add isaac sim 2023 support and dockerfiles

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.

docker/README.md

@@ -10,27 +10,5 @@ its affiliates is strictly prohibited.
 -->
 # Docker Instructions
 
-## Running docker from NGC (Recommended)
+Check [Docker Development](https://curobo.org/source/getting_started/5_docker_development.html) for
-1. `sh build_user_docker.sh $UID`
+instructions.
-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
-

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
+

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`
-
+# If you want to build a isaac sim docker, run this script with `bash build_dev_docker.sh isaac`
-# Make sure you have pulled all required repos into pkgs folder (see pull_repos.sh script)
 
 # 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}
+    echo "Unknown Architecture"
-    dockerfile="x86.dockerfile"
+    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} . 

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 . 
+
+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 . 

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
+
+

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

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

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

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

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

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

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'
 

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
+  && add-apt-repository -y ppa:git-core/ppa \
-
+  && apt-get update && apt-get install -y \
-RUN 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

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
+parser = argparse.ArgumentParser()
-# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
+
-from curobo.geom.sdf.world import CollisionCheckerType
+parser.add_argument(
-from curobo.geom.types import WorldConfig
+    "--headless_mode",
-from curobo.types.base import TensorDeviceType
+    type=str,
-from curobo.types.math import Pose
+    default=None,
-from curobo.util_file import get_world_configs_path, join_path, load_yaml
+    help="To run headless, use one of [native, websocket], webrtc might not work.",
-from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
+)
+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
+from omni.isaac.core.objects import sphere
-
-########### 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.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():

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
     )

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
-
+from curobo.util_file import get_world_configs_path, join_path, load_yaml
-parser = argparse.ArgumentParser()
+from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
-parser.add_argument(
-    "--nvblox", action="store_true", help="When True, enables headless mode", default=False
-)
-args = parser.parse_args()
 
 
 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():

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_p = Robot(
-    robot = my_world.scene.add(
-        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
 

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)

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 sphere
-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.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"]

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
     )

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 (
+# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
-    get_assets_path,
+from curobo.geom.sdf.world import CollisionCheckerType
-    get_filename,
+from curobo.geom.types import WorldConfig
-    get_path_of_dir,
+from curobo.types.base import TensorDeviceType
-    get_robot_configs_path,
+from curobo.types.math import Pose
-    join_path,
+from curobo.types.robot import JointState
-    load_yaml,
+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
     )

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.objects import cuboid
-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.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 helper import add_extensions, 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.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)

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})
+        "headless": args.headless_mode is not None,
-# Third Party
+        "width": "1920",
-# Enable the layers and stage windows in the UI
+        "height": "1080",
-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
 
 # 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.objects import cuboid
-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.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):

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)

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

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

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

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,9 +253,10 @@ 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
+
             # get full dof state
             art_action = ArticulationAction(
                 cmd_state.position.cpu().numpy(),
@@ -258,7 +266,9 @@ class CuroboController(BaseController):
             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):
+            # for _ in range(2):
-                my_world.step(render=False)
+            #    my_world.step(render=False)
 
 simulation_app.close()

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,10 +84,17 @@ def save_curobo_robot_world_to_usd(robot_file="franka.yml"):
     dt = 1 / 60.0
 
     q_traj = get_trajectory(robot_file, dt)
+    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"
+            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",
         )
 
 
@@ -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")

pyproject.toml

@@ -56,6 +56,7 @@ exclude_lines = [
     "torch.jit.script",
     "abstractmethod",
     "def backward",
+    "staticmethod\ndef forward",
 ]
 omit = [
     "src/curobo/util/error_metrics.py",

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:

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"]

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:

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."
     )
 
 

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]
+        m_list = self.get_robot_link_meshes()
-        pose = self.get_link_poses(q, self.mesh_link_names)
+        pose = self.get_link_poses(q, self.kinematics_config.mesh_link_names)
-        for li, l in enumerate(self.mesh_link_names):
+        for li, l in enumerate(self.kinematics_config.mesh_link_names):
-            m_list[li].pose = pose.get_index(0, li).tolist()
+            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:

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,
+        )

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:

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:

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())
+                c_p, c_q = get_position_quat(pose.get_index(t, i).tolist(), use_float)
-                form.Set(time=t * self.interpolation_steps, value=c_t)
+                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,8 +795,13 @@ 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,
     ):
+        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(
@@ -794,9 +809,10 @@ class UsdHelper:
             )
             kin_model = CudaRobotModel(robot_cfg)
 
-        m = kin_model.get_robot_as_mesh(q_start.position)
+        m = kin_model.get_robot_link_meshes()
-
+        offsets = [x.pose for x in m]
         robot_mesh_model = WorldConfig(mesh=m)
+        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()
@@ -805,13 +821,38 @@ class UsdHelper:
             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)
+        # add offsets for visual mesh:
-        usd_helper.write_stage_to_file(save_path + ".usd")
+        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:
+        if goal_object is None:
-        #    log_warn("Using franka gripper as goal object")
+            log_warn("Using franka gripper as goal object")
-        #    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()

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