From 3690d28c54ee4ba23e2d7cc03a89a5845e4f9546 Mon Sep 17 00:00:00 2001 From: Balakumar Sundaralingam Date: Sat, 20 Jul 2024 14:51:43 -0700 Subject: [PATCH] Isaac Sim 4.0 support, Kinematics API doc, Windows support --- CHANGELOG.md | 48 +- benchmark/ik_benchmark.py | 16 +- docker/aarch64.dockerfile | 9 +- docker/build_dev_docker.sh | 2 +- docker/build_docker.sh | 17 +- docker/isaac_sim.dockerfile | 13 +- docker/start_docker.sh | 1 + docker/user.dockerfile | 2 +- docker/user_isaac_sim.dockerfile | 6 +- docker/x86.dockerfile | 2 +- examples/isaac_sim/batch_collision_checker.py | 6 + .../isaac_sim/batch_motion_gen_reacher.py | 6 + examples/isaac_sim/collision_checker.py | 6 + examples/isaac_sim/constrained_reacher.py | 13 +- examples/isaac_sim/helper.py | 24 +- examples/isaac_sim/ik_reachability.py | 6 + examples/isaac_sim/load_all_robots.py | 6 + examples/isaac_sim/motion_gen_reacher.py | 9 +- .../isaac_sim/motion_gen_reacher_nvblox.py | 6 + examples/isaac_sim/mpc_example.py | 9 +- examples/isaac_sim/mpc_nvblox_example.py | 11 +- examples/isaac_sim/multi_arm_reacher.py | 14 +- examples/isaac_sim/realsense_collision.py | 49 +- examples/isaac_sim/realsense_mpc.py | 19 +- examples/isaac_sim/realsense_reacher.py | 14 +- examples/isaac_sim/simple_stacking.py | 12 +- .../isaac_sim/util/convert_urdf_to_usd.py | 7 + examples/isaac_sim/util/dowload_assets.py | 7 + examples/mpc_example.py | 6 - setup.py | 7 + .../franka_description/franka_panda.urdf | 45 +- .../content/configs/robot/kinova_gen3.yml | 12 +- src/curobo/content/configs/robot/ur10e.xrdf | 100 ++++ src/curobo/cuda_robot_model/__init__.py | 85 +++- .../cuda_robot_model/cuda_robot_generator.py | 295 +++++++++--- .../cuda_robot_model/cuda_robot_model.py | 443 ++++++++++++++++-- .../cuda_robot_model/kinematics_parser.py | 107 ++++- src/curobo/cuda_robot_model/types.py | 315 +++++++++++-- .../urdf_kinematics_parser.py | 94 +++- .../cuda_robot_model/usd_kinematics_parser.py | 59 ++- src/curobo/cuda_robot_model/util.py | 63 +++ .../curobolib/cpp/kinematics_fused_kernel.cu | 142 ++++-- .../curobolib/cpp/pose_distance_kernel.cu | 24 +- .../curobolib/cpp/self_collision_kernel.cu | 8 +- src/curobo/curobolib/geom.py | 2 +- src/curobo/curobolib/kinematics.py | 12 +- src/curobo/geom/sdf/world.py | 16 + src/curobo/geom/sdf/world_blox.py | 4 + src/curobo/geom/sdf/world_mesh.py | 4 + src/curobo/geom/sdf/world_voxel.py | 4 + src/curobo/geom/types.py | 26 +- src/curobo/graph/graph_base.py | 2 + src/curobo/opt/newton/newton_base.py | 4 +- src/curobo/opt/opt_base.py | 3 + src/curobo/opt/particle/particle_opt_base.py | 12 +- src/curobo/rollout/arm_base.py | 18 +- src/curobo/rollout/arm_reacher.py | 5 +- src/curobo/rollout/cost/bound_cost.py | 26 +- src/curobo/rollout/cost/dist_cost.py | 12 +- src/curobo/rollout/cost/pose_cost.py | 28 +- src/curobo/rollout/rollout_base.py | 10 +- src/curobo/types/file_path.py | 173 +++++++ src/curobo/util/torch_utils.py | 11 + src/curobo/util/trajectory.py | 9 +- src/curobo/util/usd_helper.py | 35 +- src/curobo/util/warp.py | 9 + src/curobo/util/xrdf_utils.py | 170 +++++++ src/curobo/util_file.py | 23 +- src/curobo/wrap/reacher/evaluator.py | 1 + src/curobo/wrap/reacher/ik_solver.py | 41 +- src/curobo/wrap/reacher/motion_gen.py | 66 ++- src/curobo/wrap/reacher/mpc.py | 42 +- src/curobo/wrap/reacher/trajopt.py | 107 +++-- src/curobo/wrap/wrap_base.py | 12 +- src/curobo/wrap/wrap_mpc.py | 2 - tests/conftest.py | 1 + tests/cuda_graph_test.py | 75 +++ tests/kinematics_test.py | 46 +- tests/motion_gen_constrained_test.py | 5 +- tests/motion_gen_module_test.py | 96 +++- tests/motion_gen_speed_test.py | 25 +- tests/self_collision_test.py | 6 +- tests/xrdf_test.py | 33 ++ 83 files changed, 2821 insertions(+), 500 deletions(-) create mode 100644 src/curobo/content/configs/robot/ur10e.xrdf create mode 100644 src/curobo/cuda_robot_model/util.py create mode 100644 src/curobo/types/file_path.py create mode 100644 src/curobo/util/xrdf_utils.py create mode 100644 tests/cuda_graph_test.py create mode 100644 tests/xrdf_test.py diff --git a/CHANGELOG.md b/CHANGELOG.md index 8d7b4019..17b461bd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,16 +10,58 @@ its affiliates is strictly prohibited. --> # Changelog -## Latest Commit +## Version 0.7.4 + +### Changes in Default Behavior + +- Cuda graph capture of optimization iterations resets solver before recording. +- ``join_path(a, b)`` now requires ``a`` to not have a trailing slash to make the file compatible with Windows. +- Drop examples support for Isaac Sim < 4.0.0. +- asset_root_path can be either empty string or None. +- Order of variables in ``SelfCollisionKinematicsConfig`` has changed. Unused variables +moved to bottom. +- Remove requirement of warmup for using ``offset_waypoint`` in ``PoseCost``. + +### New Features + +- Interpolated metrics calculation now recreates cuda graph if interpolation steps exceed existing buffer size. +- Add experimental ``CUDAGraph.reset`` usage as ``cuda>=12.0`` is not crashing when an existing captured CUDAGraph is freed and recaptured with new memory pointers. Try this experimental feature by +setting an environment variable ``export CUROBO_TORCH_CUDA_GRAPH_RESET=1``. This feature will allow for changing the problem type in ``motion_gen`` and ``ik_solver`` without requiring recreation of the class. +- Add partial support for Windows. +- Add Isaac Sim 4.0.0 docker support. +- Examples now work with Isaac Sim 4.0.0. +- Add XRDF support. +- Add curobo.types.file_path.ContentPath to store paths for files representing robot and world. This +improves development on top of cuRobo with custom robots living external of cuRobo library. +- Add attach external objects to robot link API to CudaRobotModel. +- Add MotionGenStatus.DT_EXCEPTION to report failures due to trajectory exceeding user specified +maximum trajectory dt. +- Add reading of end-effector mesh if available when rendering trajectory with ``UsdHelper``, also +supports goalset rendering. +- Kinematics module (`curobo.cuda_robot_model`) has complete API documentation. ### BugFixes & Misc. -- Add support for older warp versions (<1.0.0) as it's not possible to run older isaac sim with -newer warp versions. + +- Minor documentation fixes to install instructions. +- Add support for older warp versions (<1.0.0) as it's not possible to run older isaac sim with newer warp versions. - Add override option to mpc dataclass. - Fix bug in ``PoseCost.forward_pose()`` which caused ``torch_layers_example.py`` to fail. - Add warp constants to make module hash depend on robot dof, for modules that generate runtime warp kernels. This fixes issues using cuRobo in isaac sim. - Add ``plan_config.timeout`` check to ``plan_single_js()``. +- Recreation of interpolation buffer now copies the joint names from raw trajectory. +- Fix bug in running captured cuda graph on deleted memory pointers +when getting metrics on interpolated trajectory +- Change order of operations in cuda graph capture of particle opt to get correct results +during graph capture phase. +- Franka Panda now works in Isaac Sim 4.0.0. The fix was to add inertial parameters to all links in +the urdf. +- Create new instances of rollouts in wrap classes to ensure cuda graph rollouts are not +accidentally used in other pipelines. +- Add cuda graph check for ``get_metrics``. +- Remove aligned address assumption for float arrays inside kernel (local memory). +- Add check for existing warp kernel in a module before creating a new one to avoid corruption of +existing cuda graphs. ## Version 0.7.3 diff --git a/benchmark/ik_benchmark.py b/benchmark/ik_benchmark.py index a649c1df..aed926ca 100644 --- a/benchmark/ik_benchmark.py +++ b/benchmark/ik_benchmark.py @@ -109,7 +109,7 @@ def run_full_config_collision_free_ik( parser.add_argument( "--save_path", type=str, - default=".", + default=None, help="path to save file", ) parser.add_argument( @@ -132,9 +132,9 @@ def run_full_config_collision_free_ik( ) args = parser.parse_args() - b_list = [1, 10, 100, 500, 2000][:] + b_list = [1, 10, 100, 2000][-1:] - robot_list = get_motion_gen_robot_list() + get_multi_arm_robot_list()[:2] + robot_list = get_motion_gen_robot_list() world_file = "collision_test.yml" print("running...") @@ -187,7 +187,13 @@ def run_full_config_collision_free_ik( data["Orientation-Error-Collision-Free-IK"].append(q_err_c) data["Collision-Free-IK-time(ms)"].append(dt_cu_ik_cfree * 1000.0) - write_yaml(data, join_path(args.save_path, args.file_name + ".yml")) + + if args.save_path is not None: + file_path = join_path(args.save_path, args.file_name) + else: + file_path = args.file_name + + write_yaml(data, file_path + ".yml") try: # Third Party @@ -195,7 +201,7 @@ def run_full_config_collision_free_ik( df = pd.DataFrame(data) print("Reported errors are 98th percentile") - df.to_csv(join_path(args.save_path, args.file_name + ".csv")) + df.to_csv(file_path + ".csv") try: # Third Party from tabulate import tabulate diff --git a/docker/aarch64.dockerfile b/docker/aarch64.dockerfile index 152aa197..b69ae91c 100644 --- a/docker/aarch64.dockerfile +++ b/docker/aarch64.dockerfile @@ -124,15 +124,16 @@ RUN pip3 install trimesh \ numpy-quaternion \ networkx \ pyyaml \ - rospkg \ + rospkg \ rosdep \ empy # Add cache date to avoid using cached layers older than this -ARG CACHE_DATE=2024-04-11 +ARG CACHE_DATE=2024-07-19 + # install warp: -# +# RUN pip3 install warp-lang # install curobo: @@ -165,7 +166,7 @@ RUN cd /pkgs && git clone https://github.com/nvlabs/nvblox_torch.git && \ sh install.sh $(python3 -c 'import torch.utils; print(torch.utils.cmake_prefix_path)') && \ python3 -m pip install -e . - + RUN python3 -m pip install "robometrics[evaluator] @ git+https://github.com/fishbotics/robometrics.git" # upgrade typing extensions: diff --git a/docker/build_dev_docker.sh b/docker/build_dev_docker.sh index 843a76c4..125502c0 100755 --- a/docker/build_dev_docker.sh +++ b/docker/build_dev_docker.sh @@ -35,4 +35,4 @@ fi echo $input_arg echo $USER_ID -docker build --build-arg USERNAME=$USER --build-arg USER_ID=${USER_ID} --build-arg IMAGE_TAG=$input_arg -f $user_dockerfile --tag curobo_docker:user_$input_arg . \ No newline at end of file +docker build --build-arg USERNAME=$USER --build-arg USER_ID=${USER_ID} --build-arg IMAGE_TAG=$input_arg -f $user_dockerfile --tag curobo_docker:user_$input_arg . \ No newline at end of file diff --git a/docker/build_docker.sh b/docker/build_docker.sh index 8159476c..74c556a5 100755 --- a/docker/build_docker.sh +++ b/docker/build_docker.sh @@ -32,16 +32,11 @@ if [ -z "$input_arg" ]; then fi fi -if [ "$input_arg" == "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 [ "$input_arg" == "isaac_sim_2023.1.0" ]; then +if [ "$input_arg" == "isaac_sim_4.0.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" + image_tag="isaac_sim_4.0.0" + isaac_sim_version="4.0.0" elif [ "$input_arg" == "x86" ]; then echo "Building for X86 Architecture" dockerfile="x86.dockerfile" @@ -63,11 +58,11 @@ fi # "nvidia": { # "path": "/usr/bin/nvidia-container-runtime", # "runtimeArgs": [] -# } +# } # }, # "default-runtime": "nvidia" # ADD this line (the above lines will already exist in your json file) # } -# +# echo "${dockerfile}" -docker build --build-arg ISAAC_SIM_VERSION=${isaac_sim_version} -t curobo_docker:${image_tag} -f ${dockerfile} . +docker build --build-arg ISAAC_SIM_VERSION=${isaac_sim_version} -t curobo_docker:${image_tag} -f ${dockerfile} . diff --git a/docker/isaac_sim.dockerfile b/docker/isaac_sim.dockerfile index 7ca0e0c4..07d901d1 100644 --- a/docker/isaac_sim.dockerfile +++ b/docker/isaac_sim.dockerfile @@ -11,7 +11,7 @@ ARG DEBIAN_FRONTEND=noninteractive ARG BASE_DIST=ubuntu20.04 ARG CUDA_VERSION=11.4.2 -ARG ISAAC_SIM_VERSION=2022.2.1 +ARG ISAAC_SIM_VERSION=4.0.0 FROM nvcr.io/nvidia/isaac-sim:${ISAAC_SIM_VERSION} AS isaac-sim @@ -20,7 +20,7 @@ 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} +#$FROM nvcr.io/nvidia/cuda:${CUDA_VERSION}-cudnn8-devel-${BASE_DIST} LABEL maintainer "User Name" @@ -102,7 +102,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ 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 \ + -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 \ @@ -118,7 +118,7 @@ RUN wget -q --show-progress \ && rm /tmp/vulkansdk-linux-x86_64-${VULKAN_SDK_VERSION}.tar.gz && rm -rf /opt/vulkan -# Setup the required capabilities for the container runtime +# Setup the required capabilities for the container runtime ENV NVIDIA_VISIBLE_DEVICES=all NVIDIA_DRIVER_CAPABILITIES=all # Open ports for live streaming @@ -157,16 +157,15 @@ 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 echo "alias omni_python='/isaac-sim/python.sh'" >> ~/.bashrc # Add cache date to avoid using cached layers older than this ARG CACHE_DATE=2024-04-11 -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 +# version of curobo there. Then uncomment below line and comment the next line that clones from # github # COPY pkgs /pkgs diff --git a/docker/start_docker.sh b/docker/start_docker.sh index d8963fa5..548dfe88 100755 --- a/docker/start_docker.sh +++ b/docker/start_docker.sh @@ -56,6 +56,7 @@ elif [ "$input_arg" == "aarch64" ]; then elif [[ "$input_arg" == *isaac_sim* ]] ; then + docker run --name container_$input_arg --entrypoint bash -it --gpus all -e "ACCEPT_EULA=Y" --rm --network=host \ --privileged \ -e "PRIVACY_CONSENT=Y" \ diff --git a/docker/user.dockerfile b/docker/user.dockerfile index 0b385e1d..595edb1a 100644 --- a/docker/user.dockerfile +++ b/docker/user.dockerfile @@ -15,7 +15,7 @@ FROM curobo_docker:${IMAGE_TAG} # Set variables ARG USERNAME ARG USER_ID -ARG CACHE_DATE=2024-04-25 +ARG CACHE_DATE=2024-07-19 # Set environment variables diff --git a/docker/user_isaac_sim.dockerfile b/docker/user_isaac_sim.dockerfile index 5f8a109f..939c72e4 100644 --- a/docker/user_isaac_sim.dockerfile +++ b/docker/user_isaac_sim.dockerfile @@ -10,11 +10,13 @@ ## # Check architecture and load: -ARG IMAGE_TAG +ARG IMAGE_TAG FROM curobo_docker:${IMAGE_TAG} # Set variables ARG USERNAME ARG USER_ID +ARG CACHE_DATE=2024-07-19 + # Set environment variables @@ -44,7 +46,7 @@ RUN chown -R $USERNAME:users /isaac-sim/exts/omni.isaac.synthetic_recorder/ RUN chown -R $USERNAME:users /isaac-sim/kit/exts/omni.gpu_foundation RUN mkdir -p /home/$USERNAME/.cache && cp -r /root/.cache/* /home/$USERNAME/.cache && chown -R $USERNAME:users /home/$USERNAME/.cache RUN mkdir -p /isaac-sim/kit/data/documents/Kit && mkdir -p /isaac-sim/kit/data/documents/Kit/apps/Isaac-Sim/scripts/ &&chown -R $USERNAME:users /isaac-sim/kit/data/documents/Kit /isaac-sim/kit/data/documents/Kit/apps/Isaac-Sim/scripts/ -RUN mkdir -p /home/$USERNAME/.local +RUN mkdir -p /home/$USERNAME/.local RUN echo "alias omni_python='/isaac-sim/python.sh'" >> /home/$USERNAME/.bashrc diff --git a/docker/x86.dockerfile b/docker/x86.dockerfile index 0bf970a9..9758c17d 100644 --- a/docker/x86.dockerfile +++ b/docker/x86.dockerfile @@ -78,7 +78,7 @@ ENV TORCH_CUDA_ARCH_LIST "7.0+PTX" ENV LD_LIBRARY_PATH="/usr/local/lib:${LD_LIBRARY_PATH}" # Add cache date to avoid using cached layers older than this -ARG CACHE_DATE=2024-04-25 +ARG CACHE_DATE=2024-07-19 RUN pip install "robometrics[evaluator] @ git+https://github.com/fishbotics/robometrics.git" diff --git a/examples/isaac_sim/batch_collision_checker.py b/examples/isaac_sim/batch_collision_checker.py index c377f8f7..19d40d9a 100644 --- a/examples/isaac_sim/batch_collision_checker.py +++ b/examples/isaac_sim/batch_collision_checker.py @@ -9,6 +9,12 @@ # its affiliates is strictly prohibited. # +try: + # Third Party + import isaacsim +except ImportError: + pass + # Third Party import torch diff --git a/examples/isaac_sim/batch_motion_gen_reacher.py b/examples/isaac_sim/batch_motion_gen_reacher.py index d488f68d..9b93944b 100644 --- a/examples/isaac_sim/batch_motion_gen_reacher.py +++ b/examples/isaac_sim/batch_motion_gen_reacher.py @@ -10,6 +10,12 @@ # +try: + # Third Party + import isaacsim +except ImportError: + pass + # Third Party import torch diff --git a/examples/isaac_sim/collision_checker.py b/examples/isaac_sim/collision_checker.py index d6a2b981..ca28f27a 100644 --- a/examples/isaac_sim/collision_checker.py +++ b/examples/isaac_sim/collision_checker.py @@ -9,6 +9,12 @@ # its affiliates is strictly prohibited. # +try: + # Third Party + import isaacsim +except ImportError: + pass + # Third Party import torch diff --git a/examples/isaac_sim/constrained_reacher.py b/examples/isaac_sim/constrained_reacher.py index 83f6041c..7b21b204 100644 --- a/examples/isaac_sim/constrained_reacher.py +++ b/examples/isaac_sim/constrained_reacher.py @@ -9,6 +9,12 @@ # its affiliates is strictly prohibited. # +try: + # Third Party + import isaacsim +except ImportError: + pass + # Third Party import torch @@ -156,7 +162,6 @@ tensor_args, collision_checker_type=CollisionCheckerType.MESH, collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh}, - velocity_scale=0.75, interpolation_dt=0.02, ee_link_name="right_gripper", ) @@ -180,7 +185,11 @@ cmd_plan = None articulation_controller = robot.get_articulation_controller() plan_config = MotionGenPlanConfig( - enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True + enable_graph=False, + enable_graph_attempt=4, + max_attempts=2, + enable_finetune_trajopt=True, + time_dilation_factor=0.5, ) plan_idx = 0 diff --git a/examples/isaac_sim/helper.py b/examples/isaac_sim/helper.py index b6c2f8e2..77a1c47c 100644 --- a/examples/isaac_sim/helper.py +++ b/examples/isaac_sim/helper.py @@ -31,7 +31,7 @@ from omni.isaac.urdf import _urdf # isaacsim 2022.2 except ImportError: # Third Party - from omni.importer.urdf import _urdf # isaac sim 2023.1 + from omni.importer.urdf import _urdf # isaac sim 2023.1 or above ISAAC_SIM_23 = True # Standard Library @@ -68,22 +68,23 @@ def add_robot_to_scene( robot_name: str = "robot", position: np.array = np.array([0, 0, 0]), ): - urdf_interface = _urdf.acquire_urdf_interface() + urdf_interface = _urdf.acquire_urdf_interface() + # Set the settings in the import config import_config = _urdf.ImportConfig() import_config.merge_fixed_joints = False import_config.convex_decomp = False - import_config.import_inertia_tensor = True import_config.fix_base = True - import_config.make_default_prim = False + import_config.make_default_prim = True import_config.self_collision = False import_config.create_physics_scene = True import_config.import_inertia_tensor = False - import_config.default_drive_strength = 20000 - import_config.default_position_drive_damping = 500 + import_config.default_drive_strength = 1047.19751 + import_config.default_position_drive_damping = 52.35988 import_config.default_drive_type = _urdf.UrdfJointTargetType.JOINT_DRIVE_POSITION import_config.distance_scale = 1 import_config.density = 0.0 + asset_path = get_assets_path() if ( "external_asset_path" in robot_config["kinematics"] @@ -115,18 +116,7 @@ def add_robot_to_scene( linkp = stage.GetPrimAtPath(robot_path) set_prim_transform(linkp, [position[0], position[1], position[2], 1, 0, 0, 0]) - if False and ISAAC_SIM_23: # this doesn't work in isaac sim 2023.1.1 - robot_p.set_solver_velocity_iteration_count(0) - robot_p.set_solver_position_iteration_count(44) - - my_world._physics_context.set_solver_type("PGS") - - if ISAAC_SIM_23: # fix to load robot correctly in isaac sim 2023.1.1 - linkp = stage.GetPrimAtPath(robot_path + "/" + base_link_name) - mass = UsdPhysics.MassAPI(linkp) - mass.GetMassAttr().Set(0) robot = my_world.scene.add(robot_p) - # robot_path = robot.prim_path return robot, robot_path diff --git a/examples/isaac_sim/ik_reachability.py b/examples/isaac_sim/ik_reachability.py index eb45ee50..a0e43355 100644 --- a/examples/isaac_sim/ik_reachability.py +++ b/examples/isaac_sim/ik_reachability.py @@ -9,6 +9,12 @@ # its affiliates is strictly prohibited. # +try: + # Third Party + import isaacsim +except ImportError: + pass + # Third Party import torch diff --git a/examples/isaac_sim/load_all_robots.py b/examples/isaac_sim/load_all_robots.py index d12812a5..64d6da24 100644 --- a/examples/isaac_sim/load_all_robots.py +++ b/examples/isaac_sim/load_all_robots.py @@ -9,6 +9,12 @@ # its affiliates is strictly prohibited. # +try: + # Third Party + import isaacsim +except ImportError: + pass + # Third Party import torch diff --git a/examples/isaac_sim/motion_gen_reacher.py b/examples/isaac_sim/motion_gen_reacher.py index 4704a77f..eaaa4bd0 100644 --- a/examples/isaac_sim/motion_gen_reacher.py +++ b/examples/isaac_sim/motion_gen_reacher.py @@ -10,6 +10,12 @@ # +try: + # Third Party + import isaacsim +except ImportError: + pass + # Third Party import torch @@ -238,6 +244,7 @@ def main(): max_attempts=max_attempts, enable_finetune_trajopt=True, parallel_finetune=True, + time_dilation_factor=0.5, ) usd_help.load_stage(my_world.stage) @@ -407,7 +414,7 @@ def main(): cmd_idx = 0 else: - carb.log_warn("Plan did not converge to a solution. No action is being taken.") + carb.log_warn("Plan did not converge to a solution: " + str(result.status)) target_pose = cube_position target_orientation = cube_orientation past_pose = cube_position diff --git a/examples/isaac_sim/motion_gen_reacher_nvblox.py b/examples/isaac_sim/motion_gen_reacher_nvblox.py index 1e97e08e..72703d91 100644 --- a/examples/isaac_sim/motion_gen_reacher_nvblox.py +++ b/examples/isaac_sim/motion_gen_reacher_nvblox.py @@ -10,6 +10,12 @@ # +try: + # Third Party + import isaacsim +except ImportError: + pass + # Third Party import torch diff --git a/examples/isaac_sim/mpc_example.py b/examples/isaac_sim/mpc_example.py index 7858d50d..0f287fe3 100644 --- a/examples/isaac_sim/mpc_example.py +++ b/examples/isaac_sim/mpc_example.py @@ -11,10 +11,11 @@ # -# script running (ubuntu): -# - -############################################################ +try: + # Third Party + import isaacsim +except ImportError: + pass # Third Party diff --git a/examples/isaac_sim/mpc_nvblox_example.py b/examples/isaac_sim/mpc_nvblox_example.py index 487d6146..d3ead0a5 100644 --- a/examples/isaac_sim/mpc_nvblox_example.py +++ b/examples/isaac_sim/mpc_nvblox_example.py @@ -11,10 +11,11 @@ # -# script running (ubuntu): -# - -############################################################ +try: + # Third Party + import isaacsim +except ImportError: + pass # Third Party @@ -241,7 +242,7 @@ def main(): step_index = my_world.current_time_step_index - if step_index == 0: + if step_index <= 2: my_world.reset() idx_list = [robot.get_dof_index(x) for x in j_names] robot.set_joint_positions(default_config, idx_list) diff --git a/examples/isaac_sim/multi_arm_reacher.py b/examples/isaac_sim/multi_arm_reacher.py index 4545a89f..4ecc17e4 100644 --- a/examples/isaac_sim/multi_arm_reacher.py +++ b/examples/isaac_sim/multi_arm_reacher.py @@ -9,6 +9,12 @@ # its affiliates is strictly prohibited. # +try: + # Third Party + import isaacsim +except ImportError: + pass + # Third Party import torch @@ -138,14 +144,11 @@ def main(): tensor_args, collision_checker_type=CollisionCheckerType.MESH, use_cuda_graph=True, - num_trajopt_seeds=12, - num_graph_seeds=12, interpolation_dt=0.03, collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh}, collision_activation_distance=0.025, - acceleration_scale=1.0, fixed_iters_trajopt=True, - trajopt_tsteps=40, + maximum_trajectory_dt=0.5, ) motion_gen = MotionGen(motion_gen_config) print("warming up...") @@ -157,6 +160,7 @@ def main(): enable_graph=False, enable_graph_attempt=4, max_attempts=10, + time_dilation_factor=0.5, ) usd_help.load_stage(my_world.stage) @@ -329,7 +333,7 @@ def main(): cmd_idx = 0 else: - carb.log_warn("Plan did not converge to a solution. No action is being taken.") + carb.log_warn("Plan did not converge to a solution: " + str(result.status)) target_pose = cube_position past_pose = cube_position if cmd_plan is not None: diff --git a/examples/isaac_sim/realsense_collision.py b/examples/isaac_sim/realsense_collision.py index 008ff537..dd720ef2 100644 --- a/examples/isaac_sim/realsense_collision.py +++ b/examples/isaac_sim/realsense_collision.py @@ -9,6 +9,13 @@ # its affiliates is strictly prohibited. # + +try: + # Third Party + import isaacsim +except ImportError: + pass + # Third Party import torch @@ -38,11 +45,24 @@ from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig simulation_app.update() +# Standard Library +import argparse + # Third Party from omni.isaac.core import World from omni.isaac.core.materials import OmniPBR from omni.isaac.core.objects import cuboid, sphere +parser = argparse.ArgumentParser() + +parser.add_argument( + "--show-window", + action="store_true", + help="When True, shows camera image in a CV window", + default=False, +) +args = parser.parse_args() + def draw_points(voxels): # Third Party @@ -216,20 +236,21 @@ def draw_line(start, gradient): bounding = Cuboid("t", dims=[1, 1, 1], pose=[0, 0, 0, 1, 0, 0, 0]) voxels = model.world_model.get_voxels_in_bounding_box(bounding, 0.025) # print(data_camera.depth_image) - depth_image = data["raw_depth"] - color_image = data["raw_rgb"] - depth_colormap = cv2.applyColorMap( - cv2.convertScaleAbs(depth_image, alpha=100), cv2.COLORMAP_VIRIDIS - ) - images = np.hstack((color_image, depth_colormap)) - - cv2.namedWindow("Align Example", cv2.WINDOW_NORMAL) - cv2.imshow("Align Example", images) - key = cv2.waitKey(1) - # Press esc or 'q' to close the image window - if key & 0xFF == ord("q") or key == 27: - cv2.destroyAllWindows() - break + if args.show_window: + depth_image = data["raw_depth"] + color_image = data["raw_rgb"] + depth_colormap = cv2.applyColorMap( + cv2.convertScaleAbs(depth_image, alpha=100), cv2.COLORMAP_VIRIDIS + ) + images = np.hstack((color_image, depth_colormap)) + + cv2.namedWindow("Align Example", cv2.WINDOW_NORMAL) + cv2.imshow("Align Example", images) + key = cv2.waitKey(1) + # Press esc or 'q' to close the image window + if key & 0xFF == ord("q") or key == 27: + cv2.destroyAllWindows() + break draw_points(voxels) d, d_vec = model.get_collision_vector(x_sph) diff --git a/examples/isaac_sim/realsense_mpc.py b/examples/isaac_sim/realsense_mpc.py index 9a650d8b..c19fd8e0 100644 --- a/examples/isaac_sim/realsense_mpc.py +++ b/examples/isaac_sim/realsense_mpc.py @@ -9,6 +9,12 @@ # its affiliates is strictly prohibited. # +try: + # Third Party + import isaacsim +except ImportError: + pass + # Third Party import torch @@ -64,9 +70,17 @@ parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load") + parser.add_argument( "--waypoints", action="store_true", help="When True, sets robot in static mode", default=False ) +parser.add_argument( + "--show-window", + action="store_true", + help="When True, shows camera image in a CV window", + default=False, +) + parser.add_argument( "--use-debug-draw", action="store_true", @@ -330,7 +344,7 @@ def draw_line(start, gradient): if cmd_step_idx == 0: draw_rollout_points(mpc.get_visual_rollouts(), clear=not args.use_debug_draw) - if step_index < 2: + 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) @@ -374,7 +388,7 @@ def draw_line(start, gradient): if not args.use_debug_draw: voxel_viewer.clear() - if True: + if args.show_window: depth_image = data["raw_depth"] color_image = data["raw_rgb"] depth_colormap = cv2.applyColorMap( @@ -384,7 +398,6 @@ def draw_line(start, gradient): depth_colormap = cv2.flip(depth_colormap, 1) images = np.hstack((color_image, depth_colormap)) - cv2.namedWindow("NVBLOX Example", cv2.WINDOW_NORMAL) cv2.imshow("NVBLOX Example", images) key = cv2.waitKey(1) diff --git a/examples/isaac_sim/realsense_reacher.py b/examples/isaac_sim/realsense_reacher.py index e20d92ac..5e8aa45f 100644 --- a/examples/isaac_sim/realsense_reacher.py +++ b/examples/isaac_sim/realsense_reacher.py @@ -9,6 +9,12 @@ # its affiliates is strictly prohibited. # +try: + # Third Party + import isaacsim +except ImportError: + pass + # Third Party import torch @@ -60,6 +66,12 @@ parser = argparse.ArgumentParser() +parser.add_argument( + "--show-window", + action="store_true", + help="When True, shows camera image in a CV window", + default=False, +) parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load") parser.add_argument( @@ -316,7 +328,7 @@ def draw_line(start, gradient): voxel_viewer.clear() # draw_points(voxels) - if True: + if args.show_window: depth_image = data["raw_depth"] color_image = data["raw_rgb"] depth_colormap = cv2.applyColorMap( diff --git a/examples/isaac_sim/simple_stacking.py b/examples/isaac_sim/simple_stacking.py index aaadc479..790987da 100644 --- a/examples/isaac_sim/simple_stacking.py +++ b/examples/isaac_sim/simple_stacking.py @@ -9,6 +9,14 @@ # its affiliates is strictly prohibited. # + +try: + # Third Party + import isaacsim +except ImportError: + pass + + # Third Party import torch @@ -154,7 +162,6 @@ def __init__( collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh}, store_ik_debug=self._save_log, store_trajopt_debug=self._save_log, - velocity_scale=0.75, ) self.motion_gen = MotionGen(motion_gen_config) print("warming up...") @@ -173,6 +180,7 @@ def __init__( partial_ik_opt=False, parallel_finetune=True, pose_cost_metric=pose_metric, + time_dilation_factor=0.75, ) self.usd_help.load_stage(self.my_world.stage) self.cmd_plan = None @@ -455,7 +463,7 @@ def set_robot(self) -> Franka: print(articulation_controller.get_max_efforts()) robot = my_franka print("**********************") -if True: +if False: robot.enable_gravity() articulation_controller.set_gains( kps=np.array( diff --git a/examples/isaac_sim/util/convert_urdf_to_usd.py b/examples/isaac_sim/util/convert_urdf_to_usd.py index f2c0f2f6..5648941c 100644 --- a/examples/isaac_sim/util/convert_urdf_to_usd.py +++ b/examples/isaac_sim/util/convert_urdf_to_usd.py @@ -9,6 +9,13 @@ # its affiliates is strictly prohibited. # + +try: + # Third Party + import isaacsim +except ImportError: + pass + # Third Party import torch diff --git a/examples/isaac_sim/util/dowload_assets.py b/examples/isaac_sim/util/dowload_assets.py index b5e38122..f8458f7f 100644 --- a/examples/isaac_sim/util/dowload_assets.py +++ b/examples/isaac_sim/util/dowload_assets.py @@ -12,6 +12,13 @@ # This script downloads robot usd assets from isaac sim for using in CuRobo. +try: + # Third Party + import isaacsim +except ImportError: + pass + + # Third Party import torch diff --git a/examples/mpc_example.py b/examples/mpc_example.py index 06bb92ec..1bf6b2c3 100644 --- a/examples/mpc_example.py +++ b/examples/mpc_example.py @@ -55,12 +55,6 @@ def demo_full_config_mpc(): mpc_config = MpcSolverConfig.load_from_robot_config( robot_cfg, world_file, - use_cuda_graph=True, - use_cuda_graph_metrics=True, - use_cuda_graph_full_step=False, - use_lbfgs=False, - use_es=False, - use_mppi=True, store_rollouts=True, step_dt=0.03, ) diff --git a/setup.py b/setup.py index 11201a9a..393d4894 100644 --- a/setup.py +++ b/setup.py @@ -17,6 +17,9 @@ # References: # * https://setuptools.pypa.io/en/latest/setuptools.html#setup-cfg-only-projects +# Standard Library +import sys + # Third Party import setuptools from torch.utils.cpp_extension import BuildExtension, CUDAExtension @@ -31,6 +34,10 @@ "--prec-sqrt=false", ] } + +if sys.platform == "win32": + extra_cuda_args["nvcc"].append("--allow-unsupported-compiler") + # create a list of modules to be compiled: ext_modules = [ CUDAExtension( diff --git a/src/curobo/content/assets/robot/franka_description/franka_panda.urdf b/src/curobo/content/assets/robot/franka_description/franka_panda.urdf index 1821c7a9..91a5a9fa 100644 --- a/src/curobo/content/assets/robot/franka_description/franka_panda.urdf +++ b/src/curobo/content/assets/robot/franka_description/franka_panda.urdf @@ -19,6 +19,8 @@ + + @@ -31,6 +33,11 @@ + + + + + @@ -52,6 +59,11 @@ + + + + + @@ -73,6 +85,11 @@ + + + + + @@ -94,13 +111,18 @@ + + + + + - +