├── motion_policy_configs └── ur3e │ ├── rmpflow │ ├── config.json │ ├── ur3e_gripper_rmpflow_config.yaml │ └── ur3e_gripper_robot_description.yaml │ └── ur3e_gripper.urdf ├── Isaac_launch ├── ur3e │ ├── __init__.py │ └── ur3e.py ├── rtde │ ├── __init__.py │ ├── rtde_config.py │ ├── csv_writer.py │ ├── csv_reader.py │ ├── csv_binary_writer.py │ ├── serialize.py │ └── rtde.py ├── Gripper_control.py ├── isaac_rtde_gripper.py ├── isaac_rtde_dual_arm.py ├── helper.py ├── motion_gen_reacher.py ├── multi_arm_reacher_MT.py ├── record_configuration.xml └── simple_stacking_ur.py ├── Docker ├── build_docker.sh ├── start_docker.sh └── isaac_sim.dockerfile └── README.md /motion_policy_configs/ur3e/rmpflow/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "end_effector_frame_name": "gripper_center", 3 | "maximum_substep_size" : 0.00334, 4 | "ignore_robot_state_updates" : false, 5 | "relative_asset_paths": { 6 | "robot_description_path" : "ur3e_gripper_robot_description.yaml", 7 | "urdf_path": "../ur3e_gripper.urdf", 8 | "rmpflow_config_path" : "ur3e_gripper_rmpflow_config.yaml" 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /Isaac_launch/ur3e/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021-2023, NVIDIA CORPORATION. All rights reserved. 2 | # 3 | # NVIDIA CORPORATION and its licensors retain all intellectual property 4 | # and proprietary rights in and to this software, related documentation 5 | # and any modifications thereto. Any use, reproduction, disclosure or 6 | # distribution of this software and related documentation without an express 7 | # license agreement from NVIDIA CORPORATION is strictly prohibited. 8 | # 9 | from .ur3e import ur3e 10 | # from omni.isaac.ur3e.kinematics_solver import KinematicsSolver 11 | -------------------------------------------------------------------------------- /Isaac_launch/rtde/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | from .rtde import * 25 | from .rtde_config import * 26 | -------------------------------------------------------------------------------- /Isaac_launch/rtde/rtde_config.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import xml.etree.ElementTree as ET 25 | 26 | 27 | class Recipe(object): 28 | __slots__ = ["key", "names", "types"] 29 | 30 | @staticmethod 31 | def parse(recipe_node): 32 | rmd = Recipe() 33 | rmd.key = recipe_node.get("key") 34 | rmd.names = [f.get("name") for f in recipe_node.findall("field")] 35 | rmd.types = [f.get("type") for f in recipe_node.findall("field")] 36 | return rmd 37 | 38 | 39 | class ConfigFile(object): 40 | def __init__(self, filename): 41 | self.__filename = filename 42 | tree = ET.parse(self.__filename) 43 | root = tree.getroot() 44 | recipes = [Recipe.parse(r) for r in root.findall("recipe")] 45 | self.__dictionary = dict() 46 | for r in recipes: 47 | self.__dictionary[r.key] = r 48 | 49 | def get_recipe(self, key): 50 | r = self.__dictionary[key] 51 | return r.names, r.types 52 | -------------------------------------------------------------------------------- /Docker/build_docker.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | ## 3 | ## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 4 | ## 5 | ## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 6 | ## property and proprietary rights in and to this material, related 7 | ## documentation and any modifications thereto. Any use, reproduction, 8 | ## disclosure or distribution of this material and related documentation 9 | ## without an express license agreement from NVIDIA CORPORATION or 10 | ## its affiliates is strictly prohibited. 11 | ## 12 | 13 | 14 | # This script will create a dev docker. Run this script by calling `bash build_dev_docker.sh` 15 | # If you want to build a isaac sim docker, run this script with `bash build_dev_docker.sh isaac` 16 | 17 | # Check architecture to build: 18 | 19 | image_tag="x86" 20 | isaac_sim_version="" 21 | input_arg="$1" 22 | 23 | if [ -z "$input_arg" ]; then 24 | arch=$(uname -m) 25 | echo "Argument empty, trying to build based on architecture" 26 | if [ "$arch" == "x86_64" ]; then 27 | input_arg="x86" 28 | elif [ "$arch" == "arm64" ]; then 29 | input_arg="aarch64" 30 | elif [ "$arch" == "aarch64" ]; then 31 | input_arg="aarch64" 32 | fi 33 | fi 34 | 35 | if [ "$input_arg" == "isaac_sim_2022.2.1" ]; then 36 | echo "Building Isaac Sim docker" 37 | dockerfile="isaac_sim.dockerfile" 38 | image_tag="isaac_sim_2022.2.1" 39 | isaac_sim_version="2022.2.1" 40 | elif [ "$input_arg" == "isaac_sim_2023.1.0" ]; then 41 | echo "Building Isaac Sim headless docker" 42 | dockerfile="isaac_sim.dockerfile" 43 | image_tag="isaac_sim_2023.1.0" 44 | isaac_sim_version="2023.1.0" 45 | elif [ "$input_arg" == "x86" ]; then 46 | echo "Building for X86 Architecture" 47 | dockerfile="x86.dockerfile" 48 | image_tag="x86" 49 | elif [ "$input_arg" = "aarch64" ]; then 50 | echo "Building for ARM Architecture" 51 | dockerfile="aarch64.dockerfile" 52 | image_tag="aarch64" 53 | else 54 | echo "Unknown Argument. Please pass one of [x86, aarch64, isaac_sim_2022.2.1, isaac_sim_2023.1.0]" 55 | exit 56 | fi 57 | 58 | # build docker file: 59 | # Make sure you enable nvidia runtime by: 60 | # Edit/create the /etc/docker/daemon.json with content: 61 | # { 62 | # "runtimes": { 63 | # "nvidia": { 64 | # "path": "/usr/bin/nvidia-container-runtime", 65 | # "runtimeArgs": [] 66 | # } 67 | # }, 68 | # "default-runtime": "nvidia" # ADD this line (the above lines will already exist in your json file) 69 | # } 70 | # 71 | echo "${dockerfile}" 72 | 73 | docker build --build-arg ISAAC_SIM_VERSION=${isaac_sim_version} -t curobo_docker:${image_tag} -f ${dockerfile} . 74 | -------------------------------------------------------------------------------- /Isaac_launch/rtde/csv_writer.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import csv 25 | 26 | import sys 27 | 28 | sys.path.append("..") 29 | 30 | from rtde import serialize 31 | 32 | 33 | class CSVWriter(object): 34 | def __init__(self, csvfile, names, types, delimiter=" "): 35 | if len(names) != len(types): 36 | raise ValueError("List sizes are not identical.") 37 | self.__names = names 38 | self.__types = types 39 | self.__header_names = [] 40 | self.__columns = 0 41 | for i in range(len(self.__names)): 42 | size = serialize.get_item_size(self.__types[i]) 43 | self.__columns += size 44 | if size > 1: 45 | for j in range(size): 46 | name = self.__names[i] + "_" + str(j) 47 | self.__header_names.append(name) 48 | else: 49 | name = self.__names[i] 50 | self.__header_names.append(name) 51 | self.__writer = csv.writer(csvfile, delimiter=delimiter) 52 | 53 | def writeheader(self): 54 | self.__writer.writerow(self.__header_names) 55 | 56 | def writerow(self, data_object): 57 | data = [] 58 | for i in range(len(self.__names)): 59 | size = serialize.get_item_size(self.__types[i]) 60 | value = data_object.__dict__[self.__names[i]] 61 | if size > 1: 62 | data.extend(value) 63 | else: 64 | data.append(value) 65 | self.__writer.writerow(data) 66 | -------------------------------------------------------------------------------- /Docker/start_docker.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | ## 3 | ## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 4 | ## 5 | ## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 6 | ## property and proprietary rights in and to this material, related 7 | ## documentation and any modifications thereto. Any use, reproduction, 8 | ## disclosure or distribution of this material and related documentation 9 | ## without an express license agreement from NVIDIA CORPORATION or 10 | ## its affiliates is strictly prohibited. 11 | ## 12 | 13 | input_arg="$1" 14 | 15 | if [ -z "$input_arg" ]; then 16 | echo "Argument empty, trying to run based on architecture" 17 | arch=$(uname -m) 18 | if [ "$arch" == "x86_64" ]; then 19 | input_arg="x86" 20 | elif [ "$arch" == "arm64" ]; then 21 | input_arg="aarch64" 22 | elif [ "$arch" == "aarch64" ]; then 23 | input_arg="aarch64" 24 | fi 25 | fi 26 | 27 | if [ "$input_arg" == "x86" ]; then 28 | 29 | docker run --rm -it \ 30 | --privileged \ 31 | -e NVIDIA_DISABLE_REQUIRE=1 \ 32 | -e NVIDIA_DRIVER_CAPABILITIES=all --device /dev/dri \ 33 | --hostname ros1-docker \ 34 | --add-host ros1-docker:127.0.0.1 \ 35 | --gpus all \ 36 | --network host \ 37 | --env DISPLAY=unix$DISPLAY \ 38 | --volume /tmp/.X11-unix:/tmp/.X11-unix \ 39 | --volume /dev:/dev \ 40 | curobo_docker:$input_arg 41 | 42 | elif [ "$input_arg" == "aarch64" ]; then 43 | 44 | docker run --rm -it \ 45 | --privileged \ 46 | --runtime nvidia \ 47 | --hostname ros1-docker \ 48 | --add-host ros1-docker:127.0.0.1 \ 49 | --network host \ 50 | --gpus all \ 51 | --env ROS_HOSTNAME=localhost \ 52 | --env DISPLAY=$DISPLAY \F 53 | --volume /tmp/.X11-unix:/tmp/.X11-unix \ 54 | --volume /dev/input:/dev/input \ 55 | curobo_docker:$input_arg 56 | 57 | elif [[ "$input_arg" == *isaac_sim* ]] ; then 58 | SCRIPT_DIR="$(dirname "$(realpath "$0")")" 59 | docker run --name container_$input_arg --entrypoint bash -it --gpus all -e "ACCEPT_EULA=Y" --rm --network=host \ 60 | --privileged \ 61 | -e "PRIVACY_CONSENT=Y" \ 62 | -e "RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" \ 63 | -e "LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/isaac-sim/exts/omni.isaac.ros2_bridge/foxy/lib" \ 64 | -v $HOME/.Xauthority:/root/.Xauthority \ 65 | -e DISPLAY \ 66 | -v ~/docker/isaac-sim/cache/kit:/isaac-sim/kit/cache:rw \ 67 | -v ~/docker/isaac-sim/cache/ov:/root/.cache/ov:rw \ 68 | -v ~/docker/isaac-sim/cache/pip:/root/.cache/pip:rw \ 69 | -v ~/docker/isaac-sim/cache/glcache:/root/.cache/nvidia/GLCache:rw \ 70 | -v ~/docker/isaac-sim/cache/computecache:/root/.nv/ComputeCache:rw \ 71 | -v ~/docker/isaac-sim/logs:/root/.nvidia-omniverse/logs:rw \ 72 | -v ~/docker/isaac-sim/data:/root/.local/share/ov/data:rw \ 73 | -v ~/docker/isaac-sim/documents:/home/$USER/Documents:rw \ 74 | -v "$SCRIPT_DIR../../Isaac_launch/:/pkgs/curobo/examples/Isaac_launch:rw"\ 75 | --volume /dev:/dev \ 76 | curobo_docker:$input_arg 77 | 78 | else 79 | echo "Unknown docker" 80 | fi 81 | -------------------------------------------------------------------------------- /motion_policy_configs/ur3e/rmpflow/ur3e_gripper_rmpflow_config.yaml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved. 2 | # 3 | # NVIDIA CORPORATION and its licensors retain all intellectual property 4 | # and proprietary rights in and to this software, related documentation 5 | # and any modifications thereto. Any use, reproduction, disclosure or 6 | # distribution of this software and related documentation without an express 7 | # license agreement from NVIDIA CORPORATION is strictly prohibited. 8 | 9 | api_version: 1.0 10 | 11 | joint_limit_buffers: [.01, .01, .01, .01, .01, .01] 12 | 13 | rmp_params: 14 | cspace_target_rmp: 15 | metric_scalar: 50. 16 | position_gain: 100. 17 | damping_gain: 50. 18 | robust_position_term_thresh: .5 19 | inertia: 1. 20 | cspace_trajectory_rmp: 21 | p_gain: 80. 22 | d_gain: 10. 23 | ff_gain: .25 24 | weight: 50. 25 | cspace_affine_rmp: 26 | final_handover_time_std_dev: .25 27 | weight: 2000. 28 | joint_limit_rmp: 29 | metric_scalar: 1000. 30 | metric_length_scale: .01 31 | metric_exploder_eps: 1e-3 32 | metric_velocity_gate_length_scale: .01 33 | accel_damper_gain: 200. 34 | accel_potential_gain: 1. 35 | accel_potential_exploder_length_scale: .1 36 | accel_potential_exploder_eps: 1e-2 37 | joint_velocity_cap_rmp: 38 | max_velocity: 2.15 39 | velocity_damping_region: 0.5 40 | damping_gain: 300. 41 | metric_weight: 100. 42 | target_rmp: 43 | accel_p_gain: 80. 44 | accel_d_gain: 120. 45 | accel_norm_eps: .075 46 | metric_alpha_length_scale: .05 47 | min_metric_alpha: .01 48 | max_metric_scalar: 10000. 49 | min_metric_scalar: 2500. 50 | proximity_metric_boost_scalar: 20. 51 | proximity_metric_boost_length_scale: .02 52 | accept_user_weights: false 53 | axis_target_rmp: 54 | accel_p_gain: 200. 55 | accel_d_gain: 40. 56 | metric_scalar: 10. 57 | proximity_metric_boost_scalar: 3000. 58 | proximity_metric_boost_length_scale: .05 59 | accept_user_weights: false 60 | collision_rmp: 61 | damping_gain: 50. 62 | damping_std_dev: .04 63 | damping_robustness_eps: 1e-2 64 | damping_velocity_gate_length_scale: .01 65 | repulsion_gain: 1200. 66 | repulsion_std_dev: .01 67 | metric_modulation_radius: .5 68 | metric_scalar: 10000. 69 | metric_exploder_std_dev: .02 70 | metric_exploder_eps: .001 71 | damping_rmp: 72 | accel_d_gain: 30. 73 | metric_scalar: 50. 74 | inertia: 100. 75 | 76 | canonical_resolve: 77 | max_acceleration_norm: 50. 78 | projection_tolerance: .01 79 | verbose: false 80 | 81 | body_cylinders: 82 | - name: base_link 83 | pt1: [0, 0, 0.15] 84 | pt2: [0, 0, 0] 85 | radius: .065 86 | 87 | body_collision_controllers: 88 | - name: wrist_2_link 89 | radius: .04 90 | - name: wrist_3_link 91 | radius: .04 92 | - name: tool0 93 | radius: .04 94 | - name: single_finger_left 95 | radius: .04 96 | - name: single_finger_right 97 | radius: .04 98 | -------------------------------------------------------------------------------- /motion_policy_configs/ur3e/rmpflow/ur3e_gripper_robot_description.yaml: -------------------------------------------------------------------------------- 1 | # The robot description defines the generalized coordinates and how to map those 2 | # to the underlying URDF dofs. 3 | 4 | api_version: 1.0 5 | 6 | # Defines the generalized coordinates. Each generalized coordinate is assumed 7 | # to have an entry in the URDF. 8 | # Lula will only use these joints to control the robot position. 9 | cspace: 10 | - shoulder_pan_joint 11 | - shoulder_lift_joint 12 | - elbow_joint 13 | - wrist_1_joint 14 | - wrist_2_joint 15 | - wrist_3_joint 16 | default_q: [ 17 | 0.0,-1.0,0.9,0.0,0.0,0.0 18 | ] 19 | 20 | # Most dimensions of the cspace have a direct corresponding element 21 | # in the URDF. This list of rules defines how unspecified coordinates 22 | # should be extracted or how values in the URDF should be overwritten. 23 | 24 | cspace_to_urdf_rules: 25 | 26 | # Lula uses collision spheres to define the robot geometry in order to avoid 27 | # collisions with external obstacles. If no spheres are specified, Lula will 28 | # not be able to avoid obstacles. 29 | 30 | collision_spheres: 31 | - shoulder_link: 32 | - "center": [-0.0, 0.0, -0.02] 33 | "radius": 0.055 34 | - "center": [0.01, -0.019, -0.0] 35 | "radius": 0.045 36 | - "center": [0.004, -0.007, 0.019] 37 | "radius": 0.05 38 | - wrist_1_link: 39 | - "center": [0.0, -0.009, -0.002] 40 | "radius": 0.041 41 | - "center": [-0.003, 0.019, 0.001] 42 | "radius": 0.037 43 | - "center": [0.006, 0.007, -0.024] 44 | "radius": 0.033 45 | - wrist_2_link: 46 | - "center": [-0.0, 0.0, -0.015] 47 | "radius": 0.041 48 | - "center": [-0.0, 0.012, 0.001] 49 | "radius": 0.039 50 | - "center": [-0.0, -0.018, -0.001] 51 | "radius": 0.04 52 | - wrist_3_link: 53 | - "center": [0.0, 0.002, -0.025] 54 | "radius": 0.035 55 | - upper_arm_link: 56 | - "center": [-0.008, 0.0, 0.127] 57 | "radius": 0.056 58 | - "center": [-0.091, 0.0, 0.127] 59 | "radius": 0.054 60 | - "center": [-0.174, -0.0, 0.13] 61 | "radius": 0.051 62 | - "center": [-0.242, -0.0, 0.106] 63 | "radius": 0.048 64 | - "center": [-0.15, 0.0, 0.105] 65 | "radius": 0.051 66 | - "center": [0.0, 0.0, 0.11] 67 | "radius": 0.056 68 | - "center": [-0.245, 0.005, 0.143] 69 | "radius": 0.043 70 | - "center": [-0.058, -0.002, 0.105] 71 | "radius": 0.052 72 | - "center": [-0.055, 0.001, 0.132] 73 | "radius": 0.055 74 | - "center": [-0.14, 0.0, 0.133] 75 | "radius": 0.052 76 | - forearm_link: 77 | - "center": [-0.084, -0.0, 0.033] 78 | "radius": 0.044 79 | - "center": [-0.157, -0.0, 0.035] 80 | "radius": 0.043 81 | - "center": [-0.008, -0.0, 0.053] 82 | "radius": 0.043 83 | - "center": [-0.213, 0.0, 0.074] 84 | "radius": 0.042 85 | - "center": [-0.213, -0.0, 0.021] 86 | "radius": 0.042 87 | - "center": [-0.13, -0.0, 0.022] 88 | "radius": 0.044 89 | - "center": [-0.003, -0.003, 0.041] 90 | "radius": 0.037 91 | - "center": [-0.118, 0.001, 0.039] 92 | "radius": 0.044 93 | - "center": [-0.059, -0.001, 0.037] 94 | "radius": 0.044 95 | - "center": [-0.168, -0.0, 0.016] 96 | "radius": 0.043 97 | - single_base: 98 | - "center": [-0.0, 0.0, -0.02] 99 | "radius": 0.055 100 | - "center": [0.01, -0.019, -0.0] 101 | "radius": 0.045 102 | - "center": [0.004, -0.007, 0.019] 103 | "radius": 0.05 104 | -------------------------------------------------------------------------------- /Isaac_launch/rtde/csv_reader.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import csv 25 | import numpy as np 26 | import logging 27 | 28 | from .rtde import LOGNAME 29 | 30 | _log = logging.getLogger(LOGNAME) 31 | 32 | 33 | runtime_state = "runtime_state" 34 | runtime_state_running = "2" 35 | 36 | 37 | class CSVReader(object): 38 | __samples = None 39 | __filename = None 40 | 41 | def get_header_data(self, __reader): 42 | header = next(__reader) 43 | return header 44 | 45 | def __init__(self, csvfile, delimiter=" ", filter_running_program=False): 46 | self.__filename = csvfile.name 47 | 48 | csvfile = [ 49 | csvfile for csvfile in csvfile.readlines() if csvfile.strip() 50 | ] # remove any empty lines 51 | 52 | reader = csv.reader(csvfile, delimiter=delimiter) 53 | header = self.get_header_data(reader) 54 | 55 | # read csv file 56 | data = [row for row in reader] 57 | 58 | if len(data) == 0: 59 | _log.warn("No data read from file: " + self.__filename) 60 | 61 | # filter data 62 | if filter_running_program: 63 | if runtime_state not in header: 64 | _log.warn( 65 | "Unable to filter data since runtime_state field is missing in data set" 66 | ) 67 | else: 68 | idx = header.index(runtime_state) 69 | data = [row for row in data if row[idx] == runtime_state_running] 70 | 71 | self.__samples = len(data) 72 | 73 | if self.__samples == 0: 74 | _log.warn("No data left from file: " + self.__filename + " after filtering") 75 | 76 | # transpose data 77 | data = list(zip(*data)) 78 | 79 | # create dictionary from header elements (keys) to float arrays 80 | self.__dict__.update( 81 | { 82 | header[i]: np.array(list(map(float, data[:][i]))) 83 | for i in range(len(header)) 84 | } 85 | ) 86 | 87 | def get_samples(self): 88 | return self.__samples 89 | 90 | def get_name(self): 91 | return self.__filename 92 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Overview 2 | 3 | 🤖 Simulate and control two Universal Robots (UR) arms concurrently using NVIDIA Isaac Sim. This project provides a digital twin environment for realistic robotic simulation and testing control algorithms. 4 | 5 | 6 | https://github.com/MetaToolEU/MT_Isaac_sim/assets/28174056/f53435bb-87a1-4ff9-9b28-50b225b56b98 7 | 8 | 9 | ## Features 10 | 11 | - **Dual UR Arm Simulation**: Simulate UR3e arms simultaneously. 12 | - **Real-Time Digital Twin Control**: Implement real-time control using UR RTDE interface. 13 | - **Collision Avoidance**: Integrate collision detection for safe operations. 14 | - **Path Planning**: Develop optimized trajectories using Isaac Sim extensions. 15 | 16 | 17 | ## System Requirements 18 | 19 | To run the Docker-based simulation tutorial, ensure your system meets the following requirements: 20 | 21 | - **Operating System**: Ubuntu 20.04 LTS or later (preferred); Docker is also available for Windows and macOS. 22 | - **Docker**: Docker Engine and Docker Compose installed (version 1.27+ recommended). 23 | - **CPU**: Multi-core processor (e.g., Intel Core i7 or equivalent). 24 | - **GPU**: NVIDIA GPU with CUDA support (e.g., RTX 2060 or higher) and driver version **525.60.11** or later. 25 | - **RAM**: Minimum 8 GB (16 GB recommended). 26 | - **Storage**: At least 50 GB of free disk space for Docker images and data. 27 | - **Network**: Stable internet connection for downloading Docker images and dependencies. 28 | 29 | **Important**: Ensure Docker is properly configured to utilize the GPU if required and that your system has adequate resources for smooth operation. Verify GPU availability with the following command: 30 | 31 | ```bash 32 | docker run --rm --gpus all nvidia/cuda:11.2.2-base-ubuntu20.04 nvidia-smi 33 | ``` 34 | ### Additional Requirements 35 | - [Isaac Sim](https://docs.omniverse.nvidia.com/isaacsim/latest/installation/install_workstation.html) 36 | - Python 3.7 or later 37 | - [UR RTDE](https://sdurobotics.gitlab.io/ur_rtde/) 38 | - Curobo v0.6.2 (The project is developed and tested on Curobo version 0.6.2) 39 | ## Setup using Docker (RECOMENDED) 40 | 1. **Setup NVIDIA GPU Cloud (NGC) Environment:** 41 | 42 | Ensure you have an NGC API key. Follow the instructions [here](https://docs.nvidia.com/ngc/gpu-cloud/ngc-user-guide/index.html#generating-api-key) to generate one. 43 | 44 | Log in to NGC using Docker: 45 | 46 | ```bash 47 | docker login nvcr.io 48 | Username: $oauthtoken 49 | Password: [Your NGC API Key] 50 | 2. **Install and configure your omniverse Nucleus user:** 51 | 52 | Ensure that you have [Omniverse Launcher](https://www.nvidia.com/es-es/omniverse/download/) installed 53 | 54 | After installing Omniverse Launcher, configure your local [Nucleus Server](https://docs.omniverse.nvidia.com/nucleus/latest/workstation/installation.html) 55 | ## Getting Started 56 | #### 1. Clone the repository: 57 | 58 | ```bash 59 | git clone https://github.com/MetaToolEU/MT_Isaac_sim.git 60 | cd MT_Isaac_sim/Docker 61 | ``` 62 | #### 2 Build the Docker image: 63 | Run the provided script to set up the environment and build the Docker image: 64 | 65 | ```bash 66 | bash build_docker.sh isaac_sim_2023.1.0 67 | ``` 68 | #### 3 Run the Docker Container: 69 | Enable GUI visualization on your host machine: 70 | ```bash 71 | xhost + # this will enable gui visualization 72 | ``` 73 | Start the Isaac Sim container: 74 | ```bash 75 | bash start_docker.sh isaac_sim_2023.1.0 76 | cd examples/Isaac_launch 77 | ``` 78 | ### If you are using Curobo local installation : 79 | #### 1. Add an alias to Isaac Sim’s python in your bashrc file: 80 | ```bash 81 | echo "alias omni_python='~/.local/share/ov/pkg/isaac_sim-2022.2.1/python.sh'" >> ~/.bashrc 82 | ``` 83 | 84 | #### 2. Update your bash environment to include the new alias: 85 | ```bash 86 | source ~/.bashrc 87 | ``` 88 | ## Launch Demos 89 | #### 1. To launch dual robot digital twin run the Python script with Isaac Sim: 90 | Replace "yyy.yyy.yyy.yyy" with the actual IP addresses of your first and second robots. 91 | ```bash 92 | omni_python isaac_rtde_dual_arm.py --robot-ip "yyy.yyy.yyy.yyy" --robot-ip2 "yyy.yyy.yyy.yyy" 93 | ``` 94 | 95 | #### 2. To launch the Curobo multi_arm_reacher_MT example run : 96 | ```bash 97 | omni_python multi_arm_reacher_MT.py 98 | ``` 99 | #### 3. To launch the Curobo Simple_stacking_ur example run : 100 | ```bash 101 | omni_python simple_stacking_ur.py 102 | ``` 103 | -------------------------------------------------------------------------------- /Isaac_launch/Gripper_control.py: -------------------------------------------------------------------------------- 1 | from rtde_io import RTDEIOInterface # Import the RTDEIOInterface class 2 | import logging 3 | import sys 4 | import rtde as rtde 5 | import rtde as rtde_config 6 | import argparse 7 | import time 8 | 9 | class UR3eGripper: 10 | def __init__(self, ip_address, rtde_receive_interface, port): 11 | # Initialize the RTDEIOInterface with the specified IP address 12 | self.rtde_io = RTDEIOInterface(ip_address) 13 | self.rtde_r = rtde_receive_interface 14 | 15 | # Set the initial state of tool digital outs 16 | self.rtde_io.setToolDigitalOut(1, False) 17 | self.rtde_io.setToolDigitalOut(0, False) 18 | 19 | # Parse command-line arguments 20 | parser = argparse.ArgumentParser() 21 | parser.add_argument("--frequency", type=int, default=125, help="the sampling frequency in Herz") 22 | parser.add_argument("--config", default="record_configuration.xml", help="data configuration file to use (record_configuration.xml)") 23 | parser.add_argument("--verbose", help="increase output verbosity", action="store_true") 24 | parser.add_argument("--buffered", help="Use buffered receive which doesn't skip data", action="store_true") 25 | parser.add_argument("--binary", help="save the data in binary format", action="store_true") 26 | self.args = parser.parse_args() 27 | 28 | # Set IP address and port for RTDE communication 29 | self.ip_address = ip_address 30 | 31 | # RTDE connection setup 32 | if self.args.verbose: 33 | logging.basicConfig(level=logging.INFO) 34 | 35 | self.conf = rtde_config.ConfigFile(self.args.config) 36 | self.output_names, _ = self.conf.get_recipe("out") 37 | self.con = rtde.RTDE(ip_address, port) 38 | 39 | self.connect_and_start() 40 | 41 | def connect_and_start(self): 42 | # Establish the RTDE connection and start synchronization 43 | self.con.connect() 44 | self.con.get_controller_version() 45 | 46 | # Set up the output configuration 47 | if not self.con.send_output_setup(self.output_names, frequency=self.args.frequency): 48 | logging.error("Unable to configure output") 49 | sys.exit() 50 | 51 | # Start the data synchronization 52 | if not self.con.send_start(): 53 | logging.error("Unable to start synchronization") 54 | sys.exit() 55 | 56 | def open(self): 57 | # Inform that the gripper is opening 58 | print("Gripper is opening") 59 | 60 | # Deactivate the tool digital out 1 and activate tool digital out 0 61 | self.rtde_io.setToolDigitalOut(1, False) 62 | self.rtde_io.setToolDigitalOut(0, True) 63 | 64 | def close(self): 65 | # Inform that the gripper is closing 66 | print("Gripper is closing") 67 | 68 | # Deactivate the tool digital out 0 and activate tool digital out 1 69 | self.rtde_io.setToolDigitalOut(0, False) 70 | self.rtde_io.setToolDigitalOut(1, True) 71 | 72 | def extract_position(self): 73 | try: 74 | state = self.con.receive(binary=self.args.binary) 75 | actual_q = None 76 | gripper_position = None 77 | 78 | if state is not None: 79 | for name in self.output_names: 80 | value = getattr(state, name, None) 81 | 82 | if name == "tool_analog_input0": 83 | if not isinstance(value, list): 84 | tool_analog_input0 = value 85 | gripper_position = round(map_value(tool_analog_input0, 0, 10, 0.0, -0.013), 3) 86 | 87 | elif name == "actual_q": 88 | actual_q = value 89 | 90 | return actual_q, gripper_position 91 | except Exception as e: 92 | logging.error(f"Error receiving data: {e}") 93 | self.reconnect() 94 | return None, None 95 | 96 | def reconnect(self): 97 | logging.info("Attempting to reconnect...") 98 | try: 99 | self.con.disconnect() 100 | except Exception as e: 101 | logging.error(f"Error during disconnect: {e}") 102 | 103 | time.sleep(2) # Wait before trying to reconnect 104 | self.connect_and_start() 105 | 106 | def map_value(value, min_input, max_input, min_out, max_out): 107 | # Ensure the value is within the input range 108 | value = max(min(value, max_input), min_input) 109 | 110 | # Map the value from the input range to the output range 111 | mapped_value = (value - min_input) / (max_input - min_input) * (max_out - min_out) + min_out 112 | 113 | return mapped_value 114 | 115 | def map_values_in_list(input_list, min_input, max_input, min_out, max_out): 116 | return [map_value(value, min_input, max_input, min_out, max_out) for value in input_list] 117 | -------------------------------------------------------------------------------- /Isaac_launch/isaac_rtde_gripper.py: -------------------------------------------------------------------------------- 1 | # Isaac Sim app library 2 | from omni.isaac.kit import SimulationApp 3 | from Gripper_control import UR3eGripper 4 | # from Gripper_control import extract_data 5 | simulation_app = SimulationApp({"headless": False}) 6 | 7 | # Isaac Sim extenstions + core libraries 8 | from omni.isaac.motion_generation.lula import RmpFlow 9 | from omni.isaac.motion_generation import ArticulationMotionPolicy 10 | from omni.isaac.core.robots import Robot 11 | from omni.isaac.core.objects import cuboid 12 | from omni.isaac.core import World 13 | from omni.isaac.core.utils.stage import add_reference_to_stage 14 | from omni.isaac.core.utils.nucleus import get_assets_root_path 15 | from omni.isaac.motion_generation.interface_config_loader import ( 16 | load_supported_motion_policy_config, 17 | ) 18 | 19 | # ur rtde communication 20 | import rtde_control 21 | import rtde_receive 22 | 23 | import numpy as np 24 | import argparse 25 | import sys 26 | 27 | import argparse 28 | import logging 29 | import sys 30 | 31 | import time 32 | import os 33 | script_dir = os.path.dirname(os.path.abspath(__file__)) 34 | 35 | parser = argparse.ArgumentParser() 36 | parser.add_argument( 37 | "--host", 38 | type=str, 39 | default="192.168.10.23", 40 | help="IP adress of robot Real world UR Polyscope or VM UR Polyscope", 41 | ) 42 | 43 | arg = parser.parse_args() 44 | rtde_receive_interface = rtde_receive.RTDEReceiveInterface(arg.host) 45 | 46 | ur3e_gripper_instance = UR3eGripper(arg.host,rtde_receive_interface,30004) 47 | 48 | # set up paths and prims 49 | robot_name = "UR3e" 50 | prim_path = "/UR3e" 51 | usd_path = os.path.join(script_dir, "../usd/flatten.usd") 52 | 53 | # set references to staget in isaac 54 | add_reference_to_stage(usd_path=usd_path, prim_path=prim_path) 55 | 56 | # add world 57 | my_world = World(stage_units_in_meters=1.0) 58 | my_world.scene.add_default_ground_plane() 59 | 60 | # add robot to world 61 | robot = my_world.scene.add(Robot(prim_path=prim_path, name=robot_name)) 62 | 63 | # loading rmp_config 64 | rmp_config = load_supported_motion_policy_config(robot_name, "RMPflow") 65 | 66 | # Initialize an RmpFlow object and set up 67 | rmpflow = RmpFlow( 68 | robot_description_path = os.path.join(script_dir, "../motion_policy_configs/ur3e/rmpflow/ur3e_gripper_robot_description.yaml"), 69 | urdf_path = os.path.join(script_dir, "../motion_policy_configs/ur3e/ur3e_gripper.urdf"), 70 | rmpflow_config_path = os.path.join(script_dir, "../motion_policy_configs/ur3e/rmpflow/ur3e_gripper_rmpflow_config.yaml"), 71 | end_effector_frame_name = "gripper_center", 72 | maximum_substep_size = 0.00334 73 | ) 74 | physics_dt = 1.0/60 75 | articulation_rmpflow = ArticulationMotionPolicy(robot, rmpflow, physics_dt) 76 | articulation_controller = robot.get_articulation_controller() 77 | 78 | # Make a target to follow 79 | target_cube = cuboid.VisualCuboid( 80 | "/World/target", position=np.array([0.09, 0.22, 0.7]), color=np.array([1.0, 0, 0]), size=0.1, scale=np.array([0.5,0.5,0.5]) 81 | ) 82 | 83 | # Make an obstacle to avoid 84 | ground = cuboid.VisualCuboid( 85 | "/World/ground", position=np.array([0.0, 0, -0.0525]), color=np.array([0, 1.0, 0]), size=0.1, scale=np.array([40,40,1]) 86 | ) 87 | # rmpflow.visualize_end_effector_position() 88 | # rmpflow.visualize_collision_spheres() 89 | rmpflow.add_obstacle(ground) 90 | 91 | # prereset world 92 | my_world.reset() 93 | 94 | 95 | try: 96 | rtde_r = rtde_receive.RTDEReceiveInterface(arg.host) 97 | rtde_c = rtde_control.RTDEControlInterface(arg.host,500.0,rtde_control.RTDEControlInterface.FLAG_USE_EXT_UR_CAP) 98 | 99 | except: 100 | print("[ERROR] Robot is not connected") 101 | # close isaac sim 102 | simulation_app.close() 103 | sys.exit() 104 | 105 | ur3e_gripper_instance.open() 106 | while simulation_app.is_running(): 107 | # on step render 108 | my_world.step(render=True) 109 | if my_world.is_playing(): 110 | # first frame -> reset world 111 | if my_world.current_time_step_index == 0: 112 | my_world.reset() 113 | 114 | # set target to RMP Flow 115 | rmpflow.set_end_effector_target( 116 | target_position=target_cube.get_world_pose()[0], target_orientation=target_cube.get_world_pose()[1] 117 | ) 118 | 119 | # Parameters 120 | velocity = 1 121 | acceleration = 1 122 | dt = 0.1/500 # 2ms 123 | lookahead_time = 0.1 124 | gain = 300 125 | end_effector_pose=rmpflow.get_end_effector_pose(robot.get_joint_positions(joint_indices=np.array([0,1,2,3,4,5]))) 126 | # print(end_effector_pose) 127 | rounded_translation = np.round(end_effector_pose[0], 2) 128 | target_translation = np.array([0.09, 0.22, 0.7]) 129 | # print(rounded_translation,target_translation) 130 | offset_error = np.array([0.01, 0.01, 0.01]) 131 | translation_with_offset = rounded_translation + offset_error 132 | position_threshold = 0.01 # Adjust the threshold as needed 133 | 134 | if np.linalg.norm(translation_with_offset - target_translation) < position_threshold: 135 | ur3e_gripper_instance.close() 136 | 137 | joint_q = robot.get_joint_positions(joint_indices=np.array([0,1,2,3,4,5])) 138 | 139 | # time start period 140 | t_start = rtde_c.initPeriod() 141 | 142 | # run servoJ 143 | rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain) 144 | rtde_c.waitPeriod(t_start) 145 | 146 | rmpflow.update_world() 147 | 148 | actions = articulation_rmpflow.get_next_articulation_action() 149 | articulation_controller.apply_action(actions) 150 | actual_q, gripper_position = ur3e_gripper_instance.extract_position() 151 | 152 | robot.set_joint_positions(np.array(list(actual_q)+[gripper_position,gripper_position])) 153 | 154 | # rtde control stop script and disconnect 155 | rtde_c.servoStop() 156 | rtde_c.stopScript() 157 | rtde_r.disconnect() 158 | 159 | # close isaac sim 160 | simulation_app.close() 161 | -------------------------------------------------------------------------------- /Isaac_launch/isaac_rtde_dual_arm.py: -------------------------------------------------------------------------------- 1 | from omni.isaac.kit import SimulationApp 2 | simulation_app = SimulationApp({"headless": False}) 3 | 4 | from omni.isaac.motion_generation.lula import RmpFlow 5 | from omni.isaac.motion_generation import ArticulationMotionPolicy 6 | from omni.isaac.core.robots import Robot 7 | from omni.isaac.core.objects import cuboid 8 | from omni.isaac.core import World 9 | from omni.isaac.core.utils.stage import add_reference_to_stage 10 | from omni.isaac.core.utils.nucleus import get_assets_root_path 11 | from omni.isaac.motion_generation.interface_config_loader import load_supported_motion_policy_config 12 | import rtde_control 13 | import rtde_receive 14 | from Gripper_control import UR3eGripper 15 | import numpy as np 16 | import argparse 17 | import sys 18 | import time 19 | import os 20 | script_dir = os.path.dirname(os.path.abspath(__file__)) 21 | 22 | # Set up argument parsers 23 | parser = argparse.ArgumentParser() 24 | parser.add_argument("--robot-ip", type=str, default="192.168.10.23", help="IP address of the first robot") 25 | arg = parser.parse_args() 26 | rtde_receive_interface = rtde_receive.RTDEReceiveInterface(arg.robot_ip) 27 | 28 | ur3e_gripper_instance = UR3eGripper(arg.robot_ip,rtde_receive_interface,30004) 29 | 30 | # ur3e_gripper_instance = UR3eGripper(ip_address=arg.robot_ip) 31 | 32 | parser2 = argparse.ArgumentParser() 33 | parser2.add_argument("--robot-ip2", type=str, default="192.168.10.25", help="IP address of the second robot") 34 | arg_2 = parser2.parse_args() 35 | rtde_receive_interface_2 = rtde_receive.RTDEReceiveInterface(arg_2.robot_ip2) 36 | 37 | ur3e_gripper_instance_2 = UR3eGripper(arg_2.robot_ip2,rtde_receive_interface_2,30004) 38 | 39 | # ur3e_gripper_instance_2 = UR3eGripper(ip_address=arg_2.robot_ip2) 40 | # Set up paths and prims 41 | robot_name = "UR3e" 42 | prim_path = "/UR3e" 43 | usd_path = os.path.join(script_dir, "../usd/flatten_updated_1.usd") 44 | 45 | robot_name_2 = "UR3e_2" 46 | prim_path_2 = "/UR3e_2" 47 | 48 | # Set references to stage in Isaac Sim 49 | add_reference_to_stage(usd_path=usd_path, prim_path=prim_path) 50 | add_reference_to_stage(usd_path=usd_path, prim_path=prim_path_2) 51 | 52 | # Add world 53 | my_world = World(stage_units_in_meters=1.0) 54 | my_world.scene.add_default_ground_plane() 55 | 56 | # Add robots to the world 57 | robot = my_world.scene.add(Robot(prim_path=prim_path, name=robot_name, position=np.array([0, 1.0, 0]))) 58 | robot_2 = my_world.scene.add(Robot(prim_path=prim_path_2, name=robot_name_2)) 59 | 60 | # Load supported motion policy configurations 61 | rmp_config = load_supported_motion_policy_config(robot_name, "RMPflow") 62 | 63 | # Initialize RmpFlow objects and set up 64 | # Robot 1 65 | rmpflow = RmpFlow( 66 | robot_description_path = os.path.join(script_dir, "../motion_policy_configs/ur3e/rmpflow/ur3e_gripper_robot_description.yaml"), 67 | urdf_path = os.path.join(script_dir, "../motion_policy_configs/ur3e/ur3e_gripper.urdf"), 68 | rmpflow_config_path = os.path.join(script_dir, "../motion_policy_configs/ur3e/rmpflow/ur3e_gripper_rmpflow_config.yaml"), 69 | end_effector_frame_name = "gripper_center", 70 | maximum_substep_size = 0.00334 71 | ) 72 | physics_dt = 1.0 / 60 73 | 74 | articulation_rmpflow = ArticulationMotionPolicy(robot, rmpflow, physics_dt) 75 | articulation_controller = robot.get_articulation_controller() 76 | 77 | # Robot 2 78 | rmpflow_2 = RmpFlow( 79 | robot_description_path = os.path.join(script_dir, "../motion_policy_configs/ur3e/rmpflow/ur3e_gripper_robot_description.yaml"), 80 | urdf_path = os.path.join(script_dir, "../motion_policy_configs/ur3e/ur3e_gripper.urdf"), 81 | rmpflow_config_path = os.path.join(script_dir, "../motion_policy_configs/ur3e/rmpflow/ur3e_gripper_rmpflow_config.yaml"), 82 | end_effector_frame_name = "gripper_center", 83 | maximum_substep_size = 0.00334 84 | ) 85 | articulation_rmpflow_2 = ArticulationMotionPolicy(robot_2, rmpflow_2, physics_dt) 86 | articulation_controller_2 = robot_2.get_articulation_controller() 87 | 88 | # Make a target to follow 89 | target_cube = cuboid.VisualCuboid( 90 | "/World/target", position=np.array([0.5, 0, 0.5]), color=np.array([1.0, 0, 0]), size=0.1, scale=np.array([0.5, 0.5, 0.5]) 91 | ) 92 | 93 | # Make an obstacle to avoid 94 | ground = cuboid.VisualCuboid( 95 | "/World/ground", position=np.array([0.0, 0, -0.0525]), color=np.array([0, 1.0, 0]), size=0.1, scale=np.array([40, 40, 1]) 96 | ) 97 | 98 | # Pre-reset world 99 | my_world.reset() 100 | 101 | # IP address of the first robot 102 | try: 103 | rtde_r = rtde_receive.RTDEReceiveInterface(arg.robot_ip) 104 | rtde_c = rtde_control.RTDEControlInterface(arg.robot_ip, 500.0, rtde_control.RTDEControlInterface.FLAG_USE_EXT_UR_CAP, 50002) 105 | # robot.set_joint_positions(np.array(rtde_r.getActualQ())) 106 | 107 | except: 108 | print("[ERROR] First robot is not connected") 109 | simulation_app.close() 110 | sys.exit() 111 | 112 | # IP address of the second robot 113 | try: 114 | rtde_r_2 = rtde_receive.RTDEReceiveInterface(arg_2.robot_ip2) 115 | rtde_c_2 = rtde_control.RTDEControlInterface(arg_2.robot_ip2, 500.0, rtde_control.RTDEControlInterface.FLAG_USE_EXT_UR_CAP, 50007) 116 | # robot_2.set_joint_positions(np.array(rtde_r_2.getActualQ())) 117 | 118 | except: 119 | print("[ERROR] Second robot is not connected") 120 | simulation_app.close() 121 | sys.exit() 122 | 123 | while simulation_app.is_running(): 124 | # On step render 125 | my_world.step(render=True) 126 | if my_world.is_playing(): 127 | # First frame -> reset world 128 | if my_world.current_time_step_index == 0: 129 | my_world.reset() 130 | 131 | # Query the current obstacle position 132 | rmpflow.update_world() 133 | rmpflow_2.update_world() 134 | 135 | # Get actual joint positions from robots and update Isaac model 136 | actual_q, gripper_position = ur3e_gripper_instance.extract_position() 137 | actual_q_2, gripper_position_2 = ur3e_gripper_instance_2.extract_position() 138 | 139 | robot.set_joint_positions(np.array(list(actual_q)+[gripper_position,gripper_position])) 140 | robot_2.set_joint_positions(np.array(list(actual_q_2)+[gripper_position_2,gripper_position_2])) 141 | 142 | # RTDE control stop script and disconnect 143 | rtde_c.servoStop() 144 | rtde_c.stopScript() 145 | rtde_r.disconnect() 146 | 147 | rtde_c_2.servoStop() 148 | rtde_c_2.stopScript() 149 | rtde_r_2.disconnect() 150 | 151 | # Close Isaac Sim 152 | simulation_app.close() -------------------------------------------------------------------------------- /Isaac_launch/ur3e/ur3e.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021-2023, NVIDIA CORPORATION. All rights reserved. 2 | # 3 | # NVIDIA CORPORATION and its licensors retain all intellectual property 4 | # and proprietary rights in and to this software, related documentation 5 | # and any modifications thereto. Any use, reproduction, disclosure or 6 | # distribution of this software and related documentation without an express 7 | # license agreement from NVIDIA CORPORATION is strictly prohibited. 8 | # 9 | from typing import List, Optional 10 | 11 | import carb 12 | import numpy as np 13 | import os 14 | from omni.isaac.core.prims.rigid_prim import RigidPrim 15 | from omni.isaac.core.robots.robot import Robot 16 | from omni.isaac.core.utils.nucleus import get_assets_root_path 17 | from omni.isaac.core.utils.prims import get_prim_at_path 18 | from omni.isaac.core.utils.stage import add_reference_to_stage, get_stage_units 19 | from omni.isaac.manipulators.grippers.parallel_gripper import ParallelGripper 20 | 21 | current_dir = os.path.dirname(os.path.realpath(__file__)) 22 | default_path = os.path.abspath(os.path.join(current_dir, '../../../usd/ur3e_gripper.usd')) 23 | 24 | class ur3e(Robot): 25 | """[summary] 26 | 27 | Args: 28 | prim_path (str): [description] 29 | name (str, optional): [description]. Defaults to "franka_robot". 30 | usd_path (Optional[str], optional): [description]. Defaults to None. 31 | position (Optional[np.ndarray], optional): [description]. Defaults to None. 32 | orientation (Optional[np.ndarray], optional): [description]. Defaults to None. 33 | end_effector_prim_name (Optional[str], optional): [description]. Defaults to None. 34 | gripper_dof_names (Optional[List[str]], optional): [description]. Defaults to None. 35 | gripper_open_position (Optional[np.ndarray], optional): [description]. Defaults to None. 36 | gripper_closed_position (Optional[np.ndarray], optional): [description]. Defaults to None. 37 | """ 38 | 39 | def __init__( 40 | self, 41 | prim_path: str, 42 | name: str = "ur3e_robot", 43 | usd_path: Optional[str] = None, 44 | position: Optional[np.ndarray] = None, 45 | orientation: Optional[np.ndarray] = None, 46 | end_effector_prim_name: Optional[str] = None, 47 | gripper_dof_names: Optional[List[str]] = None, 48 | gripper_open_position: Optional[np.ndarray] = None, 49 | gripper_closed_position: Optional[np.ndarray] = None, 50 | deltas: Optional[np.ndarray] = None, 51 | ) -> None: 52 | prim = get_prim_at_path(prim_path) 53 | self._end_effector = None 54 | self._gripper = None 55 | self._end_effector_prim_name = end_effector_prim_name 56 | if not prim.IsValid(): 57 | if usd_path: 58 | add_reference_to_stage(usd_path=usd_path, prim_path=prim_path) 59 | else: 60 | assets_root_path = get_assets_root_path() 61 | if assets_root_path is None: 62 | carb.log_error("Could not find Isaac Sim assets folder") 63 | usd_path = default_path 64 | add_reference_to_stage(usd_path=usd_path, prim_path=prim_path) 65 | if self._end_effector_prim_name is None: 66 | self._end_effector_prim_path = prim_path + "/single_base" 67 | else: 68 | self._end_effector_prim_path = prim_path + "/" + end_effector_prim_name 69 | if gripper_dof_names is None: 70 | gripper_dof_names = ["single_right_finger_joint", "single_left_finger_joint"] 71 | if gripper_open_position is None: 72 | gripper_open_position = np.array([-0.013, -0.013]) / get_stage_units() 73 | if gripper_closed_position is None: 74 | gripper_closed_position = np.array([-0.009, -0.009]) 75 | else: 76 | if self._end_effector_prim_name is None: 77 | self._end_effector_prim_path = prim_path + "/single_base" 78 | else: 79 | self._end_effector_prim_path = prim_path + "/" + end_effector_prim_name 80 | if gripper_dof_names is None: 81 | gripper_dof_names = ["single_right_finger_joint", "single_left_finger_joint"] 82 | if gripper_open_position is None: 83 | gripper_open_position = np.array([-0.013, -0.013]) / get_stage_units() 84 | if gripper_closed_position is None: 85 | gripper_closed_position = np.array([-0.009, -0.009]) 86 | super().__init__( 87 | prim_path=prim_path, name=name, position=position, orientation=orientation, articulation_controller=None 88 | ) 89 | if gripper_dof_names is not None: 90 | if deltas is None: 91 | deltas = np.array([0.013, 0.013]) / get_stage_units() 92 | self._gripper = ParallelGripper( 93 | end_effector_prim_path=self._end_effector_prim_path, 94 | joint_prim_names=gripper_dof_names, 95 | joint_opened_positions=gripper_open_position, 96 | joint_closed_positions=gripper_closed_position, 97 | action_deltas=deltas, 98 | ) 99 | return 100 | 101 | @property 102 | def end_effector(self) -> RigidPrim: 103 | """[summary] 104 | 105 | Returns: 106 | RigidPrim: [description] 107 | """ 108 | return self._end_effector 109 | 110 | @property 111 | def gripper(self) -> ParallelGripper: 112 | """[summary] 113 | 114 | Returns: 115 | ParallelGripper: [description] 116 | """ 117 | return self._gripper 118 | 119 | def initialize(self, physics_sim_view=None) -> None: 120 | """[summary]""" 121 | super().initialize(physics_sim_view) 122 | self._end_effector = RigidPrim(prim_path=self._end_effector_prim_path, name=self.name + "_end_effector") 123 | self._end_effector.initialize(physics_sim_view) 124 | self._gripper.initialize( 125 | physics_sim_view=physics_sim_view, 126 | articulation_apply_action_func=self.apply_action, 127 | get_joint_positions_func=self.get_joint_positions, 128 | set_joint_positions_func=self.set_joint_positions, 129 | dof_names=self.dof_names, 130 | ) 131 | return 132 | 133 | def post_reset(self) -> None: 134 | """[summary]""" 135 | super().post_reset() 136 | self._gripper.post_reset() 137 | self._articulation_controller.switch_dof_control_mode( 138 | dof_index=self.gripper.joint_dof_indicies[0], mode="position" 139 | ) 140 | self._articulation_controller.switch_dof_control_mode( 141 | dof_index=self.gripper.joint_dof_indicies[1], mode="position" 142 | ) 143 | return 144 | -------------------------------------------------------------------------------- /Isaac_launch/helper.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 5 | # property and proprietary rights in and to this material, related 6 | # documentation and any modifications thereto. Any use, reproduction, 7 | # disclosure or distribution of this material and related documentation 8 | # without an express license agreement from NVIDIA CORPORATION or 9 | # its affiliates is strictly prohibited. 10 | # 11 | 12 | # Standard Library 13 | from typing import Dict, List 14 | 15 | # Third Party 16 | import numpy as np 17 | from matplotlib import cm 18 | from omni.isaac.core import World 19 | from omni.isaac.core.materials import OmniPBR 20 | from omni.isaac.core.objects import cuboid 21 | from omni.isaac.core.robots import Robot 22 | from pxr import UsdPhysics 23 | 24 | # CuRobo 25 | from curobo.util.logger import log_warn 26 | from curobo.util.usd_helper import set_prim_transform 27 | 28 | ISAAC_SIM_23 = False 29 | try: 30 | # Third Party 31 | from omni.isaac.urdf import _urdf # isaacsim 2022.2 32 | except ImportError: 33 | # Third Party 34 | from omni.importer.urdf import _urdf # isaac sim 2023.1 35 | 36 | ISAAC_SIM_23 = True 37 | # Standard Library 38 | from typing import Optional 39 | 40 | # Third Party 41 | from omni.isaac.core.utils.extensions import enable_extension 42 | 43 | # CuRobo 44 | from curobo.util_file import get_assets_path, get_filename, get_path_of_dir, join_path 45 | 46 | 47 | def add_extensions(simulation_app, headless_mode: Optional[str] = None): 48 | ext_list = [ 49 | "omni.kit.asset_converter", 50 | "omni.kit.tool.asset_importer", 51 | "omni.isaac.asset_browser", 52 | ] 53 | if headless_mode is not None: 54 | log_warn("Running in headless mode: " + headless_mode) 55 | ext_list += ["omni.kit.livestream." + headless_mode] 56 | [enable_extension(x) for x in ext_list] 57 | simulation_app.update() 58 | 59 | return True 60 | 61 | 62 | ############################################################ 63 | def add_robot_to_scene( 64 | robot_config: Dict, 65 | my_world: World, 66 | load_from_usd: bool = False, 67 | subroot: str = "", 68 | robot_name: str = "robot", 69 | position: np.array = np.array([0, 0, 0]), 70 | ): 71 | urdf_interface = _urdf.acquire_urdf_interface() 72 | 73 | import_config = _urdf.ImportConfig() 74 | import_config.merge_fixed_joints = False 75 | import_config.convex_decomp = False 76 | import_config.import_inertia_tensor = True 77 | import_config.fix_base = True 78 | import_config.make_default_prim = False 79 | import_config.self_collision = False 80 | import_config.create_physics_scene = True 81 | import_config.import_inertia_tensor = False 82 | import_config.default_drive_strength = 20000 83 | import_config.default_position_drive_damping = 500 84 | import_config.default_drive_type = _urdf.UrdfJointTargetType.JOINT_DRIVE_POSITION 85 | import_config.distance_scale = 1 86 | import_config.density = 0.0 87 | asset_path = get_assets_path() 88 | if ( 89 | "external_asset_path" in robot_config["kinematics"] 90 | and robot_config["kinematics"]["external_asset_path"] is not None 91 | ): 92 | asset_path = robot_config["kinematics"]["external_asset_path"] 93 | full_path = join_path(asset_path, robot_config["kinematics"]["urdf_path"]) 94 | robot_path = get_path_of_dir(full_path) 95 | filename = get_filename(full_path) 96 | imported_robot = urdf_interface.parse_urdf(robot_path, filename, import_config) 97 | dest_path = subroot 98 | robot_path = urdf_interface.import_robot( 99 | robot_path, 100 | filename, 101 | imported_robot, 102 | import_config, 103 | dest_path, 104 | ) 105 | 106 | base_link_name = robot_config["kinematics"]["base_link"] 107 | 108 | robot_p = Robot( 109 | prim_path=robot_path + "/" + base_link_name, 110 | name=robot_name, 111 | ) 112 | 113 | robot_prim = robot_p.prim 114 | stage = robot_prim.GetStage() 115 | linkp = stage.GetPrimAtPath(robot_path) 116 | set_prim_transform(linkp, [position[0], position[1], position[2], 1, 0, 0, 0]) 117 | 118 | if False and ISAAC_SIM_23: # this doesn't work in isaac sim 2023.1.1 119 | robot_p.set_solver_velocity_iteration_count(0) 120 | robot_p.set_solver_position_iteration_count(44) 121 | 122 | my_world._physics_context.set_solver_type("PGS") 123 | 124 | if ISAAC_SIM_23: # fix to load robot correctly in isaac sim 2023.1.1 125 | linkp = stage.GetPrimAtPath(robot_path + "/" + base_link_name) 126 | mass = UsdPhysics.MassAPI(linkp) 127 | mass.GetMassAttr().Set(0) 128 | robot = my_world.scene.add(robot_p) 129 | # robot_path = robot.prim_path 130 | return robot, robot_path 131 | 132 | 133 | class VoxelManager: 134 | def __init__( 135 | self, 136 | num_voxels: int = 5000, 137 | size: float = 0.02, 138 | color: List[float] = [1, 1, 1], 139 | prefix_path: str = "/World/curobo/voxel_", 140 | material_path: str = "/World/looks/v_", 141 | ) -> None: 142 | self.cuboid_list = [] 143 | self.cuboid_material_list = [] 144 | self.disable_idx = num_voxels 145 | for i in range(num_voxels): 146 | target_material = OmniPBR("/World/looks/v_" + str(i), color=np.ravel(color)) 147 | 148 | cube = cuboid.VisualCuboid( 149 | prefix_path + str(i), 150 | position=np.array([0, 0, -10]), 151 | orientation=np.array([1, 0, 0, 0]), 152 | size=size, 153 | visual_material=target_material, 154 | ) 155 | self.cuboid_list.append(cube) 156 | self.cuboid_material_list.append(target_material) 157 | cube.set_visibility(True) 158 | 159 | def update_voxels(self, voxel_position: np.ndarray, color_axis: int = 0): 160 | max_index = min(voxel_position.shape[0], len(self.cuboid_list)) 161 | 162 | jet = cm.get_cmap("hot") # .reversed() 163 | z_val = voxel_position[:, 0] 164 | 165 | jet_colors = jet(z_val) 166 | 167 | for i in range(max_index): 168 | self.cuboid_list[i].set_visibility(True) 169 | 170 | self.cuboid_list[i].set_local_pose(translation=voxel_position[i]) 171 | self.cuboid_material_list[i].set_color(jet_colors[i][:3]) 172 | 173 | for i in range(max_index, len(self.cuboid_list)): 174 | self.cuboid_list[i].set_local_pose(translation=np.ravel([0, 0, -10.0])) 175 | 176 | # self.cuboid_list[i].set_visibility(False) 177 | 178 | def clear(self): 179 | for i in range(len(self.cuboid_list)): 180 | self.cuboid_list[i].set_local_pose(translation=np.ravel([0, 0, -10.0])) 181 | -------------------------------------------------------------------------------- /Isaac_launch/rtde/csv_binary_writer.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import sys 25 | 26 | sys.path.append("..") 27 | 28 | import struct 29 | from rtde import serialize 30 | 31 | 32 | class CSVBinaryWriter(object): 33 | def __init__(self, file, names, types, delimiter=" "): 34 | if len(names) != len(types): 35 | raise ValueError("List sizes are not identical.") 36 | self.__file = file 37 | self.__names = names 38 | self.__types = types 39 | self.__delimiter = delimiter 40 | self.__header_names = [] 41 | self.__columns = 0 42 | for i in range(len(self.__names)): 43 | size = serialize.get_item_size(self.__types[i]) 44 | self.__columns += size 45 | if size > 1: 46 | for j in range(size): 47 | name = self.__names[i] + "_" + str(j) 48 | self.__header_names.append(name) 49 | else: 50 | name = self.__names[i] 51 | self.__header_names.append(name) 52 | 53 | def getType(self, vtype): 54 | if vtype == "VECTOR3D": 55 | return "DOUBLE" + self.__delimiter + "DOUBLE" + self.__delimiter + "DOUBLE" 56 | elif vtype == "VECTOR6D": 57 | return ( 58 | "DOUBLE" 59 | + self.__delimiter 60 | + "DOUBLE" 61 | + self.__delimiter 62 | + "DOUBLE" 63 | + self.__delimiter 64 | + "DOUBLE" 65 | + self.__delimiter 66 | + "DOUBLE" 67 | + self.__delimiter 68 | + "DOUBLE" 69 | ) 70 | elif vtype == "VECTOR6INT32": 71 | return ( 72 | "INT32" 73 | + self.__delimiter 74 | + "INT32" 75 | + self.__delimiter 76 | + "INT32" 77 | + self.__delimiter 78 | + "INT32" 79 | + self.__delimiter 80 | + "INT32" 81 | + self.__delimiter 82 | + "INT32" 83 | ) 84 | elif vtype == "VECTOR6UINT32": 85 | return ( 86 | "UINT32" 87 | + self.__delimiter 88 | + "UINT32" 89 | + self.__delimiter 90 | + "UINT32" 91 | + self.__delimiter 92 | + "UINT32" 93 | + self.__delimiter 94 | + "UINT32" 95 | + self.__delimiter 96 | + "UINT32" 97 | ) 98 | else: 99 | return str(vtype) 100 | 101 | def writeheader(self): 102 | # Header names 103 | headerStr = str("") 104 | for i in range(len(self.__header_names)): 105 | if i != 0: 106 | headerStr += self.__delimiter 107 | 108 | headerStr += self.__header_names[i] 109 | 110 | headerStr += "\n" 111 | self.__file.write(struct.pack(str(len(headerStr)) + "s", headerStr if sys.version_info[0] < 3 else headerStr.encode("utf-8"))) 112 | 113 | # Header types 114 | typeStr = str("") 115 | for i in range(len(self.__names)): 116 | if i != 0: 117 | typeStr += self.__delimiter 118 | 119 | typeStr += self.getType(self.__types[i]) 120 | 121 | typeStr += "\n" 122 | self.__file.write(struct.pack(str(len(typeStr)) + "s", typeStr if sys.version_info[0] < 3 else typeStr.encode("utf-8"))) 123 | 124 | def packToBinary(self, vtype, value): 125 | print(vtype) 126 | if vtype == "BOOL": 127 | print("isBOOL" + str(value)) 128 | if vtype == "UINT8": 129 | print("isUINT8" + str(value)) 130 | elif vtype == "INT32": 131 | print("isINT32" + str(value)) 132 | elif vtype == "INT64": 133 | print("isINT64" + str(value)) 134 | elif vtype == "UINT32": 135 | print("isUINT32" + str(value)) 136 | elif vtype == "UINT64": 137 | print("isUINT64" + str(value)) 138 | elif vtype == "DOUBLE": 139 | print( 140 | "isDOUBLE" + str(value) + str(type(value)) + str(sys.getsizeof(value)) 141 | ) 142 | elif vtype == "VECTOR3D": 143 | print( 144 | "isVECTOR3D" + str(value[0]) + "," + str(value[1]) + "," + str(value[2]) 145 | ) 146 | elif vtype == "VECTOR6D": 147 | print( 148 | "isVECTOR6D" 149 | + str(value[0]) 150 | + "," 151 | + str(value[1]) 152 | + "," 153 | + str(value[2]) 154 | + "," 155 | + str(value[3]) 156 | + "," 157 | + str(value[4]) 158 | + "," 159 | + str(value[5]) 160 | ) 161 | elif vtype == "VECTOR6INT32": 162 | print( 163 | "isVECTOR6INT32" 164 | + str(value[0]) 165 | + "," 166 | + str(value[1]) 167 | + "," 168 | + str(value[2]) 169 | + "," 170 | + str(value[3]) 171 | + "," 172 | + str(value[4]) 173 | + "," 174 | + str(value[5]) 175 | ) 176 | elif vtype == "VECTOR6UINT32": 177 | print( 178 | "isVECTOR6UINT32" 179 | + str(value[0]) 180 | + "," 181 | + str(value[1]) 182 | + "," 183 | + str(value[2]) 184 | + "," 185 | + str(value[3]) 186 | + "," 187 | + str(value[4]) 188 | + "," 189 | + str(value[5]) 190 | ) 191 | 192 | def writerow(self, data_object): 193 | self.__file.write(data_object) 194 | -------------------------------------------------------------------------------- /Isaac_launch/rtde/serialize.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import struct 25 | 26 | 27 | class ControlHeader(object): 28 | __slots__ = [ 29 | "command", 30 | "size", 31 | ] 32 | 33 | @staticmethod 34 | def unpack(buf): 35 | rmd = ControlHeader() 36 | (rmd.size, rmd.command) = struct.unpack_from(">HB", buf) 37 | return rmd 38 | 39 | 40 | class ControlVersion(object): 41 | __slots__ = ["major", "minor", "bugfix", "build"] 42 | 43 | @staticmethod 44 | def unpack(buf): 45 | rmd = ControlVersion() 46 | (rmd.major, rmd.minor, rmd.bugfix, rmd.build) = struct.unpack_from(">IIII", buf) 47 | return rmd 48 | 49 | 50 | class ReturnValue(object): 51 | __slots__ = ["success"] 52 | 53 | @staticmethod 54 | def unpack(buf): 55 | rmd = ReturnValue() 56 | rmd.success = bool(struct.unpack_from(">B", buf)[0]) 57 | return rmd 58 | 59 | 60 | class MessageV1(object): 61 | @staticmethod 62 | def unpack(buf): 63 | rmd = Message() # use V2 message object 64 | offset = 0 65 | rmd.level = struct.unpack_from(">B", buf, offset)[0] 66 | offset = offset + 1 67 | rmd.message = str(buf[offset:]) 68 | rmd.source = "" 69 | 70 | return rmd 71 | 72 | 73 | class Message(object): 74 | __slots__ = ["level", "message", "source"] 75 | EXCEPTION_MESSAGE = 0 76 | ERROR_MESSAGE = 1 77 | WARNING_MESSAGE = 2 78 | INFO_MESSAGE = 3 79 | 80 | @staticmethod 81 | def unpack(buf): 82 | rmd = Message() 83 | offset = 0 84 | msg_length = struct.unpack_from(">B", buf, offset)[0] 85 | offset = offset + 1 86 | rmd.message = str(buf[offset : offset + msg_length]) 87 | offset = offset + msg_length 88 | 89 | src_length = struct.unpack_from(">B", buf, offset)[0] 90 | offset = offset + 1 91 | rmd.source = str(buf[offset : offset + src_length]) 92 | offset = offset + src_length 93 | rmd.level = struct.unpack_from(">B", buf, offset)[0] 94 | 95 | return rmd 96 | 97 | 98 | def get_item_size(data_type): 99 | if data_type.startswith("VECTOR6"): 100 | return 6 101 | elif data_type.startswith("VECTOR3"): 102 | return 3 103 | return 1 104 | 105 | 106 | def unpack_field(data, offset, data_type): 107 | size = get_item_size(data_type) 108 | if data_type == "VECTOR6D" or data_type == "VECTOR3D": 109 | return [float(data[offset + i]) for i in range(size)] 110 | elif data_type == "VECTOR6UINT32": 111 | return [int(data[offset + i]) for i in range(size)] 112 | elif data_type == "DOUBLE": 113 | return float(data[offset]) 114 | elif data_type == "UINT32" or data_type == "UINT64": 115 | return int(data[offset]) 116 | elif data_type == "VECTOR6INT32": 117 | return [int(data[offset + i]) for i in range(size)] 118 | elif data_type == "INT32" or data_type == "UINT8": 119 | return int(data[offset]) 120 | elif data_type == "BOOL": 121 | return bool(data[offset]) 122 | raise ValueError("unpack_field: unknown data type: " + data_type) 123 | 124 | 125 | class DataObject(object): 126 | recipe_id = None 127 | 128 | def pack(self, names, types): 129 | if len(names) != len(types): 130 | raise ValueError("List sizes are not identical.") 131 | l = [] 132 | if self.recipe_id is not None: 133 | l.append(self.recipe_id) 134 | for i in range(len(names)): 135 | if self.__dict__[names[i]] is None: 136 | raise ValueError("Uninitialized parameter: " + names[i]) 137 | if types[i].startswith("VECTOR"): 138 | l.extend(self.__dict__[names[i]]) 139 | else: 140 | l.append(self.__dict__[names[i]]) 141 | return l 142 | 143 | @staticmethod 144 | def unpack(data, names, types): 145 | if len(names) != len(types): 146 | raise ValueError("List sizes are not identical.") 147 | obj = DataObject() 148 | offset = 0 149 | obj.recipe_id = data[0] 150 | for i in range(len(names)): 151 | obj.__dict__[names[i]] = unpack_field(data[1:], offset, types[i]) 152 | offset += get_item_size(types[i]) 153 | return obj 154 | 155 | @staticmethod 156 | def create_empty(names, recipe_id): 157 | obj = DataObject() 158 | for i in range(len(names)): 159 | obj.__dict__[names[i]] = None 160 | obj.recipe_id = recipe_id 161 | return obj 162 | 163 | 164 | class DataConfig(object): 165 | __slots__ = ["id", "names", "types", "fmt"] 166 | 167 | @staticmethod 168 | def unpack_recipe(buf): 169 | rmd = DataConfig() 170 | rmd.id = struct.unpack_from(">B", buf)[0] 171 | rmd.types = buf.decode("utf-8")[1:].split(",") 172 | rmd.fmt = ">B" 173 | for i in rmd.types: 174 | if i == "INT32": 175 | rmd.fmt += "i" 176 | elif i == "UINT32": 177 | rmd.fmt += "I" 178 | elif i == "VECTOR6D": 179 | rmd.fmt += "d" * 6 180 | elif i == "VECTOR3D": 181 | rmd.fmt += "d" * 3 182 | elif i == "VECTOR6INT32": 183 | rmd.fmt += "i" * 6 184 | elif i == "VECTOR6UINT32": 185 | rmd.fmt += "I" * 6 186 | elif i == "DOUBLE": 187 | rmd.fmt += "d" 188 | elif i == "UINT64": 189 | rmd.fmt += "Q" 190 | elif i == "UINT8": 191 | rmd.fmt += "B" 192 | elif i == "BOOL": 193 | rmd.fmt += "?" 194 | elif i == "IN_USE": 195 | raise ValueError("An input parameter is already in use.") 196 | else: 197 | raise ValueError("Unknown data type: " + i) 198 | return rmd 199 | 200 | def pack(self, state): 201 | l = state.pack(self.names, self.types) 202 | return struct.pack(self.fmt, *l) 203 | 204 | def unpack(self, data): 205 | li = struct.unpack_from(self.fmt, data) 206 | return DataObject.unpack(li, self.names, self.types) 207 | -------------------------------------------------------------------------------- /Docker/isaac_sim.dockerfile: -------------------------------------------------------------------------------- 1 | ## 2 | ## Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | ## 4 | ## NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 5 | ## property and proprietary rights in and to this material, related 6 | ## documentation and any modifications thereto. Any use, reproduction, 7 | ## disclosure or distribution of this material and related documentation 8 | ## without an express license agreement from NVIDIA CORPORATION or 9 | ## its affiliates is strictly prohibited. 10 | ## 11 | ARG DEBIAN_FRONTEND=noninteractive 12 | ARG BASE_DIST=ubuntu20.04 13 | ARG CUDA_VERSION=11.4.2 14 | ARG ISAAC_SIM_VERSION=2022.2.1 15 | 16 | 17 | FROM nvcr.io/nvidia/isaac-sim:${ISAAC_SIM_VERSION} AS isaac-sim 18 | 19 | FROM nvcr.io/nvidia/cudagl:${CUDA_VERSION}-devel-${BASE_DIST} 20 | 21 | 22 | # this does not work for 2022.2.1 23 | #$FROM nvcr.io/nvidia/cuda:${CUDA_VERSION}-cudnn8-devel-${BASE_DIST} 24 | 25 | LABEL maintainer "User Name" 26 | 27 | ARG VULKAN_SDK_VERSION=1.3.224.1 28 | 29 | 30 | 31 | # Deal with getting tons of debconf messages 32 | # See: https://github.com/phusion/baseimage-docker/issues/58 33 | RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections 34 | 35 | 36 | # add GL if using a cuda docker instead of cudagl: 37 | #RUN apt-get update && apt-get install -y --no-install-recommends \ 38 | # pkg-config \ 39 | # libglvnd-dev \ 40 | # libgl1-mesa-dev \ 41 | # libegl1-mesa-dev \ 42 | # libgles2-mesa-dev && \ 43 | # rm -rf /var/lib/apt/lists/* 44 | 45 | # Set timezone info 46 | RUN apt-get update && apt-get install -y \ 47 | tzdata \ 48 | software-properties-common \ 49 | && rm -rf /var/lib/apt/lists/* \ 50 | && ln -fs /usr/share/zoneinfo/America/Los_Angeles /etc/localtime \ 51 | && echo "America/Los_Angeles" > /etc/timezone \ 52 | && dpkg-reconfigure -f noninteractive tzdata \ 53 | && add-apt-repository -y ppa:git-core/ppa \ 54 | && apt-get update && apt-get install -y \ 55 | curl \ 56 | lsb-core \ 57 | wget \ 58 | build-essential \ 59 | cmake \ 60 | git \ 61 | git-lfs \ 62 | iputils-ping \ 63 | make \ 64 | openssh-server \ 65 | openssh-client \ 66 | libeigen3-dev \ 67 | libssl-dev \ 68 | python3-pip \ 69 | python3-ipdb \ 70 | python3-tk \ 71 | python3-wstool \ 72 | sudo git bash unattended-upgrades \ 73 | apt-utils \ 74 | terminator \ 75 | && rm -rf /var/lib/apt/lists/* 76 | 77 | 78 | # https://catalog.ngc.nvidia.com/orgs/nvidia/containers/cudagl 79 | 80 | RUN apt-get update && apt-get install -y --no-install-recommends \ 81 | libatomic1 \ 82 | libegl1 \ 83 | libglu1-mesa \ 84 | libgomp1 \ 85 | libsm6 \ 86 | libxi6 \ 87 | libxrandr2 \ 88 | libxt6 \ 89 | libfreetype-dev \ 90 | libfontconfig1 \ 91 | openssl \ 92 | libssl1.1 \ 93 | wget \ 94 | vulkan-utils \ 95 | && apt-get -y autoremove \ 96 | && apt-get clean autoclean \ 97 | && rm -rf /var/lib/apt/lists/* 98 | 99 | 100 | 101 | # Download the Vulkan SDK and extract the headers, loaders, layers and binary utilities 102 | RUN wget -q --show-progress \ 103 | --progress=bar:force:noscroll \ 104 | https://sdk.lunarg.com/sdk/download/${VULKAN_SDK_VERSION}/linux/vulkansdk-linux-x86_64-${VULKAN_SDK_VERSION}.tar.gz \ 105 | -O /tmp/vulkansdk-linux-x86_64-${VULKAN_SDK_VERSION}.tar.gz \ 106 | && echo "Installing Vulkan SDK ${VULKAN_SDK_VERSION}" \ 107 | && mkdir -p /opt/vulkan \ 108 | && tar -xf /tmp/vulkansdk-linux-x86_64-${VULKAN_SDK_VERSION}.tar.gz -C /opt/vulkan \ 109 | && mkdir -p /usr/local/include/ && cp -ra /opt/vulkan/${VULKAN_SDK_VERSION}/x86_64/include/* /usr/local/include/ \ 110 | && mkdir -p /usr/local/lib && cp -ra /opt/vulkan/${VULKAN_SDK_VERSION}/x86_64/lib/* /usr/local/lib/ \ 111 | && cp -a /opt/vulkan/${VULKAN_SDK_VERSION}/x86_64/lib/libVkLayer_*.so /usr/local/lib \ 112 | && mkdir -p /usr/local/share/vulkan/explicit_layer.d \ 113 | && cp /opt/vulkan/${VULKAN_SDK_VERSION}/x86_64/etc/vulkan/explicit_layer.d/VkLayer_*.json /usr/local/share/vulkan/explicit_layer.d \ 114 | && mkdir -p /usr/local/share/vulkan/registry \ 115 | && cp -a /opt/vulkan/${VULKAN_SDK_VERSION}/x86_64/share/vulkan/registry/* /usr/local/share/vulkan/registry \ 116 | && cp -a /opt/vulkan/${VULKAN_SDK_VERSION}/x86_64/bin/* /usr/local/bin \ 117 | && ldconfig \ 118 | && rm /tmp/vulkansdk-linux-x86_64-${VULKAN_SDK_VERSION}.tar.gz && rm -rf /opt/vulkan 119 | 120 | 121 | 122 | 123 | # Update and upgrade base packages 124 | RUN apt-get update && apt-get upgrade -y && apt-get install -y \ 125 | curl \ 126 | gnupg2 \ 127 | lsb-release \ 128 | && apt-get clean && rm -rf /var/lib/apt/lists/* 129 | 130 | # Add ROS2 Foxy repository (correct URL for Ubuntu 20.04) 131 | RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key | apt-key add - && \ 132 | echo "deb [arch=amd64] http://packages.ros.org/ros2/ubuntu focal main" | tee /etc/apt/sources.list.d/ros2.list && \ 133 | apt-get update 134 | 135 | # Install ROS2 Foxy 136 | RUN apt-get install -y ros-foxy-desktop python3-colcon-common-extensions && \ 137 | apt-get clean && rm -rf /var/lib/apt/lists/* 138 | 139 | # Source ROS2 setup 140 | RUN echo "source /opt/ros/foxy/setup.bash" >> ~/.bashrc 141 | 142 | # Install additional ROS2 dependencies as needed 143 | RUN apt-get update && \ 144 | apt-get clean && rm -rf /var/lib/apt/lists/* 145 | 146 | 147 | # Setup the required capabilities for the container runtime 148 | ENV NVIDIA_VISIBLE_DEVICES=all NVIDIA_DRIVER_CAPABILITIES=all 149 | 150 | # Open ports for live streaming 151 | EXPOSE 47995-48012/udp \ 152 | 47995-48012/tcp \ 153 | 49000-49007/udp \ 154 | 49000-49007/tcp \ 155 | 49100/tcp \ 156 | 8011/tcp \ 157 | 8012/tcp \ 158 | 8211/tcp \ 159 | 8899/tcp \ 160 | 8891/tcp 161 | 162 | ENV OMNI_SERVER http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/${ISAAC_SIM_VERSION} 163 | ENV OMNI_SERVER omniverse://localhost/NVIDIA/Assets/Isaac/2022.1 164 | ENV OMNI_USER admin 165 | ENV OMNI_PASS admin 166 | ENV MIN_DRIVER_VERSION 525.60.11 167 | 168 | # Copy Isaac Sim files 169 | COPY --from=isaac-sim /isaac-sim /isaac-sim 170 | RUN mkdir -p /root/.nvidia-omniverse/config 171 | COPY --from=isaac-sim /root/.nvidia-omniverse/config /root/.nvidia-omniverse/config 172 | COPY --from=isaac-sim /etc/vulkan/icd.d/nvidia_icd.json /etc/vulkan/icd.d/nvidia_icd.json 173 | COPY --from=isaac-sim /etc/vulkan/icd.d/nvidia_icd.json /etc/vulkan/implicit_layer.d/nvidia_layers.json 174 | 175 | WORKDIR /isaac-sim 176 | 177 | 178 | ENV TORCH_CUDA_ARCH_LIST="7.0+PTX" 179 | 180 | 181 | 182 | 183 | # create an alias for omniverse python 184 | ENV omni_python='/isaac-sim/python.sh' 185 | 186 | RUN echo "alias omni_python='/isaac-sim/python.sh'" >> /.bashrc 187 | 188 | 189 | # Add cache date to avoid using cached layers older than this 190 | ARG CACHE_DATE=2024-04-11 191 | 192 | RUN $omni_python -m pip install "robometrics[evaluator] @ git+https://github.com/fishbotics/robometrics.git" 193 | 194 | # if you want to use a different version of curobo, create folder as docker/pkgs and put your 195 | # version of curobo there. Then uncomment below line and comment the next line that clones from 196 | # github 197 | 198 | # COPY pkgs /pkgs 199 | 200 | # RUN mkdir /pkgs && cd /pkgs && git clone https://github.com/MetaToolEU/curobo.git -b isaac-3.0 201 | 202 | RUN mkdir -p /pkgs && \ 203 | cd /pkgs && \ 204 | git clone https://github.com/MetaToolEU/curobo.git -b isaac-3.0 205 | 206 | 207 | RUN $omni_python -m pip install ninja wheel tomli 208 | 209 | 210 | RUN cd /pkgs/curobo && $omni_python -m pip install .[dev] --no-build-isolation 211 | 212 | WORKDIR /pkgs/curobo 213 | 214 | # Optionally install nvblox: 215 | 216 | RUN apt-get update && \ 217 | apt-get install -y curl tcl && \ 218 | rm -rf /var/lib/apt/lists/* 219 | 220 | 221 | 222 | # install gflags and glog statically, instructions from: https://github.com/nvidia-isaac/nvblox/blob/public/docs/redistributable.md 223 | 224 | 225 | RUN cd /pkgs && wget https://cmake.org/files/v3.27/cmake-3.27.1.tar.gz && \ 226 | tar -xvzf cmake-3.27.1.tar.gz && \ 227 | apt update && apt install -y build-essential checkinstall zlib1g-dev libssl-dev && \ 228 | cd cmake-3.27.1 && ./bootstrap && \ 229 | make -j8 && \ 230 | make install && rm -rf /var/lib/apt/lists/* 231 | 232 | 233 | ENV USE_CX11_ABI=0 234 | ENV PRE_CX11_ABI=ON 235 | 236 | 237 | 238 | RUN cd /pkgs && git clone https://github.com/sqlite/sqlite.git -b version-3.39.4 && \ 239 | cd /pkgs/sqlite && CFLAGS=-fPIC ./configure --prefix=/pkgs/sqlite/install/ && \ 240 | make && make install 241 | 242 | 243 | 244 | RUN cd /pkgs && git clone https://github.com/google/glog.git -b v0.6.0 && \ 245 | cd glog && \ 246 | mkdir build && cd build && \ 247 | cmake .. -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ 248 | -DCMAKE_INSTALL_PREFIX=/pkgs/glog/install/ \ 249 | -DWITH_GFLAGS=OFF -DWITH_GTEST=OFF -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS=-D_GLIBCXX_USE_CXX11_ABI=${USE_CX11_ABI} \ 250 | && make -j8 && make install 251 | 252 | 253 | RUN cd /pkgs && git clone https://github.com/gflags/gflags.git -b v2.2.2 && \ 254 | cd gflags && \ 255 | mkdir build && cd build && \ 256 | cmake .. -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ 257 | -DCMAKE_INSTALL_PREFIX=/pkgs/gflags/install/ \ 258 | -DGFLAGS_BUILD_STATIC_LIBS=ON -DCMAKE_CXX_FLAGS=-D_GLIBCXX_USE_CXX11_ABI=${USE_CX11_ABI} \ 259 | && make -j8 && make install 260 | 261 | RUN cd /pkgs && git clone https://github.com/valtsblukis/nvblox.git && cd /pkgs/nvblox/nvblox && \ 262 | mkdir build && cd build && \ 263 | cmake .. -DBUILD_REDISTRIBUTABLE=ON \ 264 | -DCMAKE_CXX_FLAGS=-D_GLIBCXX_USE_CXX11_ABI=${USE_CX11_ABI} -DPRE_CXX11_ABI_LINKABLE=${PRE_CX11_ABI} \ 265 | -DSQLITE3_BASE_PATH="/pkgs/sqlite/install/" -DGLOG_BASE_PATH="/pkgs/glog/install/" \ 266 | -DGFLAGS_BASE_PATH="/pkgs/gflags/install/" -DCMAKE_CUDA_FLAGS=-D_GLIBCXX_USE_CXX11_ABI=${USE_CX11_ABI} \ 267 | -DBUILD_TESTING=OFF && \ 268 | make -j32 && \ 269 | make install 270 | 271 | # we also need libglog for pytorch: 272 | RUN cd /pkgs/glog && \ 273 | mkdir build_isaac && cd build_isaac && \ 274 | cmake .. -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ 275 | -DWITH_GFLAGS=OFF -DWITH_GTEST=OFF -DBUILD_SHARED_LIBS=OFF -DCMAKE_CXX_FLAGS=-D_GLIBCXX_USE_CXX11_ABI=${USE_CX11_ABI} \ 276 | && make -j8 && make install 277 | 278 | RUN cd /pkgs && git clone https://github.com/nvlabs/nvblox_torch.git && \ 279 | cd /pkgs/nvblox_torch && \ 280 | sh install_isaac_sim.sh $($omni_python -c 'import torch.utils; print(torch.utils.cmake_prefix_path)') && \ 281 | $omni_python -m pip install -e . 282 | 283 | # install realsense for nvblox demos: 284 | RUN $omni_python -m pip install pyrealsense2 opencv-python transforms3d 285 | 286 | RUN $omni_python -m pip install "robometrics[evaluator] @ git+https://github.com/fishbotics/robometrics.git" 287 | 288 | -------------------------------------------------------------------------------- /Isaac_launch/motion_gen_reacher.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 5 | # property and proprietary rights in and to this material, related 6 | # documentation and any modifications thereto. Any use, reproduction, 7 | # disclosure or distribution of this material and related documentation 8 | # without an express license agreement from NVIDIA CORPORATION or 9 | # its affiliates is strictly prohibited. 10 | # 11 | 12 | 13 | # Third Party 14 | import torch 15 | import os 16 | script_dir = os.path.dirname(os.path.abspath(__file__)) 17 | 18 | a = torch.zeros(4, device="cuda:0") 19 | 20 | # Standard Library 21 | import argparse 22 | 23 | parser = argparse.ArgumentParser() 24 | parser.add_argument( 25 | "--headless_mode", 26 | type=str, 27 | default=None, 28 | help="To run headless, use one of [native, websocket], webrtc might not work.", 29 | ) 30 | parser.add_argument("--robot", type=str, default="ur3e_gripper.yml", help="robot configuration to load") 31 | 32 | parser.add_argument( 33 | "--visualize_spheres", 34 | action="store_true", 35 | help="When True, visualizes robot spheres", 36 | default=True, 37 | ) 38 | 39 | args = parser.parse_args() 40 | 41 | ############################################################ 42 | 43 | # Third Party 44 | from omni.isaac.kit import SimulationApp 45 | 46 | simulation_app = SimulationApp( 47 | { 48 | "headless": args.headless_mode is not None, 49 | "width": "1920", 50 | "height": "1080", 51 | } 52 | ) 53 | # Standard Library 54 | from typing import Dict 55 | 56 | # Third Party 57 | import carb 58 | import numpy as np 59 | from helper import add_extensions, add_robot_to_scene 60 | from omni.isaac.core import World 61 | from omni.isaac.core.objects import cuboid, sphere 62 | 63 | ########### OV ################# 64 | from omni.isaac.core.utils.types import ArticulationAction 65 | 66 | # CuRobo 67 | # from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig 68 | from curobo.geom.sdf.world import CollisionCheckerType 69 | from curobo.geom.types import WorldConfig 70 | from curobo.types.base import TensorDeviceType 71 | from curobo.types.math import Pose 72 | from curobo.types.robot import JointState 73 | from curobo.types.state import JointState 74 | from curobo.util.logger import setup_curobo_logger 75 | from curobo.util.usd_helper import UsdHelper 76 | from curobo.util_file import ( 77 | get_assets_path, 78 | get_filename, 79 | get_path_of_dir, 80 | get_robot_configs_path, 81 | get_world_configs_path, 82 | join_path, 83 | load_yaml, 84 | ) 85 | from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig 86 | 87 | ############################################################ 88 | from pxr import UsdGeom, Gf 89 | 90 | def setup_camera(stage): 91 | # Define the camera in the scene 92 | camera_prim_path = "/World/Camera" 93 | if not stage.GetPrimAtPath(camera_prim_path): 94 | UsdGeom.Camera.Define(stage, camera_prim_path) 95 | 96 | camera_prim = stage.GetPrimAtPath(camera_prim_path) 97 | camera = UsdGeom.Camera(camera_prim) 98 | 99 | # Set camera properties 100 | camera.GetFocalLengthAttr().Set(35.0) 101 | camera.GetHorizontalApertureAttr().Set(20.955) 102 | camera.GetVerticalApertureAttr().Set(15.2908) 103 | 104 | # Set the initial camera position and orientation 105 | xform = UsdGeom.Xformable(camera) 106 | translate_op = xform.AddTranslateOp() 107 | translate_op.Set(Gf.Vec3d(2.7, 0.6, 1)) 108 | 109 | rotate_op = xform.AddRotateXYZOp() 110 | rotate_op.Set(Gf.Vec3f(90, 0, 90)) 111 | 112 | return camera_prim_path 113 | # from omni.isaac.sensor import Camera 114 | # import omni.isaac.core.utils.numpy.rotations as rot_utils 115 | 116 | # def setup_camera(stage): 117 | # camera = Camera( 118 | # prim_path="/World/camera", 119 | # position=np.array([2.7, 0.6, 1.0]), 120 | # frequency=20, 121 | # resolution=(256, 256), 122 | # orientation=rot_utils.euler_angles_to_quats(np.array([90, 0, 90]), degrees=True), 123 | # ) 124 | # return camera 125 | ########### OV #################;;;;; 126 | 127 | 128 | def main(): 129 | # create a curobo motion gen instance: 130 | 131 | # assuming obstacles are in objects_path: 132 | my_world = World(stage_units_in_meters=1.0) 133 | stage = my_world.stage 134 | 135 | xform = stage.DefinePrim("/World", "Xform") 136 | stage.SetDefaultPrim(xform) 137 | stage.DefinePrim("/curobo", "Xform") 138 | # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World")) 139 | stage = my_world.stage 140 | # stage.SetDefaultPrim(stage.GetPrimAtPath("/World")) 141 | # Set up the camera 142 | camera_prim_path = setup_camera(stage) 143 | # Make a target to follow 144 | target = cuboid.VisualCuboid( 145 | "/World/target", 146 | position=np.array([-0.5, 0.6, 0.85]), 147 | orientation=np.array([0, 1, 0, 0]), 148 | color=np.array([1.0, 0, 0]), 149 | size=0.05, 150 | ) 151 | 152 | setup_curobo_logger("warn") 153 | past_pose = None 154 | n_obstacle_cuboids = 30 155 | n_obstacle_mesh = 10 156 | 157 | # warmup curobo instance 158 | usd_help = UsdHelper() 159 | target_pose = None 160 | 161 | tensor_args = TensorDeviceType() 162 | 163 | robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"] 164 | 165 | j_names = robot_cfg["kinematics"]["cspace"]["joint_names"] 166 | # default_config = robot_cfg["kinematics"]["cspace"]["retract_config"] 167 | default_config =robot_cfg["kinematics"]["cspace"]["retract_config"] = [-0.0, -0.0, 0.0, -1.0, 0.0, -0.0] 168 | 169 | robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world) 170 | 171 | articulation_controller = robot.get_articulation_controller() 172 | world_config_path = os.path.join(script_dir, "../curobo/content/configs/world") 173 | world_cfg_table = WorldConfig.from_dict( 174 | load_yaml(os.path.join(world_config_path, "collision_mesh_scene_Lab.yml")) 175 | ).get_mesh_world() 176 | world_cfg_table.mesh[0].name += "_mesh" 177 | # world_cfg1.mesh[0].pose[2] = -10.5 178 | 179 | world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg_table.mesh) 180 | 181 | motion_gen_config = MotionGenConfig.load_from_robot_config( 182 | robot_cfg, 183 | world_cfg, 184 | tensor_args, 185 | trajopt_tsteps=32, 186 | collision_checker_type=CollisionCheckerType.MESH, 187 | use_cuda_graph=True, 188 | num_trajopt_seeds=12, 189 | num_graph_seeds=12, 190 | interpolation_dt=0.03, 191 | collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh}, 192 | collision_activation_distance=0.025, 193 | acceleration_scale=1.0, 194 | self_collision_check=True, 195 | maximum_trajectory_dt=0.25, 196 | fixed_iters_trajopt=True, 197 | finetune_dt_scale=1.05, 198 | velocity_scale=[0.25, 1, 1, 1, 1.0, 1.0, 1.0, 1.0, 1.0], 199 | ) 200 | motion_gen = MotionGen(motion_gen_config) 201 | print("warming up...") 202 | motion_gen.warmup(enable_graph=False, warmup_js_trajopt=False) 203 | 204 | print("Curobo is Ready") 205 | 206 | add_extensions(simulation_app, args.headless_mode) 207 | 208 | plan_config = MotionGenPlanConfig( 209 | enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True 210 | ) 211 | 212 | usd_help.load_stage(my_world.stage) 213 | usd_help.add_world_to_stage(world_cfg, base_frame="/World") 214 | 215 | cmd_plan = None 216 | cmd_idx = 0 217 | my_world.scene.add_default_ground_plane() 218 | i = 0 219 | spheres = None 220 | 221 | while simulation_app.is_running(): 222 | my_world.step(render=True) 223 | if not my_world.is_playing(): 224 | if i % 100 == 0: 225 | print("**** Click Play to start simulation *****") 226 | i += 1 227 | # if step_index == 0: 228 | # my_world.play() 229 | continue 230 | 231 | step_index = my_world.current_time_step_index 232 | # print(step_index) 233 | if step_index < 2: 234 | my_world.reset() 235 | robot._articulation_view.initialize() 236 | idx_list = [robot.get_dof_index(x) for x in j_names] 237 | robot.set_joint_positions(default_config, idx_list) 238 | 239 | robot._articulation_view.set_max_efforts( 240 | values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list 241 | ) 242 | if step_index < 20: 243 | continue 244 | 245 | if step_index == 50 or step_index % 1000 == 0.0: 246 | print("Updating world, reading w.r.t.", robot_prim_path) 247 | obstacles = usd_help.get_obstacles_from_stage( 248 | # only_paths=[obstacles_path], 249 | reference_prim_path=robot_prim_path, 250 | ignore_substring=[ 251 | robot_prim_path, 252 | "/World/target", 253 | "/World/defaultGroundPlane", 254 | "/curobo", 255 | ], 256 | ).get_collision_check_world() 257 | 258 | motion_gen.update_world(obstacles) 259 | print("Updated World") 260 | carb.log_info("Synced CuRobo world from stage.") 261 | 262 | # position and orientation of target virtual cube: 263 | cube_position, cube_orientation = target.get_world_pose() 264 | 265 | if past_pose is None: 266 | past_pose = cube_position 267 | if target_pose is None: 268 | target_pose = cube_position 269 | 270 | sim_js = robot.get_joints_state() 271 | sim_js_names = robot.dof_names 272 | cu_js = JointState( 273 | position=tensor_args.to_device(sim_js.positions), 274 | velocity=tensor_args.to_device(sim_js.velocities) * 0.0, 275 | acceleration=tensor_args.to_device(sim_js.velocities) * 0.0, 276 | jerk=tensor_args.to_device(sim_js.velocities) * 0.0, 277 | joint_names=sim_js_names, 278 | ) 279 | cu_js = cu_js.get_ordered_joint_state(motion_gen.kinematics.joint_names) 280 | 281 | if args.visualize_spheres and step_index % 2 == 0: 282 | sph_list = motion_gen.kinematics.get_robot_as_spheres(cu_js.position) 283 | 284 | if spheres is None: 285 | spheres = [] 286 | # create spheres: 287 | 288 | for si, s in enumerate(sph_list[0]): 289 | sp = sphere.VisualSphere( 290 | prim_path="/curobo/robot_sphere_" + str(si), 291 | position=np.ravel(s.position), 292 | radius=float(s.radius), 293 | color=np.array([0, 0.8, 0.2]), 294 | ) 295 | spheres.append(sp) 296 | else: 297 | for si, s in enumerate(sph_list[0]): 298 | spheres[si].set_world_pose(position=np.ravel(s.position)) 299 | spheres[si].set_radius(float(s.radius)) 300 | if ( 301 | np.linalg.norm(cube_position - target_pose) > 1e-3 302 | and np.linalg.norm(past_pose - cube_position) == 0.0 303 | and np.linalg.norm(sim_js.velocities) < 0.2 304 | ): 305 | # Set EE teleop goals, use cube for simple non-vr init: 306 | ee_translation_goal = cube_position 307 | ee_orientation_teleop_goal = cube_orientation 308 | 309 | # compute curobo solution: 310 | ik_goal = Pose( 311 | position=tensor_args.to_device(ee_translation_goal), 312 | quaternion=tensor_args.to_device(ee_orientation_teleop_goal), 313 | ) 314 | 315 | result = motion_gen.plan_single(cu_js.unsqueeze(0), ik_goal, plan_config) 316 | # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1)) 317 | 318 | succ = result.success.item() # ik_result.success.item() 319 | if succ: 320 | cmd_plan = result.get_interpolated_plan() 321 | cmd_plan = motion_gen.get_full_js(cmd_plan) 322 | # get only joint names that are in both: 323 | idx_list = [] 324 | common_js_names = [] 325 | for x in sim_js_names: 326 | if x in cmd_plan.joint_names: 327 | idx_list.append(robot.get_dof_index(x)) 328 | common_js_names.append(x) 329 | # idx_list = [robot.get_dof_index(x) for x in sim_js_names] 330 | 331 | cmd_plan = cmd_plan.get_ordered_joint_state(common_js_names) 332 | cmd_idx = 0 333 | else: 334 | carb.log_warn("Plan did not converge to a solution. No action is being taken.") 335 | target_pose = cube_position 336 | past_pose = cube_position 337 | if cmd_plan is not None: 338 | cmd_state = cmd_plan[cmd_idx] 339 | 340 | # get full dof state 341 | art_action = ArticulationAction( 342 | cmd_state.position.cpu().numpy(), 343 | cmd_state.velocity.cpu().numpy(), 344 | joint_indices=idx_list, 345 | ) 346 | # set desired joint angles obtained from IK: 347 | print(art_action) 348 | articulation_controller.apply_action(art_action) 349 | cmd_idx += 1 350 | for _ in range(2): 351 | my_world.step(render=False) 352 | if cmd_idx >= len(cmd_plan.position): 353 | cmd_idx = 0 354 | cmd_plan = None 355 | simulation_app.close() 356 | 357 | 358 | if __name__ == "__main__": 359 | main() 360 | 361 | -------------------------------------------------------------------------------- /motion_policy_configs/ur3e/ur3e_gripper.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 57 | 62 | 63 | transmission_interface/SimpleTransmission 64 | 65 | hardware_interface/PositionJointInterface 66 | 67 | 68 | 1 69 | 70 | 71 | 72 | transmission_interface/SimpleTransmission 73 | 74 | hardware_interface/PositionJointInterface 75 | 76 | 77 | 1 78 | 79 | 80 | 81 | transmission_interface/SimpleTransmission 82 | 83 | hardware_interface/PositionJointInterface 84 | 85 | 86 | 1 87 | 88 | 89 | 90 | transmission_interface/SimpleTransmission 91 | 92 | hardware_interface/PositionJointInterface 93 | 94 | 95 | 1 96 | 97 | 98 | 99 | transmission_interface/SimpleTransmission 100 | 101 | hardware_interface/PositionJointInterface 102 | 103 | 104 | 1 105 | 106 | 107 | 108 | transmission_interface/SimpleTransmission 109 | 110 | hardware_interface/PositionJointInterface 111 | 112 | 113 | 1 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | -------------------------------------------------------------------------------- /Isaac_launch/rtde/rtde.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020-2022, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import struct 25 | import socket 26 | import select 27 | import sys 28 | import logging 29 | 30 | if sys.version_info[0] < 3: 31 | import serialize 32 | else: 33 | from . import serialize 34 | 35 | DEFAULT_TIMEOUT = 1.0 36 | 37 | LOGNAME = "rtde" 38 | _log = logging.getLogger(LOGNAME) 39 | 40 | 41 | class Command: 42 | RTDE_REQUEST_PROTOCOL_VERSION = 86 # ascii V 43 | RTDE_GET_URCONTROL_VERSION = 118 # ascii v 44 | RTDE_TEXT_MESSAGE = 77 # ascii M 45 | RTDE_DATA_PACKAGE = 85 # ascii U 46 | RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS = 79 # ascii O 47 | RTDE_CONTROL_PACKAGE_SETUP_INPUTS = 73 # ascii I 48 | RTDE_CONTROL_PACKAGE_START = 83 # ascii S 49 | RTDE_CONTROL_PACKAGE_PAUSE = 80 # ascii P 50 | 51 | 52 | RTDE_PROTOCOL_VERSION_1 = 1 53 | RTDE_PROTOCOL_VERSION_2 = 2 54 | 55 | 56 | class ConnectionState: 57 | DISCONNECTED = 0 58 | CONNECTED = 1 59 | STARTED = 2 60 | PAUSED = 3 61 | 62 | 63 | class RTDEException(Exception): 64 | def __init__(self, msg): 65 | self.msg = msg 66 | 67 | def __str__(self): 68 | return repr(self.msg) 69 | 70 | 71 | class RTDETimeoutException(RTDEException): 72 | def __init__(self, msg): 73 | super(RTDETimeoutException, self).__init__(msg) 74 | 75 | 76 | class RTDE(object): 77 | def __init__(self, hostname, port=30004): 78 | self.hostname = hostname 79 | self.port = port 80 | self.__conn_state = ConnectionState.DISCONNECTED 81 | self.__sock = None 82 | self.__output_config = None 83 | self.__input_config = {} 84 | self.__skipped_package_count = 0 85 | self.__protocolVersion = RTDE_PROTOCOL_VERSION_1 86 | 87 | def connect(self): 88 | if self.__sock: 89 | return 90 | 91 | self.__buf = b"" # buffer data in binary format 92 | try: 93 | self.__sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 94 | self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 95 | self.__sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) 96 | self.__sock.settimeout(DEFAULT_TIMEOUT) 97 | self.__skipped_package_count = 0 98 | self.__sock.connect((self.hostname, self.port)) 99 | self.__conn_state = ConnectionState.CONNECTED 100 | except (socket.timeout, socket.error): 101 | self.__sock = None 102 | raise 103 | if not self.negotiate_protocol_version(): 104 | raise RTDEException("Unable to negotiate protocol version") 105 | 106 | def disconnect(self): 107 | if self.__sock: 108 | self.__sock.close() 109 | self.__sock = None 110 | self.__conn_state = ConnectionState.DISCONNECTED 111 | 112 | def is_connected(self): 113 | return self.__conn_state is not ConnectionState.DISCONNECTED 114 | 115 | def get_controller_version(self): 116 | cmd = Command.RTDE_GET_URCONTROL_VERSION 117 | version = self.__sendAndReceive(cmd) 118 | if version: 119 | _log.info( 120 | "Controller version: " 121 | + str(version.major) 122 | + "." 123 | + str(version.minor) 124 | + "." 125 | + str(version.bugfix) 126 | + "." 127 | + str(version.build) 128 | ) 129 | if version.major == 3 and version.minor <= 2 and version.bugfix < 19171: 130 | _log.error( 131 | "Please upgrade your controller to minimally version 3.2.19171" 132 | ) 133 | sys.exit() 134 | return version.major, version.minor, version.bugfix, version.build 135 | return None, None, None, None 136 | 137 | def negotiate_protocol_version(self): 138 | cmd = Command.RTDE_REQUEST_PROTOCOL_VERSION 139 | payload = struct.pack(">H", RTDE_PROTOCOL_VERSION_2) 140 | success = self.__sendAndReceive(cmd, payload) 141 | if success: 142 | self.__protocolVersion = RTDE_PROTOCOL_VERSION_2 143 | return success 144 | 145 | def send_input_setup(self, variables, types=[]): 146 | cmd = Command.RTDE_CONTROL_PACKAGE_SETUP_INPUTS 147 | payload = bytearray(",".join(variables), "utf-8") 148 | result = self.__sendAndReceive(cmd, payload) 149 | if len(types) != 0 and not self.__list_equals(result.types, types): 150 | _log.error( 151 | "Data type inconsistency for input setup: " 152 | + str(types) 153 | + " - " 154 | + str(result.types) 155 | ) 156 | return None 157 | result.names = variables 158 | self.__input_config[result.id] = result 159 | return serialize.DataObject.create_empty(variables, result.id) 160 | 161 | def send_output_setup(self, variables, types=[], frequency=125): 162 | cmd = Command.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS 163 | payload = struct.pack(">d", frequency) 164 | payload = payload + (",".join(variables).encode("utf-8")) 165 | result = self.__sendAndReceive(cmd, payload) 166 | if len(types) != 0 and not self.__list_equals(result.types, types): 167 | _log.error( 168 | "Data type inconsistency for output setup: " 169 | + str(types) 170 | + " - " 171 | + str(result.types) 172 | ) 173 | return False 174 | result.names = variables 175 | self.__output_config = result 176 | return True 177 | 178 | def send_start(self): 179 | cmd = Command.RTDE_CONTROL_PACKAGE_START 180 | success = self.__sendAndReceive(cmd) 181 | if success: 182 | _log.info("RTDE synchronization started") 183 | self.__conn_state = ConnectionState.STARTED 184 | else: 185 | _log.error("RTDE synchronization failed to start") 186 | return success 187 | 188 | def send_pause(self): 189 | cmd = Command.RTDE_CONTROL_PACKAGE_PAUSE 190 | success = self.__sendAndReceive(cmd) 191 | if success: 192 | _log.info("RTDE synchronization paused") 193 | self.__conn_state = ConnectionState.PAUSED 194 | else: 195 | _log.error("RTDE synchronization failed to pause") 196 | return success 197 | 198 | def send(self, input_data): 199 | if self.__conn_state != ConnectionState.STARTED: 200 | _log.error("Cannot send when RTDE synchronization is inactive") 201 | return 202 | if not input_data.recipe_id in self.__input_config: 203 | _log.error("Input configuration id not found: " + str(input_data.recipe_id)) 204 | return 205 | config = self.__input_config[input_data.recipe_id] 206 | return self.__sendall(Command.RTDE_DATA_PACKAGE, config.pack(input_data)) 207 | 208 | def receive(self, binary=False): 209 | """Recieve the latest data package. 210 | If muliple packages has been received, older ones are discarded 211 | and only the newest one will be returned. Will block untill a package 212 | is received or the connection is lost 213 | """ 214 | if self.__output_config is None: 215 | raise RTDEException("Output configuration not initialized") 216 | if self.__conn_state != ConnectionState.STARTED: 217 | raise RTDEException("Cannot receive when RTDE synchronization is inactive") 218 | return self.__recv(Command.RTDE_DATA_PACKAGE, binary) 219 | 220 | def receive_buffered(self, binary=False, buffer_limit=None): 221 | """Recieve the next data package. 222 | If muliple packages has been received they are buffered and will 223 | be returned on subsequent calls to this function. 224 | Returns None if no data is available. 225 | """ 226 | 227 | if self._RTDE__output_config is None: 228 | logging.error("Output configuration not initialized") 229 | return None 230 | 231 | try: 232 | while ( 233 | self.is_connected() 234 | and (buffer_limit == None or len(self.__buf) < buffer_limit) 235 | and self.__recv_to_buffer(0) 236 | ): 237 | pass 238 | except RTDEException as e: 239 | data = self.__recv_from_buffer(Command.RTDE_DATA_PACKAGE, binary) 240 | if data == None: 241 | raise e 242 | else: 243 | data = self.__recv_from_buffer(Command.RTDE_DATA_PACKAGE, binary) 244 | 245 | return data 246 | 247 | def send_message( 248 | self, message, source="Python Client", type=serialize.Message.INFO_MESSAGE 249 | ): 250 | cmd = Command.RTDE_TEXT_MESSAGE 251 | fmt = ">B%dsB%dsB" % (len(message), len(source)) 252 | payload = struct.pack(fmt, len(message), message, len(source), source, type) 253 | return self.__sendall(cmd, payload) 254 | 255 | def __on_packet(self, cmd, payload): 256 | if cmd == Command.RTDE_REQUEST_PROTOCOL_VERSION: 257 | return self.__unpack_protocol_version_package(payload) 258 | elif cmd == Command.RTDE_GET_URCONTROL_VERSION: 259 | return self.__unpack_urcontrol_version_package(payload) 260 | elif cmd == Command.RTDE_TEXT_MESSAGE: 261 | return self.__unpack_text_message(payload) 262 | elif cmd == Command.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: 263 | return self.__unpack_setup_outputs_package(payload) 264 | elif cmd == Command.RTDE_CONTROL_PACKAGE_SETUP_INPUTS: 265 | return self.__unpack_setup_inputs_package(payload) 266 | elif cmd == Command.RTDE_CONTROL_PACKAGE_START: 267 | return self.__unpack_start_package(payload) 268 | elif cmd == Command.RTDE_CONTROL_PACKAGE_PAUSE: 269 | return self.__unpack_pause_package(payload) 270 | elif cmd == Command.RTDE_DATA_PACKAGE: 271 | return self.__unpack_data_package(payload, self.__output_config) 272 | else: 273 | _log.error("Unknown package command: " + str(cmd)) 274 | 275 | def __sendAndReceive(self, cmd, payload=b""): 276 | if self.__sendall(cmd, payload): 277 | return self.__recv(cmd) 278 | else: 279 | return None 280 | 281 | def __sendall(self, command, payload=b""): 282 | fmt = ">HB" 283 | size = struct.calcsize(fmt) + len(payload) 284 | buf = struct.pack(fmt, size, command) + payload 285 | 286 | if self.__sock is None: 287 | _log.error("Unable to send: not connected to Robot") 288 | return False 289 | 290 | _, writable, _ = select.select([], [self.__sock], [], DEFAULT_TIMEOUT) 291 | if len(writable): 292 | self.__sock.sendall(buf) 293 | return True 294 | else: 295 | self.__trigger_disconnected() 296 | return False 297 | 298 | def has_data(self): 299 | timeout = 0 300 | readable, _, _ = select.select([self.__sock], [], [], timeout) 301 | return len(readable) != 0 302 | 303 | def __recv(self, command, binary=False): 304 | while self.is_connected(): 305 | try: 306 | self.__recv_to_buffer(DEFAULT_TIMEOUT) 307 | except RTDETimeoutException: 308 | return None 309 | 310 | # unpack_from requires a buffer of at least 3 bytes 311 | while len(self.__buf) >= 3: 312 | # Attempts to extract a packet 313 | packet_header = serialize.ControlHeader.unpack(self.__buf) 314 | 315 | if len(self.__buf) >= packet_header.size: 316 | packet, self.__buf = ( 317 | self.__buf[3 : packet_header.size], 318 | self.__buf[packet_header.size :], 319 | ) 320 | data = self.__on_packet(packet_header.command, packet) 321 | if len(self.__buf) >= 3 and command == Command.RTDE_DATA_PACKAGE: 322 | next_packet_header = serialize.ControlHeader.unpack(self.__buf) 323 | if next_packet_header.command == command: 324 | _log.debug("skipping package(1)") 325 | self.__skipped_package_count += 1 326 | continue 327 | if packet_header.command == command: 328 | if binary: 329 | return packet[1:] 330 | 331 | return data 332 | else: 333 | _log.debug("skipping package(2)") 334 | else: 335 | break 336 | raise RTDEException(" _recv() Connection lost ") 337 | 338 | def __recv_to_buffer(self, timeout): 339 | readable, _, xlist = select.select([self.__sock], [], [self.__sock], timeout) 340 | if len(readable): 341 | more = self.__sock.recv(4096) 342 | # When the controller stops while the script is running 343 | if len(more) == 0: 344 | _log.error( 345 | "received 0 bytes from Controller, probable cause: Controller has stopped" 346 | ) 347 | self.__trigger_disconnected() 348 | raise RTDEException("received 0 bytes from Controller") 349 | 350 | self.__buf = self.__buf + more 351 | return True 352 | 353 | if ( 354 | len(xlist) or len(readable) == 0 355 | ) and timeout != 0: # Effectively a timeout of timeout seconds 356 | _log.warning("no data received in last %d seconds ", timeout) 357 | raise RTDETimeoutException("no data received within timeout") 358 | 359 | return False 360 | 361 | def __recv_from_buffer(self, command, binary=False): 362 | # unpack_from requires a buffer of at least 3 bytes 363 | while len(self.__buf) >= 3: 364 | # Attempts to extract a packet 365 | packet_header = serialize.ControlHeader.unpack(self.__buf) 366 | 367 | if len(self.__buf) >= packet_header.size: 368 | packet, self.__buf = ( 369 | self.__buf[3 : packet_header.size], 370 | self.__buf[packet_header.size :], 371 | ) 372 | data = self.__on_packet(packet_header.command, packet) 373 | if packet_header.command == command: 374 | if binary: 375 | return packet[1:] 376 | 377 | return data 378 | else: 379 | _log.debug("skipping package(2)") 380 | else: 381 | return None 382 | 383 | def __trigger_disconnected(self): 384 | _log.info("RTDE disconnected") 385 | self.disconnect() # clean-up 386 | 387 | def __unpack_protocol_version_package(self, payload): 388 | if len(payload) != 1: 389 | _log.error("RTDE_REQUEST_PROTOCOL_VERSION: Wrong payload size") 390 | return None 391 | result = serialize.ReturnValue.unpack(payload) 392 | return result.success 393 | 394 | def __unpack_urcontrol_version_package(self, payload): 395 | if len(payload) != 16: 396 | _log.error("RTDE_GET_URCONTROL_VERSION: Wrong payload size") 397 | return None 398 | version = serialize.ControlVersion.unpack(payload) 399 | return version 400 | 401 | def __unpack_text_message(self, payload): 402 | if len(payload) < 1: 403 | _log.error("RTDE_TEXT_MESSAGE: No payload") 404 | return None 405 | if self.__protocolVersion == RTDE_PROTOCOL_VERSION_1: 406 | msg = serialize.MessageV1.unpack(payload) 407 | else: 408 | msg = serialize.Message.unpack(payload) 409 | 410 | if ( 411 | msg.level == serialize.Message.EXCEPTION_MESSAGE 412 | or msg.level == serialize.Message.ERROR_MESSAGE 413 | ): 414 | _log.error(msg.source + ": " + msg.message) 415 | elif msg.level == serialize.Message.WARNING_MESSAGE: 416 | _log.warning(msg.source + ": " + msg.message) 417 | elif msg.level == serialize.Message.INFO_MESSAGE: 418 | _log.info(msg.source + ": " + msg.message) 419 | 420 | def __unpack_setup_outputs_package(self, payload): 421 | if len(payload) < 1: 422 | _log.error("RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: No payload") 423 | return None 424 | output_config = serialize.DataConfig.unpack_recipe(payload) 425 | return output_config 426 | 427 | def __unpack_setup_inputs_package(self, payload): 428 | if len(payload) < 1: 429 | _log.error("RTDE_CONTROL_PACKAGE_SETUP_INPUTS: No payload") 430 | return None 431 | input_config = serialize.DataConfig.unpack_recipe(payload) 432 | return input_config 433 | 434 | def __unpack_start_package(self, payload): 435 | if len(payload) != 1: 436 | _log.error("RTDE_CONTROL_PACKAGE_START: Wrong payload size") 437 | return None 438 | result = serialize.ReturnValue.unpack(payload) 439 | return result.success 440 | 441 | def __unpack_pause_package(self, payload): 442 | if len(payload) != 1: 443 | _log.error("RTDE_CONTROL_PACKAGE_PAUSE: Wrong payload size") 444 | return None 445 | result = serialize.ReturnValue.unpack(payload) 446 | return result.success 447 | 448 | def __unpack_data_package(self, payload, output_config): 449 | if output_config is None: 450 | _log.error("RTDE_DATA_PACKAGE: Missing output configuration") 451 | return None 452 | output = output_config.unpack(payload) 453 | return output 454 | 455 | def __list_equals(self, l1, l2): 456 | if len(l1) != len(l2): 457 | return False 458 | for i in range(len((l1))): 459 | if l1[i] != l2[i]: 460 | return False 461 | return True 462 | 463 | @property 464 | def skipped_package_count(self): 465 | """The skipped package count, resets on connect""" 466 | return self.__skipped_package_count 467 | -------------------------------------------------------------------------------- /Isaac_launch/multi_arm_reacher_MT.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 5 | # property and proprietary rights in and to this material, related 6 | # documentation and any modifications thereto. Any use, reproduction, 7 | # disclosure or distribution of this material and related documentation 8 | # without an express license agreement from NVIDIA CORPORATION or 9 | # its affiliates is strictly prohibited. 10 | # 11 | 12 | 13 | # Third Party 14 | import torch 15 | 16 | a = torch.zeros(4, device="cuda:0") 17 | 18 | # Standard Library 19 | import argparse 20 | 21 | parser = argparse.ArgumentParser() 22 | 23 | parser.add_argument( 24 | "--headless_mode", 25 | type=str, 26 | default=None, 27 | help="To run headless, use one of [native, websocket], webrtc might not work.", 28 | ) 29 | parser.add_argument( 30 | "--visualize_spheres", 31 | action="store_true", 32 | help="When True, visualizes robot spheres", 33 | default=True, 34 | ) 35 | 36 | parser.add_argument( 37 | "--robot", type=str, default="dual_ur3e_simple.yml", help="robot configuration to load" 38 | ) 39 | args = parser.parse_args() 40 | 41 | ############################################################ 42 | 43 | # Third Party 44 | from omni.isaac.kit import SimulationApp 45 | 46 | simulation_app = SimulationApp( 47 | { 48 | "headless": args.headless_mode is not None, 49 | "width": "1920", 50 | "height": "1080", 51 | } 52 | ) 53 | # Third Party 54 | import carb 55 | import numpy as np 56 | from helper import add_extensions, add_robot_to_scene 57 | from omni.isaac.core import World 58 | from omni.isaac.core.objects import cuboid, sphere 59 | # Standard Library 60 | from typing import Optional 61 | import omni.graph.core as og # noqa E402 62 | ########### OV ################# 63 | from omni.isaac.core.utils.types import ArticulationAction 64 | 65 | # CuRobo 66 | from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel 67 | 68 | # from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig 69 | from curobo.geom.sdf.world import CollisionCheckerType 70 | from curobo.geom.types import WorldConfig 71 | from curobo.rollout.rollout_base import Goal 72 | from curobo.types.base import TensorDeviceType 73 | from curobo.types.math import Pose 74 | from curobo.types.robot import JointState, RobotConfig 75 | from curobo.types.state import JointState 76 | from curobo.util.logger import setup_curobo_logger 77 | from curobo.util.usd_helper import UsdHelper 78 | from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml 79 | from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig 80 | 81 | ############################################################ 82 | 83 | 84 | ########### OV #################;;;;; 85 | 86 | 87 | ############################################################ 88 | 89 | 90 | def main(): 91 | # assuming obstacles are in objects_path: 92 | my_world = World(stage_units_in_meters=1.0) 93 | stage = my_world.stage 94 | 95 | xform = stage.DefinePrim("/World", "Xform") 96 | stage.SetDefaultPrim(xform) 97 | stage.DefinePrim("/curobo", "Xform") 98 | # my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World")) 99 | stage = my_world.stage 100 | # stage.SetDefaultPrim(stage.GetPrimAtPath("/World")) 101 | 102 | # Make a target to follow 103 | 104 | setup_curobo_logger("warn") 105 | past_pose = None 106 | n_obstacle_cuboids = 30 107 | n_obstacle_mesh = 10 108 | 109 | # warmup curobo instance 110 | usd_help = UsdHelper() 111 | target_pose = None 112 | 113 | tensor_args = TensorDeviceType() 114 | 115 | robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"] 116 | 117 | j_names = robot_cfg["kinematics"]["cspace"]["joint_names"] 118 | default_config = robot_cfg["kinematics"]["cspace"]["retract_config"] = [-0.0, -0.0, 0.0, -1.0, 0.0, -0.0, 119 | -0.0, -0.0, 0.0, -1.0, 0.0, -0.0] 120 | 121 | robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world) 122 | 123 | articulation_controller = robot.get_articulation_controller() 124 | 125 | world_cfg_table = WorldConfig.from_dict( 126 | load_yaml(join_path(get_world_configs_path(), "collision_mesh_scene_Lab.yml")) 127 | ).get_mesh_world() 128 | world_cfg_table.mesh[0].name += "_mesh" 129 | # world_cfg1.mesh[0].pose[2] = -10.5 130 | 131 | world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg_table.mesh) 132 | 133 | motion_gen_config = MotionGenConfig.load_from_robot_config( 134 | robot_cfg, 135 | world_cfg, 136 | tensor_args, 137 | trajopt_tsteps=32, 138 | collision_checker_type=CollisionCheckerType.MESH, 139 | use_cuda_graph=True, 140 | num_trajopt_seeds=12, 141 | num_graph_seeds=12, 142 | interpolation_dt=0.03, 143 | collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh}, 144 | collision_activation_distance=0.025, 145 | acceleration_scale=1.0, 146 | maximum_trajectory_dt=0.2, 147 | fixed_iters_trajopt=True, 148 | ) 149 | motion_gen = MotionGen(motion_gen_config) 150 | print("warming up...") 151 | motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False) 152 | 153 | print("Curobo is Ready") 154 | add_extensions(simulation_app, args.headless_mode) 155 | plan_config = MotionGenPlanConfig( 156 | enable_graph=False, enable_graph_attempt=4, max_attempts=10, enable_finetune_trajopt=True 157 | ) 158 | 159 | usd_help.load_stage(my_world.stage) 160 | usd_help.add_world_to_stage(world_cfg, base_frame="/World") 161 | 162 | cmd_plan = None 163 | cmd_idx = 0 164 | my_world.scene.add_default_ground_plane() 165 | i = 0 166 | spheres = None 167 | 168 | # read number of targets in link names: 169 | link_names = motion_gen.kinematics.link_names 170 | ee_link_name = motion_gen.kinematics.ee_link 171 | # get link poses at retract configuration: 172 | 173 | kin_state = motion_gen.kinematics.get_state(motion_gen.get_retract_config().view(1, -1)) 174 | 175 | link_retract_pose = kin_state.link_pose 176 | t_pos = np.ravel(kin_state.ee_pose.to_list()) 177 | target = cuboid.VisualCuboid( 178 | "/World/target", 179 | position=t_pos[:3], 180 | orientation=t_pos[3:], 181 | color=np.array([1.0, 0, 0]), 182 | size=0.05, 183 | ) 184 | 185 | # create new targets for new links: 186 | ee_idx = link_names.index(ee_link_name) 187 | target_links = {} 188 | names = [] 189 | for i in link_names: 190 | if i != ee_link_name: 191 | k_pose = np.ravel(link_retract_pose[i].to_list()) 192 | color = np.random.randn(3) * 0.2 193 | color[0] += 0.5 194 | color[1] = 0.5 195 | color[2] = 0.0 196 | target_links[i] = cuboid.VisualCuboid( 197 | "/World/target_" + i, 198 | position=np.array(k_pose[:3]), 199 | orientation=np.array(k_pose[3:]), 200 | color=color, 201 | size=0.05, 202 | ) 203 | names.append("/World/target_" + i) 204 | i = 0 205 | import omni 206 | from pxr import UsdGeom 207 | 208 | stage=omni.usd.get_context().get_stage() 209 | 210 | path="/World/Camera/MyCamera" 211 | prim_type="Camera" 212 | translation=(-0.45,0.48,4.73) 213 | rotation=(0,0,90) 214 | camera_prim = stage.DefinePrim(path, prim_type) 215 | xform_api=UsdGeom.XformCommonAPI(camera_prim) 216 | xform_api.SetTranslate(translation) 217 | xform_api.SetRotate(rotation) 218 | 219 | 220 | UR_STAGE_PATH="/World/ur3e_robot" 221 | Camera_STAGE_PATH="/World/Camera/MyCamera" 222 | simulation_app.update() 223 | from omni.isaac.core.utils.extensions import enable_extension 224 | enable_extension("omni.isaac.ros2_bridge") 225 | 226 | try: 227 | og.Controller.edit( 228 | {"graph_path": "/ActionGraph", "evaluator_name": "execution"}, 229 | { 230 | og.Controller.Keys.CREATE_NODES: [ 231 | ("OnImpulseEvent", "omni.graph.action.OnImpulseEvent"), 232 | ("ReadSimTime", "omni.isaac.core_nodes.IsaacReadSimulationTime"), 233 | ("Context", "omni.isaac.ros2_bridge.ROS2Context"), 234 | ("PublishJointState", "omni.isaac.ros2_bridge.ROS2PublishJointState"), 235 | ( 236 | "SubscribeJointState", 237 | "omni.isaac.ros2_bridge.ROS2SubscribeJointState", 238 | ), 239 | ( 240 | "ArticulationController", 241 | "omni.isaac.core_nodes.IsaacArticulationController", 242 | ), 243 | ("PublishClock", "omni.isaac.ros2_bridge.ROS2PublishClock"), 244 | 245 | ######camera graph 246 | ("IsaacCreateViewport", "omni.isaac.core_nodes.IsaacCreateViewport"), 247 | ("IsaacGetViewportRenderProduct", "omni.isaac.core_nodes.IsaacGetViewportRenderProduct"), 248 | ("IsaacSetCameraOnRenderProduct", "omni.isaac.core_nodes.IsaacSetCameraOnRenderProduct"), 249 | ("ROS2CameraHelper", "omni.isaac.ros2_bridge.ROS2CameraHelper"), 250 | ], 251 | og.Controller.Keys.CONNECT: [ 252 | ("OnImpulseEvent.outputs:execOut", "PublishJointState.inputs:execIn"), 253 | # ("OnImpulseEvent.outputs:execOut", "SubscribeJointState.inputs:execIn"), 254 | ("OnImpulseEvent.outputs:execOut", "PublishClock.inputs:execIn"), 255 | ( 256 | "OnImpulseEvent.outputs:execOut", 257 | "ArticulationController.inputs:execIn", 258 | ), 259 | ("Context.outputs:context", "PublishJointState.inputs:context"), 260 | # ("Context.outputs:context", "SubscribeJointState.inputs:context"), 261 | ("Context.outputs:context", "PublishClock.inputs:context"), 262 | ( 263 | "ReadSimTime.outputs:simulationTime", 264 | "PublishJointState.inputs:timeStamp", 265 | ), 266 | ("ReadSimTime.outputs:simulationTime", "PublishClock.inputs:timeStamp"), 267 | # ( 268 | # "SubscribeJointState.outputs:jointNames", 269 | # "ArticulationController.inputs:jointNames", 270 | # ), 271 | # ( 272 | # "SubscribeJointState.outputs:positionCommand", 273 | # "ArticulationController.inputs:positionCommand", 274 | # ), 275 | # ( 276 | # "SubscribeJointState.outputs:velocityCommand", 277 | # "ArticulationController.inputs:velocityCommand", 278 | # ), 279 | # ( 280 | # "SubscribeJointState.outputs:effortCommand", 281 | # "ArticulationController.inputs:effortCommand", 282 | # ), 283 | 284 | ###### camera connects 285 | ("OnImpulseEvent.outputs:execOut", "IsaacCreateViewport.inputs:execIn"), 286 | ("IsaacCreateViewport.outputs:execOut", "IsaacGetViewportRenderProduct.inputs:execIn"), 287 | ("IsaacCreateViewport.outputs:viewport", "IsaacGetViewportRenderProduct.inputs:viewport"), 288 | 289 | ("IsaacGetViewportRenderProduct.outputs:execOut", "IsaacSetCameraOnRenderProduct.inputs:execIn"), 290 | ("IsaacGetViewportRenderProduct.outputs:renderProductPath", "IsaacSetCameraOnRenderProduct.inputs:renderProductPath"), 291 | ("IsaacGetViewportRenderProduct.outputs:renderProductPath", "ROS2CameraHelper.inputs:renderProductPath"), 292 | 293 | ("Context.outputs:context", "ROS2CameraHelper.inputs:context"), 294 | ("IsaacSetCameraOnRenderProduct.outputs:execOut", "ROS2CameraHelper.inputs:execIn"), 295 | ], 296 | og.Controller.Keys.SET_VALUES: [ 297 | ("Context.inputs:useDomainIDEnvVar", 1), 298 | # Setting the /UR target prim to Articulation Controller node 299 | # ("ArticulationController.inputs:usePath", True), 300 | ("ArticulationController.inputs:robotPath", UR_STAGE_PATH), 301 | ("PublishJointState.inputs:targetPrim", UR_STAGE_PATH), 302 | ("PublishJointState.inputs:topicName", "isaac_joint_states"), 303 | # ("SubscribeJointState.inputs:topicName", "isaac_joint_commands"), 304 | 305 | 306 | ######### camera variables 307 | ("IsaacCreateViewport.inputs:name", "Camera_view_port"), 308 | ("IsaacSetCameraOnRenderProduct.inputs:cameraPrim",Camera_STAGE_PATH ), 309 | # # Ros2 settings 310 | ("ROS2CameraHelper.inputs:frameId", "World"), 311 | ("ROS2CameraHelper.inputs:topicName", "Camera_image"), 312 | ], 313 | },) 314 | except Exception as e: 315 | print(e) 316 | 317 | 318 | while simulation_app.is_running(): 319 | 320 | og.Controller.set( 321 | og.Controller.attribute("/ActionGraph/OnImpulseEvent.state:enableImpulse"), True 322 | ) 323 | my_world.step(render=True) 324 | if not my_world.is_playing(): 325 | if i % 100 == 0: 326 | print("**** Click Play to start simulation *****") 327 | i += 1 328 | # if step_index == 0: 329 | # my_world.play() 330 | continue 331 | 332 | step_index = my_world.current_time_step_index 333 | # print(step_index) 334 | if step_index <= 2: 335 | my_world.reset() 336 | idx_list = [robot.get_dof_index(x) for x in j_names] 337 | robot.set_joint_positions(default_config, idx_list) 338 | 339 | robot._articulation_view.set_max_efforts( 340 | values=np.array([5000 for i in range(len(idx_list))]), joint_indices=idx_list 341 | ) 342 | if step_index < 20: 343 | continue 344 | 345 | if step_index == 50 or step_index % 1000 == 0.0: 346 | print("Updating world, reading w.r.t.", robot_prim_path) 347 | obstacles = usd_help.get_obstacles_from_stage( 348 | # only_paths=[obstacles_path], 349 | reference_prim_path=robot_prim_path, 350 | ignore_substring=[ 351 | robot_prim_path, 352 | "/World/target", 353 | "/World/defaultGroundPlane", 354 | "/curobo", 355 | ] 356 | + names, 357 | ).get_collision_check_world() 358 | 359 | motion_gen.update_world(obstacles) 360 | print("Updated World") 361 | carb.log_info("Synced CuRobo world from stage.") 362 | 363 | # position and orientation of target virtual cube: 364 | cube_position, cube_orientation = target.get_world_pose() 365 | 366 | if past_pose is None: 367 | past_pose = cube_position 368 | if target_pose is None: 369 | target_pose = cube_position 370 | sim_js = robot.get_joints_state() 371 | sim_js_names = robot.dof_names 372 | cu_js = JointState( 373 | position=tensor_args.to_device(sim_js.positions), 374 | velocity=tensor_args.to_device(sim_js.velocities) * 0.0, 375 | acceleration=tensor_args.to_device(sim_js.velocities) * 0.0, 376 | jerk=tensor_args.to_device(sim_js.velocities) * 0.0, 377 | joint_names=sim_js_names, 378 | ) 379 | cu_js = cu_js.get_ordered_joint_state(motion_gen.kinematics.joint_names) 380 | 381 | if args.visualize_spheres and step_index % 2 == 0: 382 | sph_list = motion_gen.kinematics.get_robot_as_spheres(cu_js.position) 383 | 384 | if spheres is None: 385 | spheres = [] 386 | # create spheres: 387 | 388 | for si, s in enumerate(sph_list[0]): 389 | sp = sphere.VisualSphere( 390 | prim_path="/curobo/robot_sphere_" + str(si), 391 | position=np.ravel(s.position), 392 | radius=float(s.radius), 393 | color=np.array([0, 0.8, 0.2]), 394 | ) 395 | spheres.append(sp) 396 | else: 397 | for si, s in enumerate(sph_list[0]): 398 | spheres[si].set_world_pose(position=np.ravel(s.position)) 399 | spheres[si].set_radius(float(s.radius)) 400 | # print(sim_js.velocities) 401 | if ( 402 | np.linalg.norm(cube_position - target_pose) > 1e-3 403 | and np.linalg.norm(past_pose - cube_position) == 0.0 404 | and np.linalg.norm(sim_js.velocities) < 0.2 405 | ): 406 | # Set EE teleop goals, use cube for simple non-vr init: 407 | ee_translation_goal = cube_position 408 | ee_orientation_teleop_goal = cube_orientation 409 | 410 | # compute curobo solution: 411 | ik_goal = Pose( 412 | position=tensor_args.to_device(ee_translation_goal), 413 | quaternion=tensor_args.to_device(ee_orientation_teleop_goal), 414 | ) 415 | # add link poses: 416 | link_poses = {} 417 | for i in target_links.keys(): 418 | c_p, c_rot = target_links[i].get_world_pose() 419 | link_poses[i] = Pose( 420 | position=tensor_args.to_device(c_p), 421 | quaternion=tensor_args.to_device(c_rot), 422 | ) 423 | result = motion_gen.plan_single( 424 | cu_js.unsqueeze(0), ik_goal, plan_config.clone(), link_poses=link_poses 425 | ) 426 | # ik_result = ik_solver.solve_single(ik_goal, cu_js.position.view(1,-1), cu_js.position.view(1,1,-1)) 427 | 428 | succ = result.success.item() # ik_result.success.item() 429 | if succ: 430 | cmd_plan = result.get_interpolated_plan() 431 | cmd_plan = motion_gen.get_full_js(cmd_plan) 432 | print(cmd_plan) 433 | # get only joint names that are in both: 434 | idx_list = [] 435 | common_js_names = [] 436 | for x in sim_js_names: 437 | if x in cmd_plan.joint_names: 438 | idx_list.append(robot.get_dof_index(x)) 439 | common_js_names.append(x) 440 | # idx_list = [robot.get_dof_index(x) for x in sim_js_names] 441 | 442 | cmd_plan = cmd_plan.get_ordered_joint_state(common_js_names) 443 | 444 | cmd_idx = 0 445 | 446 | else: 447 | carb.log_warn("Plan did not converge to a solution. No action is being taken.") 448 | target_pose = cube_position 449 | past_pose = cube_position 450 | if cmd_plan is not None: 451 | cmd_state = cmd_plan[cmd_idx] 452 | 453 | # get full dof state 454 | art_action = ArticulationAction( 455 | cmd_state.position.cpu().numpy(), 456 | cmd_state.velocity.cpu().numpy(), 457 | joint_indices=idx_list, 458 | ) 459 | # set desired joint angles obtained from IK: 460 | articulation_controller.apply_action(art_action) 461 | cmd_idx += 1 462 | for _ in range(2): 463 | my_world.step(render=False) 464 | if cmd_idx >= len(cmd_plan.position): 465 | cmd_idx = 0 466 | cmd_plan = None 467 | simulation_app.close() 468 | 469 | 470 | if __name__ == "__main__": 471 | main() 472 | -------------------------------------------------------------------------------- /Isaac_launch/record_configuration.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 36 | 37 | 38 | 39 | 40 | 412 | 413 | 414 | -------------------------------------------------------------------------------- /Isaac_launch/simple_stacking_ur.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 5 | # property and proprietary rights in and to this material, related 6 | # documentation and any modifications thereto. Any use, reproduction, 7 | # disclosure or distribution of this material and related documentation 8 | # without an express license agreement from NVIDIA CORPORATION or 9 | # its affiliates is strictly prohibited. 10 | # 11 | 12 | # Third Party 13 | import torch 14 | 15 | a = torch.zeros( 16 | 4, device="cuda:0" 17 | ) # this is necessary to allow isaac sim to use this torch instance 18 | # Third Party 19 | import numpy as np 20 | 21 | np.set_printoptions(suppress=True) 22 | # Standard Library 23 | 24 | # Standard Library 25 | import argparse 26 | import os 27 | ## import curobo: 28 | 29 | parser = argparse.ArgumentParser() 30 | current_dir = os.path.dirname(os.path.realpath(__file__)) 31 | default_path = os.path.abspath(os.path.join(current_dir, '../../src/curobo/content')) 32 | parser.add_argument( 33 | "--headless_mode", 34 | type=str, 35 | default=None, 36 | help="To run headless, use one of [native, websocket], webrtc might not work.", 37 | ) 38 | 39 | parser.add_argument( 40 | "--constrain_grasp_approach", 41 | action="store_true", 42 | help="When True, approaches grasp with fixed orientation and motion only along z axis.", 43 | default=True, 44 | ) 45 | 46 | parser.add_argument( 47 | "--external_asset_path", 48 | type=str, 49 | default=default_path+"/assets", 50 | help="Path to external assets when loading an externally located robot", 51 | ) 52 | parser.add_argument( 53 | "--external_robot_configs_path", 54 | type=str, 55 | default=default_path+"/configs/robot", 56 | help="Path to external robot config when loading an external robot", 57 | ) 58 | 59 | args = parser.parse_args() 60 | 61 | 62 | 63 | # Third Party 64 | from omni.isaac.kit import SimulationApp 65 | 66 | simulation_app = SimulationApp( 67 | { 68 | "headless": args.headless_mode is not None, 69 | "width": "1920", 70 | "height": "1080", 71 | } 72 | ) 73 | # Standard Library 74 | from typing import Optional 75 | import omni.graph.core as og # noqa E402 76 | 77 | # Third Party 78 | import carb 79 | from helper import add_extensions 80 | from omni.isaac.core import World 81 | from omni.isaac.core.controllers import BaseController 82 | from omni.isaac.core.tasks import Stacking as BaseStacking 83 | from omni.isaac.core.utils.prims import is_prim_path_valid 84 | from omni.isaac.core.utils.stage import get_stage_units 85 | from omni.isaac.core.utils.string import find_unique_string_name 86 | from omni.isaac.core.utils.types import ArticulationAction 87 | from omni.isaac.core.utils.viewports import set_camera_view 88 | from ur3e.ur3e import ur3e as ur3e 89 | # CuRobo 90 | from curobo.geom.sdf.world import CollisionCheckerType 91 | from curobo.geom.sphere_fit import SphereFitType 92 | from curobo.geom.types import WorldConfig 93 | from curobo.rollout.rollout_base import Goal 94 | from curobo.types.base import TensorDeviceType 95 | from curobo.types.math import Pose 96 | from curobo.types.robot import JointState 97 | from curobo.types.state import JointState 98 | from curobo.util.usd_helper import UsdHelper 99 | from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml 100 | from curobo.wrap.reacher.motion_gen import ( 101 | MotionGen, 102 | MotionGenConfig, 103 | MotionGenPlanConfig, 104 | MotionGenResult, 105 | PoseCostMetric, 106 | ) 107 | from omni.isaac.core.utils.extensions import enable_extension 108 | 109 | class CuroboController(BaseController): 110 | def __init__( 111 | self, 112 | my_world: World, 113 | my_task: BaseStacking, 114 | name: str = "curobo_controller", 115 | constrain_grasp_approach: bool = False, 116 | ) -> None: 117 | BaseController.__init__(self, name=name) 118 | self._save_log = False 119 | self.my_world = my_world 120 | self.my_task = my_task 121 | self._step_idx = 0 122 | n_obstacle_cuboids = 20 123 | n_obstacle_mesh = 2 124 | # warmup curobo instance 125 | self.usd_help = UsdHelper() 126 | self.init_curobo = False 127 | self.world_file = "collision_table.yml" 128 | self.cmd_js_names = [ 129 | "shoulder_pan_joint", 130 | "shoulder_lift_joint", 131 | "elbow_joint", 132 | "wrist_1_joint", 133 | "wrist_2_joint", 134 | "wrist_3_joint", 135 | ] 136 | self.tensor_args = TensorDeviceType() 137 | robot_cfg_path = args.external_robot_configs_path 138 | 139 | self.robot_cfg = load_yaml(join_path(robot_cfg_path, "ur3e_gripper.yml"))["robot_cfg"] 140 | self.robot_cfg["kinematics"]["external_asset_path"] = args.external_asset_path 141 | 142 | self.robot_cfg["kinematics"][ 143 | "base_link" 144 | ] = "base_link" # controls which frame the controller is controlling 145 | 146 | self.robot_cfg["kinematics"][ 147 | "ee_link" 148 | ] = "single_base" # controls which frame the controller is controlling 149 | # self.robot_cfg["kinematics"]["cspace"]["max_acceleration"] = 10.0 # controls how fast robot moves 150 | self.robot_cfg["kinematics"]["extra_collision_spheres"] = {"attached_object": 100} 151 | # @self.robot_cfg["kinematics"]["collision_sphere_buffer"] = 0.0 152 | self.robot_cfg["kinematics"]["collision_spheres"] = os.path.join(robot_cfg_path, "spheres/ur3e.yml") 153 | 154 | # self.robot_cfg["kinematics"]["collision_spheres"] = "spheres/ur3e.yml" 155 | 156 | world_cfg_table = WorldConfig.from_dict( 157 | load_yaml(join_path(get_world_configs_path(), "collision_table.yml")) 158 | ) 159 | self._world_cfg_table = world_cfg_table 160 | 161 | world_cfg1 = WorldConfig.from_dict( 162 | load_yaml(join_path(get_world_configs_path(), "collision_table.yml")) 163 | ).get_mesh_world() 164 | world_cfg1.mesh[0].pose[2] = -10.5 165 | 166 | self._world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh) 167 | 168 | motion_gen_config = MotionGenConfig.load_from_robot_config( 169 | self.robot_cfg, 170 | self._world_cfg, 171 | self.tensor_args, 172 | trajopt_tsteps=32, 173 | collision_checker_type=CollisionCheckerType.MESH, 174 | use_cuda_graph=True, 175 | interpolation_dt=0.01, 176 | collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh}, 177 | store_ik_debug=self._save_log, 178 | store_trajopt_debug=self._save_log, 179 | velocity_scale=0.75, 180 | ) 181 | self.motion_gen = MotionGen(motion_gen_config) 182 | print("warming up...") 183 | self.motion_gen.warmup(parallel_finetune=True) 184 | pose_metric = None 185 | if constrain_grasp_approach: 186 | pose_metric = PoseCostMetric.create_grasp_approach_metric( 187 | offset_position=0.1, tstep_fraction=0.8 188 | ) 189 | 190 | self.plan_config = MotionGenPlanConfig( 191 | enable_graph=False, 192 | max_attempts=10, 193 | enable_graph_attempt=None, 194 | enable_finetune_trajopt=True, 195 | partial_ik_opt=False, 196 | parallel_finetune=True, 197 | pose_cost_metric=pose_metric, 198 | ) 199 | self.usd_help.load_stage(self.my_world.stage) 200 | self.cmd_plan = None 201 | self.cmd_idx = 0 202 | self._step_idx = 0 203 | self.idx_list = None 204 | 205 | def attach_obj( 206 | self, 207 | sim_js: JointState, 208 | js_names: list, 209 | ) -> None: 210 | cube_name = self.my_task.get_cube_prim(self.my_task.target_cube) 211 | 212 | cu_js = JointState( 213 | position=self.tensor_args.to_device(sim_js.positions), 214 | velocity=self.tensor_args.to_device(sim_js.velocities) * 0.0, 215 | acceleration=self.tensor_args.to_device(sim_js.velocities) * 0.0, 216 | jerk=self.tensor_args.to_device(sim_js.velocities) * 0.0, 217 | joint_names=js_names, 218 | ) 219 | 220 | self.motion_gen.attach_objects_to_robot( 221 | cu_js, 222 | [cube_name], 223 | sphere_fit_type=SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE, 224 | world_objects_pose_offset=Pose.from_list([0, 0, 0.01, 1, 0, 0, 0], self.tensor_args), 225 | ) 226 | 227 | def detach_obj(self) -> None: 228 | self.motion_gen.detach_object_from_robot() 229 | 230 | def plan( 231 | self, 232 | ee_translation_goal: np.array, 233 | ee_orientation_goal: np.array, 234 | sim_js: JointState, 235 | js_names: list, 236 | ) -> MotionGenResult: 237 | ik_goal = Pose( 238 | position=self.tensor_args.to_device(ee_translation_goal), 239 | quaternion=self.tensor_args.to_device(ee_orientation_goal), 240 | ) 241 | cu_js = JointState( 242 | position=self.tensor_args.to_device(sim_js.positions), 243 | velocity=self.tensor_args.to_device(sim_js.velocities) * 0.0, 244 | acceleration=self.tensor_args.to_device(sim_js.velocities) * 0.0, 245 | jerk=self.tensor_args.to_device(sim_js.velocities) * 0.0, 246 | joint_names=js_names, 247 | ) 248 | cu_js = cu_js.get_ordered_joint_state(self.motion_gen.kinematics.joint_names) 249 | result = self.motion_gen.plan_single(cu_js.unsqueeze(0), ik_goal, self.plan_config.clone()) 250 | if self._save_log: # and not result.success.item(): # logging for debugging 251 | UsdHelper.write_motion_gen_log( 252 | result, 253 | {"robot_cfg": self.robot_cfg}, 254 | self._world_cfg, 255 | cu_js, 256 | ik_goal, 257 | join_path("log/usd/", "cube") + "_debug", 258 | write_ik=False, 259 | write_trajopt=True, 260 | visualize_robot_spheres=True, 261 | link_spheres=self.motion_gen.kinematics.kinematics_config.link_spheres, 262 | grid_space=2, 263 | write_robot_usd_path="log/usd/assets", 264 | ) 265 | return result 266 | 267 | def forward( 268 | self, 269 | sim_js: JointState, 270 | js_names: list, 271 | ) -> ArticulationAction: 272 | assert self.my_task.target_position is not None 273 | assert self.my_task.target_cube is not None 274 | 275 | if self.cmd_plan is None: 276 | self.cmd_idx = 0 277 | self._step_idx = 0 278 | # Set EE goals 279 | ee_translation_goal = self.my_task.target_position 280 | ee_orientation_goal = np.array([0, 0, -1, 0]) 281 | # compute curobo solution: 282 | result = self.plan(ee_translation_goal, ee_orientation_goal, sim_js, js_names) 283 | succ = result.success.item() 284 | if succ: 285 | cmd_plan = result.get_interpolated_plan() 286 | self.idx_list = [i for i in range(len(self.cmd_js_names))] 287 | self.cmd_plan = cmd_plan.get_ordered_joint_state(self.cmd_js_names) 288 | else: 289 | carb.log_warn("Plan did not converge to a solution.") 290 | return None 291 | if self._step_idx % 3 == 0: 292 | cmd_state = self.cmd_plan[self.cmd_idx] 293 | self.cmd_idx += 1 294 | 295 | # get full dof state 296 | art_action = ArticulationAction( 297 | cmd_state.position.cpu().numpy(), 298 | cmd_state.velocity.cpu().numpy() * 0.0, 299 | joint_indices=self.idx_list, 300 | ) 301 | if self.cmd_idx >= len(self.cmd_plan.position): 302 | self.cmd_idx = 0 303 | self.cmd_plan = None 304 | else: 305 | art_action = None 306 | self._step_idx += 1 307 | return art_action 308 | 309 | def reached_target(self, observations: dict) -> bool: 310 | curr_ee_position = observations["ur3e"]["end_effector_position"] 311 | if np.linalg.norm( 312 | self.my_task.target_position - curr_ee_position 313 | ) < 0.07 and ( # This is half gripper width, curobo succ threshold is 0.5 cm 314 | self.cmd_plan is None 315 | ): 316 | if self.my_task.cube_in_hand is None: 317 | print("reached picking target: ", self.my_task.target_cube) 318 | else: 319 | print("reached placing target: ", self.my_task.target_cube) 320 | return True 321 | else: 322 | return False 323 | 324 | def reset( 325 | self, 326 | ignore_substring: str, 327 | robot_prim_path: str, 328 | ) -> None: 329 | # init 330 | self.update(ignore_substring, robot_prim_path) 331 | self.init_curobo = True 332 | self.cmd_plan = None 333 | self.cmd_idx = 0 334 | 335 | def update( 336 | self, 337 | ignore_substring: str, 338 | robot_prim_path: str, 339 | ) -> None: 340 | # print("updating world...") 341 | obstacles = self.usd_help.get_obstacles_from_stage( 342 | ignore_substring=ignore_substring, reference_prim_path=robot_prim_path 343 | ).get_collision_check_world() 344 | # add ground plane as it's not readable: 345 | obstacles.add_obstacle(self._world_cfg_table.cuboid[0]) 346 | self.motion_gen.update_world(obstacles) 347 | self._world_cfg = obstacles 348 | 349 | 350 | class MultiModalStacking(BaseStacking): 351 | def __init__( 352 | self, 353 | name: str = "multi_modal_stacking", 354 | offset: Optional[np.ndarray] = None, 355 | ) -> None: 356 | BaseStacking.__init__( 357 | self, 358 | name=name, 359 | cube_initial_positions=np.array( 360 | [ 361 | [0.20, 0.0, 0.1], 362 | [0.20, -0.10, 0.1], 363 | [0.20, 0.10, 0.1], 364 | ] 365 | ) 366 | / get_stage_units(), 367 | cube_initial_orientations=None, 368 | stack_target_position=None, 369 | cube_size=np.array([0.05, 0.02, 0.05]), 370 | offset=offset, 371 | ) 372 | self.cube_list = None 373 | self.target_position = None 374 | self.target_cube = None 375 | self.cube_in_hand = None 376 | 377 | def reset(self) -> None: 378 | self.cube_list = self.get_cube_names() 379 | self.target_position = None 380 | self.target_cube = None 381 | self.cube_in_hand = None 382 | 383 | def update_task(self) -> bool: 384 | # after detaching the cube in hand 385 | assert self.target_cube is not None 386 | assert self.cube_in_hand is not None 387 | self.cube_list.insert(0, self.cube_in_hand) 388 | self.target_cube = None 389 | self.target_position = None 390 | self.cube_in_hand = None 391 | if len(self.cube_list) <= 1: 392 | task_finished = True 393 | else: 394 | task_finished = False 395 | return task_finished 396 | 397 | def get_cube_prim(self, cube_name: str): 398 | for i in range(self._num_of_cubes): 399 | if cube_name == self._cubes[i].name: 400 | return self._cubes[i].prim_path 401 | 402 | def get_place_position(self, observations: dict) -> None: 403 | assert self.target_cube is not None 404 | self.cube_in_hand = self.target_cube 405 | self.target_cube = self.cube_list[0] 406 | ee_to_grasped_cube = ( 407 | observations["ur3e"]["end_effector_position"][2] 408 | - observations[self.cube_in_hand]["position"][2] 409 | ) 410 | self.target_position = observations[self.target_cube]["position"] + [ 411 | 0, 412 | 0, 413 | self._cube_size[2] + ee_to_grasped_cube + 0.01, 414 | ] 415 | self.cube_list.remove(self.target_cube) 416 | 417 | def get_pick_position(self, observations: dict) -> None: 418 | assert self.cube_in_hand is None 419 | self.target_cube = self.cube_list[1] 420 | self.target_position = observations[self.target_cube]["position"] + [ 421 | 0, 422 | 0, 423 | self._cube_size[2] / 2 + 0.085, 424 | ] 425 | self.cube_list.remove(self.target_cube) 426 | print(self.target_position) 427 | def set_robot(self) -> ur3e: 428 | ur3e_prim_path = find_unique_string_name( 429 | initial_name="/World/ur3e", is_unique_fn=lambda x: not is_prim_path_valid(x) 430 | ) 431 | ur3e_robot_name = find_unique_string_name( 432 | initial_name="ur3e", is_unique_fn=lambda x: not self.scene.object_exists(x) 433 | ) 434 | return ur3e( 435 | prim_path=ur3e_prim_path, name=ur3e_robot_name, end_effector_prim_name="single_base" 436 | ) 437 | 438 | 439 | robot_prim_path = "/World/ur3e/base_link" 440 | ignore_substring = ["ur3e", "TargetCube", "material", "Plane"] 441 | my_world = World(stage_units_in_meters=1.0) 442 | stage = my_world.stage 443 | stage.SetDefaultPrim(stage.GetPrimAtPath("/World")) 444 | 445 | my_task = MultiModalStacking() 446 | my_world.add_task(my_task) 447 | my_world.reset() 448 | robot_name = my_task.get_params()["robot_name"]["value"] 449 | my_ur3e = my_world.scene.get_object(robot_name) 450 | my_controller = CuroboController( 451 | my_world=my_world, my_task=my_task, constrain_grasp_approach=args.constrain_grasp_approach 452 | ) 453 | articulation_controller = my_ur3e.get_articulation_controller() 454 | set_camera_view(eye=[2, 0, 1], target=[0.00, 0.00, 0.00], camera_prim_path="/OmniverseKit_Persp") 455 | wait_steps = 8 456 | import omni 457 | from pxr import UsdGeom 458 | 459 | stage=omni.usd.get_context().get_stage() 460 | 461 | path="/World/Camera/MyCamera" 462 | prim_type="Camera" 463 | translation=(0,0,5) 464 | rotation=(0,0,-90) 465 | camera_prim = stage.DefinePrim(path, prim_type) 466 | 467 | xform_api=UsdGeom.XformCommonAPI(camera_prim) 468 | xform_api.SetTranslate(translation) 469 | xform_api.SetRotate(rotation) 470 | my_ur3e.set_solver_velocity_iteration_count(4) 471 | my_ur3e.set_solver_position_iteration_count(124) 472 | my_world._physics_context.set_solver_type("TGS") 473 | my_world._physics_context.set_broadphase_type("GPU") 474 | 475 | initial_steps = 100 476 | ################################################################ 477 | print("Start simulation...") 478 | robot = my_ur3e 479 | print( 480 | my_world._physics_context.get_solver_type(), 481 | robot.get_solver_position_iteration_count(), 482 | robot.get_solver_velocity_iteration_count(), 483 | ) 484 | print(my_world._physics_context.use_gpu_pipeline) 485 | print(articulation_controller.get_gains()) 486 | print(articulation_controller.get_max_efforts()) 487 | robot = my_ur3e 488 | print("**********************") 489 | if True: 490 | robot.enable_gravity() 491 | # articulation_controller.set_gains( 492 | # kps=np.array( 493 | # [12000, 12000.001, 12000.001, 12000.001, 12000.001, 12000.001, 6000.0, 6000.0] 494 | # ) 495 | # ) 496 | 497 | # articulation_controller.set_max_efforts( 498 | # values=np.array([3360, 3360, 1680, 720, 720, 720, 50, 50]) 499 | # ) 500 | 501 | print("Updated gains:") 502 | print(articulation_controller.get_gains()) 503 | print(articulation_controller.get_max_efforts()) 504 | # exit() 505 | my_ur3e.gripper.open() 506 | for _ in range(wait_steps): 507 | my_world.step(render=True) 508 | my_task.reset() 509 | task_finished = False 510 | observations = my_world.get_observations() 511 | my_task.get_pick_position(observations) 512 | 513 | i = 0 514 | 515 | add_extensions(simulation_app, args.headless_mode) 516 | # Creating a action graph with ROS component nodes 517 | UR_STAGE_PATH="/World/ur3e" 518 | Camera_STAGE_PATH="/World/Camera/MyCamera" 519 | simulation_app.update() 520 | enable_extension("omni.isaac.ros2_bridge") 521 | 522 | try: 523 | og.Controller.edit( 524 | {"graph_path": "/ActionGraph", "evaluator_name": "execution"}, 525 | { 526 | og.Controller.Keys.CREATE_NODES: [ 527 | ("OnImpulseEvent", "omni.graph.action.OnImpulseEvent"), 528 | ("ReadSimTime", "omni.isaac.core_nodes.IsaacReadSimulationTime"), 529 | ("Context", "omni.isaac.ros2_bridge.ROS2Context"), 530 | ("PublishJointState", "omni.isaac.ros2_bridge.ROS2PublishJointState"), 531 | ( 532 | "SubscribeJointState", 533 | "omni.isaac.ros2_bridge.ROS2SubscribeJointState", 534 | ), 535 | ( 536 | "ArticulationController", 537 | "omni.isaac.core_nodes.IsaacArticulationController", 538 | ), 539 | ("PublishClock", "omni.isaac.ros2_bridge.ROS2PublishClock"), 540 | 541 | ######camera graph 542 | ("IsaacCreateViewport", "omni.isaac.core_nodes.IsaacCreateViewport"), 543 | ("IsaacGetViewportRenderProduct", "omni.isaac.core_nodes.IsaacGetViewportRenderProduct"), 544 | ("IsaacSetCameraOnRenderProduct", "omni.isaac.core_nodes.IsaacSetCameraOnRenderProduct"), 545 | ("ROS2CameraHelper", "omni.isaac.ros2_bridge.ROS2CameraHelper"), 546 | ], 547 | og.Controller.Keys.CONNECT: [ 548 | ("OnImpulseEvent.outputs:execOut", "PublishJointState.inputs:execIn"), 549 | ("OnImpulseEvent.outputs:execOut", "SubscribeJointState.inputs:execIn"), 550 | ("OnImpulseEvent.outputs:execOut", "PublishClock.inputs:execIn"), 551 | ( 552 | "OnImpulseEvent.outputs:execOut", 553 | "ArticulationController.inputs:execIn", 554 | ), 555 | ("Context.outputs:context", "PublishJointState.inputs:context"), 556 | ("Context.outputs:context", "SubscribeJointState.inputs:context"), 557 | ("Context.outputs:context", "PublishClock.inputs:context"), 558 | ( 559 | "ReadSimTime.outputs:simulationTime", 560 | "PublishJointState.inputs:timeStamp", 561 | ), 562 | ("ReadSimTime.outputs:simulationTime", "PublishClock.inputs:timeStamp"), 563 | ( 564 | "SubscribeJointState.outputs:jointNames", 565 | "ArticulationController.inputs:jointNames", 566 | ), 567 | ( 568 | "SubscribeJointState.outputs:positionCommand", 569 | "ArticulationController.inputs:positionCommand", 570 | ), 571 | ( 572 | "SubscribeJointState.outputs:velocityCommand", 573 | "ArticulationController.inputs:velocityCommand", 574 | ), 575 | ( 576 | "SubscribeJointState.outputs:effortCommand", 577 | "ArticulationController.inputs:effortCommand", 578 | ), 579 | 580 | ###### camera connects 581 | ("OnImpulseEvent.outputs:execOut", "IsaacCreateViewport.inputs:execIn"), 582 | ("IsaacCreateViewport.outputs:execOut", "IsaacGetViewportRenderProduct.inputs:execIn"), 583 | ("IsaacCreateViewport.outputs:viewport", "IsaacGetViewportRenderProduct.inputs:viewport"), 584 | 585 | ("IsaacGetViewportRenderProduct.outputs:execOut", "IsaacSetCameraOnRenderProduct.inputs:execIn"), 586 | ("IsaacGetViewportRenderProduct.outputs:renderProductPath", "IsaacSetCameraOnRenderProduct.inputs:renderProductPath"), 587 | ("IsaacGetViewportRenderProduct.outputs:renderProductPath", "ROS2CameraHelper.inputs:renderProductPath"), 588 | 589 | ("Context.outputs:context", "ROS2CameraHelper.inputs:context"), 590 | ("IsaacSetCameraOnRenderProduct.outputs:execOut", "ROS2CameraHelper.inputs:execIn"), 591 | ], 592 | og.Controller.Keys.SET_VALUES: [ 593 | ("Context.inputs:useDomainIDEnvVar", 1), 594 | # Setting the /UR target prim to Articulation Controller node 595 | ("ArticulationController.inputs:usePath", True), 596 | ("ArticulationController.inputs:robotPath", UR_STAGE_PATH), 597 | ("PublishJointState.inputs:topicName", "isaac_joint_states"), 598 | ("SubscribeJointState.inputs:topicName", "isaac_joint_commands"), 599 | 600 | 601 | ######### camera variables 602 | ("IsaacCreateViewport.inputs:name", "Camera_view_port"), 603 | ("IsaacSetCameraOnRenderProduct.inputs:cameraPrim",Camera_STAGE_PATH ), 604 | # # Ros2 settings 605 | ("ROS2CameraHelper.inputs:frameId", "World"), 606 | ("ROS2CameraHelper.inputs:topicName", "Camera_image"), 607 | ], 608 | },) 609 | except Exception as e: 610 | print(e) 611 | from omni.isaac.core_nodes.scripts.utils import set_target_prims # noqa E402 612 | 613 | # Setting the /UR target prim to Publish JointState node 614 | set_target_prims( 615 | primPath="/ActionGraph/PublishJointState", targetPrimPaths=[UR_STAGE_PATH] 616 | ) 617 | 618 | 619 | simulation_app.update() 620 | while simulation_app.is_running(): 621 | 622 | og.Controller.set( 623 | og.Controller.attribute("/ActionGraph/OnImpulseEvent.state:enableImpulse"), True 624 | ) 625 | my_world.step(render=True) # necessary to visualize changes 626 | i += 1 627 | 628 | if task_finished or i < initial_steps: 629 | continue 630 | 631 | if not my_controller.init_curobo: 632 | my_controller.reset(ignore_substring, robot_prim_path) 633 | 634 | step_index = my_world.current_time_step_index 635 | observations = my_world.get_observations() 636 | sim_js = my_ur3e.get_joints_state() 637 | 638 | if my_controller.reached_target(observations): 639 | print(my_ur3e.gripper.get_joint_positions()[0]) 640 | if my_ur3e.gripper.get_joint_positions()[0] < -0.005: # reached placing target 641 | print('here') 642 | my_ur3e.gripper.open() 643 | for _ in range(wait_steps): 644 | my_world.step(render=True) 645 | my_controller.detach_obj() 646 | my_controller.update( 647 | ignore_substring, robot_prim_path 648 | ) # update world collision configuration 649 | task_finished = my_task.update_task() 650 | if task_finished: 651 | print("\nTASK DONE\n") 652 | for _ in range(wait_steps): 653 | my_world.step(render=True) 654 | continue 655 | else: 656 | my_task.get_pick_position(observations) 657 | 658 | else: # reached picking target 659 | my_ur3e.gripper.close() 660 | for _ in range(wait_steps): 661 | my_world.step(render=True) 662 | sim_js = my_ur3e.get_joints_state() 663 | my_controller.update(ignore_substring, robot_prim_path) 664 | my_controller.attach_obj(sim_js, my_ur3e.dof_names) 665 | my_task.get_place_position(observations) 666 | 667 | else: # target position has been set 668 | sim_js = my_ur3e.get_joints_state() 669 | art_action = my_controller.forward(sim_js, my_ur3e.dof_names) 670 | if art_action is not None: 671 | articulation_controller.apply_action(art_action) 672 | # for _ in range(2): 673 | # my_world.step(render=False) 674 | 675 | simulation_app.close() 676 | 677 | --------------------------------------------------------------------------------