diff --git a/CHANGELOG.md b/CHANGELOG.md index 33b2ea8..f863336 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,14 @@ its affiliates is strictly prohibited. --> # Changelog -## Version 0.0.0 +## Version 0.6.1 + +- Added changes to `examples/isaac_sim` to support Isaac Sim 2023.1.0 +- Added dockerfiles and notes to run cuRobo from a docker +- Minor cleanup of examples +- Added option to generate log with UsdHelper from URDF file (check `examples/usd_example.py`) +- Fix typos in robot sphere generation tutorial (thanks @cedricgoubard) + +## Version 0.6.0 - First version of CuRobo. diff --git a/docker/README.md b/docker/README.md index 413413c..ee53e46 100644 --- a/docker/README.md +++ b/docker/README.md @@ -10,27 +10,5 @@ its affiliates is strictly prohibited. --> # Docker Instructions -## Running docker from NGC (Recommended) -1. `sh build_user_docker.sh $UID` -2. `sh start_docker_x86.sh` will start the docker - -## Building your own docker image with CuRobo -1. Add default nvidia runtime to enable cuda compilation during docker build: - ``` - Edit/create the /etc/docker/daemon.json with content: - { - "runtimes": { - "nvidia": { - "path": "/usr/bin/nvidia-container-runtime", - "runtimeArgs": [] - } - }, - "default-runtime": "nvidia" # ADD this line (the above lines will already exist in your json file) - } - ``` -2. `sh pull_repos.sh` -3. `bash build_dev_docker.sh` -4. Change the docker image name in `user.dockerfile` -5. `sh build_user_docker.sh` -6. `sh start_docker_x86.sh` will start the docker - +Check [Docker Development](https://curobo.org/source/getting_started/5_docker_development.html) for +instructions. \ No newline at end of file diff --git a/docker/aarch64.dockerfile b/docker/aarch64.dockerfile new file mode 100644 index 0000000..b2e4b63 --- /dev/null +++ b/docker/aarch64.dockerfile @@ -0,0 +1,167 @@ +## +## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +## +## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual +## property and proprietary rights in and to this material, related +## documentation and any modifications thereto. Any use, reproduction, +## disclosure or distribution of this material and related documentation +## without an express license agreement from NVIDIA CORPORATION or +## its affiliates is strictly prohibited. +## +FROM nvcr.io/nvidia/l4t-pytorch:r35.1.0-pth1.13-py3 AS l4t_pytorch + +# Install ros components: +RUN apt-get update &&\ + apt-get install -y sudo git bash unattended-upgrades glmark2 &&\ + rm -rf /var/lib/apt/lists/* + + +# Deal with getting tons of debconf messages +# See: https://github.com/phusion/baseimage-docker/issues/58 +RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections + +# TODO: Don't hardcode timezone setting to Los_Angeles, pull from host computer +# Set timezone info +RUN apt-get update && apt-get install -y \ + tzdata \ + && rm -rf /var/lib/apt/lists/* \ + && ln -fs /usr/share/zoneinfo/America/Los_Angeles /etc/localtime \ + && echo "America/Los_Angeles" > /etc/timezone \ + && dpkg-reconfigure -f noninteractive tzdata + +# Install apt-get packages necessary for building, downloading, etc +# NOTE: Dockerfile best practices recommends having apt-get update +# and install commands in one line to avoid apt-get caching issues. +# https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#run +RUN apt-get update && apt-get install -y \ + curl \ + lsb-core \ + software-properties-common \ + wget \ + && rm -rf /var/lib/apt/lists/* + +RUN add-apt-repository -y ppa:git-core/ppa + +RUN apt-get update && apt-get install -y \ + build-essential \ + cmake \ + git \ + git-lfs \ + iputils-ping \ + make \ + openssh-server \ + openssh-client \ + libeigen3-dev \ + libssl-dev \ + python3-pip \ + python3-ipdb \ + python3-tk \ + python3-wstool \ + sudo git bash unattended-upgrades \ + apt-utils \ + terminator \ + && rm -rf /var/lib/apt/lists/* + +ARG ROS_PKG=ros_base # desktop does not work +ENV ROS_DISTRO=noetic +ENV ROS_ROOT=/opt/ros/${ROS_DISTRO} +ENV ROS_PYTHON_VERSION=3 + +ENV DEBIAN_FRONTEND=noninteractive + +WORKDIR /workspace + + +# +# add the ROS deb repo to the apt sources list +# +RUN apt-get update && \ + apt-get install -y --no-install-recommends \ + git \ + cmake \ + build-essential \ + curl \ + wget \ + gnupg2 \ + lsb-release \ + ca-certificates \ + && rm -rf /var/lib/apt/lists/* + +RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' +RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - + + +# +# install bootstrap dependencies +# +RUN apt-get update && \ + apt-get install -y --no-install-recommends \ + libpython3-dev \ + python3-rosdep \ + python3-rosinstall-generator \ + python3-vcstool \ + build-essential && \ + rosdep init && \ + rosdep update && \ + rm -rf /var/lib/apt/lists/* + + +# +# download/build the ROS source +# +RUN mkdir ros_catkin_ws && \ + cd ros_catkin_ws && \ + rosinstall_generator ${ROS_PKG} vision_msgs --rosdistro ${ROS_DISTRO} --deps --tar > ${ROS_DISTRO}-${ROS_PKG}.rosinstall && \ + mkdir src && \ + vcs import --input ${ROS_DISTRO}-${ROS_PKG}.rosinstall ./src && \ + apt-get update && \ + rosdep install --from-paths ./src --ignore-packages-from-source --rosdistro ${ROS_DISTRO} --skip-keys python3-pykdl -y && \ + python3 ./src/catkin/bin/catkin_make_isolated --install --install-space ${ROS_ROOT} -DCMAKE_BUILD_TYPE=Release && \ + rm -rf /var/lib/apt/lists/* + + +RUN pip3 install trimesh \ + numpy-quaternion \ + networkx \ + pyyaml \ + rospkg \ + rosdep \ + empy + +# warp from https://github.com/NVIDIA/warp needs to be compiled locally and then +# placed in curobo/docker/pkgs. +# Run the following from your terminal: +# cd curobo/docker && mkdir pkgs && cd pkgs && git clone https://github.com/NVIDIA/warp.git +# cd warp && python build_libs.py +# +# copy pkgs directory: +COPY pkgs /pkgs + +# install warp: +# +RUN cd /pkgs/warp && python3 build_lib.py && pip3 install . + +# install curobo: + +RUN cd /pkgs && git clone https://github.com/NVlabs/curobo.git + + +RUN cd /pkgs/curobo && pip3 install . --no-build-isolation + +# Optionally install nvblox: + +RUN apt-get update && \ + apt-get install -y libgoogle-glog-dev libgtest-dev curl libsqlite3-dev && \ + cd /usr/src/googletest && cmake . && cmake --build . --target install && \ + rm -rf /var/lib/apt/lists/* + +RUN cd /pkgs && git clone https://github.com/valtsblukis/nvblox.git && \ + cd nvblox && cd nvblox && mkdir build && cd build && \ + cmake .. -DPRE_CXX11_ABI_LINKABLE=ON && \ + make -j32 && \ + make install + +RUN cd /pkgs && git clone https://github.com/nvlabs/nvblox_torch.git && \ + cd nvblox_torch && \ + sh install.sh + diff --git a/docker/build_dev_docker.sh b/docker/build_dev_docker.sh index 66daa8e..f221fdf 100644 --- a/docker/build_dev_docker.sh +++ b/docker/build_dev_docker.sh @@ -12,21 +12,35 @@ # This script will create a dev docker. Run this script by calling `bash build_dev_docker.sh` - -# Make sure you have pulled all required repos into pkgs folder (see pull_repos.sh script) +# If you want to build a isaac sim docker, run this script with `bash build_dev_docker.sh isaac` # Check architecture to build: arch=`uname -m` -if [ ${arch} == "x86_64" ]; then +image_tag="x86" +isaac_sim_version="" + +if [ $1 == "isaac_sim_2022.2.1" ]; then + echo "Building Isaac Sim docker" + dockerfile="isaac_sim.dockerfile" + image_tag="isaac_sim_2022.2.1" + isaac_sim_version="2022.2.1" +elif [ $1 == "isaac_sim_2023.1.0" ]; then + echo "Building Isaac Sim headless docker" + dockerfile="isaac_sim.dockerfile" + image_tag="isaac_sim_2023.1.0" + isaac_sim_version="2023.1.0" +elif [ ${arch} == "x86" ]; then echo "Building for X86 Architecture" dockerfile="x86.dockerfile" + image_tag="x86" elif [ ${arch} = "aarch64" ]; then echo "Building for ARM Architecture" - dockerfile="arm64.dockerfile" + dockerfile="aarch64.dockerfile" + image_tag="aarch64" else - echo "Unknown Architecture, defaulting to " + ${arch} - dockerfile="x86.dockerfile" + echo "Unknown Architecture" + exit fi # build docker file: @@ -43,4 +57,5 @@ fi # } # echo "${dockerfile}" -docker build -t curobo_docker:latest -f ${dockerfile} . + +docker build --build-arg ISAAC_SIM_VERSION=${isaac_sim_version} -t curobo_docker:${image_tag} -f ${dockerfile} . diff --git a/docker/build_user_docker.sh b/docker/build_user_docker.sh index f3cc236..95dbe81 100644 --- a/docker/build_user_docker.sh +++ b/docker/build_user_docker.sh @@ -8,4 +8,8 @@ ## without an express license agreement from NVIDIA CORPORATION or ## its affiliates is strictly prohibited. ## -docker build --build-arg USERNAME=$USER --no-cache --build-arg USER_ID=$1 --tag curobo_user_docker:latest -f user.dockerfile . \ No newline at end of file + +echo $1 +echo $2 + +docker build --build-arg USERNAME=$USER --no-cache --build-arg USER_ID=$1 --build-arg IMAGE_TAG=$2 -f user.dockerfile --tag curobo_docker:user_$2 . \ No newline at end of file diff --git a/docker/isaac_sim.dockerfile b/docker/isaac_sim.dockerfile new file mode 100644 index 0000000..02eca42 --- /dev/null +++ b/docker/isaac_sim.dockerfile @@ -0,0 +1,247 @@ +## +## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +## +## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual +## property and proprietary rights in and to this material, related +## documentation and any modifications thereto. Any use, reproduction, +## disclosure or distribution of this material and related documentation +## without an express license agreement from NVIDIA CORPORATION or +## its affiliates is strictly prohibited. +## +ARG DEBIAN_FRONTEND=noninteractive +ARG BASE_DIST=ubuntu20.04 +ARG CUDA_VERSION=11.4.2 +ARG ISAAC_SIM_VERSION=2022.2.1 + + +FROM nvcr.io/nvidia/isaac-sim:${ISAAC_SIM_VERSION} AS isaac-sim + +FROM nvcr.io/nvidia/cudagl:${CUDA_VERSION}-devel-${BASE_DIST} + +# this does not work for 2022.2.1 +#$FROM nvcr.io/nvidia/cuda:${CUDA_VERSION}-cudnn8-devel-${BASE_DIST} + +LABEL maintainer "User Name" + +ARG VULKAN_SDK_VERSION=1.3.224.1 + + + +# Deal with getting tons of debconf messages +# See: https://github.com/phusion/baseimage-docker/issues/58 +RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections + + +# add GL if using a cuda docker instead of cudagl: +#RUN apt-get update && apt-get install -y --no-install-recommends \ +# pkg-config \ +# libglvnd-dev \ +# libgl1-mesa-dev \ +# libegl1-mesa-dev \ +# libgles2-mesa-dev && \ +# rm -rf /var/lib/apt/lists/* + +# Set timezone info +RUN apt-get update && apt-get install -y \ + tzdata \ + software-properties-common \ + && rm -rf /var/lib/apt/lists/* \ + && ln -fs /usr/share/zoneinfo/America/Los_Angeles /etc/localtime \ + && echo "America/Los_Angeles" > /etc/timezone \ + && dpkg-reconfigure -f noninteractive tzdata \ + && add-apt-repository -y ppa:git-core/ppa \ + && apt-get update && apt-get install -y \ + curl \ + lsb-core \ + wget \ + build-essential \ + cmake \ + git \ + git-lfs \ + iputils-ping \ + make \ + openssh-server \ + openssh-client \ + libeigen3-dev \ + libssl-dev \ + python3-pip \ + python3-ipdb \ + python3-tk \ + python3-wstool \ + sudo git bash unattended-upgrades \ + apt-utils \ + terminator \ + && rm -rf /var/lib/apt/lists/* + + +# https://catalog.ngc.nvidia.com/orgs/nvidia/containers/cudagl + +RUN apt-get update && apt-get install -y --no-install-recommends \ + libatomic1 \ + libegl1 \ + libglu1-mesa \ + libgomp1 \ + libsm6 \ + libxi6 \ + libxrandr2 \ + libxt6 \ + libfreetype-dev \ + libfontconfig1 \ + openssl \ + libssl1.1 \ + wget \ + vulkan-utils \ +&& apt-get -y autoremove \ +&& apt-get clean autoclean \ +&& rm -rf /var/lib/apt/lists/* + + + +# Download the Vulkan SDK and extract the headers, loaders, layers and binary utilities +RUN wget -q --show-progress \ + --progress=bar:force:noscroll \ + https://sdk.lunarg.com/sdk/download/${VULKAN_SDK_VERSION}/linux/vulkansdk-linux-x86_64-${VULKAN_SDK_VERSION}.tar.gz \ + -O /tmp/vulkansdk-linux-x86_64-${VULKAN_SDK_VERSION}.tar.gz \ + && echo "Installing Vulkan SDK ${VULKAN_SDK_VERSION}" \ + && mkdir -p /opt/vulkan \ + && tar -xf /tmp/vulkansdk-linux-x86_64-${VULKAN_SDK_VERSION}.tar.gz -C /opt/vulkan \ + && mkdir -p /usr/local/include/ && cp -ra /opt/vulkan/${VULKAN_SDK_VERSION}/x86_64/include/* /usr/local/include/ \ + && mkdir -p /usr/local/lib && cp -ra /opt/vulkan/${VULKAN_SDK_VERSION}/x86_64/lib/* /usr/local/lib/ \ + && cp -a /opt/vulkan/${VULKAN_SDK_VERSION}/x86_64/lib/libVkLayer_*.so /usr/local/lib \ + && mkdir -p /usr/local/share/vulkan/explicit_layer.d \ + && cp /opt/vulkan/${VULKAN_SDK_VERSION}/x86_64/etc/vulkan/explicit_layer.d/VkLayer_*.json /usr/local/share/vulkan/explicit_layer.d \ + && mkdir -p /usr/local/share/vulkan/registry \ + && cp -a /opt/vulkan/${VULKAN_SDK_VERSION}/x86_64/share/vulkan/registry/* /usr/local/share/vulkan/registry \ + && cp -a /opt/vulkan/${VULKAN_SDK_VERSION}/x86_64/bin/* /usr/local/bin \ + && ldconfig \ + && rm /tmp/vulkansdk-linux-x86_64-${VULKAN_SDK_VERSION}.tar.gz && rm -rf /opt/vulkan + + +# Setup the required capabilities for the container runtime +ENV NVIDIA_VISIBLE_DEVICES=all NVIDIA_DRIVER_CAPABILITIES=all + +# Open ports for live streaming +EXPOSE 47995-48012/udp \ + 47995-48012/tcp \ + 49000-49007/udp \ + 49000-49007/tcp \ + 49100/tcp \ + 8011/tcp \ + 8012/tcp \ + 8211/tcp \ + 8899/tcp \ + 8891/tcp + +ENV OMNI_SERVER http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/${ISAAC_SIM_VERSION} +# ENV OMNI_SERVER omniverse://localhost/NVIDIA/Assets/Isaac/2022.1 +# ENV OMNI_USER admin +# ENV OMNI_PASS admin +ENV MIN_DRIVER_VERSION 525.60.11 + +# Copy Isaac Sim files +COPY --from=isaac-sim /isaac-sim /isaac-sim +RUN mkdir -p /root/.nvidia-omniverse/config +COPY --from=isaac-sim /root/.nvidia-omniverse/config /root/.nvidia-omniverse/config +COPY --from=isaac-sim /etc/vulkan/icd.d/nvidia_icd.json /etc/vulkan/icd.d/nvidia_icd.json +COPY --from=isaac-sim /etc/vulkan/icd.d/nvidia_icd.json /etc/vulkan/implicit_layer.d/nvidia_layers.json + +WORKDIR /isaac-sim + + +ENV TORCH_CUDA_ARCH_LIST="7.0+PTX" + + + + +# create an alias for omniverse python +ENV omni_python='/isaac-sim/python.sh' + +RUN echo "alias omni_python='/isaac-sim/python.sh'" >> /.bashrc + +RUN $omni_python -m pip install "robometrics[evaluator] @ git+https://github.com/fishbotics/robometrics.git" + +# if you want to use a different version of curobo, create folder as docker/pkgs and put your +# version of curobo there. Then uncomment below line and comment the next line that clones from +# github + +# COPY pkgs /pkgs + +RUN mkdir /pkgs && cd /pkgs && git clone https://github.com/NVlabs/curobo.git + +RUN $omni_python -m pip install ninja wheel tomli + + +RUN cd /pkgs/curobo && $omni_python -m pip install .[dev, isaac_sim] --no-build-isolation + +# Optionally install nvblox: + +RUN apt-get update && \ + apt-get install -y curl tcl && \ + rm -rf /var/lib/apt/lists/* + + + +# install gflags and glog statically, instructions from: https://github.com/nvidia-isaac/nvblox/blob/public/docs/redistributable.md + +RUN cd /pkgs && git clone https://github.com/sqlite/sqlite.git -b version-3.39.4 && \ + cd /pkgs/sqlite && CFLAGS=-fPIC ./configure --prefix=/pkgs/sqlite/install/ && \ + make && make install + + +RUN cd /pkgs && git clone https://github.com/google/glog.git -b v0.4.0 && \ + cd glog && \ + mkdir build && cd build && \ + cmake .. -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ + -DCMAKE_INSTALL_PREFIX=/pkgs/glog/install/ \ + -DWITH_GFLAGS=OFF -DBUILD_SHARED_LIBS=OFF \ + && make -j8 && make install + + +RUN cd /pkgs && git clone https://github.com/gflags/gflags.git -b v2.2.2 && \ + cd gflags && \ + mkdir build && cd build && \ + cmake .. -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ + -DCMAKE_INSTALL_PREFIX=/pkgs/gflags/install/ \ + -DGFLAGS_BUILD_STATIC_LIBS=ON -DGFLAGS=google \ + && make -j8 && make install + +RUN cd /pkgs && git clone https://github.com/google/googletest.git -b v1.14.0 && \ + cd googletest && mkdir build && cd build && cmake .. && make -j8 && make install + +RUN cd /pkgs && git clone https://github.com/valtsblukis/nvblox.git + +RUN cd /pkgs/nvblox/nvblox && mkdir build && cd build && \ + cmake .. -DPRE_CXX11_ABI_LINKABLE=ON -DBUILD_REDISTRIBUTABLE=ON -DSQLITE3_BASE_PATH="/pkgs/sqlite/install/" -DGLOG_BASE_PATH="/pkgs/glog/install/" -DGFLAGS_BASE_PATH="/pkgs/gflags/install/" && \ + make -j32 && \ + make install + +# install newer cmake and glog for pytorch: +# TODO: use libgoogle from source that was compiled instead. + +RUN apt-get update && \ + apt-get install -y libgoogle-glog-dev && \ + rm -rf /var/lib/apt/lists/* + +RUN cd /pkgs && wget https://cmake.org/files/v3.19/cmake-3.19.5.tar.gz && \ + tar -xvzf cmake-3.19.5.tar.gz && \ + apt update && apt install -y build-essential checkinstall zlib1g-dev libssl-dev && \ + cd cmake-3.19.5 && ./bootstrap && \ + make -j8 && \ + make install && rm -rf /var/lib/apt/lists/* + + +ENV cudnn_version=8.2.4.15 +ENV cuda_version=cuda11.4 +RUN apt update && apt-get install -y libcudnn8=${cudnn_version}-1+${cuda_version} libcudnn8-dev=${cudnn_version}-1+${cuda_version} && \ + rm -rf /var/lib/apt/lists/* + + +RUN cd /pkgs && git clone https://github.com/nvlabs/nvblox_torch.git && \ + cd /pkgs/nvblox_torch && \ + sh install_isaac_sim.sh $($omni_python -c 'import torch.utils; print(torch.utils.cmake_prefix_path)') && \ + $omni_python -m pip install -e . + +# install realsense for nvblox demos: +RUN $omni_python -m pip install pyrealsense2 opencv-python transforms3d + + diff --git a/docker/start_dev_docker.sh b/docker/start_dev_docker.sh new file mode 100644 index 0000000..6ea0fdf --- /dev/null +++ b/docker/start_dev_docker.sh @@ -0,0 +1,49 @@ +#!/bin/bash +## +## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +## +## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual +## property and proprietary rights in and to this material, related +## documentation and any modifications thereto. Any use, reproduction, +## disclosure or distribution of this material and related documentation +## without an express license agreement from NVIDIA CORPORATION or +## its affiliates is strictly prohibited. +## + + +if [ $1 == "x86" ]; then + + docker run --rm -it \ + --privileged \ + -e NVIDIA_DISABLE_REQUIRE=1 \ + -e NVIDIA_DRIVER_CAPABILITIES=all --device /dev/dri \ + --mount type=bind,src=/home/$USER/code,target=/home/$USER/code \ + --hostname ros1-docker \ + --add-host ros1-docker:127.0.0.1 \ + --gpus all \ + --network host \ + --env DISPLAY=unix$DISPLAY \ + --volume /tmp/.X11-unix:/tmp/.X11-unix \ + --volume /dev:/dev \ + curobo_docker:user_$1 + +elif [ $1 == "aarch64" ]; then + + docker run --rm -it \ + --runtime nvidia \ + --hostname ros1-docker \ + --add-host ros1-docker:127.0.0.1 \ + --network host \ + --gpus all \ + --env ROS_HOSTNAME=localhost \ + --env DISPLAY=$DISPLAY \ + --volume /tmp/.X11-unix:/tmp/.X11-unix \ + --volume /dev/input:/dev/input \ + --mount type=bind,src=/home/$USER/code,target=/home/$USER/code \ + curobo_docker:user_$1 + +elif [[ $1 == *isaac_sim* ]] ; then + echo "Isaac Sim Dev Docker is not supported" +else + echo "Unknown docker" +fi diff --git a/docker/start_docker.sh b/docker/start_docker.sh new file mode 100644 index 0000000..83ed94e --- /dev/null +++ b/docker/start_docker.sh @@ -0,0 +1,62 @@ +#!/bin/bash +## +## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +## +## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual +## property and proprietary rights in and to this material, related +## documentation and any modifications thereto. Any use, reproduction, +## disclosure or distribution of this material and related documentation +## without an express license agreement from NVIDIA CORPORATION or +## its affiliates is strictly prohibited. +## + +if [ $1 == "x86" ]; then + + docker run --rm -it \ + --privileged \ + -e NVIDIA_DISABLE_REQUIRE=1 \ + -e NVIDIA_DRIVER_CAPABILITIES=all --device /dev/dri \ + --hostname ros1-docker \ + --add-host ros1-docker:127.0.0.1 \ + --gpus all \ + --network host \ + --env DISPLAY=unix$DISPLAY \ + --volume /tmp/.X11-unix:/tmp/.X11-unix \ + --volume /dev:/dev \ + curobo_docker:$1 + +elif [ $1 == "aarch64" ]; then + + docker run --rm -it \ + --runtime nvidia \ + --hostname ros1-docker \ + --add-host ros1-docker:127.0.0.1 \ + --network host \ + --gpus all \ + --env ROS_HOSTNAME=localhost \ + --env DISPLAY=$DISPLAY \ + --volume /tmp/.X11-unix:/tmp/.X11-unix \ + --volume /dev/input:/dev/input \ + curobo_docker:$1 + +elif [[ $1 == *isaac_sim* ]] ; then + + docker run --name container_$1 --entrypoint bash -it --gpus all -e "ACCEPT_EULA=Y" --rm --network=host \ + --privileged \ + -e "PRIVACY_CONSENT=Y" \ + -v $HOME/.Xauthority:/root/.Xauthority \ + -e DISPLAY \ + -v ~/docker/isaac-sim/cache/kit:/isaac-sim/kit/cache:rw \ + -v ~/docker/isaac-sim/cache/ov:/root/.cache/ov:rw \ + -v ~/docker/isaac-sim/cache/pip:/root/.cache/pip:rw \ + -v ~/docker/isaac-sim/cache/glcache:/root/.cache/nvidia/GLCache:rw \ + -v ~/docker/isaac-sim/cache/computecache:/root/.nv/ComputeCache:rw \ + -v ~/docker/isaac-sim/logs:/root/.nvidia-omniverse/logs:rw \ + -v ~/docker/isaac-sim/data:/root/.local/share/ov/data:rw \ + -v ~/docker/isaac-sim/documents:/root/Documents:rw \ + --volume /dev:/dev \ + curobo_docker:$1 + +else + echo "Unknown docker" +fi \ No newline at end of file diff --git a/docker/start_docker_aarch64.sh b/docker/start_docker_aarch64.sh new file mode 100644 index 0000000..c4b5810 --- /dev/null +++ b/docker/start_docker_aarch64.sh @@ -0,0 +1,22 @@ +## +## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +## +## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual +## property and proprietary rights in and to this material, related +## documentation and any modifications thereto. Any use, reproduction, +## disclosure or distribution of this material and related documentation +## without an express license agreement from NVIDIA CORPORATION or +## its affiliates is strictly prohibited. +## +docker run --rm -it \ +--runtime nvidia \ +--mount type=bind,src=/home/$USER/code,target=/home/$USER/code \ +--hostname ros1-docker \ +--add-host ros1-docker:127.0.0.1 \ +--network host \ +--gpus all \ +--env ROS_HOSTNAME=localhost \ +--env DISPLAY=$DISPLAY \ +--volume /tmp/.X11-unix:/tmp/.X11-unix \ +--volume /dev/input:/dev/input \ +curobo_docker:aarch64 diff --git a/docker/start_docker_isaac_sim.sh b/docker/start_docker_isaac_sim.sh new file mode 100644 index 0000000..c98ae94 --- /dev/null +++ b/docker/start_docker_isaac_sim.sh @@ -0,0 +1,27 @@ +## +## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +## +## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual +## property and proprietary rights in and to this material, related +## documentation and any modifications thereto. Any use, reproduction, +## disclosure or distribution of this material and related documentation +## without an express license agreement from NVIDIA CORPORATION or +## its affiliates is strictly prohibited. +## + + +docker run --name container_$1 --entrypoint bash -it --gpus all -e "ACCEPT_EULA=Y" --rm --network=host \ + --privileged \ + -e "PRIVACY_CONSENT=Y" \ + -v $HOME/.Xauthority:/root/.Xauthority \ + -e DISPLAY \ + -v ~/docker/isaac-sim/cache/kit:/isaac-sim/kit/cache:rw \ + -v ~/docker/isaac-sim/cache/ov:/root/.cache/ov:rw \ + -v ~/docker/isaac-sim/cache/pip:/root/.cache/pip:rw \ + -v ~/docker/isaac-sim/cache/glcache:/root/.cache/nvidia/GLCache:rw \ + -v ~/docker/isaac-sim/cache/computecache:/root/.nv/ComputeCache:rw \ + -v ~/docker/isaac-sim/logs:/root/.nvidia-omniverse/logs:rw \ + -v ~/docker/isaac-sim/data:/root/.local/share/ov/data:rw \ + -v ~/docker/isaac-sim/documents:/root/Documents:rw \ + --volume /dev:/dev \ + curobo_docker:$1 diff --git a/docker/start_docker_isaac_sim_headless.sh b/docker/start_docker_isaac_sim_headless.sh new file mode 100644 index 0000000..166f778 --- /dev/null +++ b/docker/start_docker_isaac_sim_headless.sh @@ -0,0 +1,22 @@ +## +## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +## +## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual +## property and proprietary rights in and to this material, related +## documentation and any modifications thereto. Any use, reproduction, +## disclosure or distribution of this material and related documentation +## without an express license agreement from NVIDIA CORPORATION or +## its affiliates is strictly prohibited. +## + +docker run --name container_$1 --entrypoint bash -it --gpus all -e "ACCEPT_EULA=Y" --rm --network=host \ + -e "PRIVACY_CONSENT=Y" \ + -v ~/docker/isaac-sim/cache/kit:/isaac-sim/kit/cache:rw \ + -v ~/docker/isaac-sim/cache/ov:/root/.cache/ov:rw \ + -v ~/docker/isaac-sim/cache/pip:/root/.cache/pip:rw \ + -v ~/docker/isaac-sim/cache/glcache:/root/.cache/nvidia/GLCache:rw \ + -v ~/docker/isaac-sim/cache/computecache:/root/.nv/ComputeCache:rw \ + -v ~/docker/isaac-sim/logs:/root/.nvidia-omniverse/logs:rw \ + -v ~/docker/isaac-sim/data:/root/.local/share/ov/data:rw \ + -v ~/docker/isaac-sim/documents:/root/Documents:rw \ + curobo_docker:$1 diff --git a/docker/start_docker_x86.sh b/docker/start_docker_x86.sh index 3400e97..e12a26c 100644 --- a/docker/start_docker_x86.sh +++ b/docker/start_docker_x86.sh @@ -8,8 +8,9 @@ ## without an express license agreement from NVIDIA CORPORATION or ## its affiliates is strictly prohibited. ## + docker run --rm -it \ ---privileged --mount type=bind,src=/home/$USER/code,target=/home/$USER/code \ +--privileged \ -e NVIDIA_DISABLE_REQUIRE=1 \ -e NVIDIA_DRIVER_CAPABILITIES=all --device /dev/dri \ --hostname ros1-docker \ @@ -19,4 +20,4 @@ docker run --rm -it \ --env DISPLAY=unix$DISPLAY \ --volume /tmp/.X11-unix:/tmp/.X11-unix \ --volume /dev:/dev \ -curobo_user_docker:latest +curobo_docker:x86 diff --git a/docker/user.dockerfile b/docker/user.dockerfile index 6ab23a1..ad17095 100644 --- a/docker/user.dockerfile +++ b/docker/user.dockerfile @@ -10,7 +10,8 @@ ## # Check architecture and load: -FROM curobo_docker:latest +ARG IMAGE_TAG +FROM curobo_docker:${IMAGE_TAG} # Set variables ARG USERNAME ARG USER_ID @@ -24,11 +25,13 @@ RUN useradd -l -u $USER_ID -g users $USERNAME RUN /sbin/adduser $USERNAME sudo RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers + # Set user USER $USERNAME WORKDIR /home/$USERNAME ENV USER=$USERNAME ENV PATH="${PATH}:/home/${USER}/.local/bin" + RUN echo 'completed' diff --git a/docker/x86.dockerfile b/docker/x86.dockerfile index c35289d..54f07af 100644 --- a/docker/x86.dockerfile +++ b/docker/x86.dockerfile @@ -12,6 +12,11 @@ FROM nvcr.io/nvidia/pytorch:23.08-py3 AS torch_cuda_base LABEL maintainer "User Name" + +# Deal with getting tons of debconf messages +# See: https://github.com/phusion/baseimage-docker/issues/58 +RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections + # add GL: RUN apt-get update && apt-get install -y --no-install-recommends \ pkg-config \ @@ -24,16 +29,6 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ ENV NVIDIA_VISIBLE_DEVICES all ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute - -RUN apt-get update &&\ - apt-get install -y sudo git bash unattended-upgrades glmark2 &&\ - rm -rf /var/lib/apt/lists/* - - -# Deal with getting tons of debconf messages -# See: https://github.com/phusion/baseimage-docker/issues/58 -RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections - # Set timezone info RUN apt-get update && apt-get install -y \ tzdata \ @@ -42,9 +37,8 @@ RUN apt-get update && apt-get install -y \ && ln -fs /usr/share/zoneinfo/America/Los_Angeles /etc/localtime \ && echo "America/Los_Angeles" > /etc/timezone \ && dpkg-reconfigure -f noninteractive tzdata \ - && add-apt-repository -y ppa:git-core/ppa - -RUN apt-get update && apt-get install -y \ + && add-apt-repository -y ppa:git-core/ppa \ + && apt-get update && apt-get install -y \ curl \ lsb-core \ wget \ @@ -65,6 +59,7 @@ RUN apt-get update && apt-get install -y \ sudo git bash unattended-upgrades \ apt-utils \ terminator \ + glmark2 \ && rm -rf /var/lib/apt/lists/* # push defaults to bashrc: @@ -83,11 +78,43 @@ ENV TORCH_CUDA_ARCH_LIST "7.0+PTX" ENV LD_LIBRARY_PATH="/usr/local/lib:${LD_LIBRARY_PATH}" -# copy pkgs directory: clone curobo into docker/pkgs folder. -COPY pkgs /pkgs - RUN pip install "robometrics[evaluator] @ git+https://github.com/fishbotics/robometrics.git" +# if you want to use a different version of curobo, create folder as docker/pkgs and put your +# version of curobo there. Then uncomment below line and comment the next line that clones from +# github + +# COPY pkgs /pkgs + +RUN mkdir /pkgs && cd /pkgs && git clone https://github.com/NVlabs/curobo.git RUN cd /pkgs/curobo && pip3 install .[dev,usd] --no-build-isolation +WORKDIR /pkgs/curobo + +# Optionally install nvblox: + +# we require this environment variable to render images in unit test curobo/tests/nvblox_test.py + +ENV PYOPENGL_PLATFORM=egl + +# add this file to enable EGL for rendering + +RUN echo '{"file_format_version": "1.0.0", "ICD": {"library_path": "libEGL_nvidia.so.0"}}' >> /usr/share/glvnd/egl_vendor.d/10_nvidia.json + +RUN apt-get update && \ + apt-get install -y libgoogle-glog-dev libgtest-dev curl libsqlite3-dev && \ + cd /usr/src/googletest && cmake . && cmake --build . --target install && \ + rm -rf /var/lib/apt/lists/* + +RUN cd /pkgs && git clone https://github.com/valtsblukis/nvblox.git && \ + cd nvblox && cd nvblox && mkdir build && cd build && \ + cmake .. -DPRE_CXX11_ABI_LINKABLE=ON && \ + make -j32 && \ + make install + +RUN cd /pkgs && git clone https://github.com/nvlabs/nvblox_torch.git && \ + cd nvblox_torch && \ + sh install.sh + +RUN python -m pip install pyrealsense2 opencv-python transforms3d diff --git a/examples/isaac_sim/batch_collision_checker.py b/examples/isaac_sim/batch_collision_checker.py index 619982a..c377f8f 100644 --- a/examples/isaac_sim/batch_collision_checker.py +++ b/examples/isaac_sim/batch_collision_checker.py @@ -17,51 +17,48 @@ a = torch.zeros(4, device="cuda:0") # Standard Library +# Standard Library +import argparse + # Third Party from omni.isaac.kit import SimulationApp -# CuRobo -# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig -from curobo.geom.sdf.world import CollisionCheckerType -from curobo.geom.types import WorldConfig -from curobo.types.base import TensorDeviceType -from curobo.types.math import Pose -from curobo.util_file import get_world_configs_path, join_path, load_yaml -from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig +parser = argparse.ArgumentParser() + +parser.add_argument( + "--headless_mode", + type=str, + default=None, + help="To run headless, use one of [native, websocket], webrtc might not work.", +) +args = parser.parse_args() simulation_app = SimulationApp( { - "headless": False, + "headless": args.headless_mode is not None, "width": "1920", "height": "1080", } ) -# Third Party -from omni.isaac.core.utils.extensions import enable_extension - -ext_list = [ - "omni.kit.asset_converter", - # "omni.kit.livestream.native", - "omni.kit.tool.asset_importer", - "omni.isaac.asset_browser", -] -[enable_extension(x) for x in ext_list] - - # Third Party import carb import numpy as np +from helper import add_extensions from omni.isaac.core import World from omni.isaac.core.materials import OmniPBR -from omni.isaac.core.objects import cuboid, sphere - -########### OV ################# -from omni.isaac.core.utils.extensions import enable_extension -from omni.isaac.core.utils.types import ArticulationAction +from omni.isaac.core.objects import sphere # CuRobo +# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig +from curobo.geom.types import WorldConfig +from curobo.types.base import TensorDeviceType +from curobo.types.math import Pose from curobo.util.logger import setup_curobo_logger from curobo.util.usd_helper import UsdHelper +from curobo.util_file import get_world_configs_path, join_path, load_yaml +from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig + +########### OV ################# def main(): @@ -79,10 +76,7 @@ def main(): stage.SetDefaultPrim(xform) stage.DefinePrim("/curobo", "Xform") - # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World")) stage = my_world.stage - # stage.SetDefaultPrim(stage.GetPrimAtPath("/World")) - # Make a target to follow target_list = [] target_material_list = [] offset_x = 3.5 @@ -131,6 +125,7 @@ def main(): x_sph = torch.zeros((n_envs, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype) x_sph[..., 3] = radius env_query_idx = torch.arange(n_envs, device=tensor_args.device, dtype=torch.int32) + add_extensions(simulation_app, args.headless_mode) while simulation_app.is_running(): my_world.step(render=True) if not my_world.is_playing(): diff --git a/examples/isaac_sim/batch_motion_gen_reacher.py b/examples/isaac_sim/batch_motion_gen_reacher.py index 02299e7..ed0f401 100644 --- a/examples/isaac_sim/batch_motion_gen_reacher.py +++ b/examples/isaac_sim/batch_motion_gen_reacher.py @@ -17,62 +17,18 @@ a = torch.zeros(4, device="cuda:0") # Standard Library -# Third Party -from omni.isaac.kit import SimulationApp - -# CuRobo -# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig -from curobo.geom.sdf.world import CollisionCheckerType -from curobo.geom.types import WorldConfig -from curobo.types.base import TensorDeviceType -from curobo.types.math import Pose -from curobo.types.state import JointState -from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml -from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig -from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig - -simulation_app = SimulationApp( - { - "headless": False, - "width": "1920", - "height": "1080", - } -) -# Third Party -from omni.isaac.core.utils.extensions import enable_extension - -ext_list = [ - "omni.kit.asset_converter", - # "omni.kit.livestream.native", - "omni.kit.tool.asset_importer", - "omni.isaac.asset_browser", -] -[enable_extension(x) for x in ext_list] - - # Standard Library import argparse -# Third Party -import carb -import numpy as np -from helper import add_robot_to_scene -from omni.isaac.core import World -from omni.isaac.core.materials import OmniPBR -from omni.isaac.core.objects import cuboid, sphere - -########### OV ################# -from omni.isaac.core.utils.extensions import enable_extension -from omni.isaac.core.utils.types import ArticulationAction - -# CuRobo -from curobo.util.logger import setup_curobo_logger -from curobo.util.usd_helper import UsdHelper - parser = argparse.ArgumentParser() + parser.add_argument( - "--headless", action="store_true", help="When True, enables headless mode", default=False + "--headless_mode", + type=str, + default=None, + help="To run headless, use one of [native, websocket], webrtc might not work.", ) + parser.add_argument( "--visualize_spheres", action="store_true", @@ -83,6 +39,37 @@ parser.add_argument( parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load") args = parser.parse_args() +simulation_app = SimulationApp( + { + "headless": args.headless_mode is not None, + "width": "1920", + "height": "1080", + } +) +# Third Party +import carb +import numpy as np +from helper import add_extensions, add_robot_to_scene +from omni.isaac.core import World +from omni.isaac.core.objects import cuboid + +########### OV ################# +from omni.isaac.core.utils.types import ArticulationAction +from omni.isaac.kit import SimulationApp + +# CuRobo +# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig +from curobo.geom.sdf.world import CollisionCheckerType +from curobo.geom.types import WorldConfig +from curobo.types.base import TensorDeviceType +from curobo.types.math import Pose +from curobo.types.state import JointState +from curobo.util.logger import setup_curobo_logger +from curobo.util.usd_helper import UsdHelper +from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml +from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig +from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig + def main(): usd_help = UsdHelper() @@ -180,6 +167,7 @@ def main(): enable_graph=False, warmup_js_trajopt=False, batch=n_envs, batch_env_mode=True ) + add_extensions(simulation_app, args.headless_mode) config = RobotWorldConfig.load_from_config( robot_file, world_cfg_list, collision_activation_distance=act_distance ) diff --git a/examples/isaac_sim/collision_checker.py b/examples/isaac_sim/collision_checker.py index 96dec2e..d6a2b98 100644 --- a/examples/isaac_sim/collision_checker.py +++ b/examples/isaac_sim/collision_checker.py @@ -17,60 +17,52 @@ a = torch.zeros(4, device="cuda:0") # Standard Library +# Standard Library +import argparse + # Third Party from omni.isaac.kit import SimulationApp +parser = argparse.ArgumentParser() +parser.add_argument( + "--nvblox", action="store_true", help="When True, enables headless mode", default=False +) + +parser.add_argument( + "--headless_mode", + type=str, + default=None, + help="To run headless, use one of [native, websocket], webrtc might not work.", +) +args = parser.parse_args() + +simulation_app = SimulationApp( + { + "headless": args.headless_mode is not None, + "width": "1920", + "height": "1080", + } +) +# Third Party +import carb +import numpy as np +from helper import add_extensions +from omni.isaac.core import World +from omni.isaac.core.materials import OmniPBR +from omni.isaac.core.objects import sphere + # CuRobo # from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig from curobo.geom.sdf.world import CollisionCheckerType from curobo.geom.types import WorldConfig from curobo.types.base import TensorDeviceType from curobo.types.math import Pose -from curobo.util_file import get_world_configs_path, join_path, load_yaml -from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig - -simulation_app = SimulationApp( - { - "headless": False, - "width": "1920", - "height": "1080", - } -) -# Third Party -from omni.isaac.core.utils.extensions import enable_extension - -ext_list = [ - "omni.kit.asset_converter", - # "omni.kit.livestream.native", - "omni.kit.tool.asset_importer", - "omni.isaac.asset_browser", -] -[enable_extension(x) for x in ext_list] - - -# Standard Library -import argparse - -# Third Party -import carb -import numpy as np -from omni.isaac.core import World -from omni.isaac.core.materials import OmniPBR -from omni.isaac.core.objects import cuboid, sphere ########### OV ################# -from omni.isaac.core.utils.extensions import enable_extension -from omni.isaac.core.utils.types import ArticulationAction - -# CuRobo from curobo.util.logger import setup_curobo_logger from curobo.util.usd_helper import UsdHelper - -parser = argparse.ArgumentParser() -parser.add_argument( - "--nvblox", action="store_true", help="When True, enables headless mode", default=False -) -args = parser.parse_args() +from curobo.util_file import get_world_configs_path, join_path, load_yaml +from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig def draw_line(start, gradient): @@ -154,6 +146,7 @@ def main(): x_sph = torch.zeros((1, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype) x_sph[..., 3] = radius + add_extensions(simulation_app, args.headless_mode) while simulation_app.is_running(): my_world.step(render=True) if not my_world.is_playing(): diff --git a/examples/isaac_sim/helper.py b/examples/isaac_sim/helper.py index a1eb9a7..d778378 100644 --- a/examples/isaac_sim/helper.py +++ b/examples/isaac_sim/helper.py @@ -20,15 +20,43 @@ from omni.isaac.core.materials import OmniPBR from omni.isaac.core.objects import cuboid from omni.isaac.core.robots import Robot +# CuRobo +from curobo.util.logger import log_warn + +ISAAC_SIM_23 = False try: # Third Party from omni.isaac.urdf import _urdf # isaacsim 2022.2 except ImportError: + # Third Party from omni.importer.urdf import _urdf # isaac sim 2023.1 + + ISAAC_SIM_23 = True +# Standard Library +from typing import Optional + +# Third Party +from omni.isaac.core.utils.extensions import enable_extension + # CuRobo from curobo.util_file import get_assets_path, get_filename, get_path_of_dir, join_path +def add_extensions(simulation_app, headless_mode: Optional[str] = None): + ext_list = [ + "omni.kit.asset_converter", + "omni.kit.tool.asset_importer", + "omni.isaac.asset_browser", + ] + if headless_mode is not None: + log_warn("Running in headless mode: " + headless_mode) + ext_list += ["omni.kit.livestream." + headless_mode] + [enable_extension(x) for x in ext_list] + simulation_app.update() + + return True + + ############################################################ def add_robot_to_scene( robot_config: Dict, @@ -72,14 +100,18 @@ def add_robot_to_scene( # print(prim_path) # robot_prim = my_world.scene.stage.OverridePrim(prim_path) # robot_prim.GetReferences().AddReference(dest_path) - - robot = my_world.scene.add( - Robot( - prim_path=robot_path, - name=robot_name, - position=position, - ) + robot_p = Robot( + prim_path=robot_path, + name=robot_name, + position=position, ) + if ISAAC_SIM_23: + robot_p.set_solver_velocity_iteration_count(4) + robot_p.set_solver_position_iteration_count(44) + + my_world._physics_context.set_solver_type("PGS") + + robot = my_world.scene.add(robot_p) return robot, robot_path diff --git a/examples/isaac_sim/ik_reachability.py b/examples/isaac_sim/ik_reachability.py index 6da6c62..eb45ee5 100644 --- a/examples/isaac_sim/ik_reachability.py +++ b/examples/isaac_sim/ik_reachability.py @@ -17,25 +17,12 @@ a = torch.zeros(4, device="cuda:0") # Standard Library import argparse -# CuRobo -from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel - -# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig -from curobo.geom.sdf.world import CollisionCheckerType -from curobo.geom.types import WorldConfig -from curobo.rollout.rollout_base import Goal -from curobo.types.base import TensorDeviceType -from curobo.types.math import Pose -from curobo.types.robot import JointState, RobotConfig -from curobo.types.state import JointState -from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml -from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig -from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig -from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig - parser = argparse.ArgumentParser() parser.add_argument( - "--headless", action="store_true", help="When True, enables headless mode", default=False + "--headless_mode", + type=str, + default=None, + help="To run headless, use one of [native, websocket], webrtc might not work.", ) parser.add_argument( "--visualize_spheres", @@ -54,52 +41,49 @@ from omni.isaac.kit import SimulationApp simulation_app = SimulationApp( { - "headless": args.headless, + "headless": args.headless_mode is not None, "width": "1920", "height": "1080", } ) -# Third Party -from omni.isaac.core.utils.extensions import enable_extension - -# CuRobo -from curobo.util_file import ( - get_assets_path, - get_filename, - get_path_of_dir, - get_robot_configs_path, - join_path, - load_yaml, -) - -ext_list = [ - "omni.kit.asset_converter", - # "omni.kit.livestream.native", - "omni.kit.tool.asset_importer", - "omni.isaac.asset_browser", - "omni.isaac.urdf", -] -[enable_extension(x) for x in ext_list] -simulation_app.update() - # Standard Library from typing import Dict # Third Party import carb import numpy as np -from helper import add_robot_to_scene +from helper import add_extensions, add_robot_to_scene from omni.isaac.core import World from omni.isaac.core.objects import cuboid, sphere -from omni.isaac.core.robots import Robot - -########### OV ################# -from omni.isaac.core.utils.extensions import enable_extension -from omni.isaac.core.utils.types import ArticulationAction # CuRobo +from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel + +# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig +from curobo.geom.sdf.world import CollisionCheckerType +from curobo.geom.types import WorldConfig +from curobo.rollout.rollout_base import Goal +from curobo.types.base import TensorDeviceType +from curobo.types.math import Pose +from curobo.types.robot import JointState, RobotConfig +from curobo.types.state import JointState from curobo.util.logger import setup_curobo_logger from curobo.util.usd_helper import UsdHelper +from curobo.util_file import ( + get_assets_path, + get_filename, + get_path_of_dir, + get_robot_configs_path, + get_world_configs_path, + join_path, + load_yaml, +) +from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig +from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig +from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig + +########### OV ################# + ############################################################ @@ -225,6 +209,7 @@ def main(): result = ik_solver.solve_batch(goal_pose) print("Curobo is Ready") + add_extensions(simulation_app, args.headless_mode) usd_help.load_stage(my_world.stage) usd_help.add_world_to_stage(world_cfg, base_frame="/World") @@ -246,7 +231,7 @@ def main(): step_index = my_world.current_time_step_index # print(step_index) - if step_index == 0: + if step_index <= 2: my_world.reset() idx_list = [robot.get_dof_index(x) for x in j_names] robot.set_joint_positions(default_config, idx_list) diff --git a/examples/isaac_sim/load_all_robots.py b/examples/isaac_sim/load_all_robots.py index aaf0c0c..d12812a 100644 --- a/examples/isaac_sim/load_all_robots.py +++ b/examples/isaac_sim/load_all_robots.py @@ -22,30 +22,13 @@ import argparse # Third Party from omni.isaac.kit import SimulationApp -# CuRobo -from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelConfig - -# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig -from curobo.geom.sdf.world import CollisionCheckerType -from curobo.geom.types import WorldConfig -from curobo.types.base import TensorDeviceType -from curobo.types.math import Pose -from curobo.types.robot import RobotConfig -from curobo.types.state import JointState -from curobo.util_file import ( - get_motion_gen_robot_list, - get_robot_configs_path, - get_robot_path, - get_world_configs_path, - join_path, - load_yaml, -) -from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig -from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig - parser = argparse.ArgumentParser() + parser.add_argument( - "--headless", action="store_true", help="When True, enables headless mode", default=False + "--headless_mode", + type=str, + default=None, + help="To run headless, use one of [native, websocket], webrtc might not work.", ) parser.add_argument( "--visualize_spheres", @@ -57,38 +40,31 @@ parser.add_argument( args = parser.parse_args() simulation_app = SimulationApp( { - "headless": args.headless, + "headless": args.headless_mode is not None, "width": "1920", "height": "1080", } ) -# Third Party -from omni.isaac.core.utils.extensions import enable_extension - -ext_list = [ - "omni.kit.asset_converter", - # "omni.kit.livestream.native", - "omni.kit.tool.asset_importer", - "omni.isaac.asset_browser", -] -[enable_extension(x) for x in ext_list] - # Third Party import carb import numpy as np -from helper import add_robot_to_scene +from helper import add_extensions, add_robot_to_scene from omni.isaac.core import World -from omni.isaac.core.materials import OmniPBR -from omni.isaac.core.objects import cuboid, sphere - -########### OV ################# -from omni.isaac.core.utils.extensions import enable_extension -from omni.isaac.core.utils.types import ArticulationAction +from omni.isaac.core.objects import sphere # CuRobo +from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel + +# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig +from curobo.types.base import TensorDeviceType +from curobo.types.math import Pose +from curobo.types.robot import RobotConfig + +########### OV ################# from curobo.util.logger import setup_curobo_logger from curobo.util.usd_helper import UsdHelper +from curobo.util_file import get_motion_gen_robot_list, get_robot_configs_path, join_path, load_yaml def main(): @@ -153,6 +129,7 @@ def main(): setup_curobo_logger("warn") + add_extensions(simulation_app, args.headless_mode) while simulation_app.is_running(): my_world.step(render=True) @@ -163,7 +140,7 @@ def main(): continue step_index = my_world.current_time_step_index - if step_index == 0: + if step_index <= 2: my_world.reset() for ri, robot in enumerate(robot_list): j_names = robot_cfg_list[ri]["kinematics"]["cspace"]["joint_names"] diff --git a/examples/isaac_sim/motion_gen_reacher.py b/examples/isaac_sim/motion_gen_reacher.py index 9dbdf8d..51f5081 100644 --- a/examples/isaac_sim/motion_gen_reacher.py +++ b/examples/isaac_sim/motion_gen_reacher.py @@ -18,23 +18,15 @@ a = torch.zeros(4, device="cuda:0") # Standard Library import argparse -# CuRobo -from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel - -# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig -from curobo.geom.sdf.world import CollisionCheckerType -from curobo.geom.types import WorldConfig -from curobo.types.base import TensorDeviceType -from curobo.types.math import Pose -from curobo.types.robot import JointState, RobotConfig -from curobo.types.state import JointState -from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml -from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig - parser = argparse.ArgumentParser() parser.add_argument( - "--headless", action="store_true", help="When True, enables headless mode", default=False + "--headless_mode", + type=str, + default=None, + help="To run headless, use one of [native, websocket], webrtc might not work.", ) +parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load") + parser.add_argument( "--visualize_spheres", action="store_true", @@ -42,7 +34,6 @@ parser.add_argument( default=False, ) -parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load") args = parser.parse_args() ############################################################ @@ -52,51 +43,44 @@ from omni.isaac.kit import SimulationApp simulation_app = SimulationApp( { - "headless": args.headless, + "headless": args.headless_mode is not None, "width": "1920", "height": "1080", } ) -# Third Party -from omni.isaac.core.utils.extensions import enable_extension - -# CuRobo -from curobo.util_file import ( - get_assets_path, - get_filename, - get_path_of_dir, - get_robot_configs_path, - join_path, - load_yaml, -) - -ext_list = [ - "omni.kit.asset_converter", - # "omni.kit.livestream.native", - "omni.kit.tool.asset_importer", - "omni.isaac.asset_browser", -] -[enable_extension(x) for x in ext_list] -# simulation_app.update() - # Standard Library from typing import Dict # Third Party import carb import numpy as np -from helper import add_robot_to_scene +from helper import add_extensions, add_robot_to_scene from omni.isaac.core import World from omni.isaac.core.objects import cuboid, sphere -from omni.isaac.core.robots import Robot ########### OV ################# -from omni.isaac.core.utils.extensions import enable_extension from omni.isaac.core.utils.types import ArticulationAction # CuRobo +# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig +from curobo.geom.sdf.world import CollisionCheckerType +from curobo.geom.types import WorldConfig +from curobo.types.base import TensorDeviceType +from curobo.types.math import Pose +from curobo.types.robot import JointState +from curobo.types.state import JointState from curobo.util.logger import setup_curobo_logger from curobo.util.usd_helper import UsdHelper +from curobo.util_file import ( + get_assets_path, + get_filename, + get_path_of_dir, + get_robot_configs_path, + get_world_configs_path, + join_path, + load_yaml, +) +from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig ############################################################ @@ -105,6 +89,8 @@ from curobo.util.usd_helper import UsdHelper def main(): + # create a curobo motion gen instance: + # assuming obstacles are in objects_path: my_world = World(stage_units_in_meters=1.0) stage = my_world.stage @@ -181,6 +167,9 @@ def main(): motion_gen.warmup(enable_graph=False, warmup_js_trajopt=False) print("Curobo is Ready") + + add_extensions(simulation_app, args.headless_mode) + plan_config = MotionGenPlanConfig( enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True ) diff --git a/examples/isaac_sim/motion_gen_reacher_nvblox.py b/examples/isaac_sim/motion_gen_reacher_nvblox.py index 81cc207..8afc57d 100644 --- a/examples/isaac_sim/motion_gen_reacher_nvblox.py +++ b/examples/isaac_sim/motion_gen_reacher_nvblox.py @@ -18,21 +18,13 @@ a = torch.zeros(4, device="cuda:0") # Standard Library import argparse -# CuRobo -# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig -from curobo.geom.sdf.world import CollisionCheckerType -from curobo.geom.types import WorldConfig -from curobo.types.base import TensorDeviceType -from curobo.types.math import Pose -from curobo.types.robot import JointState, RobotConfig -from curobo.types.state import JointState -from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml -from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig -from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig - parser = argparse.ArgumentParser() + parser.add_argument( - "--headless", action="store_true", help="When True, enables headless mode", default=False + "--headless_mode", + type=str, + default=None, + help="To run headless, use one of [native, websocket], webrtc might not work.", ) parser.add_argument( "--visualize_spheres", @@ -51,23 +43,21 @@ from omni.isaac.kit import SimulationApp simulation_app = SimulationApp( { - "headless": args.headless, + "headless": args.headless_mode is not None, "width": "1920", "height": "1080", } ) -# Third Party -from omni.isaac.core.utils.extensions import enable_extension - # CuRobo -from curobo.util_file import ( - get_assets_path, - get_filename, - get_path_of_dir, - get_robot_configs_path, - join_path, - load_yaml, -) +# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig +from curobo.geom.sdf.world import CollisionCheckerType +from curobo.geom.types import WorldConfig +from curobo.types.base import TensorDeviceType +from curobo.types.math import Pose +from curobo.types.robot import JointState +from curobo.types.state import JointState +from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml +from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig ext_list = [ "omni.kit.asset_converter", @@ -78,19 +68,15 @@ ext_list = [ # [enable_extension(x) for x in ext_list] # simulation_app.update() -# Standard Library -from typing import Dict # Third Party import carb import numpy as np -from helper import add_robot_to_scene +from helper import add_extensions, add_robot_to_scene from omni.isaac.core import World from omni.isaac.core.objects import cuboid, sphere -from omni.isaac.core.robots import Robot ########### OV ################# -from omni.isaac.core.utils.extensions import enable_extension from omni.isaac.core.utils.types import ArticulationAction # CuRobo @@ -177,6 +163,7 @@ def main(): motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False) print("Curobo is Ready") + add_extensions(simulation_app, args.headless_mode) plan_config = MotionGenPlanConfig( enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True ) diff --git a/examples/isaac_sim/mpc_example.py b/examples/isaac_sim/mpc_example.py index 7dbbdff..7858d50 100644 --- a/examples/isaac_sim/mpc_example.py +++ b/examples/isaac_sim/mpc_example.py @@ -24,15 +24,16 @@ a = torch.zeros(4, device="cuda:0") # Standard Library import argparse -import pickle -import shutil -import sys ## import curobo: parser = argparse.ArgumentParser() + parser.add_argument( - "--headless", action="store_true", help="When True, enables headless mode", default=False + "--headless_mode", + type=str, + default=None, + help="To run headless, use one of [native, websocket], webrtc might not work.", ) parser.add_argument( "--visualize_spheres", @@ -50,20 +51,15 @@ args = parser.parse_args() from omni.isaac.kit import SimulationApp simulation_app = SimulationApp( - {"headless": False, "width": 1920, "height": 1080} -) # _make_simulation_app({"headless": False}) + { + "headless": args.headless_mode is not None, + "width": "1920", + "height": "1080", + } +) + # Third Party # Enable the layers and stage windows in the UI -from omni.isaac.core.utils.extensions import enable_extension - -ext_list = [ - "omni.kit.asset_converter", - # "omni.kit.livestream.native", - "omni.kit.tool.asset_importer", - "omni.isaac.asset_browser", -] -[enable_extension(x) for x in ext_list] -simulation_app.update() # Standard Library import os @@ -72,24 +68,8 @@ import carb import numpy as np from helper import add_robot_to_scene from omni.isaac.core import World -from omni.isaac.core.objects import DynamicCuboid, VisualCuboid, cuboid, sphere -from omni.isaac.core.prims.xform_prim import XFormPrim - -########### OV ################# -from omni.isaac.core.utils.extensions import enable_extension - -# from omni.isaac.cortex.motion_commander import MotionCommand, PosePq, open_gripper, close_gripper -from omni.isaac.core.utils.rotations import euler_angles_to_quat - -########### frame prim ################# -from omni.isaac.core.utils.stage import get_stage_units +from omni.isaac.core.objects import cuboid from omni.isaac.core.utils.types import ArticulationAction -from omni.isaac.cortex.cortex_object import CortexObject -from omni.isaac.franka import KinematicsSolver -from omni.isaac.franka.franka import Franka -from omni.isaac.franka.tasks import FollowTarget -from pxr import Gf -from scipy.spatial.transform import Rotation as R # CuRobo from curobo.util.logger import setup_curobo_logger @@ -111,23 +91,16 @@ DATA_DIR = os.path.join(EXT_DIR, "data") from typing import Optional # Third Party -from helper import add_robot_to_scene -from omni.isaac.core.materials import PhysicsMaterial, VisualMaterial -from omni.isaac.core.utils.prims import add_reference_to_stage, get_prim_at_path -from pxr import Sdf, UsdShade +from helper import add_extensions, add_robot_to_scene # CuRobo -from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel -from curobo.curobolib import geom_cu - # from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig from curobo.geom.sdf.world import CollisionCheckerType from curobo.geom.types import WorldConfig -from curobo.rollout.cost.self_collision_cost import SelfCollisionCost, SelfCollisionCostConfig from curobo.rollout.rollout_base import Goal from curobo.types.base import TensorDeviceType from curobo.types.math import Pose -from curobo.types.robot import JointState, RobotConfig +from curobo.types.robot import JointState from curobo.types.state import JointState from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig @@ -271,11 +244,13 @@ def main(): goal_buffer = mpc.setup_solve_single(goal, 1) mpc.update_goal(goal_buffer) + mpc_result = mpc.step(current_state, max_attempts=2) usd_help.load_stage(my_world.stage) init_world = False cmd_state_full = None step = 0 + add_extensions(simulation_app, args.headless_mode) while simulation_app.is_running(): if not init_world: for _ in range(10): @@ -289,7 +264,7 @@ def main(): step_index = my_world.current_time_step_index - if step_index == 0: + if step_index <= 2: my_world.reset() idx_list = [robot.get_dof_index(x) for x in j_names] robot.set_joint_positions(default_config, idx_list) diff --git a/examples/isaac_sim/mpc_nvblox_example.py b/examples/isaac_sim/mpc_nvblox_example.py index 4627a5f..487d614 100644 --- a/examples/isaac_sim/mpc_nvblox_example.py +++ b/examples/isaac_sim/mpc_nvblox_example.py @@ -24,15 +24,16 @@ a = torch.zeros(4, device="cuda:0") # Standard Library import argparse -import pickle -import shutil -import sys ## import curobo: parser = argparse.ArgumentParser() + parser.add_argument( - "--headless", action="store_true", help="When True, enables headless mode", default=False + "--headless_mode", + type=str, + default=None, + help="To run headless, use one of [native, websocket], webrtc might not work.", ) parser.add_argument( "--visualize_spheres", @@ -50,50 +51,41 @@ args = parser.parse_args() from omni.isaac.kit import SimulationApp simulation_app = SimulationApp( - {"headless": False, "width": 1920, "height": 1080} -) # _make_simulation_app({"headless": False}) -# Third Party -# Enable the layers and stage windows in the UI -from omni.isaac.core.utils.extensions import enable_extension - -ext_list = [ - "omni.kit.asset_converter", - # "omni.kit.livestream.native", - "omni.kit.tool.asset_importer", - "omni.isaac.asset_browser", -] -[enable_extension(x) for x in ext_list] -simulation_app.update() + { + "headless": args.headless_mode is not None, + "width": "1920", + "height": "1080", + } +) # Standard Library import os # Third Party import carb import numpy as np -from helper import add_robot_to_scene +from helper import add_extensions, add_robot_to_scene from omni.isaac.core import World -from omni.isaac.core.objects import DynamicCuboid, VisualCuboid, cuboid, sphere -from omni.isaac.core.prims.xform_prim import XFormPrim - -########### OV ################# -from omni.isaac.core.utils.extensions import enable_extension - -# from omni.isaac.cortex.motion_commander import MotionCommand, PosePq, open_gripper, close_gripper -from omni.isaac.core.utils.rotations import euler_angles_to_quat +from omni.isaac.core.objects import cuboid ########### frame prim ################# -from omni.isaac.core.utils.stage import get_stage_units from omni.isaac.core.utils.types import ArticulationAction -from omni.isaac.cortex.cortex_object import CortexObject -from omni.isaac.franka import KinematicsSolver -from omni.isaac.franka.franka import Franka -from omni.isaac.franka.tasks import FollowTarget -from pxr import Gf -from scipy.spatial.transform import Rotation as R # CuRobo +# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig +from curobo.geom.sdf.world import CollisionCheckerType +from curobo.geom.types import WorldConfig +from curobo.rollout.rollout_base import Goal +from curobo.types.base import TensorDeviceType +from curobo.types.math import Pose +from curobo.types.robot import JointState +from curobo.types.state import JointState from curobo.util.logger import setup_curobo_logger from curobo.util.usd_helper import UsdHelper +from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml +from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig + +########### OV ################# + ############################################################ @@ -101,37 +93,6 @@ from curobo.util.usd_helper import UsdHelper ########### OV #################;;;;; -########### -EXT_DIR = os.path.abspath(os.path.join(os.path.abspath(os.path.dirname(__file__)))) -DATA_DIR = os.path.join(EXT_DIR, "data") -########### frame prim #################;;;;; - - -# Standard Library -from typing import Optional - -# Third Party -from helper import add_robot_to_scene -from omni.isaac.core.materials import PhysicsMaterial, VisualMaterial -from omni.isaac.core.utils.prims import add_reference_to_stage, get_prim_at_path -from pxr import Sdf, UsdShade - -# CuRobo -from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel -from curobo.curobolib import geom_cu - -# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig -from curobo.geom.sdf.world import CollisionCheckerType -from curobo.geom.types import WorldConfig -from curobo.rollout.cost.self_collision_cost import SelfCollisionCost, SelfCollisionCostConfig -from curobo.rollout.rollout_base import Goal -from curobo.types.base import TensorDeviceType -from curobo.types.math import Pose -from curobo.types.robot import JointState, RobotConfig -from curobo.types.state import JointState -from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml -from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig - ############################################################ @@ -187,12 +148,9 @@ def main(): setup_curobo_logger("warn") past_pose = None - n_obstacle_cuboids = 30 - n_obstacle_mesh = 10 # warmup curobo instance usd_help = UsdHelper() - target_pose = None tensor_args = TensorDeviceType() @@ -202,7 +160,7 @@ def main(): default_config = robot_cfg["kinematics"]["cspace"]["retract_config"] robot_cfg["kinematics"]["collision_sphere_buffer"] += 0.02 - robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world) + robot, _ = add_robot_to_scene(robot_cfg, my_world) articulation_controller = robot.get_articulation_controller() @@ -260,12 +218,16 @@ def main(): goal_buffer = mpc.setup_solve_single(goal, 1) mpc.update_goal(goal_buffer) + mpc_result = mpc.step(current_state, max_attempts=2) + + add_extensions(simulation_app, args.headless_mode) usd_help.load_stage(my_world.stage) usd_help.add_world_to_stage(world_cfg.get_mesh_world(), base_frame="/World") init_world = False cmd_state_full = None step = 0 + add_extensions(simulation_app, args.headless_mode) while simulation_app.is_running(): if not init_world: for _ in range(10): diff --git a/examples/isaac_sim/multi_arm_reacher.py b/examples/isaac_sim/multi_arm_reacher.py index e4feb8b..7caf61a 100644 --- a/examples/isaac_sim/multi_arm_reacher.py +++ b/examples/isaac_sim/multi_arm_reacher.py @@ -18,23 +18,13 @@ a = torch.zeros(4, device="cuda:0") # Standard Library import argparse -# CuRobo -from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel - -# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig -from curobo.geom.sdf.world import CollisionCheckerType -from curobo.geom.types import WorldConfig -from curobo.rollout.rollout_base import Goal -from curobo.types.base import TensorDeviceType -from curobo.types.math import Pose -from curobo.types.robot import JointState, RobotConfig -from curobo.types.state import JointState -from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml -from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig - parser = argparse.ArgumentParser() + parser.add_argument( - "--headless", action="store_true", help="When True, enables headless mode", default=False + "--headless_mode", + type=str, + default=None, + help="To run headless, use one of [native, websocket], webrtc might not work.", ) parser.add_argument( "--visualize_spheres", @@ -55,51 +45,36 @@ from omni.isaac.kit import SimulationApp simulation_app = SimulationApp( { - "headless": args.headless, + "headless": args.headless_mode is not None, "width": "1920", "height": "1080", } ) -# Third Party -from omni.isaac.core.utils.extensions import enable_extension - -# CuRobo -from curobo.util_file import ( - get_assets_path, - get_filename, - get_path_of_dir, - get_robot_configs_path, - join_path, - load_yaml, -) - -ext_list = [ - "omni.kit.asset_converter", - "omni.kit.livestream.native", - "omni.kit.tool.asset_importer", - "omni.isaac.asset_browser", -] -# [enable_extension(x) for x in ext_list] -# simulation_app.update() - -# Standard Library -from typing import Dict - # Third Party import carb import numpy as np -from helper import add_robot_to_scene +from helper import add_extensions, add_robot_to_scene from omni.isaac.core import World from omni.isaac.core.objects import cuboid, sphere -from omni.isaac.core.robots import Robot ########### OV ################# -from omni.isaac.core.utils.extensions import enable_extension from omni.isaac.core.utils.types import ArticulationAction # CuRobo +from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel + +# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig +from curobo.geom.sdf.world import CollisionCheckerType +from curobo.geom.types import WorldConfig +from curobo.rollout.rollout_base import Goal +from curobo.types.base import TensorDeviceType +from curobo.types.math import Pose +from curobo.types.robot import JointState, RobotConfig +from curobo.types.state import JointState from curobo.util.logger import setup_curobo_logger from curobo.util.usd_helper import UsdHelper +from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml +from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig ############################################################ @@ -178,6 +153,7 @@ def main(): motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False) print("Curobo is Ready") + add_extensions(simulation_app, args.headless_mode) plan_config = MotionGenPlanConfig( enable_graph=False, enable_graph_attempt=4, max_attempts=10, enable_finetune_trajopt=True ) @@ -240,7 +216,7 @@ def main(): step_index = my_world.current_time_step_index # print(step_index) - if step_index == 0: + if step_index <= 2: my_world.reset() idx_list = [robot.get_dof_index(x) for x in j_names] robot.set_joint_positions(default_config, idx_list) diff --git a/examples/isaac_sim/realsense_collision.py b/examples/isaac_sim/realsense_collision.py index a804fa1..008ff53 100644 --- a/examples/isaac_sim/realsense_collision.py +++ b/examples/isaac_sim/realsense_collision.py @@ -21,6 +21,13 @@ from matplotlib import cm from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader from omni.isaac.kit import SimulationApp +simulation_app = SimulationApp( + { + "headless": False, + "width": "1920", + "height": "1080", + } +) # CuRobo from curobo.geom.sdf.world import CollisionCheckerType from curobo.geom.types import Cuboid, WorldConfig @@ -30,13 +37,6 @@ from curobo.types.math import Pose from curobo.util_file import get_world_configs_path, join_path, load_yaml from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig -simulation_app = SimulationApp( - { - "headless": False, - "width": "1920", - "height": "1080", - } -) simulation_app.update() # Third Party from omni.isaac.core import World diff --git a/examples/isaac_sim/realsense_mpc.py b/examples/isaac_sim/realsense_mpc.py index 77f9cce..3009ecd 100644 --- a/examples/isaac_sim/realsense_mpc.py +++ b/examples/isaac_sim/realsense_mpc.py @@ -13,13 +13,23 @@ import torch a = torch.zeros(4, device="cuda:0") + +# Third Party +from omni.isaac.kit import SimulationApp + +simulation_app = SimulationApp( + { + "headless": False, + "width": "1920", + "height": "1080", + } +) # Third Party import cv2 import numpy as np import torch from matplotlib import cm from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader -from omni.isaac.kit import SimulationApp # CuRobo from curobo.geom.sdf.world import CollisionCheckerType @@ -33,13 +43,6 @@ from curobo.util_file import get_robot_configs_path, get_world_configs_path, joi from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig -simulation_app = SimulationApp( - { - "headless": False, - "width": "1920", - "height": "1080", - } -) simulation_app.update() # Standard Library import argparse diff --git a/examples/isaac_sim/realsense_reacher.py b/examples/isaac_sim/realsense_reacher.py index 374605e..e2f5fd9 100644 --- a/examples/isaac_sim/realsense_reacher.py +++ b/examples/isaac_sim/realsense_reacher.py @@ -13,13 +13,23 @@ import torch a = torch.zeros(4, device="cuda:0") +# Third Party +from omni.isaac.kit import SimulationApp + +simulation_app = SimulationApp( + { + "headless": False, + "width": "1920", + "height": "1080", + } +) + # Third Party import cv2 import numpy as np import torch from matplotlib import cm from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader -from omni.isaac.kit import SimulationApp # CuRobo from curobo.geom.sdf.world import CollisionCheckerType @@ -33,13 +43,6 @@ from curobo.util_file import get_robot_configs_path, get_world_configs_path, joi from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig -simulation_app = SimulationApp( - { - "headless": False, - "width": "1920", - "height": "1080", - } -) simulation_app.update() # Standard Library import argparse diff --git a/examples/isaac_sim/simple_stacking.py b/examples/isaac_sim/simple_stacking.py index f3d1438..b76de9c 100644 --- a/examples/isaac_sim/simple_stacking.py +++ b/examples/isaac_sim/simple_stacking.py @@ -20,21 +20,41 @@ import numpy as np np.set_printoptions(suppress=True) # Standard Library -import os.path as osp + +# Standard Library +import argparse + +## import curobo: + +parser = argparse.ArgumentParser() + +parser.add_argument( + "--headless_mode", + type=str, + default=None, + help="To run headless, use one of [native, websocket], webrtc might not work.", +) +args = parser.parse_args() # Third Party from omni.isaac.kit import SimulationApp -simulation_app = SimulationApp({"headless": False}) # This adds paths for the following importing +simulation_app = SimulationApp( + { + "headless": args.headless_mode is not None, + "width": "1920", + "height": "1080", + } +) # Standard Library from typing import Optional # Third Party import carb +from helper import add_extensions from omni.isaac.core import World from omni.isaac.core.controllers import BaseController from omni.isaac.core.tasks import Stacking as BaseStacking -from omni.isaac.core.utils.extensions import enable_extension from omni.isaac.core.utils.prims import is_prim_path_valid from omni.isaac.core.utils.stage import get_stage_units from omni.isaac.core.utils.string import find_unique_string_name @@ -43,14 +63,13 @@ from omni.isaac.core.utils.viewports import set_camera_view from omni.isaac.franka import Franka # CuRobo -from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel from curobo.geom.sdf.world import CollisionCheckerType from curobo.geom.sphere_fit import SphereFitType from curobo.geom.types import WorldConfig from curobo.rollout.rollout_base import Goal from curobo.types.base import TensorDeviceType from curobo.types.math import Pose -from curobo.types.robot import JointState, RobotConfig +from curobo.types.robot import JointState from curobo.types.state import JointState from curobo.util.usd_helper import UsdHelper from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml @@ -60,18 +79,6 @@ from curobo.wrap.reacher.motion_gen import ( MotionGenPlanConfig, MotionGenResult, ) -from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig - -ext_list = [ - "omni.kit.asset_converter", - "omni.kit.livestream.native", - "omni.kit.tool.asset_importer", - "omni.isaac.asset_browser", - # "omni.kit.window.sequencer.scripts.sequencer_extension", - "omni.kit.window.movie_capture", -] -[enable_extension(x) for x in ext_list] # toggle this for remote streaming -simulation_app.update() class CuroboController(BaseController): @@ -85,6 +92,7 @@ class CuroboController(BaseController): self._save_log = False self.my_world = my_world self.my_task = my_task + self._step_idx = 0 n_obstacle_cuboids = 20 n_obstacle_mesh = 2 # warmup curobo instance @@ -111,7 +119,7 @@ class CuroboController(BaseController): ] = "panda_hand" # controls which frame the controller is controlling # self.robot_cfg["kinematics"]["cspace"]["max_acceleration"] = 10.0 # controls how fast robot moves self.robot_cfg["kinematics"]["extra_collision_spheres"] = {"attached_object": 100} - self.robot_cfg["kinematics"]["collision_sphere_buffer"] = 0.0 + # @self.robot_cfg["kinematics"]["collision_sphere_buffer"] = 0.0 self.robot_cfg["kinematics"]["collision_spheres"] = "spheres/franka_collision_mesh.yml" world_cfg_table = WorldConfig.from_dict( @@ -133,16 +141,13 @@ class CuroboController(BaseController): trajopt_tsteps=32, collision_checker_type=CollisionCheckerType.MESH, use_cuda_graph=True, - num_ik_seeds=40, - num_trajopt_seeds=10, - num_graph_seeds=10, interpolation_dt=0.01, collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh}, store_ik_debug=self._save_log, store_trajopt_debug=self._save_log, state_finite_difference_mode="CENTRAL", minimize_jerk=True, - acceleration_scale=0.3, + acceleration_scale=0.5, fixed_iters_trajopt=True, ) self.motion_gen = MotionGen(motion_gen_config) @@ -159,6 +164,7 @@ class CuroboController(BaseController): self.usd_help.load_stage(self.my_world.stage) self.cmd_plan = None self.cmd_idx = 0 + self._step_idx = 0 self.idx_list = None def attach_obj( @@ -233,6 +239,7 @@ class CuroboController(BaseController): if self.cmd_plan is None: self.cmd_idx = 0 + self._step_idx = 0 # Set EE goals ee_translation_goal = self.my_task.target_position ee_orientation_goal = np.array([0, 0, -1, 0]) @@ -246,19 +253,22 @@ class CuroboController(BaseController): else: carb.log_warn("Plan did not converge to a solution.") return None + if self._step_idx % 3 == 0: + cmd_state = self.cmd_plan[self.cmd_idx] + self.cmd_idx += 1 - cmd_state = self.cmd_plan[self.cmd_idx] - self.cmd_idx += 1 - # get full dof state - art_action = ArticulationAction( - cmd_state.position.cpu().numpy(), - # cmd_state.velocity.cpu().numpy(), - joint_indices=self.idx_list, - ) - if self.cmd_idx >= len(self.cmd_plan.position): - self.cmd_idx = 0 - self.cmd_plan = None - + # get full dof state + art_action = ArticulationAction( + cmd_state.position.cpu().numpy(), + # cmd_state.velocity.cpu().numpy(), + joint_indices=self.idx_list, + ) + if self.cmd_idx >= len(self.cmd_plan.position): + self.cmd_idx = 0 + self.cmd_plan = None + else: + art_action = None + self._step_idx += 1 return art_action def reached_target(self, observations: dict) -> bool: @@ -413,9 +423,39 @@ articulation_controller = my_franka.get_articulation_controller() set_camera_view(eye=[2, 0, 1], target=[0.00, 0.00, 0.00], camera_prim_path="/OmniverseKit_Persp") wait_steps = 8 -initial_steps = 10 +my_franka.set_solver_velocity_iteration_count(4) +my_franka.set_solver_position_iteration_count(124) +my_world._physics_context.set_solver_type("TGS") +initial_steps = 100 ################################################################ print("Start simulation...") +robot = my_franka +print( + my_world._physics_context.get_solver_type(), + robot.get_solver_position_iteration_count(), + robot.get_solver_velocity_iteration_count(), +) +print(my_world._physics_context.use_gpu_pipeline) +print(articulation_controller.get_gains()) +print(articulation_controller.get_max_efforts()) +robot = my_franka +print("**********************") +robot.enable_gravity() +articulation_controller.set_gains( + kps=np.array( + [100000000, 6000000.0, 10000000, 600000.0, 25000.0, 15000.0, 50000.0, 6000.0, 6000.0] + ) +) + + +articulation_controller.set_max_efforts( + values=np.array([100000, 52.199997, 100000, 52.199997, 7.2, 7.2, 7.2, 50.0, 50]) +) + +print("Updated gains:") +print(articulation_controller.get_gains()) +print(articulation_controller.get_max_efforts()) +# exit() my_franka.gripper.open() for _ in range(wait_steps): my_world.step(render=True) @@ -423,18 +463,11 @@ my_task.reset() task_finished = False observations = my_world.get_observations() my_task.get_pick_position(observations) -robot = my_franka -print("**********************") -# print(robot.get_solver_position_iteration_count(), robot.get_solver_velocity_iteration_count()) -robot.enable_gravity() -robot._articulation_view.set_gains( - kps=np.array([100000000, 10000000]), joint_indices=np.array([0, 2]) -) -robot._articulation_view.set_max_efforts( - values=np.array([10000, 10000]), joint_indices=np.array([0, 2]) -) i = 0 + +add_extensions(simulation_app, args.headless_mode) + while simulation_app.is_running(): my_world.step(render=True) # necessary to visualize changes i += 1 @@ -481,7 +514,7 @@ while simulation_app.is_running(): art_action = my_controller.forward(sim_js, my_franka.dof_names) if art_action is not None: articulation_controller.apply_action(art_action) - for _ in range(2): - my_world.step(render=False) + # for _ in range(2): + # my_world.step(render=False) simulation_app.close() diff --git a/examples/usd_example.py b/examples/usd_example.py index dbc2c05..632ef3a 100644 --- a/examples/usd_example.py +++ b/examples/usd_example.py @@ -70,17 +70,7 @@ def get_trajectory(robot_file="franka.yml", dt=1.0 / 60.0): retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze()) start_state = JointState.from_position(retract_cfg.view(1, -1).clone() + 0.5) - # start_state.position[0,2] = 0.5 - # start_state.position[0,1] = 0.5 - # start_state.position[0,0] = 0.5 - # print(start_state.position) result = motion_gen.plan_single(start_state, retract_pose) - # print(result.plan_state.position) - print(result.success) - # print(result.position_error) - # result = motion_gen.plan( - # start_state, retract_pose, enable_graph=False, enable_opt=True, max_attempts=10 - # ) traj = result.get_interpolated_plan() # optimized plan return traj @@ -94,11 +84,18 @@ def save_curobo_robot_world_to_usd(robot_file="franka.yml"): dt = 1 / 60.0 q_traj = get_trajectory(robot_file, dt) - q_start = q_traj[0] + if q_traj is not None: + q_start = q_traj[0] - UsdHelper.write_trajectory_animation_with_robot_usd( - robot_file, world_model, q_start, q_traj, save_path="test.usd" - ) + UsdHelper.write_trajectory_animation_with_robot_usd( + robot_file, + world_model, + q_start, + q_traj, + save_path="test.usda", + robot_color=[0.5, 0.5, 0.2, 1.0], + base_frame="/grid_world_1", + ) def save_curobo_robot_to_usd(robot_file="franka.yml"): @@ -168,4 +165,6 @@ def read_robot_from_usd(robot_file: str = "franka.yml"): if __name__ == "__main__": - save_curobo_world_to_usd() + # save_curobo_world_to_usd() + + save_curobo_robot_world_to_usd("franka.yml") diff --git a/pyproject.toml b/pyproject.toml index e337471..f91f8f4 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -56,6 +56,7 @@ exclude_lines = [ "torch.jit.script", "abstractmethod", "def backward", + "staticmethod\ndef forward", ] omit = [ "src/curobo/util/error_metrics.py", diff --git a/setup.cfg b/setup.cfg index c9eb209..2b15a36 100644 --- a/setup.cfg +++ b/setup.cfg @@ -80,8 +80,6 @@ ci = sphinx sphinx_rtd_theme graphviz>=0.20.1 - sphinxcontrib-youtube - sphinxcontrib-video>=0.2.0 # this is only available in 3.8+ smooth = @@ -100,13 +98,15 @@ dev = pytest>6.2.5 pytest-cov +isaac_sim = + tomli + wheel + ninja doc = sphinx sphinx_rtd_theme graphviz>=0.20.1 - sphinxcontrib-youtube - sphinxcontrib-video>=0.2.0 [options.entry_points] # Add here console scripts like: diff --git a/src/curobo/content/configs/robot/franka.yml b/src/curobo/content/configs/robot/franka.yml index d9f9aaa..f8accef 100644 --- a/src/curobo/content/configs/robot/franka.yml +++ b/src/curobo/content/configs/robot/franka.yml @@ -13,7 +13,7 @@ robot_cfg: kinematics: use_usd_kinematics: False isaac_usd_path: "/Isaac/Robots/Franka/franka.usd" - usd_path: "robot/non_shipping/franka/franka_panda_meters1.usda" + usd_path: "robot/non_shipping/franka/franka_panda_meters.usda" usd_robot_root: "/panda" usd_flip_joints: ["panda_joint1","panda_joint2","panda_joint3","panda_joint4", "panda_joint5", "panda_joint6","panda_joint7","panda_finger_joint1", "panda_finger_joint2"] diff --git a/src/curobo/content/configs/robot/ur10e.yml b/src/curobo/content/configs/robot/ur10e.yml index b58eeef..7c7424c 100644 --- a/src/curobo/content/configs/robot/ur10e.yml +++ b/src/curobo/content/configs/robot/ur10e.yml @@ -40,6 +40,7 @@ robot_cfg: 'wrist_3_link' : 0, 'tool0': 0, } + mesh_link_names: [ 'shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link' ,'wrist_3_link' ] lock_joints: null cspace: diff --git a/src/curobo/cuda_robot_model/cuda_robot_generator.py b/src/curobo/cuda_robot_model/cuda_robot_generator.py index ac70fb6..522e058 100644 --- a/src/curobo/cuda_robot_model/cuda_robot_generator.py +++ b/src/curobo/cuda_robot_model/cuda_robot_generator.py @@ -43,9 +43,9 @@ try: # CuRobo from curobo.cuda_robot_model.usd_kinematics_parser import UsdKinematicsParser except ImportError: - log_warn( + log_info( "USDParser failed to import, install curobo with pip install .[usd] " - + "or pip install usd-core" + + "or pip install usd-core, NOTE: Do not install this if using with ISAAC SIM." ) diff --git a/src/curobo/cuda_robot_model/cuda_robot_model.py b/src/curobo/cuda_robot_model/cuda_robot_model.py index cb82f7a..b246023 100644 --- a/src/curobo/cuda_robot_model/cuda_robot_model.py +++ b/src/curobo/cuda_robot_model/cuda_robot_model.py @@ -252,12 +252,19 @@ class CudaRobotModel(CudaRobotModelConfig): ) return state + def get_robot_link_meshes(self): + m_list = [self.get_link_mesh(l) for l in self.kinematics_config.mesh_link_names] + + return m_list + def get_robot_as_mesh(self, q: torch.Tensor): # get all link meshes: - m_list = [self.get_link_mesh(l) for l in self.mesh_link_names] - pose = self.get_link_poses(q, self.mesh_link_names) - for li, l in enumerate(self.mesh_link_names): - m_list[li].pose = pose.get_index(0, li).tolist() + m_list = self.get_robot_link_meshes() + pose = self.get_link_poses(q, self.kinematics_config.mesh_link_names) + for li, l in enumerate(self.kinematics_config.mesh_link_names): + m_list[li].pose = ( + Pose.from_list(m_list[li].pose).multiply(pose.get_index(0, li)).tolist() + ) return m_list @@ -344,7 +351,6 @@ class CudaRobotModel(CudaRobotModelConfig): def get_link_mesh(self, link_name: str) -> Mesh: mesh = self.kinematics_parser.get_link_mesh(link_name) - mesh.pose = [0, 0, 0, 1, 0, 0, 0] return mesh def get_link_transform(self, link_name: str) -> Pose: diff --git a/src/curobo/cuda_robot_model/urdf_kinematics_parser.py b/src/curobo/cuda_robot_model/urdf_kinematics_parser.py index bbe7155..43f5340 100644 --- a/src/curobo/cuda_robot_model/urdf_kinematics_parser.py +++ b/src/curobo/cuda_robot_model/urdf_kinematics_parser.py @@ -21,6 +21,7 @@ from curobo.cuda_robot_model.kinematics_parser import KinematicsParser, LinkPara from curobo.cuda_robot_model.types import JointType from curobo.geom.types import Mesh as CuroboMesh from curobo.types.base import TensorDeviceType +from curobo.types.math import Pose from curobo.util.logger import log_error, log_warn from curobo.util_file import join_path @@ -164,4 +165,16 @@ class UrdfKinematicsParser(KinematicsParser): def get_link_mesh(self, link_name): m = self._robot.link_map[link_name].visuals[0].geometry.mesh - return CuroboMesh(name=link_name, pose=None, scale=m.scale, file_path=m.filename) + mesh_pose = self._robot.link_map[link_name].visuals[0].origin + # read visual material: + if mesh_pose is None: + mesh_pose = [0, 0, 0, 1, 0, 0, 0] + else: + # convert to list: + mesh_pose = Pose.from_matrix(mesh_pose).to_list() + return CuroboMesh( + name=link_name, + pose=mesh_pose, + scale=m.scale, + file_path=m.filename, + ) diff --git a/src/curobo/geom/types.py b/src/curobo/geom/types.py index 5955418..96e2434 100644 --- a/src/curobo/geom/types.py +++ b/src/curobo/geom/types.py @@ -381,7 +381,6 @@ class Mesh(Obstacle): m = trimesh.load(self.file_path, process=process, force="mesh") if isinstance(m, trimesh.Scene): m = m.dump(concatenate=True) - if isinstance(m.visual, trimesh.visual.texture.TextureVisuals): m.visual = m.visual.to_color() else: @@ -397,6 +396,26 @@ class Mesh(Obstacle): return m + def update_material(self): + if ( + self.color is None + and self.vertex_colors is None + and self.face_colors is None + and self.file_path is not None + ): + # try to load material: + m = trimesh.load(self.file_path, process=False, force="mesh") + if isinstance(m, trimesh.Scene): + m = m.dump(concatenate=True) + if isinstance(m.visual, trimesh.visual.texture.TextureVisuals): + m.visual = m.visual.to_color() + if isinstance(m.visual, trimesh.visual.color.ColorVisuals): + if isinstance(m.visual.vertex_colors[0], np.ndarray): + self.vertex_colors = m.visual.vertex_colors + self.face_colors = m.visual.face_colors + else: + self.vertex_colors = [m.visual.vertex_colors for x in range(len(m.vertices))] + def get_mesh_data(self, process: bool = True): verts = faces = None if self.file_path is not None: diff --git a/src/curobo/types/math.py b/src/curobo/types/math.py index a4015d1..0e8f16c 100644 --- a/src/curobo/types/math.py +++ b/src/curobo/types/math.py @@ -83,10 +83,16 @@ class Pose(Sequence): self.quaternion = normalize_quaternion(self.quaternion) @staticmethod - def from_matrix(matrix: np.ndarray): + def from_matrix(matrix: Union[np.ndarray, torch.Tensor]): + if not isinstance(matrix, torch.Tensor): + tensor_args = TensorDeviceType() + matrix = torch.as_tensor(matrix, device=tensor_args.device, dtype=tensor_args.dtype) + if len(matrix.shape) == 2: matrix = matrix.view(-1, 4, 4) - return Pose(position=matrix[..., :3, 3], rotation=matrix[..., :3, :3]) + return Pose( + position=matrix[..., :3, 3], rotation=matrix[..., :3, :3], normalize_rotation=True + ) def get_rotation(self): if self.rotation is not None: diff --git a/src/curobo/util/usd_helper.py b/src/curobo/util/usd_helper.py index ac51d61..f953699 100644 --- a/src/curobo/util/usd_helper.py +++ b/src/curobo/util/usd_helper.py @@ -38,6 +38,7 @@ from curobo.types.state import JointState from curobo.util.logger import log_error, log_warn from curobo.util_file import ( file_exists, + get_assets_path, get_filename, get_files_from_dir, get_robot_configs_path, @@ -314,7 +315,6 @@ def get_sphere_attrs(prim, cache=None, transform=None) -> Sphere: size = 1.0 radius = prim.GetAttribute("radius").Get() scale = prim.GetAttribute("xformOp:scale").Get() - print(radius, scale) if scale is not None: radius = radius * max(list(scale)) * size @@ -620,7 +620,7 @@ class UsdHelper: root_path = join_path(base_frame, obstacle.name) obj_geom = UsdGeom.Mesh.Define(self.stage, root_path) obj_prim = self.stage.GetPrimAtPath(root_path) - + # obstacle.update_material() # This does not get the correct materials set_geom_mesh_attrs(obj_geom, obstacle, timestep=timestep) obj_prim.CreateAttribute("physics:rigidBodyEnabled", Sdf.ValueTypeNames.Bool, custom=False) @@ -664,10 +664,20 @@ class UsdHelper: ) for i, i_val in enumerate(prim_names): curr_prim = self.stage.GetPrimAtPath(i_val) - form = UsdGeom.Xformable(curr_prim).GetOrderedXformOps()[0] + form = UsdGeom.Xformable(curr_prim).GetOrderedXformOps() + if len(form) < 2: + log_warn("Pose transformation not found" + i_val) + continue + + pos_form = form[0] + quat_form = form[1] + use_float = True # default is float for t in range(pose.batch): - c_t = get_transform(pose.get_index(t, i).tolist()) - form.Set(time=t * self.interpolation_steps, value=c_t) + c_p, c_q = get_position_quat(pose.get_index(t, i).tolist(), use_float) + pos_form.Set(time=t * self.interpolation_steps, value=c_p) + quat_form.Set(time=t * self.interpolation_steps, value=c_q) + # c_t = get_transform(pose.get_index(t, i).tolist()) + # form.Set(time=t * self.interpolation_steps, value=c_t) def create_obstacle_animation( self, @@ -785,33 +795,64 @@ class UsdHelper: save_path: str = "out.usd", tensor_args: TensorDeviceType = TensorDeviceType(), interpolation_steps: float = 1.0, - robot_color: List[float] = [0.8, 0.8, 0.8, 1.0], + robot_base_frame="robot", + base_frame="/world", + kin_model: Optional[CudaRobotModel] = None, + visualize_robot_spheres: bool = True, + robot_color: Optional[List[float]] = None, ): - config_file = load_yaml(join_path(get_robot_configs_path(), robot_model_file)) - config_file["robot_cfg"]["kinematics"]["load_link_names_with_mesh"] = True - robot_cfg = CudaRobotModelConfig.from_data_dict( - config_file["robot_cfg"]["kinematics"], tensor_args=tensor_args - ) - kin_model = CudaRobotModel(robot_cfg) - - m = kin_model.get_robot_as_mesh(q_start.position) + if kin_model is None: + config_file = load_yaml(join_path(get_robot_configs_path(), robot_model_file)) + config_file["robot_cfg"]["kinematics"]["load_link_names_with_mesh"] = True + robot_cfg = CudaRobotModelConfig.from_data_dict( + config_file["robot_cfg"]["kinematics"], tensor_args=tensor_args + ) + kin_model = CudaRobotModel(robot_cfg) + m = kin_model.get_robot_link_meshes() + offsets = [x.pose for x in m] robot_mesh_model = WorldConfig(mesh=m) - robot_mesh_model.add_color(robot_color) - robot_mesh_model.add_material(Material(metallic=0.4)) + if robot_color is not None: + robot_mesh_model.add_color(robot_color) + robot_mesh_model.add_material(Material(metallic=0.4)) usd_helper = UsdHelper() usd_helper.create_stage( save_path, timesteps=q_traj.position.shape[0], dt=dt, interpolation_steps=interpolation_steps, + base_frame=base_frame, ) - usd_helper.add_world_to_stage(world_model) + if world_model is not None: + usd_helper.add_world_to_stage(world_model, base_frame=base_frame) - animation_links = kin_model.mesh_link_names + animation_links = kin_model.kinematics_config.mesh_link_names animation_poses = kin_model.get_link_poses(q_traj.position, animation_links) - usd_helper.create_animation(robot_mesh_model, animation_poses) - usd_helper.write_stage_to_file(save_path + ".usd") + # add offsets for visual mesh: + for i, ival in enumerate(offsets): + offset_pose = Pose.from_list(ival) + new_pose = Pose( + animation_poses.position[:, i, :], animation_poses.quaternion[:, i, :] + ).multiply(offset_pose) + animation_poses.position[:, i, :] = new_pose.position + animation_poses.quaternion[:, i, :] = new_pose.quaternion + + robot_base_frame = join_path(base_frame, robot_base_frame) + + usd_helper.create_animation( + robot_mesh_model, animation_poses, base_frame, robot_frame=robot_base_frame + ) + if visualize_robot_spheres: + # visualize robot spheres: + sphere_traj = kin_model.get_robot_as_spheres(q_traj.position) + # change color: + for s in sphere_traj: + for k in s: + k.color = [0, 0.27, 0.27, 1.0] + usd_helper.create_obstacle_animation( + sphere_traj, base_frame=base_frame, obstacles_frame="curobo/robot_collision" + ) + usd_helper.write_stage_to_file(save_path) @staticmethod def load_robot( @@ -848,7 +889,11 @@ class UsdHelper: kin_model: Optional[CudaRobotModel] = None, visualize_robot_spheres: bool = True, robot_asset_prim_path=None, + robot_color: Optional[List[float]] = None, ): + usd_exists = False + # if usd file doesn't exist, fall back to urdf animation script + if kin_model is None: config_file = load_yaml(join_path(get_robot_configs_path(), robot_model_file)) if "robot_cfg" in config_file: @@ -856,6 +901,36 @@ class UsdHelper: config_file["kinematics"]["load_link_names_with_mesh"] = True config_file["kinematics"]["use_usd_kinematics"] = True + usd_exists = file_exists( + join_path(get_assets_path(), config_file["kinematics"]["usd_path"]) + ) + else: + if kin_model.generator_config.usd_path is not None: + usd_exists = file_exists(kin_model.generator_config.usd_path) + if robot_color is not None: + log_warn( + "robot_color is not supported when using robot from usd, " + + "using urdf mode instead to write usd file" + ) + usd_exists = False + if not usd_exists: + log_warn("robot usd not found, using urdf animation instead") + return UsdHelper.write_trajectory_animation( + robot_model_file, + world_model, + q_start, + q_traj, + dt, + save_path, + tensor_args, + interpolation_steps, + robot_base_frame=robot_base_frame, + base_frame=base_frame, + kin_model=kin_model, + visualize_robot_spheres=visualize_robot_spheres, + robot_color=robot_color, + ) + if kin_model is None: robot_cfg = CudaRobotModelConfig.from_data_dict( config_file["kinematics"], tensor_args=tensor_args ) @@ -1064,9 +1139,17 @@ class UsdHelper: write_robot_usd_path="assets/", robot_asset_prim_path="/panda", ): - # if goal_object is None: - # log_warn("Using franka gripper as goal object") - # goal_object = + if goal_object is None: + log_warn("Using franka gripper as goal object") + goal_object = Mesh( + name="target_gripper", + file_path=join_path( + get_assets_path(), + "robot/franka_description/meshes/visual/hand.dae", + ), + color=[0.0, 0.8, 0.1, 1.0], + pose=goal_pose.to_list(), + ) if goal_object is not None: goal_object.pose = np.ravel(goal_pose.tolist()).tolist() world_config = world_config.clone() diff --git a/src/curobo/wrap/reacher/trajopt.py b/src/curobo/wrap/reacher/trajopt.py index b607aa8..f3f2e74 100644 --- a/src/curobo/wrap/reacher/trajopt.py +++ b/src/curobo/wrap/reacher/trajopt.py @@ -23,7 +23,7 @@ import torch.autograd.profiler as profiler # CuRobo from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelState from curobo.geom.sdf.utils import create_collision_checker -from curobo.geom.sdf.world import WorldCollision, WorldCollisionConfig +from curobo.geom.sdf.world import CollisionCheckerType, WorldCollision, WorldCollisionConfig from curobo.geom.types import WorldConfig from curobo.opt.newton.lbfgs import LBFGSOpt, LBFGSOptConfig from curobo.opt.newton.newton_base import NewtonOptBase, NewtonOptConfig @@ -104,6 +104,7 @@ class TrajOptSolverConfig: num_seeds: int = 2, seed_ratio: Dict[str, int] = {"linear": 1.0, "bias": 0.0, "start": 0.0, "end": 0.0}, use_particle_opt: bool = True, + collision_checker_type: Optional[CollisionCheckerType] = CollisionCheckerType.PRIMITIVE, traj_evaluator_config: TrajEvaluatorConfig = TrajEvaluatorConfig(), traj_evaluator: Optional[TrajEvaluator] = None, minimize_jerk: Optional[bool] = None, @@ -144,6 +145,9 @@ class TrajOptSolverConfig: if not minimize_jerk: filter_robot_command = False + if collision_checker_type is not None: + base_config_data["world_collision_checker_cfg"]["checker_type"] = collision_checker_type + if world_coll_checker is None and world_model is not None: world_cfg = WorldCollisionConfig.load_from_dict( base_config_data["world_collision_checker_cfg"], world_model, tensor_args