From b4b3600846a7396f8889296ada2ce29361dd4676 Mon Sep 17 00:00:00 2001 From: Jaiveer Singh Date: Tue, 10 Dec 2024 17:43:18 -0800 Subject: [PATCH] Isaac ROS 3.2 --- .gitattributes | 1 - .gitignore | 1 - .gitmodules | 2 +- CHANGELOG | 39 - README.md | 16 +- curobo_core/curobo | 2 +- curobo_core/package.xml | 3 +- curobo_core/setup.py | 33 +- .../isaac_ros_cumotion/__init__.py | 21 + .../cumotion_goal_set_client.py | 316 +++ .../cumotion_goal_set_planner.py | 332 +++ .../isaac_ros_cumotion/cumotion_planner.py | 415 ++- .../isaac_ros_cumotion/robot_segmenter.py | 125 +- .../isaac_ros_cumotion/update_kinematics.py | 271 ++ isaac_ros_cumotion/isaac_ros_cumotion/util.py | 17 +- .../isaac_ros_cumotion/xrdf_utils.py | 158 -- .../launch/isaac_ros_cumotion.launch.py | 2 +- isaac_ros_cumotion/package.xml | 7 +- .../params/isaac_ros_cumotion_params.yaml | 20 +- .../params/robot_segmentation_params.yaml | 10 +- isaac_ros_cumotion/setup.py | 1 + .../start_isaac_sim_franka.py | 1 + .../launch/franka.launch.py | 1 + .../launch/ur_isaac_sim.launch.py | 351 +++ isaac_ros_cumotion_examples/package.xml | 3 +- .../rviz/ur_moveit_config.rviz | 508 ++++ isaac_ros_cumotion_examples/setup.py | 4 + .../ur_config/ros2_controllers.yaml | 44 + .../ur_config/ur.ros2_control.xacro | 102 + .../ur_config/ur.urdf.xacro | 17 + .../ur_config/ur_sim.urdf.xacro | 126 + .../CMakeLists.txt | 32 +- .../action/AttachObject.action | 17 + .../action/MotionPlan.action | 35 + .../action/UpdateLinkSpheres.action | 17 + isaac_ros_cumotion_interfaces/package.xml | 52 + isaac_ros_cumotion_moveit/package.xml | 3 +- .../__init__.py | 16 + .../attach_object_client.py | 192 ++ .../attach_object_server.py | 2406 +++++++++++++++++ .../launch/cumotion_bringup.launch.py | 283 ++ .../launch/object_attachment.launch.py | 66 + .../package.xml | 54 + .../params/object_attachment_params.yaml | 48 + .../isaac_ros_cumotion_object_attachment | 0 .../setup.cfg | 4 + isaac_ros_cumotion_object_attachment/setup.py | 58 + .../test/test_copyright.py | 25 + .../test/test_flake8.py | 25 + .../test/test_pep257.py | 23 + .../__init__.py | 16 + .../isaac_ros_cumotion_python_utils/utils.py | 102 + isaac_ros_cumotion_python_utils/package.xml | 16 + .../resource/isaac_ros_cumotion_python_utils | 0 isaac_ros_cumotion_python_utils/setup.cfg | 4 + isaac_ros_cumotion_python_utils/setup.py | 65 + .../package.xml | 3 +- isaac_ros_cumotion_robot_description/setup.py | 4 + .../urdf/ur10e_robotiq_2f_140.urdf | 705 +++++ .../urdf/ur5e_robotiq_2f_140.urdf | 704 +++++ .../urdf/ur5e_robotiq_2f_85.urdf | 830 ++++++ .../xrdf/ur10e_robotiq_2f_140.xrdf | 328 +++ .../xrdf/ur5e_robotiq_2f_140.xrdf | 270 ++ .../xrdf/ur5e_robotiq_2f_85.xrdf | 246 ++ .../esdf_visualizer.py | 287 +- isaac_ros_esdf_visualizer/package.xml | 4 +- .../CMakeLists.txt | 6 + isaac_ros_goal_setter_interfaces/package.xml | 2 +- .../goal_setter_node.hpp | 66 - .../isaac_ros_moveit_goal_setter/__init__.py | 0 .../goal_initializer.py | 148 + .../move_group_client.py | 120 + .../pose_to_pose.py | 41 +- .../launch/isaac_ros_goal_setter.launch.py | 109 - isaac_ros_moveit_goal_setter/package.xml | 7 +- .../resource/isaac_ros_moveit_goal_setter | 0 isaac_ros_moveit_goal_setter/setup.cfg | 4 + isaac_ros_moveit_goal_setter/setup.py | 47 + .../src/goal_setter_node.cpp | 104 - 79 files changed, 9759 insertions(+), 784 deletions(-) delete mode 100644 CHANGELOG create mode 100644 isaac_ros_cumotion/isaac_ros_cumotion/cumotion_goal_set_client.py create mode 100644 isaac_ros_cumotion/isaac_ros_cumotion/cumotion_goal_set_planner.py create mode 100644 isaac_ros_cumotion/isaac_ros_cumotion/update_kinematics.py delete mode 100644 isaac_ros_cumotion/isaac_ros_cumotion/xrdf_utils.py create mode 100644 isaac_ros_cumotion_examples/launch/ur_isaac_sim.launch.py create mode 100644 isaac_ros_cumotion_examples/rviz/ur_moveit_config.rviz create mode 100644 isaac_ros_cumotion_examples/ur_config/ros2_controllers.yaml create mode 100644 isaac_ros_cumotion_examples/ur_config/ur.ros2_control.xacro create mode 100644 isaac_ros_cumotion_examples/ur_config/ur.urdf.xacro create mode 100644 isaac_ros_cumotion_examples/ur_config/ur_sim.urdf.xacro rename {isaac_ros_moveit_goal_setter => isaac_ros_cumotion_interfaces}/CMakeLists.txt (55%) create mode 100644 isaac_ros_cumotion_interfaces/action/AttachObject.action create mode 100644 isaac_ros_cumotion_interfaces/action/MotionPlan.action create mode 100644 isaac_ros_cumotion_interfaces/action/UpdateLinkSpheres.action create mode 100644 isaac_ros_cumotion_interfaces/package.xml create mode 100644 isaac_ros_cumotion_object_attachment/isaac_ros_cumotion_object_attachment/__init__.py create mode 100644 isaac_ros_cumotion_object_attachment/isaac_ros_cumotion_object_attachment/attach_object_client.py create mode 100644 isaac_ros_cumotion_object_attachment/isaac_ros_cumotion_object_attachment/attach_object_server.py create mode 100644 isaac_ros_cumotion_object_attachment/launch/cumotion_bringup.launch.py create mode 100755 isaac_ros_cumotion_object_attachment/launch/object_attachment.launch.py create mode 100644 isaac_ros_cumotion_object_attachment/package.xml create mode 100755 isaac_ros_cumotion_object_attachment/params/object_attachment_params.yaml create mode 100644 isaac_ros_cumotion_object_attachment/resource/isaac_ros_cumotion_object_attachment create mode 100644 isaac_ros_cumotion_object_attachment/setup.cfg create mode 100644 isaac_ros_cumotion_object_attachment/setup.py create mode 100644 isaac_ros_cumotion_object_attachment/test/test_copyright.py create mode 100644 isaac_ros_cumotion_object_attachment/test/test_flake8.py create mode 100644 isaac_ros_cumotion_object_attachment/test/test_pep257.py create mode 100644 isaac_ros_cumotion_python_utils/isaac_ros_cumotion_python_utils/__init__.py create mode 100644 isaac_ros_cumotion_python_utils/isaac_ros_cumotion_python_utils/utils.py create mode 100644 isaac_ros_cumotion_python_utils/package.xml create mode 100644 isaac_ros_cumotion_python_utils/resource/isaac_ros_cumotion_python_utils create mode 100644 isaac_ros_cumotion_python_utils/setup.cfg create mode 100644 isaac_ros_cumotion_python_utils/setup.py create mode 100644 isaac_ros_cumotion_robot_description/urdf/ur10e_robotiq_2f_140.urdf create mode 100644 isaac_ros_cumotion_robot_description/urdf/ur5e_robotiq_2f_140.urdf create mode 100644 isaac_ros_cumotion_robot_description/urdf/ur5e_robotiq_2f_85.urdf create mode 100644 isaac_ros_cumotion_robot_description/xrdf/ur10e_robotiq_2f_140.xrdf create mode 100644 isaac_ros_cumotion_robot_description/xrdf/ur5e_robotiq_2f_140.xrdf create mode 100644 isaac_ros_cumotion_robot_description/xrdf/ur5e_robotiq_2f_85.xrdf delete mode 100644 isaac_ros_moveit_goal_setter/include/isaac_ros_moveit_goal_setter/goal_setter_node.hpp create mode 100644 isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/__init__.py create mode 100755 isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/goal_initializer.py create mode 100644 isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/move_group_client.py rename isaac_ros_moveit_goal_setter/{scripts => isaac_ros_moveit_goal_setter}/pose_to_pose.py (81%) delete mode 100644 isaac_ros_moveit_goal_setter/launch/isaac_ros_goal_setter.launch.py create mode 100644 isaac_ros_moveit_goal_setter/resource/isaac_ros_moveit_goal_setter create mode 100644 isaac_ros_moveit_goal_setter/setup.cfg create mode 100644 isaac_ros_moveit_goal_setter/setup.py delete mode 100644 isaac_ros_moveit_goal_setter/src/goal_setter_node.cpp diff --git a/.gitattributes b/.gitattributes index df3861e..e3453ff 100644 --- a/.gitattributes +++ b/.gitattributes @@ -25,6 +25,5 @@ **/resources/**/*.yaml filter=lfs diff=lfs merge=lfs -text **/resources/**/*.bag filter=lfs diff=lfs merge=lfs -text -# FIXME: Only for DNN packages # DNN Model files *.onnx filter=lfs diff=lfs merge=lfs -text diff --git a/.gitignore b/.gitignore index cb3a863..8ef8ee6 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,6 @@ # Ignore all pycache files **/__pycache__/** -# FIXME: Only for DNN-based packages # Ignore TensorRT plan files *.plan *.engine diff --git a/.gitmodules b/.gitmodules index 294f962..620199f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +1,4 @@ [submodule "curobo_core/curobo"] path = curobo_core/curobo url = https://github.com/NVlabs/curobo.git - branch = isaac-3.0 + branch = isaac-3.2 diff --git a/CHANGELOG b/CHANGELOG deleted file mode 100644 index bfc2f54..0000000 --- a/CHANGELOG +++ /dev/null @@ -1,39 +0,0 @@ -.. - FIXME: Remove this before publishing! - - HOW TO USE THIS TEMPLATE: - - - Set up your editor alongside a live ReStructuredText rendering tool - - Suggestion: use the VSCode integrations provided as part of the `isaac_ros-dev` dev environment - - Create a copy of this file at the ROOT level of a new Isaac ROS package - - Look for `FIXME` tags throughout the file and resolve accordingly - - Use Semantic Versioning (https://semver.org/) to update version numbers as (Major).(Minor).(Patch) - - Carefully read through the rendered output to ensure all text is displayed appropriately - - Remove this comment before publishing README! - -isaac_ros_[FIXME: package name] -================================= - -(#major).(#minor).(#patch) (YYY2-M2-D2) ---------------------------------------- -* FIXME: One-liner description for change 1 - - FIXME: Optional additional text, empty lines to create paragraphs under the same bullet point - -* (...) - -* FIXME: One-liner description for change N - - FIXME: Optional additional text, empty lines to create paragraphs under the same bullet point - -(#major).(#minor).(#patch) (YYY1-M1-D1) ---------------------------------------- -* FIXME: One-liner description for change 1 - - FIXME: Optional additional text, empty lines to create paragraphs under the same bullet point - -* (...) - -* FIXME: One-liner description for change N - - FIXME: Optional additional text, empty lines to create paragraphs under the same bullet point diff --git a/README.md b/README.md index 805c869..d71415a 100644 --- a/README.md +++ b/README.md @@ -46,6 +46,16 @@ The key advantages of using Isaac ROS cuMotion are: > and be prepared to give necessary warnings, instructions, or take other necessary actions. > 5. Take extra caution when testing or deploying new features or code. +Examples are provided for three modes of operation: + +* **Standalone MoveIt:** MoveIt’s RViz interface allows trajectories to be planned and visualized even without a physical + robot or external simulator. +* **Simulated robot (Isaac Sim):** Trajectories may be planned and executed on a simulated robot in + [Isaac Sim](https://developer.nvidia.com/isaac/sim), allowing convenient development and rapid iteration without + use of physical hardware. Simulated sensors enable testing of perception, e.g., for the purpose of collision avoidance. +* **Physical robot:** For on-robot testing and final deployment, trajectories are planned and executed on a physical + robot. + The Isaac ROS cuMotion repository currently contains the following packages: `isaac_ros_cumotion`: @@ -78,6 +88,10 @@ Please visit the [Isaac ROS Documentation](https://nvidia-isaac-ros.github.io/re * [Quickstart](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_moveit/index.html#quickstart) * [Try More Examples](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_moveit/index.html#try-more-examples) * [Troubleshooting](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_moveit/index.html#troubleshooting) +* [`isaac_ros_cumotion_object_attachment`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_object_attachment/index.html) + * [Object Attachment](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_object_attachment/index.html#object-attachment) + * [Troubleshooting](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_object_attachment/index.html#troubleshooting) + * [API](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_object_attachment/index.html#api) * [`isaac_ros_esdf_visualizer`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_esdf_visualizer/index.html) * [Overview](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_esdf_visualizer/index.html#overview) * [API](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_cumotion/isaac_ros_esdf_visualizer/index.html#api) @@ -88,4 +102,4 @@ Please visit the [Isaac ROS Documentation](https://nvidia-isaac-ros.github.io/re ## Latest -Update 2024-09-26: Updated for Isaac ROS 3.1 +Update 2024-12-10: Added object following and pick-and-place workflows diff --git a/curobo_core/curobo b/curobo_core/curobo index 3bfed9d..36ea382 160000 --- a/curobo_core/curobo +++ b/curobo_core/curobo @@ -1 +1 @@ -Subproject commit 3bfed9d7737fc3865595e4a50b1fe84a32cf627f +Subproject commit 36ea382dabbde2a2431951fc9048f7e5ffd8106b diff --git a/curobo_core/package.xml b/curobo_core/package.xml index 0c4746d..01a6a28 100644 --- a/curobo_core/package.xml +++ b/curobo_core/package.xml @@ -2,10 +2,11 @@ curobo_core - 3.1.0 + 3.2.0 This package wraps the cuRobo library as a ROS 2 package. cuRobo serves as the current backend for cuMotion. Isaac ROS Maintainers NVIDIA Isaac ROS Software License + isaac_ros_common ament_copyright ament_flake8 diff --git a/curobo_core/setup.py b/curobo_core/setup.py index 956c8a4..9360730 100644 --- a/curobo_core/setup.py +++ b/curobo_core/setup.py @@ -15,9 +15,32 @@ # # SPDX-License-Identifier: Apache-2.0 -from setuptools import find_namespace_packages, setup +import importlib.util +from pathlib import Path +import sys from torch.utils.cpp_extension import BuildExtension, CUDAExtension +from ament_index_python.packages import get_resource +from setuptools import find_namespace_packages, setup + +ISAAC_ROS_COMMON_PATH = get_resource( + 'isaac_ros_common_scripts_path', + 'isaac_ros_common' +)[0] + +ISAAC_ROS_COMMON_VERSION_INFO = Path(ISAAC_ROS_COMMON_PATH) / 'isaac_ros_common-version-info.py' + +spec = importlib.util.spec_from_file_location( + 'isaac_ros_common_version_info', + ISAAC_ROS_COMMON_VERSION_INFO +) + +isaac_ros_common_version_info = importlib.util.module_from_spec(spec) +sys.modules['isaac_ros_common_version_info'] = isaac_ros_common_version_info +spec.loader.exec_module(isaac_ros_common_version_info) + +from isaac_ros_common_version_info import GenerateVersionInfoCommand # noqa: E402, I100 + package_name = 'curobo_core' extra_cuda_args = { @@ -91,7 +114,8 @@ zip_safe=True, maintainer='Isaac ROS Maintainers', maintainer_email='isaac-ros-maintainers@nvidia.com', - description='This package wraps the cuRobo library as a ROS 2 package. cuRobo serves as the current backend for cuMotion.', + description='This package wraps the cuRobo library as a ROS 2 package. ' + 'cuRobo serves as the current backend for cuMotion.', license='NVIDIA Isaac ROS Software License', tests_require=['pytest'], entry_points={ @@ -99,7 +123,10 @@ ], }, ext_modules=ext_modules, - cmdclass={'build_ext': BuildExtension}, + cmdclass={ + 'build_ext': BuildExtension, + 'build_py': GenerateVersionInfoCommand, + }, package_data={ 'curobo': ['**/*.*'], }, diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/__init__.py b/isaac_ros_cumotion/isaac_ros_cumotion/__init__.py index e69de29..342f377 100644 --- a/isaac_ros_cumotion/isaac_ros_cumotion/__init__.py +++ b/isaac_ros_cumotion/isaac_ros_cumotion/__init__.py @@ -0,0 +1,21 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 +from .cumotion_goal_set_client import CumotionGoalSetClient + +__all__ = [ + 'CumotionGoalSetClient', +] diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_goal_set_client.py b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_goal_set_client.py new file mode 100644 index 0000000..ef461b8 --- /dev/null +++ b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_goal_set_client.py @@ -0,0 +1,316 @@ +#!/usr/bin/env python3 + +import copy +import time +from typing import List, Optional, Union + +from curobo.types.math import Pose as CuPose +from curobo.types.state import JointState as CuJointState +from curobo.wrap.reacher.motion_gen import MotionGenPlanConfig +from geometry_msgs.msg import Pose +from isaac_ros_cumotion_interfaces.action import MotionPlan +from moveit_msgs.action import ExecuteTrajectory +from moveit_msgs.msg import DisplayTrajectory +import numpy as np +from rclpy.action import ActionClient +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from sensor_msgs.msg import JointState +from std_msgs.msg import Header + + +class CumotionGoalSetClient: + """Client for sending goals to CumotionGoalSetServer.""" + + def __init__(self, node, cgroup=None): + self.node = node + self.cgroup = cgroup + self.node.declare_parameter('joint_states_topic', '/joint_states') + + if cgroup is None: + self.cgroup = MutuallyExclusiveCallbackGroup() + + self.action_client = ActionClient(self.node, MotionPlan, '/cumotion/motion_plan', + callback_group=self.cgroup) + + self.__use_sim_time = ( + self.node.get_parameter('use_sim_time').get_parameter_value().bool_value + ) + + self.__traj_pub = self.node.create_publisher( + DisplayTrajectory, '/cumotion/display_trajectory', 1) + self.__joint_states_topic = ( + self.node.get_parameter('joint_states_topic').get_parameter_value().string_value) + self.subscription = self.node.create_subscription( + JointState, self.__joint_states_topic, self.js_callback, 10, + callback_group=self.cgroup,) + self.__js_buffer = None + self.__robot_executing = False + self.execute_plan_client = ActionClient( + self.node, ExecuteTrajectory, '/execute_trajectory', callback_group=self.cgroup,) + + self.result = None + + def js_callback(self, msg): + self.__js_buffer = { + 'joint_names': msg.name, + 'position': msg.position, + 'velocity': msg.velocity, + } + + def send_plan_goal(self, goal_msg, visualize_trajectory): + self.node.get_logger().info('Sending goal') + self.result = None + + self.action_client.wait_for_server() + self.node.get_logger().info('Found action server') + + self.send_goal_future = self.action_client.send_goal_async(goal_msg) + self.send_goal_future.add_done_callback(self.goal_response_callback) + + while self.result is None: + time.sleep(0.01) + + if visualize_trajectory and self.result.success: + msg = DisplayTrajectory() + msg.trajectory += self.result.planned_trajectory + self.__traj_pub.publish(msg) + + return self.result + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.node.get_logger().info('goal rejected') + return + + self.node.get_logger().info('goal accepted') + + self.get_result_future = goal_handle.get_result_async() + self.get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + self.result = future.result().result + + def move_pose( + self, + goal_pose: CuPose, + link_name: str, + start_state: Optional[CuJointState] = None, + plan_config: Optional[MotionGenPlanConfig] = None, + visualize_trajectory: bool = True, + execute: bool = False, + goal_pose_array=None, + disable_collision_links: List[str] = [], + update_planning_scene: bool = False, + + ): + # generate request: + self.node.get_logger().info('Moving to pose') + goal_msg = MotionPlan.Goal() + if goal_pose_array is None: + position = goal_pose.position.cpu().view(1, goal_pose.n_goalset, 3) + orientation = goal_pose.quaternion.cpu().view(1, goal_pose.n_goalset, 4) + for i in range(goal_pose.n_goalset): + pose = Pose() + pose.position.x = float(position[0, i, 0]) + pose.position.y = float(position[0, i, 1]) + pose.position.z = float(position[0, i, 2]) + pose.orientation.w = float(orientation[0, i, 0]) + pose.orientation.x = float(orientation[0, i, 1]) + pose.orientation.y = float(orientation[0, i, 2]) + pose.orientation.z = float(orientation[0, i, 3]) + + goal_msg.goal_pose.poses.append(pose) + + goal_msg.goal_pose.header.frame_id = link_name + else: + goal_msg.goal_pose = goal_pose_array + goal_msg.plan_pose = True + goal_msg.use_current_state = True + if start_state is not None: + goal_msg.use_current_state = False + goal_msg.start_state.position = start_state.position.cpu().flatten().to_list() + goal_msg.start_state.name = start_state.joint_names + goal_msg.use_planning_scene = update_planning_scene + goal_msg.hold_partial_pose = False + if plan_config is not None: + if plan_config.time_dilation_factor is not None: + goal_msg.time_dilation_factor = plan_config.time_dilation_factor + if plan_config.pose_cost_metric is not None: + if plan_config.pose_cost_metric.hold_partial_pose: + goal_msg.hold_partial_pose = plan_config.pose_cost_metric.hold_partial_pose + goal_msg.hold_partial_pose_vec_weight = ( + plan_config.pose_cost_metric.hold_vec_weight.cpu().flatten().tolist() + ) + + # send goal to client: + goal_msg.disable_collision_links = disable_collision_links + result = self.send_plan_goal(goal_msg, visualize_trajectory) + if execute: + if result.success: + self.execute_plan(result.planned_trajectory[0]) + # return trajectory: + return result + + def move_grasp( + self, + goal_pose: CuPose, + link_name: str, + start_state: Optional[CuJointState] = None, + grasp_approach_offset_distance: List[float] = [0, 0, -0.15], + grasp_approach_path_constraint: Union[None, List[float]] = [0.1, 0.1, 0.1, 0.1, 0.1, 0.0], + retract_offset_distance: List[float] = [0, 0, -0.15], + retract_path_constraint: Union[None, List[float]] = [0.1, 0.1, 0.1, 0.1, 0.1, 0.0], + grasp_approach_constraint_in_goal_frame: bool = True, + retract_constraint_in_goal_frame: bool = True, + time_dilation_factor: float = 0.2, + visualize_trajectory: bool = True, + execute: bool = False, + goal_pose_array=None, + disable_collision_links: List[str] = [], + offset_linear_grasp: bool = True, + update_planning_scene: bool = False, + plan_approach_to_grasp: bool = True, + plan_grasp_to_retract: bool = True, + ): + # generate request: + self.node.get_logger().info('Moving to grasp') + goal_msg = MotionPlan.Goal() + if goal_pose_array is None: + position = goal_pose.position.cpu().view(1, goal_pose.n_goalset, 3) + orientation = goal_pose.quaternion.cpu().view(1, goal_pose.n_goalset, 4) + for i in range(goal_pose.n_goalset): + pose = Pose() + pose.position.x = float(position[0, i, 0]) + pose.position.y = float(position[0, i, 1]) + pose.position.z = float(position[0, i, 2]) + pose.orientation.w = float(orientation[0, i, 0]) + pose.orientation.x = float(orientation[0, i, 1]) + pose.orientation.y = float(orientation[0, i, 2]) + pose.orientation.z = float(orientation[0, i, 3]) + + goal_msg.goal_pose.poses.append(pose) + + goal_msg.goal_pose.header.frame_id = link_name + else: + goal_msg.goal_pose = goal_pose_array + goal_msg.goal_pose.header.frame_id = link_name + goal_msg.plan_pose = True + goal_msg.use_current_state = True + if start_state is not None: + goal_msg.use_current_state = False + goal_msg.start_state.position = start_state.position.cpu().flatten().to_list() + goal_msg.start_state.name = start_state.joint_names + goal_msg.use_planning_scene = update_planning_scene + goal_msg.hold_partial_pose = False + goal_msg.time_dilation_factor = time_dilation_factor + # Fill the six new parameters in here + goal_msg.grasp_partial_pose_vec_weight = grasp_approach_path_constraint + grasp_offset_pose = Pose() + # Keeping the orientation identity + grasp_offset_pose.position.x = grasp_approach_offset_distance[0] + grasp_offset_pose.position.y = grasp_approach_offset_distance[1] + grasp_offset_pose.position.z = grasp_approach_offset_distance[2] + goal_msg.grasp_offset_pose = grasp_offset_pose + goal_msg.grasp_approach_constraint_in_goal_frame = grasp_approach_constraint_in_goal_frame + + goal_msg.retract_partial_pose_vec_weight = retract_path_constraint + retract_offset_pose = Pose() + # Keeping the orientation identity + retract_offset_pose.position.x = retract_offset_distance[0] + retract_offset_pose.position.y = retract_offset_distance[1] + retract_offset_pose.position.z = retract_offset_distance[2] + goal_msg.retract_offset_pose = retract_offset_pose + goal_msg.retract_constraint_in_goal_frame = retract_constraint_in_goal_frame + + goal_msg.disable_collision_links = disable_collision_links + goal_msg.plan_grasp = True + goal_msg.plan_approach_to_grasp = plan_approach_to_grasp + goal_msg.plan_grasp_to_retract = plan_grasp_to_retract + goal_msg.grasp_offset_pose = grasp_offset_pose + result = self.send_plan_goal(goal_msg, visualize_trajectory) + if execute: + if result.success: + self.execute_plan(result.planned_trajectory[0]) + return result + + def move_joint( + self, + goal_state: CuJointState, + start_state: Optional[CuJointState] = None, + plan_config: Optional[MotionGenPlanConfig] = None, + ): + # Reserving for future use + pass + + def execute_plan(self, robot_trajectory, wait_until_complete: bool = True): + # check if robot's current state is within start state of plan: + self.node.get_logger().info('Executing plan') + if self.__robot_executing: + self.node.get_logger().error('Robot is still executing previous command') + return False, None + self.node.get_logger().info('waiting for js') + self.__js_buffer = None + while self.__js_buffer is None: + time.sleep(0.001) + self.node.get_logger().info('received js') + + current_js = copy.deepcopy(self.__js_buffer) + self.__js_buffer = None + start_point = robot_trajectory.joint_trajectory.points[0] + start_point_names = robot_trajectory.joint_trajectory.joint_names + current_names = current_js['joint_names'] + joint_order = [] + for j in start_point_names: + joint_order.append(current_names.index(j)) + + current_position = [current_js['position'][i] for i in joint_order] + current_velocity = [current_js['velocity'][i] for i in joint_order] + + position_error = np.linalg.norm( + np.ravel(current_position) - np.ravel(start_point.positions)) + + velocity_error = np.linalg.norm( + np.ravel(current_velocity) - np.ravel(start_point.velocities)) + + if not self.__use_sim_time: + if position_error > 0.05: + self.node.get_logger().error('Start joint position has large error from current \ + robot state, l2 error is ' + str(position_error)) + return False, None + + if velocity_error > 0.05: + self.node.get_logger().error('Start joint velocity has large error from current \ + robot state, l2 error is ' + str(velocity_error)) + return False, None + + self.node.get_logger().info('executing goal') + self.result = None + + self.execute_plan_client.wait_for_server() + self.node.get_logger().info('Found action server') + + goal_msg = ExecuteTrajectory.Goal() + goal_msg.trajectory = robot_trajectory + goal_msg.trajectory.joint_trajectory.header = Header() + goal_msg.trajectory.multi_dof_joint_trajectory.header = Header() + + self.send_goal_future = self.execute_plan_client.send_goal_async( + goal_msg, feedback_callback=self.trajectory_execution_feedback_cb + ) + + self.__robot_executing = True + self.send_goal_future.add_done_callback(self.goal_response_callback) + if wait_until_complete: + self.wait_until_complete() + return True, self.result + + def trajectory_execution_feedback_cb(self, msg): + self.node.get_logger().info('Feedback: ' + msg.feedback.state) + + def wait_until_complete(self): + if self.__robot_executing: + while self.result is None: + time.sleep(0.01) + + self.__robot_executing = False diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_goal_set_planner.py b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_goal_set_planner.py new file mode 100644 index 0000000..fa40354 --- /dev/null +++ b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_goal_set_planner.py @@ -0,0 +1,332 @@ +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual +# property and proprietary rights in and to this material, related +# documentation and any modifications thereto. Any use, reproduction, +# disclosure or distribution of this material and related documentation +# without an express license agreement from NVIDIA CORPORATION or +# its affiliates is strictly prohibited. + +from typing import List + +from curobo.types.math import Pose +from curobo.types.state import JointState as CuJointState +from curobo.wrap.reacher.motion_gen import MotionGenPlanConfig +from curobo.wrap.reacher.motion_gen import MotionGenStatus +from curobo.wrap.reacher.motion_gen import PoseCostMetric +from isaac_ros_cumotion.cumotion_planner import CumotionActionServer +from isaac_ros_cumotion_interfaces.action import MotionPlan +from moveit_msgs.msg import MoveItErrorCodes +import rclpy +from rclpy.action import ActionServer +from rclpy.executors import MultiThreadedExecutor + + +class CumotionGoalSetPlannerServer(CumotionActionServer): + + def __init__(self): + + super().__init__() + self._goal_set_planner_server = ActionServer( + self, MotionPlan, 'cumotion/motion_plan', self.motion_plan_execute_callback + ) + + def warmup(self): + self.get_logger().info('warming up cuMotion, wait until ready') + self.motion_gen.warmup(enable_graph=True, n_goalset=100, warmup_js_trajopt=True) + self.get_logger().info('cuMotion is ready for planning queries!') + + def toggle_link_collision(self, collision_link_names: List[str], + enable_flag: bool): + if len(collision_link_names) > 0: + if enable_flag: + for k in collision_link_names: + self.motion_gen.kinematics.kinematics_config.enable_link_spheres(k) + else: + for k in collision_link_names: + self.motion_gen.kinematics.kinematics_config.disable_link_spheres(k) + + def get_cu_pose_from_ros_pose(self, ros_pose): + cu_pose = Pose.from_list( + [ros_pose.position.x, + ros_pose.position.y, + ros_pose.position.z, + ros_pose.orientation.w, + ros_pose.orientation.x, + ros_pose.orientation.y, + ros_pose.orientation.z] + ) + return cu_pose + + def get_goal_poses(self, plan_req: MotionPlan.Goal) -> Pose: + if plan_req.goal_pose.header.frame_id != self.motion_gen.kinematics.base_link: + self.get_logger().error( + 'Planning frame: ' + + plan_req.goal_pose.header.frame_id + + ' is not same as motion gen frame: ' + + self.motion_gen.kinematics.base_link + ) + return False, MoveItErrorCodes.INVALID_LINK_NAME, [] + poses = [] + for k in plan_req.goal_pose.poses: + poses.append( + [ + k.position.x, + k.position.y, + k.position.z, + k.orientation.w, + k.orientation.x, + k.orientation.y, + k.orientation.z, + ] + ) + if len(poses) == 0: + self.get_logger().error('No goal pose found') + return False, MoveItErrorCodes.INVALID_GOAL_CONSTRAINTS, poses + goal_pose = Pose.from_batch_list(poses, self.motion_gen.tensor_args) + goal_pose = Pose( + position=goal_pose.position.view(1, -1, 3), + quaternion=goal_pose.quaternion.view(1, -1, 4), + ) + return True, MoveItErrorCodes.SUCCESS, goal_pose + + def motion_plan_execute_callback(self, goal_handle): + self.get_logger().info('Executing goal...') + pose_cost_metric = None + + # check moveit scaling factors: + time_dilation_factor = goal_handle.request.time_dilation_factor + if time_dilation_factor == 0.0: + time_dilation_factor = 0.1 + self.get_logger().warn('Cannot set time_dilation_factor = 0.0') + self.get_logger().info('Planning with time_dilation_factor: ' + str(time_dilation_factor)) + + goal_handle.succeed() + self.motion_gen.reset(reset_seed=False) + + result = MotionPlan.Result() + result.success = False + if goal_handle.request.use_planning_scene: + self.get_logger().info('Updating planning scene') + scene = goal_handle.request.world + world_objects = scene.collision_objects + world_update_status = self.update_world_objects(world_objects) + if not world_update_status: + result.error_code.val = MoveItErrorCodes.COLLISION_CHECKING_UNAVAILABLE + self.get_logger().error('World update failed.') + return result + + start_state = None + plan_req = goal_handle.request + if plan_req.use_current_state: + if self._CumotionActionServer__js_buffer is None: + self.get_logger().error( + 'joint_state was not received from ' + + self._CumotionActionServer__joint_states_topic + ) + return result + # read joint state: + state = CuJointState.from_position( + position=self.tensor_args.to_device( + self._CumotionActionServer__js_buffer['position'] + ).unsqueeze(0), + joint_names=self._CumotionActionServer__js_buffer['joint_names'], + ) + state.velocity = self.tensor_args.to_device( + self._CumotionActionServer__js_buffer['velocity'] + ).unsqueeze(0) + start_state = self.motion_gen.get_active_js(state) + self._CumotionActionServer__js_buffer = None + elif len(plan_req.start_state.position) > 0: + start_state = self.motion_gen.get_active_js( + CuJointState.from_position( + position=self.tensor_args.to_device(plan_req.start_state.position).unsqueeze( + 0 + ), + joint_names=plan_req.start_state.name, + ) + ) + else: + self.get_logger().error('joint state in start state was empty') + return result + + if plan_req.plan_grasp: + self.get_logger().info('Planning to Grasp Object with stop at offset distance') + success, error_code, poses = self.get_goal_poses(plan_req) + self.get_logger().info(f'Success, Error Code): {success}, {error_code}!') + if not success: + result.error_code.val = error_code + return result + grasp_vec_weight = None + if len(plan_req.grasp_partial_pose_vec_weight) == 6: + grasp_vec_weight = [plan_req.grasp_partial_pose_vec_weight[i] for i in range(6)] + + retract_vec_weight = None + if len(plan_req.retract_partial_pose_vec_weight) == 6: + retract_vec_weight = [plan_req.retract_partial_pose_vec_weight[i] + for i in range(6)] + + grasp_constraint_in_goal_frame = plan_req.grasp_approach_constraint_in_goal_frame + retract_constraint_in_goal_frame = plan_req.retract_constraint_in_goal_frame + grasp_plan_result = self.motion_gen.plan_grasp( + start_state, + poses, + MotionGenPlanConfig( + max_attempts=self._CumotionActionServer__max_attempts, + enable_graph_attempt=1, + time_dilation_factor=time_dilation_factor, + ), + grasp_approach_offset=self.get_cu_pose_from_ros_pose(plan_req.grasp_offset_pose), + grasp_approach_path_constraint=grasp_vec_weight, + retract_offset=self.get_cu_pose_from_ros_pose(plan_req.retract_offset_pose), + retract_path_constraint=retract_vec_weight, + disable_collision_links=plan_req.disable_collision_links, + plan_approach_to_grasp=plan_req.plan_approach_to_grasp, + plan_grasp_to_retract=plan_req.plan_grasp_to_retract, + grasp_approach_constraint_in_goal_frame=grasp_constraint_in_goal_frame, + retract_constraint_in_goal_frame=retract_constraint_in_goal_frame, + ) + if grasp_plan_result.success.item(): + traj = self.get_joint_trajectory( + grasp_plan_result.grasp_trajectory, grasp_plan_result.grasp_trajectory_dt, + ) + result.planning_time = grasp_plan_result.planning_time + result.planned_trajectory.append(traj) + if plan_req.plan_grasp_to_retract: + traj = self.get_joint_trajectory( + grasp_plan_result.retract_trajectory, + grasp_plan_result.retract_trajectory_dt, + ) + result.planned_trajectory.append(traj) + result.success = True + result.goal_index = grasp_plan_result.goalset_index.item() + else: + result.success = False + result.message = grasp_plan_result.status + else: + if plan_req.plan_cspace: + self.get_logger().info('Planning CSpace target') + if len(plan_req.goal_state.position) <= 0: + self.get_logger().error('goal state is empty') + return result + goal_state = self.motion_gen.get_active_js( + CuJointState.from_position( + position=self.tensor_args.to_device( + plan_req.goal_state.position).unsqueeze(0), + joint_names=plan_req.goal_state.name, + ) + ) + self.toggle_link_collision(plan_req.disable_collision_links, False) + + motion_gen_result = self.motion_gen.plan_single_js( + start_state, + goal_state, + MotionGenPlanConfig( + max_attempts=self._CumotionActionServer__max_attempts, + enable_graph_attempt=1, + time_dilation_factor=time_dilation_factor, + ), + ) + self.toggle_link_collision(plan_req.disable_collision_links, True) + + elif plan_req.plan_pose: + self.get_logger().info('Planning Pose target') + if plan_req.hold_partial_pose: + if len(plan_req.grasp_partial_pose_vec_weight) < 6: + self.get_logger().error('Partial pose vec weight should be of length 6') + return result + + grasp_vec_weight = [plan_req.grasp_partial_pose_vec_weight[i] + for i in range(6)] + pose_cost_metric = PoseCostMetric( + hold_partial_pose=True, + grasp_vec_weight=self.motion_gen.tensor_args.to_device(grasp_vec_weight), + ) + + # read goal poses: + success, error_code, poses = self.get_goal_poses(plan_req) + if not success: + result.error_code.val = error_code + return result + + self.toggle_link_collision(plan_req.disable_collision_links, False) + if poses.shape[1] == 1: + poses.position = poses.position.view(-1, 3) + poses.quaternion = poses.quaternion.view(-1, 4) + motion_gen_result = self.motion_gen.plan_single( + start_state, + poses, + MotionGenPlanConfig( + max_attempts=self._CumotionActionServer__max_attempts, + enable_graph_attempt=1, + time_dilation_factor=time_dilation_factor, + pose_cost_metric=pose_cost_metric, + ), + ) + else: + motion_gen_result = self.motion_gen.plan_goalset( + start_state, + poses, + MotionGenPlanConfig( + max_attempts=self._CumotionActionServer__max_attempts, + enable_graph_attempt=1, + time_dilation_factor=time_dilation_factor, + pose_cost_metric=pose_cost_metric, + ), + ) + self.toggle_link_collision(plan_req.disable_collision_links, True) + + if motion_gen_result.success.item(): + result.error_code.val = MoveItErrorCodes.SUCCESS + traj = self.get_joint_trajectory( + motion_gen_result.optimized_plan, motion_gen_result.optimized_dt.item() + ) + result.planning_time = motion_gen_result.total_time + result.planned_trajectory.append(traj) + result.success = True + result.goal_index = motion_gen_result.goalset_index.item() + elif not motion_gen_result.valid_query: + self.get_logger().error(f'Invalid planning query: {motion_gen_result.status}') + if motion_gen_result.status == MotionGenStatus.INVALID_START_STATE_JOINT_LIMITS: + result.error_code.val = MoveItErrorCodes.START_STATE_INVALID + if motion_gen_result.status in [ + MotionGenStatus.INVALID_START_STATE_WORLD_COLLISION, + MotionGenStatus.INVALID_START_STATE_SELF_COLLISION, + ]: + result.error_code.val = MoveItErrorCodes.START_STATE_IN_COLLISION + else: + self.get_logger().error( + f'Motion planning failed wih status: {motion_gen_result.status}' + ) + if motion_gen_result.status == MotionGenStatus.IK_FAIL: + result.error_code.val = MoveItErrorCodes.NO_IK_SOLUTION + + self.get_logger().info( + 'returned planning result (query, success, failure_status): ' + + str(self._CumotionActionServer__query_count) + + ' ' + + str(motion_gen_result.success.item()) + + ' ' + + str(motion_gen_result.status) + ) + + self._CumotionActionServer__query_count += 1 + + return result + + +def main(args=None): + rclpy.init(args=args) + cumotion_action_server = CumotionGoalSetPlannerServer() + executor = MultiThreadedExecutor() + executor.add_node(cumotion_action_server) + try: + executor.spin() + except KeyboardInterrupt: + cumotion_action_server.get_logger().info('KeyboardInterrupt, shutting down.\n') + cumotion_action_server.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py index c2dbb2d..058d305 100644 --- a/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py +++ b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py @@ -7,10 +7,12 @@ # without an express license agreement from NVIDIA CORPORATION or # its affiliates is strictly prohibited. +from copy import deepcopy from os import path + +import threading import time -from ament_index_python.packages import get_package_share_directory from curobo.geom.sdf.world import CollisionCheckerType from curobo.geom.types import Cuboid from curobo.geom.types import Cylinder @@ -22,16 +24,17 @@ from curobo.types.math import Pose from curobo.types.state import JointState as CuJointState from curobo.util.logger import setup_curobo_logger -from curobo.util_file import get_robot_configs_path -from curobo.util_file import join_path -from curobo.util_file import load_yaml from curobo.wrap.reacher.motion_gen import MotionGen from curobo.wrap.reacher.motion_gen import MotionGenConfig from curobo.wrap.reacher.motion_gen import MotionGenPlanConfig from curobo.wrap.reacher.motion_gen import MotionGenStatus from geometry_msgs.msg import Point from geometry_msgs.msg import Vector3 -from isaac_ros_cumotion.xrdf_utils import convert_xrdf_to_curobo +from isaac_ros_cumotion.update_kinematics import get_robot_config +from isaac_ros_cumotion.update_kinematics import UpdateLinkSpheresServer +from isaac_ros_cumotion_python_utils.utils import \ + get_grid_center, get_grid_min_corner, get_grid_size, is_grid_valid, \ + load_grid_corners_from_workspace_file from moveit_msgs.action import MoveGroup from moveit_msgs.msg import CollisionObject from moveit_msgs.msg import MoveItErrorCodes @@ -45,7 +48,6 @@ from rclpy.node import Node from sensor_msgs.msg import JointState from shape_msgs.msg import SolidPrimitive -from std_msgs.msg import ColorRGBA import torch from trajectory_msgs.msg import JointTrajectory from trajectory_msgs.msg import JointTrajectoryPoint @@ -58,26 +60,40 @@ def __init__(self): super().__init__('cumotion_action_server') self.tensor_args = TensorDeviceType() self.declare_parameter('robot', 'ur5e.yml') + self.declare_parameter('urdf_path', rclpy.Parameter.Type.STRING) + self.declare_parameter('yml_file_path', rclpy.Parameter.Type.STRING) self.declare_parameter('time_dilation_factor', 0.5) + self.declare_parameter('max_attempts', 10) + self.declare_parameter('num_graph_seeds', 6) + self.declare_parameter('num_trajopt_seeds', 6) + self.declare_parameter('include_trajopt_retract_seed', True) + self.declare_parameter('num_trajopt_time_steps', 32) + self.declare_parameter('trajopt_finetune_iters', 400) + self.declare_parameter('interpolation_dt', 0.025) self.declare_parameter('collision_cache_mesh', 20) self.declare_parameter('collision_cache_cuboid', 20) - self.declare_parameter('interpolation_dt', 0.02) - self.declare_parameter('voxel_dims', [2.0, 2.0, 2.0]) self.declare_parameter('voxel_size', 0.05) self.declare_parameter('read_esdf_world', False) self.declare_parameter('publish_curobo_world_as_voxels', False) self.declare_parameter('add_ground_plane', False) self.declare_parameter('publish_voxel_size', 0.05) - self.declare_parameter('max_publish_voxels', 50000) + self.declare_parameter('max_publish_voxels', 500000) self.declare_parameter('joint_states_topic', '/joint_states') self.declare_parameter('tool_frame', rclpy.Parameter.Type.STRING) - self.declare_parameter('grid_position', [0.0, 0.0, 0.0]) + # The grid_center_m and grid_size_m parameters are loaded from the workspace file + # if the workspace_file_path is set and valid. + self.declare_parameter('workspace_file_path', '') + self.declare_parameter('grid_center_m', [0.0, 0.0, 0.0]) + self.declare_parameter('grid_size_m', [2.0, 2.0, 2.0]) + self.declare_parameter('update_esdf_on_request', True) + self.declare_parameter('use_aabb_on_request', True) self.declare_parameter('esdf_service_name', '/nvblox_node/get_esdf_and_gradient') - self.declare_parameter('urdf_path', rclpy.Parameter.Type.STRING) self.declare_parameter('enable_curobo_debug_mode', False) self.declare_parameter('override_moveit_scaling_factors', False) + self.declare_parameter('update_link_sphere_server', + 'planner_attach_object') debug_mode = ( self.get_parameter('enable_curobo_debug_mode').get_parameter_value().bool_value ) @@ -87,9 +103,10 @@ def __init__(self): setup_curobo_logger('warning') self.__voxel_pub = self.create_publisher(Marker, '/curobo/voxels', 10) - self._action_server = ActionServer( - self, MoveGroup, 'cumotion/move_group', self.execute_callback - ) + self.planner_busy = False + self.lock = threading.Lock() + + self.__robot_file = self.get_parameter('robot').get_parameter_value().string_value try: self.__urdf_path = self.get_parameter('urdf_path') @@ -99,6 +116,17 @@ def __init__(self): except rclpy.exceptions.ParameterUninitializedException: self.__urdf_path = None + try: + self.__yml_path = self.get_parameter('yml_file_path') + self.__yml_path = self.__yml_path.get_parameter_value().string_value + if self.__yml_path == '': + self.__yml_path = None + except rclpy.exceptions.ParameterUninitializedException: + self.__yml_path = None + + # If a YAML path is provided, override other XRDF/YAML file name + if self.__yml_path is not None: + self.__robot_file = self.__yml_path try: self.__tool_frame = self.get_parameter('tool_frame') self.__tool_frame = self.__tool_frame.get_parameter_value().string_value @@ -107,9 +135,6 @@ def __init__(self): except rclpy.exceptions.ParameterUninitializedException: self.__tool_frame = None - self.__xrdf_path = path.join( - get_package_share_directory('isaac_ros_cumotion_robot_description'), 'xrdf' - ) self.__joint_states_topic = ( self.get_parameter('joint_states_topic').get_parameter_value().string_value ) @@ -119,6 +144,49 @@ def __init__(self): self.__override_moveit_scaling_factors = ( self.get_parameter('override_moveit_scaling_factors').get_parameter_value().bool_value ) + + # Motion generation parameters + + self.__max_attempts = ( + self.get_parameter('max_attempts').get_parameter_value().integer_value + ) + self.__num_graph_seeds = ( + self.get_parameter('num_graph_seeds').get_parameter_value().integer_value + ) + self.__num_trajopt_seeds = ( + self.get_parameter('num_trajopt_seeds').get_parameter_value().integer_value + ) + self.__num_trajopt_time_steps = ( + self.get_parameter('num_trajopt_time_steps').get_parameter_value().integer_value + ) + self.__trajopt_finetune_iters = ( + self.get_parameter('trajopt_finetune_iters').get_parameter_value().integer_value + ) + self.__interpolation_dt = ( + self.get_parameter('interpolation_dt').get_parameter_value().double_value + ) + + include_trajopt_retract_seed = ( + self.get_parameter('include_trajopt_retract_seed').get_parameter_value().bool_value + ) + if include_trajopt_retract_seed: + self.__num_trajopt_noisy_seeds = 1 + self.__trajopt_seed_ratio = {'linear': 1.0} + else: + self.__num_trajopt_noisy_seeds = 2 + self.__trajopt_seed_ratio = {'linear': 0.5, 'bias': 0.5} + + collision_cache_cuboid = ( + self.get_parameter('collision_cache_cuboid').get_parameter_value().integer_value + ) + collision_cache_mesh = ( + self.get_parameter('collision_cache_mesh').get_parameter_value().integer_value + ) + self.__collision_cache = { + 'obb': collision_cache_cuboid, + 'mesh': collision_cache_mesh + } + # ESDF service self.__read_esdf_grid = ( @@ -127,21 +195,54 @@ def __init__(self): self.__publish_curobo_world_as_voxels = ( self.get_parameter('publish_curobo_world_as_voxels').get_parameter_value().bool_value ) - self.__grid_position = ( - self.get_parameter('grid_position').get_parameter_value().double_array_value + self.__grid_center_m = ( + self.get_parameter('grid_center_m').get_parameter_value().double_array_value ) self.__max_publish_voxels = ( self.get_parameter('max_publish_voxels').get_parameter_value().integer_value ) - self.__voxel_dims = ( - self.get_parameter('voxel_dims').get_parameter_value().double_array_value + self.__workspace_file_path = ( + self.get_parameter('workspace_file_path').get_parameter_value().string_value + ) + self.__grid_size_m = ( + self.get_parameter('grid_size_m').get_parameter_value().double_array_value + ) + self.__update_esdf_on_request = ( + self.get_parameter('update_esdf_on_request').get_parameter_value().bool_value + ) + self.__use_aabb_on_request = ( + self.get_parameter('use_aabb_on_request').get_parameter_value().bool_value ) self.__publish_voxel_size = ( self.get_parameter('publish_voxel_size').get_parameter_value().double_value ) self.__voxel_size = self.get_parameter('voxel_size').get_parameter_value().double_value + self._update_link_sphere_server = ( + self.get_parameter( + 'update_link_sphere_server').get_parameter_value().string_value + ) self.__esdf_client = None self.__esdf_req = None + + # Setup the grid position and dimension. + if path.exists(self.__workspace_file_path): + self.get_logger().info( + f'Loading grid center and dims from workspace file: {self.__workspace_file_path}.') + min_corner, max_corner = load_grid_corners_from_workspace_file( + self.__workspace_file_path) + self.__grid_size_m = get_grid_size(min_corner, max_corner, self.__voxel_size) + self.__grid_center_m = get_grid_center(min_corner, self.__grid_size_m) + + self.get_logger().info( + f'Loaded grid dims: {self.__grid_size_m}, ' + f'voxel size: {self.__voxel_size}') + else: + self.get_logger().info( + 'Loading grid position and dims from grid_center_m and grid_size_m parameters.') + + if is_grid_valid(self.__grid_size_m, self.__voxel_size): + self.get_logger().fatal('Number of voxels should be at least 1 in every dimension.') + raise SystemExit + if self.__read_esdf_grid: esdf_service_name = ( self.get_parameter('esdf_service_name').get_parameter_value().string_value @@ -156,6 +257,8 @@ def __init__(self): f'Service({esdf_service_name}) not available, waiting again...' ) self.__esdf_req = EsdfAndGradients.Request() + + self.load_motion_gen() self.warmup() self.__query_count = 0 self.__tensor_args = self.motion_gen.tensor_args @@ -164,6 +267,19 @@ def __init__(self): ) self.__js_buffer = None + # Call on_timer every 0.01 seconds + self.timer = self.create_timer(0.01, self.on_timer) + + self.__update_link_spheres_server = UpdateLinkSpheresServer( + server_node=self, + action_name=self._update_link_sphere_server, + robot_kinematics=self.motion_gen.kinematics, + robot_base_frame=self.__robot_base_frame + ) + self._action_server = ActionServer( + self, MoveGroup, 'cumotion/move_group', self.execute_callback + ) + def js_callback(self, msg): self.__js_buffer = { 'joint_names': msg.name, @@ -171,25 +287,7 @@ def js_callback(self, msg): 'velocity': msg.velocity, } - def warmup(self): - robot_file = self.get_parameter('robot').get_parameter_value().string_value - if robot_file == '': - self.get_logger().fatal('Received empty robot file') - raise SystemExit - - collision_cache_cuboid = ( - self.get_parameter('collision_cache_cuboid').get_parameter_value().integer_value - ) - collision_cache_mesh = ( - self.get_parameter('collision_cache_mesh').get_parameter_value().integer_value - ) - interpolation_dt = ( - self.get_parameter('interpolation_dt').get_parameter_value().double_value - ) - - self.get_logger().info('Loaded robot file name: ' + robot_file) - self.get_logger().info('warming up cuMotion, wait until ready') - + def load_motion_gen(self): tensor_args = self.tensor_args world_file = WorldConfig.from_dict( { @@ -201,7 +299,7 @@ def warmup(self): }, 'voxel': { 'world_voxel': { - 'dims': self.__voxel_dims, + 'dims': self.__grid_size_m, 'pose': [0, 0, 0, 1, 0, 0, 0], # x, y, z, qw, qx, qy, qz 'voxel_size': self.__voxel_size, 'feature_dtype': torch.bfloat16, @@ -210,59 +308,81 @@ def warmup(self): } ) - if robot_file.lower().endswith('.xrdf'): - if self.__urdf_path is None: - self.get_logger().fatal('urdf_path is required to load robot from .xrdf') - raise SystemExit - robot_dict = load_yaml(join_path(self.__xrdf_path, robot_file)) - robot_dict = convert_xrdf_to_curobo(self.__urdf_path, robot_dict, self.get_logger()) - else: - robot_dict = load_yaml(join_path(get_robot_configs_path(), robot_file)) - - if self.__urdf_path is not None: - robot_dict['robot_cfg']['kinematics']['urdf_path'] = self.__urdf_path + robot_config = get_robot_config( + robot_file=self.__robot_file, + urdf_file_path=self.__urdf_path, + logger=self.get_logger() + ) - robot_dict = robot_dict['robot_cfg'] + robot_dict = robot_config['robot_cfg'] motion_gen_config = MotionGenConfig.load_from_robot_config( robot_dict, world_file, tensor_args, - interpolation_dt=interpolation_dt, - collision_cache={ - 'obb': collision_cache_cuboid, - 'mesh': collision_cache_mesh, - }, + num_graph_seeds=self.__num_graph_seeds, + num_trajopt_seeds=self.__num_trajopt_seeds, + num_trajopt_noisy_seeds=self.__num_trajopt_noisy_seeds, + trajopt_tsteps=self.__num_trajopt_time_steps, + trajopt_seed_ratio=self.__trajopt_seed_ratio, + interpolation_dt=self.__interpolation_dt, + collision_cache=self.__collision_cache, collision_checker_type=CollisionCheckerType.VOXEL, ee_link_name=self.__tool_frame, + finetune_trajopt_iters=self.__trajopt_finetune_iters, ) motion_gen = MotionGen(motion_gen_config) - self.__robot_base_frame = motion_gen.kinematics.base_link - - motion_gen.warmup(enable_graph=True) + self.motion_gen = motion_gen + self.__robot_base_frame = self.motion_gen.kinematics.base_link - self.__world_collision = motion_gen.world_coll_checker + self.__world_collision = self.motion_gen.world_coll_checker if not self.__add_ground_plane: - motion_gen.clear_world_cache() - self.motion_gen = motion_gen + self.motion_gen.clear_world_cache() + self.__cumotion_grid_shape = self.__world_collision.get_voxel_grid( + 'world_voxel').get_grid_shape()[0] + + def warmup(self): + self.get_logger().info('warming up cuMotion, wait until ready') + self.motion_gen.warmup(enable_graph=True) self.get_logger().info('cuMotion is ready for planning queries!') + def on_timer(self): + with self.lock: + if self.__js_buffer is None: + return + + js = np.copy(self.__js_buffer['position']) + j_names = deepcopy(self.__js_buffer['joint_names']) + + self.__update_link_spheres_server.publish_all_active_spheres( + robot_joint_states=js, + robot_joint_names=j_names, + tensor_args=self.__tensor_args, + rgb=[0.0, 1.0, 1.0, 1.0] + ) + def update_voxel_grid(self): self.get_logger().info('Calling ESDF service') - # This is half of x,y and z dims + + # Get the AABB + min_corner = get_grid_min_corner(self.__grid_center_m, self.__grid_size_m) aabb_min = Point() - aabb_min.x = -1 * self.__voxel_dims[0] / 2 - aabb_min.y = -1 * self.__voxel_dims[1] / 2 - aabb_min.z = -1 * self.__voxel_dims[2] / 2 - # This is a voxel size. - voxel_dims = Vector3() - voxel_dims.x = self.__voxel_dims[0] - voxel_dims.y = self.__voxel_dims[1] - voxel_dims.z = self.__voxel_dims[2] - esdf_future = self.send_request(aabb_min, voxel_dims) + aabb_min.x = min_corner[0] + aabb_min.y = min_corner[1] + aabb_min.z = min_corner[2] + aabb_size = Vector3() + aabb_size.x = self.__grid_size_m[0] + aabb_size.y = self.__grid_size_m[1] + aabb_size.z = self.__grid_size_m[2] + + # Request the esdf grid + esdf_future = self.send_request(aabb_min, aabb_size) while not esdf_future.done(): time.sleep(0.001) response = esdf_future.result() + if not response.success: + self.get_logger().info('ESDF request failed, try again after few seconds.') + return False esdf_grid = self.get_esdf_voxel_grid(response) if torch.max(esdf_grid.feature_tensor) <= (-1000.0 + 0.5 * self.__voxel_size + 1e-5): self.get_logger().error('ESDF data is empty, try again after few seconds.') @@ -272,6 +392,10 @@ def update_voxel_grid(self): return True def send_request(self, aabb_min_m, aabb_size_m): + self.__esdf_req.visualize_esdf = True + self.__esdf_req.update_esdf = self.__update_esdf_on_request + self.__esdf_req.use_aabb = self.__use_aabb_on_request + self.__esdf_req.frame_id = self.__robot_base_frame self.__esdf_req.aabb_min_m = aabb_min_m self.__esdf_req.aabb_size_m = aabb_size_m self.get_logger().info( @@ -282,15 +406,43 @@ def send_request(self, aabb_min_m, aabb_size_m): return esdf_future def get_esdf_voxel_grid(self, esdf_data): + esdf_voxel_size = esdf_data.voxel_size_m + if abs(esdf_voxel_size - self.__voxel_size) > 1e-4: + self.get_logger().fatal( + 'Voxel size of esdf array is not equal to requested voxel_size, ' + f'{esdf_voxel_size} vs. {self.__voxel_size}') + raise SystemExit + + # Get the esdf and gradient data esdf_array = esdf_data.esdf_and_gradients array_shape = [ esdf_array.layout.dim[0].size, esdf_array.layout.dim[1].size, esdf_array.layout.dim[2].size, ] - array_data = np.array(esdf_array.data) + array_data = np.array(esdf_array.data, dtype=np.float32) + if (array_data.shape[0] <= 0): + self.get_logger().fatal( + 'array shape is zero: ' + str(array_data.shape) + ) + raise SystemExit + array_data = torch.as_tensor(array_data) - array_data = self.__tensor_args.to_device(array_data) + # Verify the grid shape + if array_shape != self.__cumotion_grid_shape: + self.get_logger().fatal( + 'Shape of received esdf voxel grid does not match the cumotion grid shape, ' + f'{array_shape} vs. {self.__cumotion_grid_shape}') + raise SystemExit + + # Get the origin of the grid + grid_origin = [ + esdf_data.origin_m.x, + esdf_data.origin_m.y, + esdf_data.origin_m.z, + ] + # The grid position is defined as the center point of the grid. + grid_center_m = get_grid_center(grid_origin, self.__grid_size_m) # Array data is reshaped to x y z channels array_data = array_data.view(array_shape[0], array_shape[1], array_shape[2]).contiguous() @@ -298,27 +450,20 @@ def get_esdf_voxel_grid(self, esdf_data): # Array is squeezed to 1 dimension array_data = array_data.reshape(-1, 1) - # nvblox uses negative distance inside obstacles, cuRobo needs the opposite: - array_data = -1 * array_data + # nvblox assigns a value of -1000.0 for unobserved voxels, making it positive + array_data[array_data < -999.9] = 1000.0 - # nvblox assigns a value of -1000.0 for unobserved voxels, making - array_data[array_data >= 1000.0] = -1000.0 + # nvblox uses negative distance inside obstacles, cuRobo needs the opposite: + array_data = -1.0 * array_data - # nvblox distance are at origin of each voxel, cuRobo's esdf needs it to be at faces - array_data = array_data + 0.5 * self.__voxel_size + # nvblox treats surface voxels as distance = 0.0, while cuRobo treats + # distance = 0.0 as not in collision. Adding an offset. + array_data += 0.5 * self.__voxel_size esdf_grid = CuVoxelGrid( name='world_voxel', - dims=self.__voxel_dims, - pose=[ - self.__grid_position[0], - self.__grid_position[1], - self.__grid_position[2], - 1, - 0.0, - 0.0, - 0, - ], # x, y, z, qw, qx, qy, qz + dims=self.__grid_size_m, + pose=grid_center_m + [1, 0.0, 0.0, 0.0], # x, y, z, qw, qx, qy, qz voxel_size=self.__voxel_size, feature_dtype=torch.float32, feature_tensor=array_data, @@ -475,21 +620,31 @@ def update_world_objects(self, moveit_objects): if self.__read_esdf_grid: world_update_status = self.update_voxel_grid() if self.__publish_curobo_world_as_voxels: - voxels = self.__world_collision.get_esdf_in_bounding_box( - Cuboid( - name='test', - pose=[0.0, 0.0, 0.0, 1, 0, 0, 0], # x, y, z, qw, qx, qy, qz - dims=self.__voxel_dims, - ), - voxel_size=self.__publish_voxel_size, - ) - xyzr_tensor = voxels.xyzr_tensor.clone() - xyzr_tensor[..., 3] = voxels.feature_tensor - self.publish_voxels(xyzr_tensor) + if self.__voxel_pub.get_subscription_count() > 0: + # Calculate occupancy and publish only when subscribed. + voxels = self.__world_collision.get_occupancy_in_bounding_box( + Cuboid( + name='test', + pose=[0.0, 0.0, 0.0, 1, 0, 0, 0], # x, y, z, qw, qx, qy, qz + dims=self.__grid_size_m, + ), + voxel_size=self.__publish_voxel_size, + ) + xyzr_tensor = voxels.xyzr_tensor.clone() + xyzr_tensor[..., 3] = voxels.feature_tensor + self.publish_voxels(xyzr_tensor) return world_update_status def execute_callback(self, goal_handle): + if self.planner_busy: + self.get_logger().error('Planner is busy') + goal_handle.abort() + result = MoveGroup.Result() + result.error_code.val = MoveItErrorCodes.FAILURE + return result + self.get_logger().info('Executing goal...') + # check moveit scaling factors: min_scaling_factor = min(goal_handle.request.request.max_velocity_scaling_factor, goal_handle.request.request.max_acceleration_scaling_factor) @@ -501,6 +656,7 @@ def execute_callback(self, goal_handle): self.get_logger().info('Planning with time_dilation_factor: ' + str(time_dilation_factor)) plan_req = goal_handle.request.request + goal_handle.succeed() scene = goal_handle.request.planning_options.planning_scene_diff @@ -612,15 +768,18 @@ def execute_callback(self, goal_handle): return result else: self.get_logger().error('Goal constraints not supported') + with self.lock: + self.planner_busy = True self.motion_gen.reset(reset_seed=False) motion_gen_result = self.motion_gen.plan_single( start_state, goal_pose, - MotionGenPlanConfig(max_attempts=5, enable_graph_attempt=1, + MotionGenPlanConfig(max_attempts=self.__max_attempts, enable_graph_attempt=1, time_dilation_factor=time_dilation_factor), ) - + with self.lock: + self.planner_busy = False result = MoveGroup.Result() if motion_gen_result.success.item(): result.error_code.val = MoveItErrorCodes.SUCCESS @@ -639,7 +798,7 @@ def execute_callback(self, goal_handle): if motion_gen_result.status in [ MotionGenStatus.INVALID_START_STATE_WORLD_COLLISION, MotionGenStatus.INVALID_START_STATE_SELF_COLLISION, - ]: + ]: result.error_code.val = MoveItErrorCodes.START_STATE_IN_COLLISION else: @@ -671,35 +830,36 @@ def publish_voxels(self, voxels): marker.ns = 'curobo_world' marker.action = 0 marker.pose.orientation.w = 1.0 - marker.lifetime = rclpy.duration.Duration(seconds=1000.0).to_msg() + marker.lifetime = rclpy.duration.Duration(seconds=0.0).to_msg() marker.frame_locked = False marker.scale.x = vox_size marker.scale.y = vox_size marker.scale.z = vox_size + marker.points = [] # get only voxels that are inside surfaces: - - voxels = voxels[voxels[:, 3] >= 0.0] + voxels = voxels[voxels[:, 3] > 0.0] vox = voxels.view(-1, 4).cpu().numpy() - marker.points = [] - - for i in range(min(len(vox), self.__max_publish_voxels)): - + number_of_voxels_to_publish = len(vox) + if len(vox) > self.__max_publish_voxels: + self.get_logger().warn( + f'Number of voxels to publish bigger than max_publish_voxels, ' + f'{len(vox)} > {self.__max_publish_voxels}' + ) + number_of_voxels_to_publish = self.__max_publish_voxels + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + vox = vox.astype(np.float64) + for i in range(number_of_voxels_to_publish): + # Publish the markers at the center of the voxels: pt = Point() - pt.x = float(vox[i, 0]) - pt.y = float(vox[i, 1]) - pt.z = float(vox[i, 2]) - color = ColorRGBA() - d = vox[i, 3] - - rgba = [min(1.0, 1.0 - float(d)), 0.0, 0.0, 1.0] - - color.r = rgba[0] - color.g = rgba[1] - color.b = rgba[2] - color.a = rgba[3] - marker.colors.append(color) + pt.x = vox[i, 0] + pt.y = vox[i, 1] + pt.z = vox[i, 2] marker.points.append(pt) + # publish voxels: marker.header.stamp = self.get_clock().now().to_msg() @@ -716,7 +876,8 @@ def main(args=None): except KeyboardInterrupt: cumotion_action_server.get_logger().info('KeyboardInterrupt, shutting down.\n') cumotion_action_server.destroy_node() - rclpy.shutdown() + if rclpy.ok(): + rclpy.shutdown() if __name__ == '__main__': diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/robot_segmenter.py b/isaac_ros_cumotion/isaac_ros_cumotion/robot_segmenter.py index a436df0..1d9d077 100644 --- a/isaac_ros_cumotion/isaac_ros_cumotion/robot_segmenter.py +++ b/isaac_ros_cumotion/isaac_ros_cumotion/robot_segmenter.py @@ -15,13 +15,13 @@ from curobo.types.camera import CameraObservation from curobo.types.math import Pose as CuPose from curobo.types.state import JointState as CuJointState -from curobo.util_file import get_robot_configs_path -from curobo.util_file import join_path -from curobo.util_file import load_yaml from curobo.wrap.model.robot_segmenter import RobotSegmenter +import cv2 from cv_bridge import CvBridge +from isaac_ros_common.qos import add_qos_parameter +from isaac_ros_cumotion.update_kinematics import get_robot_config +from isaac_ros_cumotion.update_kinematics import UpdateLinkSpheresServer from isaac_ros_cumotion.util import get_spheres_marker -from isaac_ros_cumotion.xrdf_utils import convert_xrdf_to_curobo from message_filters import ApproximateTimeSynchronizer from message_filters import Subscriber import numpy as np @@ -47,6 +47,8 @@ class CumotionRobotSegmenter(Node): def __init__(self): super().__init__('cumotion_robot_segmentation') self.declare_parameter('robot', 'ur5e.yml') + self.declare_parameter('urdf_path', rclpy.Parameter.Type.STRING) + self.declare_parameter('yml_file_path', rclpy.Parameter.Type.STRING) self.declare_parameter('cuda_device', 0) self.declare_parameter('distance_threshold', 0.1) self.declare_parameter('time_sync_slop', 0.1) @@ -60,15 +62,40 @@ def __init__(self): self.declare_parameter('robot_mask_publish_topics', ['/cumotion/depth_1/robot_mask']) self.declare_parameter('world_depth_publish_topics', ['/cumotion/depth_1/world_depth']) + self.declare_parameter('filter_speckles_in_mask', False) + self.declare_parameter('max_filtered_speckles_size', 1250) + self.declare_parameter('log_debug', False) - self.declare_parameter('urdf_path', rclpy.Parameter.Type.STRING) + self.declare_parameter('update_link_sphere_server', + 'segmenter_attach_object') + + depth_qos = add_qos_parameter(self, 'DEFAULT', 'depth_qos') + depth_info_qos = add_qos_parameter(self, 'DEFAULT', 'depth_info_qos') + mask_qos = add_qos_parameter(self, 'DEFAULT', 'mask_qos') + world_depth_qos = add_qos_parameter(self, 'DEFAULT', 'world_depth_qos') + + self.__robot_file = self.get_parameter('robot').get_parameter_value().string_value try: self.__urdf_path = self.get_parameter('urdf_path') self.__urdf_path = self.__urdf_path.get_parameter_value().string_value + if self.__urdf_path == '': + self.__urdf_path = None except rclpy.exceptions.ParameterUninitializedException: self.__urdf_path = None + try: + self.__yml_path = self.get_parameter('yml_file_path') + self.__yml_path = self.__yml_path.get_parameter_value().string_value + if self.__yml_path == '': + self.__yml_path = None + except rclpy.exceptions.ParameterUninitializedException: + self.__yml_path = None + + # If a YAML path is provided, override other XRDF/YAML file name + if self.__yml_path is not None: + self.__robot_file = self.__yml_path + distance_threshold = ( self.get_parameter('distance_threshold').get_parameter_value().double_value) time_sync_slop = self.get_parameter('time_sync_slop').get_parameter_value().double_value @@ -89,6 +116,13 @@ def __init__(self): world_depth_topics = ( self.get_parameter( 'world_depth_publish_topics').get_parameter_value().string_array_value) + self._filter_speckles_in_mask = ( + self.get_parameter('filter_speckles_in_mask').get_parameter_value().bool_value + ) + self._max_filtered_speckles_size = self.get_parameter( + 'max_filtered_speckles_size').get_parameter_value().integer_value + self._update_link_sphere_server = ( + self.get_parameter('update_link_sphere_server').get_parameter_value().string_value) self._log_debug = self.get_parameter('log_debug').get_parameter_value().bool_value num_cameras = len(depth_image_topics) @@ -109,7 +143,8 @@ def __init__(self): self._tensor_args = TensorDeviceType(device=torch.device('cuda', cuda_device_id)) # Create subscribers: - subscribers = [Subscriber(self, Image, topic) for topic in depth_image_topics] + subscribers = [Subscriber(self, Image, topic, qos_profile=depth_qos) + for topic in depth_image_topics] subscribers.append(Subscriber(self, JointState, joint_states_topic)) # Subscribe to topics with sync: self.approx_time_sync = ApproximateTimeSynchronizer( @@ -122,21 +157,19 @@ def __init__(self): self.info_subscribers.append( self.create_subscription( CameraInfo, depth_camera_infos[idx], - lambda msg, index=idx: self.camera_info_cb(msg, index), 10) + lambda msg, index=idx: self.camera_info_cb(msg, index), depth_info_qos) ) self.mask_publishers = [ - self.create_publisher(Image, topic, 10) for topic in publish_mask_topics] + self.create_publisher(Image, topic, mask_qos) for topic in publish_mask_topics] self.segmented_publishers = [ - self.create_publisher(Image, topic, 10) for topic in world_depth_topics] + self.create_publisher(Image, topic, world_depth_qos) for topic in world_depth_topics] self.debug_robot_publisher = self.create_publisher(MarkerArray, debug_robot_topic, 10) self.tf_buffer = Buffer(cache_time=rclpy.duration.Duration(seconds=60.0)) self.tf_listener = TransformListener(self.tf_buffer, self) - # Create a depth mask publisher: - robot_file = self.get_parameter('robot').get_parameter_value().string_value self.br = CvBridge() # Create buffers to store data: @@ -151,20 +184,24 @@ def __init__(self): self.lock = threading.Lock() self.timer = self.create_timer(0.01, self.on_timer) - robot_dict = load_yaml(join_path(get_robot_configs_path(), robot_file)) - if robot_file.lower().endswith('.xrdf'): - if self.__urdf_path is None: - self.get_logger().fatal('urdf_path is required to load robot from .xrdf') - raise SystemExit - robot_dict = convert_xrdf_to_curobo(self.__urdf_path, robot_dict, self.get_logger()) - - if self.__urdf_path is not None: - robot_dict['robot_cfg']['kinematics']['urdf_path'] = self.__urdf_path + robot_config = get_robot_config( + robot_file=self.__robot_file, + urdf_file_path=self.__urdf_path, + logger=self.get_logger() + ) self._cumotion_segmenter = RobotSegmenter.from_robot_file( - robot_dict, distance_threshold=distance_threshold) + robot_config, distance_threshold=distance_threshold) self._cumotion_base_frame = self._cumotion_segmenter.base_link + + self.__update_link_spheres_server = UpdateLinkSpheresServer( + server_node=self, + action_name=self._update_link_sphere_server, + robot_kinematics=self._cumotion_segmenter.robot_world.kinematics, + robot_base_frame=self._cumotion_base_frame + ) + self._robot_pose_cameras = None self.get_logger().info(f'Node initialized with {self._num_cameras} cameras') @@ -210,16 +247,33 @@ def is_subscribed(self) -> bool: return True return False - def publish_images(self, depth_mask, segmented_depth, camera_header, idx: int): + def filter_depth_mask(self, robot_mask, depth_image): + # pixels with depth <= 0.0 are invalid + invalid_depth_value = 0.0 + # get the invalid depth mask + invalid_depth_mask = depth_image <= invalid_depth_value + # combine the invalid depth and robot masks + combined_mask = np.logical_or(robot_mask, invalid_depth_mask).astype(np.uint8) * 255 + # filter speckles from the combined mask + filtered_combined_mask = cv2.filterSpeckles( + combined_mask, 255, self._max_filtered_speckles_size, 0)[0] + # Set depth pixels to invalid if they are masked in the filtered mask + depth_image[filtered_combined_mask.astype(bool)] = invalid_depth_value + return (filtered_combined_mask, depth_image) + + def publish_images(self, depth_masks, segmented_depth_images, camera_header, idx: int): + depth_mask = depth_masks[idx] + segmented_depth = segmented_depth_images[idx] + + if self._filter_speckles_in_mask: + depth_mask, segmented_depth = self.filter_depth_mask(depth_mask, segmented_depth) if self.mask_publishers[idx].get_subscription_count() > 0: - depth_mask = depth_mask[idx] msg = self.br.cv2_to_imgmsg(depth_mask, 'mono8') msg.header = camera_header[idx] self.mask_publishers[idx].publish(msg) if self.segmented_publishers[idx].get_subscription_count() > 0: - segmented_depth = segmented_depth[idx] if self._depth_encoding[idx] == '16UC1': segmented_depth = segmented_depth.astype(np.uint16) elif self._depth_encoding[idx] == '32FC1': @@ -269,8 +323,9 @@ def on_timer(self): ] ) except TransformException as ex: - self.get_logger().debug(f'Could not transform {camera_headers[i].frame_id} \ - to { self._cumotion_base_frame}: {ex}') + self.get_logger().debug( + f'Could not transform {camera_headers[i].frame_id}' + f'to { self._cumotion_base_frame}: {ex}') continue if None not in self._robot_pose_camera: self._robot_pose_cameras = CuPose.cat(self._robot_pose_camera) @@ -317,6 +372,13 @@ def on_timer(self): for x in range(depth_mask.shape[0]): self.publish_images(depth_mask, segmented_depth, camera_headers, x) + self.__update_link_spheres_server.publish_all_active_spheres( + robot_joint_states=js, + robot_joint_names=j_names, + tensor_args=self._tensor_args, + rgb=[1.0, 0.0, 0.0, 1.0] + ) + if self.debug_robot_publisher.get_subscription_count() > 0: self.publish_robot_spheres(q) if self._log_debug: @@ -333,14 +395,19 @@ def main(args=None): # Create the node cumotion_segmenter = CumotionRobotSegmenter() - # Spin the node so the callback function is called. - rclpy.spin(cumotion_segmenter) + try: + # Spin the node so the callback function is called. + cumotion_segmenter.get_logger().info('Starting CumotionRobotSegmenter node') + rclpy.spin(cumotion_segmenter) + except KeyboardInterrupt: + cumotion_segmenter.get_logger().info('Destroying CumotionRobotSegmenter node') # Destroy the node explicitly cumotion_segmenter.destroy_node() # Shutdown the ROS client library for Python - rclpy.shutdown() + if rclpy.ok(): + rclpy.shutdown() if __name__ == '__main__': diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/update_kinematics.py b/isaac_ros_cumotion/isaac_ros_cumotion/update_kinematics.py new file mode 100644 index 0000000..8a27ef4 --- /dev/null +++ b/isaac_ros_cumotion/isaac_ros_cumotion/update_kinematics.py @@ -0,0 +1,271 @@ +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual +# property and proprietary rights in and to this material, related +# documentation and any modifications thereto. Any use, reproduction, +# disclosure or distribution of this material and related documentation +# without an express license agreement from NVIDIA CORPORATION or +# its affiliates is strictly prohibited. + +from os import path +import time +from typing import Dict, List, Optional + +from ament_index_python.packages import get_package_share_directory +from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel +from curobo.cuda_robot_model.util import load_robot_yaml +from curobo.types.base import TensorDeviceType +from curobo.types.file_path import ContentPath +from curobo.types.state import JointState as CuJointState +from curobo.util.logger import log_error +from curobo.util.xrdf_utils import return_value_if_exists +from curobo.util_file import get_robot_configs_path +from curobo.util_file import join_path +from curobo.util_file import load_yaml +from isaac_ros_cumotion.util import get_spheres_marker +from isaac_ros_cumotion_interfaces.action import UpdateLinkSpheres +import numpy as np +from rclpy.action import ActionServer, GoalResponse +from rclpy.action.server import ServerGoalHandle +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.impl.rcutils_logger import RcutilsLogger +from rclpy.node import Node +import torch +from visualization_msgs.msg import MarkerArray + + +def get_robot_config(robot_file: str, + urdf_file_path: str, + logger: RcutilsLogger, + object_link_name: Optional[str] = None) -> Dict: + + if robot_file.lower().endswith('.yml'): + logger.warn( + 'YML files will be deprecated soon. Switch to XRDF files for future compatibility.') + + # Construct the YAML file path and convert it to a dictionary. + robot_file_path = join_path(get_robot_configs_path(), robot_file) + robot_config = load_yaml(robot_file_path) + + # If a ROS param provides the URDF path, override the YAML file's path. + if urdf_file_path is not None: + robot_config['robot_cfg']['kinematics']['urdf_path'] = urdf_file_path + + elif robot_file.lower().endswith('.xrdf'): + + # If no URDF file path is provided, stop + if urdf_file_path is None: + logger.fatal('urdf_path is required to load robot from XRDF file') + raise SystemExit + + # Construct the XRDF file path and convert it to a dictionary. + xrdf_dir_path = path.join( + get_package_share_directory('isaac_ros_cumotion_robot_description'), 'xrdf') + xrdf_file_path = join_path(xrdf_dir_path, robot_file) + content_path = ContentPath(robot_xrdf_absolute_path=xrdf_file_path, + robot_urdf_absolute_path=urdf_file_path) + robot_config = load_robot_yaml(content_path) + + update_collision_sphere_buffer( + robot_yaml=robot_config, + link_name='attached_object', + num_spheres=100, + ) + + # Add the object link for forward kinematics if specified + if object_link_name is not None: + + robot_config = append_tool_frames( + robot_yaml=robot_config, + tool_frames=[object_link_name] + ) + + else: + logger.fatal('Invalid robot file; only XRDF or YML files accepted. Halting.') + raise SystemExit + + return robot_config + + +def update_collision_sphere_buffer( + robot_yaml: Dict, + link_name: str, + num_spheres: int, +) -> Dict: + + updt_sphere_dict = return_value_if_exists( + robot_yaml['robot_cfg']['kinematics'], + 'extra_collision_spheres', + raise_error=False + ) + + if updt_sphere_dict is None: + updt_sphere_dict = {link_name: num_spheres} + else: + updt_sphere_dict[link_name] = num_spheres + + robot_yaml['robot_cfg']['kinematics']['extra_collision_spheres'] = updt_sphere_dict + return robot_yaml + + +def append_tool_frames( + robot_yaml: Dict, + tool_frames: List[str] +) -> Dict: + + robot_dict = robot_yaml['robot_cfg']['kinematics'] + link_names = return_value_if_exists(robot_dict, 'link_names', raise_error=False) + if link_names is None: + link_names = [] + link_names.extend(tool_frames) + link_names = list(set(link_names)) # Ensure link names are unique + robot_dict['link_names'] = link_names + robot_yaml = {'robot_cfg': {'kinematics': robot_dict}} + return robot_yaml + + +class UpdateLinkSpheresServer: + + def __init__(self, server_node: Node, action_name: str, robot_kinematics: CudaRobotModel, + robot_base_frame: str) -> None: + + self.__server_node = server_node + self.__action_name = action_name + self.__robot_kinematics = robot_kinematics + self.__robot_base_frame = robot_base_frame + + # Initialize a mutually exclusive callback group + self.__callback_group = MutuallyExclusiveCallbackGroup() + + # Initialize the action server + self.__action_server = ActionServer(self.__server_node, + UpdateLinkSpheres, + f'{self.__action_name}', + goal_callback=self.goal_callback, + execute_callback=self.execute_callback, + callback_group=self.__callback_group) + + # Initialize the robot sphere publisher + debug_robot_topic = f'viz_all_spheres/{self.__action_name}' + self.__debug_robot_publisher = self.__server_node.create_publisher( + MarkerArray, debug_robot_topic, 10) + + def goal_callback(self, goal_request: UpdateLinkSpheres.Goal) -> GoalResponse: + + if not self.validate_goal(goal_request): + return GoalResponse.REJECT + + return GoalResponse.ACCEPT + + def validate_goal(self, goal_request: UpdateLinkSpheres.Goal) -> bool: + + if not isinstance(goal_request.attach_object, bool): + return False + + if not isinstance(goal_request.object_link_name, str): + return False + + flattened_sphere_arr = goal_request.flattened_sphere_arr + + # Check if all elements are floats + if not all(isinstance(item, float) for item in flattened_sphere_arr): + return False + + # Check if the number of elements is a multiple of 4 + if len(flattened_sphere_arr) % 4 != 0: + return False + + return True + + def execute_callback(self, goal_handle: ServerGoalHandle) -> UpdateLinkSpheres.Result: + + feedback_msg = UpdateLinkSpheres.Feedback() + result = UpdateLinkSpheres.Result() + + # Extract information from the goal sent by the action client + attach_object = goal_handle.request.attach_object + + try: + if attach_object: + + self.handle_attachment(goal_handle, feedback_msg) + + else: + self.handle_detachment(goal_handle, feedback_msg) + + goal_handle.succeed() + result.outcome = 'Link Sphere updated' + + except Exception as e: + + feedback_msg.status = f'Error occured while update link spheres: {e}' + goal_handle.publish_feedback(feedback_msg) + + goal_handle.abort() + result.outcome = 'Failed to update Link Sphere' + + return result + + def handle_attachment(self, goal_handle: ServerGoalHandle, + feedback_msg: UpdateLinkSpheres.Feedback) -> None: + + time_start = time.time() + + feedback_msg.status = f'Attaching object for {self.__action_name}...' + goal_handle.publish_feedback(feedback_msg) + + flattened_sphere_arr = np.array(goal_handle.request.flattened_sphere_arr) + + sphere_tensor = torch.tensor(flattened_sphere_arr, device='cuda:0', dtype=torch.float32) + + # Reshape the tensor to (N, 4), where N is the number of spheres + sphere_tensor = sphere_tensor.view(-1, 4) + + self.__robot_kinematics.kinematics_config.update_link_spheres( + link_name=goal_handle.request.object_link_name, + sphere_position_radius=sphere_tensor, + start_sph_idx=0, + ) + + time_total = time.time() - time_start + feedback_msg.status = f'Attached object for {self.__action_name} in {time_total}s' + goal_handle.publish_feedback(feedback_msg) + + def handle_detachment(self, goal_handle: ServerGoalHandle, + feedback_msg: UpdateLinkSpheres.Feedback) -> None: + self.__robot_kinematics.kinematics_config.detach_object( + link_name=goal_handle.request.object_link_name) + feedback_msg.status = f'Detached Attached object for {self.__action_name}' + goal_handle.publish_feedback(feedback_msg) + + def publish_all_active_spheres(self, + robot_joint_states: np.ndarray, + robot_joint_names: List[str], + tensor_args: TensorDeviceType, + rgb: List[float] = [0.0, 1.0, 0.0, 1.0]) -> None: + + if self.__debug_robot_publisher.get_subscription_count() == 0: + return + + q = CuJointState.from_numpy(position=robot_joint_states, + joint_names=robot_joint_names, + tensor_args=tensor_args).unsqueeze(0) + + active_jnames = self.__robot_kinematics.joint_names + q = q.get_ordered_joint_state(active_jnames) + + if len(q.position.shape) == 1: + log_error('q should be of shape [b, dof]') + kin_state = self.__robot_kinematics.get_state(q.position) + spheres = kin_state.link_spheres_tensor.cpu().numpy() + + current_time = self.__server_node.get_clock().now().to_msg() + + m_arr = get_spheres_marker( + spheres[0], + self.__robot_base_frame, + current_time, + rgb, + ) + + self.__debug_robot_publisher.publish(m_arr) diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/util.py b/isaac_ros_cumotion/isaac_ros_cumotion/util.py index 64a165f..c79d7d0 100644 --- a/isaac_ros_cumotion/isaac_ros_cumotion/util.py +++ b/isaac_ros_cumotion/isaac_ros_cumotion/util.py @@ -22,22 +22,25 @@ def get_spheres_marker( radius=robot_spheres[i, -1], pose=robot_spheres[i, :3].tolist() + [1, 0, 0, 0], ) - # print(rgb[i]) - m = get_marker_sphere(r_s, base_frame, time, start_idx + i, rgb) + delete_marker = not robot_spheres[i, -1] > 0.0 + m = get_marker_sphere(r_s, base_frame, time, start_idx + i, rgb, + delete_marker=delete_marker) m_arr.markers.append(m) return m_arr -def get_marker_sphere(sphere: Sphere, base_frame: str, time, idx=0, rgb=[0.4, 0.4, 0.8, 1.0]): +def get_marker_sphere(sphere: Sphere, base_frame: str, time, idx=0, rgb=[0.4, 0.4, 0.8, 1.0], + delete_marker: bool = False): + marker_type = Marker.ADD if not delete_marker else Marker.DELETE marker = Marker() marker.header.frame_id = base_frame marker.header.stamp = time marker.id = idx marker.type = Marker.SPHERE - marker.action = Marker.ADD - marker.scale.x = sphere.radius * 2 - marker.scale.y = sphere.radius * 2 - marker.scale.z = sphere.radius * 2 + marker.action = marker_type + marker.scale.x = sphere.radius * 2 if marker_type == Marker.ADD else 0.0 + marker.scale.y = sphere.radius * 2 if marker_type == Marker.ADD else 0.0 + marker.scale.z = sphere.radius * 2 if marker_type == Marker.ADD else 0.0 marker.color.r = rgb[0] marker.color.g = rgb[1] marker.color.b = rgb[2] diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/xrdf_utils.py b/isaac_ros_cumotion/isaac_ros_cumotion/xrdf_utils.py deleted file mode 100644 index 746ab46..0000000 --- a/isaac_ros_cumotion/isaac_ros_cumotion/xrdf_utils.py +++ /dev/null @@ -1,158 +0,0 @@ -# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual -# property and proprietary rights in and to this material, related -# documentation and any modifications thereto. Any use, reproduction, -# disclosure or distribution of this material and related documentation -# without an express license agreement from NVIDIA CORPORATION or -# its affiliates is strictly prohibited. - -from typing import Any, Dict, Union - -from curobo.cuda_robot_model.urdf_kinematics_parser import UrdfKinematicsParser -from curobo.util_file import load_yaml - - -def return_value_if_exists(input_dict: Dict, key: str, logger, suffix: str = 'xrdf') -> Any: - if key not in input_dict: - logger.error(key + ' key not found in ' + suffix) - raise ValueError(key + ' key not found in ' + suffix) - return input_dict[key] - - -def convert_xrdf_to_curobo(urdf_string: str, input_dict: Union[str, Dict], logger) -> Dict: - # urdf_string needs to be a path to a urdf file. - - if isinstance(input_dict, str): - input_dict = load_yaml(input_dict) - - if return_value_if_exists(input_dict, 'format', logger) != 'xrdf': - logger.error('format is not xrdf') - raise ValueError('format is not xrdf') - - if return_value_if_exists(input_dict, 'format_version', logger) > 1.0: - logger.warn('format_version is greater than 1.0') - # Also get base link as root of urdf - kinematics_parser = UrdfKinematicsParser(urdf_string, build_scene_graph=True) - joint_names = kinematics_parser.get_controlled_joint_names() - base_link = kinematics_parser.root_link - - output_dict = {} - if 'collision' in input_dict: - - coll_name = return_value_if_exists(input_dict['collision'], 'geometry', logger) - - if 'spheres' not in input_dict['geometry'][coll_name]: - logger.error('spheres key not found in xrdf') - raise ValueError('spheres key not found in xrdf') - coll_spheres = return_value_if_exists(input_dict['geometry'][coll_name], 'spheres', logger) - output_dict['collision_spheres'] = coll_spheres - - buffer_distance = return_value_if_exists( - input_dict['collision'], 'buffer_distance', logger - ) - output_dict['collision_sphere_buffer'] = buffer_distance - output_dict['collision_link_names'] = list(coll_spheres.keys()) - - if 'self_collision' in input_dict: - if input_dict['self_collision']['geometry'] != input_dict['collision']['geometry']: - logger.error('self_collision geometry does not match collision geometry') - raise ValueError('self_collision geometry does not match collision geometry') - - self_collision_ignore = return_value_if_exists( - input_dict['self_collision'], - 'ignore', - logger, - ) - - self_collision_buffer = return_value_if_exists( - input_dict['self_collision'], - 'buffer_distance', - logger, - ) - - output_dict['self_collision_ignore'] = self_collision_ignore - output_dict['self_collision_buffer'] = self_collision_buffer - else: - logger.error('self_collision key not found in xrdf') - raise ValueError('self_collision key not found in xrdf') - else: - logger.warn('collision key not found in xrdf, collision avoidance is disabled') - - tool_frames = return_value_if_exists(input_dict, 'tool_frames', logger) - output_dict['ee_link'] = tool_frames[0] - output_dict['link_names'] = None - if len(tool_frames) > 1: - output_dict['link_names'] = input_dict['tool_frames'] - - # cspace: - cspace_dict = return_value_if_exists(input_dict, 'cspace', logger) - - active_joints = return_value_if_exists(cspace_dict, 'joint_names', logger) - - default_joint_positions = return_value_if_exists(input_dict, 'default_joint_positions', logger) - active_config = [] - locked_joints = {} - - for j in joint_names: - if j in active_joints: - if j in default_joint_positions: - active_config.append(default_joint_positions[j]) - else: - active_config.append(0.0) - else: - locked_joints[j] = 0.0 - if j in default_joint_positions: - locked_joints[j] = default_joint_positions[j] - - acceleration_limits = return_value_if_exists(cspace_dict, 'acceleration_limits', logger) - jerk_limits = return_value_if_exists(cspace_dict, 'jerk_limits', logger) - - max_acc = max(acceleration_limits) - max_jerk = max(jerk_limits) - output_dict['lock_joints'] = locked_joints - all_joint_names = active_joints + list(locked_joints.keys()) - output_cspace = { - 'joint_names': all_joint_names, - 'retract_config': active_config + list(locked_joints.values()), - 'null_space_weight': [1.0 for _ in range(len(all_joint_names))], - 'cspace_distance_weight': [1.0 for _ in range(len(all_joint_names))], - 'max_acceleration': acceleration_limits - + [max_acc for _ in range(len(all_joint_names) - len(active_joints))], - 'max_jerk': jerk_limits - + [max_jerk for _ in range(len(all_joint_names) - len(active_joints))], - } - - output_dict['cspace'] = output_cspace - - extra_links = {} - if 'modifiers' in input_dict: - for k in range(len(input_dict['modifiers'])): - mod_list = list(input_dict['modifiers'][k].keys()) - if len(mod_list) > 1: - logger.error('Each modifier should have only one key') - raise ValueError('Each modifier should have only one key') - mod_type = mod_list[0] - if mod_type == 'set_base_frame': - base_link = input_dict['modifiers'][k]['set_base_frame'] - elif mod_type == 'add_frame': - frame_data = input_dict['modifiers'][k]['add_frame'] - extra_links[frame_data['frame_name']] = { - 'parent_link_name': frame_data['parent_frame_name'], - 'link_name': frame_data['frame_name'], - 'joint_name': frame_data['joint_name'], - 'joint_type': frame_data['joint_type'], - 'fixed_transform': frame_data['fixed_transform']['position'] - + [frame_data['fixed_transform']['orientation']['w']] - + frame_data['fixed_transform']['orientation']['xyz'], - } - else: - logger.warn('XRDF modifier "' + mod_type + '" not recognized') - output_dict['extra_links'] = extra_links - - output_dict['base_link'] = base_link - - output_dict['urdf_path'] = urdf_string - - output_dict = {'robot_cfg': {'kinematics': output_dict}} - return output_dict diff --git a/isaac_ros_cumotion/launch/isaac_ros_cumotion.launch.py b/isaac_ros_cumotion/launch/isaac_ros_cumotion.launch.py index efb60e1..c81969b 100644 --- a/isaac_ros_cumotion/launch/isaac_ros_cumotion.launch.py +++ b/isaac_ros_cumotion/launch/isaac_ros_cumotion.launch.py @@ -54,7 +54,7 @@ def generate_launch_description(): name='cumotion_planner', package='isaac_ros_cumotion', namespace='', - executable='cumotion_planner_node', + executable='cumotion_goal_set_planner_node', parameters=[ launch_configs ], diff --git a/isaac_ros_cumotion/package.xml b/isaac_ros_cumotion/package.xml index 881792c..38a7aad 100644 --- a/isaac_ros_cumotion/package.xml +++ b/isaac_ros_cumotion/package.xml @@ -2,7 +2,7 @@ isaac_ros_cumotion - 3.1.0 + 3.2.0 Package adds a cuMotion planner node Isaac ROS Maintainers NVIDIA Isaac ROS Software License @@ -18,7 +18,12 @@ trajectory_msgs visualization_msgs + isaac_ros_cumotion_interfaces + curobo_core + isaac_ros_common + isaac_ros_cumotion_interfaces + isaac_ros_cumotion_python_utils isaac_ros_cumotion_robot_description ros2launch diff --git a/isaac_ros_cumotion/params/isaac_ros_cumotion_params.yaml b/isaac_ros_cumotion/params/isaac_ros_cumotion_params.yaml index ed48c41..30ca15d 100644 --- a/isaac_ros_cumotion/params/isaac_ros_cumotion_params.yaml +++ b/isaac_ros_cumotion/params/isaac_ros_cumotion_params.yaml @@ -1,11 +1,19 @@ /**: ros__parameters: robot: "" + urdf_path: "" + yml_file_path: "" time_dilation_factor: 0.5 - collision_cache_mesh: 20 + max_attempts: 10 + num_graph_seeds: 6 + num_trajopt_seeds: 6 + include_trajopt_retract_seed: True + num_trajopt_time_steps: 32 + interpolation_dt: 0.025 collision_cache_cuboid: 20 - interpolation_dt: 0.02 - voxel_dims: [2.0, 2.0, 2.0] + collision_cache_mesh: 20 + workspace_file_path: '' + grid_size_m: [2.0, 2.0, 2.0] voxel_size: 0.05 read_esdf_world: False publish_curobo_world_as_voxels: False @@ -13,8 +21,8 @@ publish_voxel_size: 0.05 max_publish_voxels: 50000 tool_frame: "" - grid_position: [0.0, 0.0, 0.0] + grid_center_m: [0.0, 0.0, 0.0] esdf_service_name: "/nvblox_node/get_esdf_and_gradient" - urdf_path: "" enable_curobo_debug_mode: False - override_moveit_scaling_factors: False \ No newline at end of file + override_moveit_scaling_factors: False + update_link_sphere_server: 'planner_attach_object' \ No newline at end of file diff --git a/isaac_ros_cumotion/params/robot_segmentation_params.yaml b/isaac_ros_cumotion/params/robot_segmentation_params.yaml index 5e64019..f0f4b85 100644 --- a/isaac_ros_cumotion/params/robot_segmentation_params.yaml +++ b/isaac_ros_cumotion/params/robot_segmentation_params.yaml @@ -1,10 +1,18 @@ /**: ros__parameters: robot: ur5e.yml + urdf_path: "" + yml_file_path: "" + depth_qos: 'DEFAULT' + depth_info_qos: 'DEFAULT' + mask_qos: 'DEFAULT' + world_depth_qos: 'DEFAULT' depth_image_topics: [/depth_image] depth_camera_infos: [/rgb/camera_info] robot_mask_publish_topics: [/cumotion/camera_1/robot_mask] world_depth_publish_topics: [/cumotion/camera_1/world_depth] + filter_speckles_in_mask: False + max_filtered_speckles_size: 1250 joint_states_topic: /joint_states debug_robot_topic: /cumotion/robot_segmenter/robot_spheres distance_threshold: 0.2 @@ -12,4 +20,4 @@ tf_lookup_duration: 5.0 cuda_device: 0 log_debug: False - urdf_path: null + update_link_sphere_server: 'segmenter_attach_object' diff --git a/isaac_ros_cumotion/setup.py b/isaac_ros_cumotion/setup.py index f03c486..01f1e33 100644 --- a/isaac_ros_cumotion/setup.py +++ b/isaac_ros_cumotion/setup.py @@ -49,6 +49,7 @@ entry_points={ 'console_scripts': [ 'cumotion_planner_node = isaac_ros_cumotion.cumotion_planner:main', + 'cumotion_goal_set_planner_node = isaac_ros_cumotion.cumotion_goal_set_planner:main', 'robot_segmenter_node = isaac_ros_cumotion.robot_segmenter:main', ], }, diff --git a/isaac_ros_cumotion_examples/isaac_sim_scripts/start_isaac_sim_franka.py b/isaac_ros_cumotion_examples/isaac_sim_scripts/start_isaac_sim_franka.py index 817d83c..58c3be6 100644 --- a/isaac_ros_cumotion_examples/isaac_sim_scripts/start_isaac_sim_franka.py +++ b/isaac_ros_cumotion_examples/isaac_sim_scripts/start_isaac_sim_franka.py @@ -277,6 +277,7 @@ except Exception as e: print(e) +simulation_app.update() # Setting the /Franka target prim to Publish JointState node set_targets( diff --git a/isaac_ros_cumotion_examples/launch/franka.launch.py b/isaac_ros_cumotion_examples/launch/franka.launch.py index 769335f..8e36960 100644 --- a/isaac_ros_cumotion_examples/launch/franka.launch.py +++ b/isaac_ros_cumotion_examples/launch/franka.launch.py @@ -36,6 +36,7 @@ def augment_moveit_config(moveit_config): config = yaml.safe_load(config_file) moveit_config.planning_pipelines['planning_pipelines'].append('isaac_ros_cumotion') moveit_config.planning_pipelines['isaac_ros_cumotion'] = config + moveit_config.planning_pipelines['default_planning_pipeline'] = 'isaac_ros_cumotion' def generate_launch_description(): diff --git a/isaac_ros_cumotion_examples/launch/ur_isaac_sim.launch.py b/isaac_ros_cumotion_examples/launch/ur_isaac_sim.launch.py new file mode 100644 index 0000000..0ac6d1e --- /dev/null +++ b/isaac_ros_cumotion_examples/launch/ur_isaac_sim.launch.py @@ -0,0 +1,351 @@ +# SPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# This launch file was originally derived from +# https://github.com/ros-planning/moveit2_tutorials/blob/efef1d3/doc/how_to_guides/isaac_panda/launch/isaac_demo.launch.py # noqa +# +# BSD 3-Clause License +# +# Copyright (c) 2008-2013, Willow Garage, Inc. All rights reserved. +# Copyright (c) 2015-2023, PickNik, LLC. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + +""" +This launch file is a minimal representative example on interfacing a UR robot with Isaac Sim. + +It runs cuMotion planning calls using the MoveIt2 interface. +""" + + +import os +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.launch_context import LaunchContext +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder +import xacro +import yaml + + +def get_robot_description_contents( + ur_type: str, + dump_to_file: bool = False, + output_file: str = None, +) -> str: + """ + Get robot description contents and optionally dump content to file. + + Args + ---- + asset_name (str): The asset name for robot description + ur_type (str): UR Type + use_sim_time (bool): Use sim time for isaac sim platform + dump_to_file (bool, optional): Dumps xml to file. Defaults to False. + output_file (str, optional): Output file path if dumps output is True. Defaults to None. + + Returns + ------- + str: XMl contents of robot model + + """ + # Update the file extension and path as needed + urdf_xacro_file = os.path.join( + get_package_share_directory('isaac_ros_cumotion_examples'), + 'ur_config', + 'ur.urdf.xacro', + ) + + # Process the .xacro file to convert it to a URDF string + xacro_processed = xacro.process_file( + urdf_xacro_file, + mappings={ + 'ur_type': ur_type, + 'name': f'{ur_type}_robot', + 'use_fake_hardware': 'true', + 'generate_ros2_control_tag': 'false', + }, + ) + robot_description = xacro_processed.toxml() + + if dump_to_file and output_file: + with open(output_file, 'w') as file: + file.write(robot_description) + + return robot_description + + +def launch_setup(context: LaunchContext, *args, **kwargs) -> List[Node]: + """ + Launch the group of nodes together in 1 process. + + Args + ---- + context (LaunchContext): Context of launch file + + Returns + ------- + List[Node]: List of nodes + + """ + ur_type = str(context.perform_substitution(LaunchConfiguration('ur_type'))) + + moveit_config = ( + MoveItConfigsBuilder(ur_type, package_name='ur_moveit_config') + .robot_description( + file_path=os.path.join( + get_package_share_directory('isaac_ros_cumotion_examples'), + 'ur_config', + 'ur.urdf.xacro', + ), + mappings={'ur_type': ur_type}, + ) + .robot_description_semantic( + file_path='srdf/ur.srdf.xacro', mappings={'name': 'ur'} + ) + .robot_description_kinematics(file_path='config/kinematics.yaml') + .trajectory_execution(file_path='config/controllers.yaml') + .planning_pipelines(pipelines=['ompl']) + .to_moveit_configs() + ) + + # Add cuMotion to list of planning pipelines. + cumotion_config_file_path = os.path.join( + get_package_share_directory('isaac_ros_cumotion_moveit'), + 'config', + 'isaac_ros_cumotion_planning.yaml', + ) + with open(cumotion_config_file_path) as cumotion_config_file: + cumotion_config = yaml.safe_load(cumotion_config_file) + moveit_config.planning_pipelines['planning_pipelines'].insert( + 0, 'isaac_ros_cumotion' + ) + moveit_config.planning_pipelines['isaac_ros_cumotion'] = cumotion_config + moveit_config.planning_pipelines['default_planning_pipeline'] = 'isaac_ros_cumotion' + + # The workarounds below are based on the following ur_moveit_config and ur_description versions + # ur_moveit_config: github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/2.2.14 + # ur_description: github.com/UniversalRobots/Universal_Robots_ROS2_Description/tree/2.1.5 + + moveit_config.trajectory_execution = { + 'moveit_simple_controller_manager': moveit_config.trajectory_execution + } + del moveit_config.trajectory_execution['moveit_simple_controller_manager'][ + 'moveit_manage_controllers' + ] + moveit_config.trajectory_execution['moveit_manage_controllers'] = True + moveit_config.trajectory_execution['trajectory_execution'] = { + 'allowed_start_tolerance': 0.01 + } + moveit_config.trajectory_execution['moveit_controller_manager'] = ( + 'moveit_simple_controller_manager/MoveItSimpleControllerManager' + ) + + moveit_config.robot_description_kinematics['robot_description_kinematics'][ + 'ur_manipulator'] = moveit_config.robot_description_kinematics[ + 'robot_description_kinematics']['/**'][ + 'ros__parameters']['robot_description_kinematics']['ur_manipulator'] + del moveit_config.robot_description_kinematics['robot_description_kinematics']['/**'] + + moveit_config.joint_limits['robot_description_planning'] = xacro.load_yaml( + os.path.join( + get_package_share_directory( + 'ur_description'), 'config', ur_type, 'joint_limits.yaml', + ) + ) + moveit_config.moveit_cpp.update({'use_sim_time': True}) + + # Add limits from ur_moveit_config joint_limits.yaml to limits from ur_description + for joint in moveit_config.joint_limits['robot_description_planning']['joint_limits']: + moveit_config.joint_limits['robot_description_planning']['joint_limits'][joint][ + 'has_acceleration_limits'] = True + moveit_config.joint_limits['robot_description_planning']['joint_limits'][joint][ + 'max_acceleration'] = 5.0 + + # Start the actual move_group node/action server + move_group_node = Node( + package='moveit_ros_move_group', + executable='move_group', + output='screen', + parameters=[moveit_config.to_dict()], + arguments=['--ros-args', '--log-level', 'info'], + ) + + # RViz + rviz_config_file = os.path.join( + get_package_share_directory('isaac_ros_cumotion_examples'), + 'rviz', + 'ur_moveit_config.rviz', + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='log', + arguments=['-d', rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, + {'use_sim_time': True}, + ], + ) + + # Static TF + world2robot_tf_node = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_transform_publisher', + output='log', + arguments=['--frame-id', 'world', '--child-frame-id', 'base_link'], + parameters=[{'use_sim_time': True}], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory('isaac_ros_cumotion_examples'), + 'ur_config', + 'ros2_controllers.yaml', + ) + ros2_control_node = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[ros2_controllers_path, {'use_sim_time': True}], + remappings=[ + ('/controller_manager/robot_description', '/robot_description'), + ], + output='screen', + ) + + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'joint_state_broadcaster', + '--controller-manager', + '/controller_manager', + ], + ) + + scaled_joint_trajectory_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['scaled_joint_trajectory_controller', + '-c', '/controller_manager'], + ) + + joint_trajectory_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_trajectory_controller', '-c', '/controller_manager'], + ) + urdf_path = '/tmp/collated_ur_urdf.urdf' + # Define robot state publisher + robot_description = get_robot_description_contents( + ur_type=ur_type, + dump_to_file=True, + output_file=urdf_path, + ) + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[ + {'robot_description': robot_description, 'use_sim_time': True}], + remappings=[('/joint_states', '/isaac_joint_states')], + ) + + # Add cumotion planner node + cumotion_planner_node = Node( + name='cumotion_planner', + package='isaac_ros_cumotion', + namespace='', + executable='cumotion_planner_node', + parameters=[ + { + 'robot': 'ur10e.xrdf', + 'urdf_path': urdf_path + } + ], + output='screen', + ) + + return [ + rviz_node, + robot_state_publisher, + world2robot_tf_node, + move_group_node, + ros2_control_node, + joint_state_broadcaster_spawner, + scaled_joint_trajectory_controller_spawner, + joint_trajectory_controller_spawner, + cumotion_planner_node + ] + + +def generate_launch_description(): + launch_args = [ + DeclareLaunchArgument( + 'ur_type', + default_value='ur10e', + description='UR robot type', + choices=[ + 'ur3', + 'ur3e', + 'ur5', + 'ur5e', + 'ur10', + 'ur10e', + 'ur16e', + 'ur20', + 'ur30', + ], + ) + ] + + return LaunchDescription(launch_args + [OpaqueFunction(function=launch_setup)]) diff --git a/isaac_ros_cumotion_examples/package.xml b/isaac_ros_cumotion_examples/package.xml index 2c03b32..d5a36a4 100644 --- a/isaac_ros_cumotion_examples/package.xml +++ b/isaac_ros_cumotion_examples/package.xml @@ -2,10 +2,11 @@ isaac_ros_cumotion_examples - 3.1.0 + 3.2.0 Examples demonstrating Isaac ROS cuMotion with MoveIt Isaac ROS Maintainers Apache-2.0 + isaac_ros_common https://developer.nvidia.com/isaac-ros-gems/ isaac_ros_cumotion_moveit diff --git a/isaac_ros_cumotion_examples/rviz/ur_moveit_config.rviz b/isaac_ros_cumotion_examples/rviz/ur_moveit_config.rviz new file mode 100644 index 0000000..3ada80d --- /dev/null +++ b/isaac_ros_cumotion_examples/rviz/ur_moveit_config.rviz @@ -0,0 +1,508 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 360 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 100 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rviz_visual_tools + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ft_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ft_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ft_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: ur_manipulator + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ft_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 5.532290935516357 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5103983283042908 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.0253980159759521 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1280 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: true + Width: 1920 + X: 1920 + Y: 0 diff --git a/isaac_ros_cumotion_examples/setup.py b/isaac_ros_cumotion_examples/setup.py index 3ec9d2e..ab18afb 100644 --- a/isaac_ros_cumotion_examples/setup.py +++ b/isaac_ros_cumotion_examples/setup.py @@ -37,6 +37,10 @@ os.path.join('share', package_name, 'rviz'), glob(os.path.join('rviz', '*.rviz')), ), + ( + os.path.join('share', package_name, 'ur_config'), + glob(os.path.join('ur_config', '*.*')), + ), ], install_requires=['setuptools'], zip_safe=True, diff --git a/isaac_ros_cumotion_examples/ur_config/ros2_controllers.yaml b/isaac_ros_cumotion_examples/ur_config/ros2_controllers.yaml new file mode 100644 index 0000000..be279f4 --- /dev/null +++ b/isaac_ros_cumotion_examples/ur_config/ros2_controllers.yaml @@ -0,0 +1,44 @@ +# This config file is used by ros2_control +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + scaled_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + +scaled_joint_trajectory_controller: + ros__parameters: + command_interfaces: + - position + state_interfaces: + - position + - velocity + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + +joint_trajectory_controller: + ros__parameters: + command_interfaces: + - position + state_interfaces: + - position + - velocity + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint diff --git a/isaac_ros_cumotion_examples/ur_config/ur.ros2_control.xacro b/isaac_ros_cumotion_examples/ur_config/ur.ros2_control.xacro new file mode 100644 index 0000000..c99bc99 --- /dev/null +++ b/isaac_ros_cumotion_examples/ur_config/ur.ros2_control.xacro @@ -0,0 +1,102 @@ + + + + + + topic_based_ros2_control/TopicBasedSystem + /isaac_joint_commands + /isaac_joint_states + + + + {-2*pi} + {2*pi} + + + -3.15 + 3.15 + + + ${initial_positions['shoulder_pan_joint']} + + + + + + + {-2*pi} + {2*pi} + + + -3.15 + 3.15 + + + ${initial_positions['shoulder_lift_joint']} + + + + + + + {-pi} + {pi} + + + -3.15 + 3.15 + + + ${initial_positions['elbow_joint']} + + + + + + + {-2*pi} + {2*pi} + + + -3.2 + 3.2 + + + ${initial_positions['wrist_1_joint']} + + + + + + + {-2*pi} + {2*pi} + + + -3.2 + 3.2 + + + ${initial_positions['wrist_2_joint']} + + + + + + + {-2*pi} + {2*pi} + + + -3.2 + 3.2 + + + ${initial_positions['wrist_3_joint']} + + + + + + + diff --git a/isaac_ros_cumotion_examples/ur_config/ur.urdf.xacro b/isaac_ros_cumotion_examples/ur_config/ur.urdf.xacro new file mode 100644 index 0000000..146e04b --- /dev/null +++ b/isaac_ros_cumotion_examples/ur_config/ur.urdf.xacro @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/isaac_ros_cumotion_examples/ur_config/ur_sim.urdf.xacro b/isaac_ros_cumotion_examples/ur_config/ur_sim.urdf.xacro new file mode 100644 index 0000000..5ec2f8c --- /dev/null +++ b/isaac_ros_cumotion_examples/ur_config/ur_sim.urdf.xacro @@ -0,0 +1,126 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(arg simulation_controllers) + + + + + + + + + + + $(arg simulation_controllers) + $(arg tf_prefix)controller_manager + + + + + \ No newline at end of file diff --git a/isaac_ros_moveit_goal_setter/CMakeLists.txt b/isaac_ros_cumotion_interfaces/CMakeLists.txt similarity index 55% rename from isaac_ros_moveit_goal_setter/CMakeLists.txt rename to isaac_ros_cumotion_interfaces/CMakeLists.txt index 2dece6e..dd948ed 100644 --- a/isaac_ros_moveit_goal_setter/CMakeLists.txt +++ b/isaac_ros_cumotion_interfaces/CMakeLists.txt @@ -16,26 +16,44 @@ # SPDX-License-Identifier: Apache-2.0 cmake_minimum_required(VERSION 3.22.1) -project(isaac_ros_moveit_goal_setter) +project(isaac_ros_cumotion_interfaces LANGUAGES C CXX) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +# The FindPythonInterp and FindPythonLibs modules are removed +if(POLICY CMP0148) + cmake_policy(SET CMP0148 OLD) +endif() + find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_executable(${PROJECT_NAME} - src/goal_setter_node.cpp +# Prepare custom goal setter interfaces +find_package(rosidl_default_generators REQUIRED) + +set(ACTION_FILES +"action/AttachObject.action" +"action/MotionPlan.action" +"action/UpdateLinkSpheres.action" ) -install(PROGRAMS scripts/pose_to_pose.py DESTINATION lib/${PROJECT_NAME}) +rosidl_generate_interfaces(${PROJECT_NAME} + ${ACTION_FILES} + DEPENDENCIES geometry_msgs moveit_msgs sensor_msgs visualization_msgs +) +ament_export_dependencies(rosidl_default_runtime) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() -ament_auto_package(INSTALL_TO_SHARE launch) + +# Embed versioning information into installed files +ament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common) +include("${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake") +generate_version_info(${PROJECT_NAME}) + +ament_auto_package() diff --git a/isaac_ros_cumotion_interfaces/action/AttachObject.action b/isaac_ros_cumotion_interfaces/action/AttachObject.action new file mode 100644 index 0000000..ed3958f --- /dev/null +++ b/isaac_ros_cumotion_interfaces/action/AttachObject.action @@ -0,0 +1,17 @@ +# Use this action to trigger the attachment or detachment of an object in the robot's kinematic model. + +# Goal: +# Set to true to attach the object, false to detach it. +bool attach_object +# Configuration of object to be attached, including its shape (sphere, cuboid, mesh), pose and scale. +visualization_msgs/Marker object_config +# Fallback sphere radius for approximating the collision geometry if automatic generation fails. +float64 fallback_radius +--- +# Result: +# Description of the final result (e.g., success or failure). +string outcome +--- +# Feedback: +# Real-time status update during the action's execution. +string status \ No newline at end of file diff --git a/isaac_ros_cumotion_interfaces/action/MotionPlan.action b/isaac_ros_cumotion_interfaces/action/MotionPlan.action new file mode 100644 index 0000000..efa7bb9 --- /dev/null +++ b/isaac_ros_cumotion_interfaces/action/MotionPlan.action @@ -0,0 +1,35 @@ +geometry_msgs/PoseArray goal_pose +sensor_msgs/JointState goal_state +sensor_msgs/JointState start_state +moveit_msgs/PlanningSceneWorld world +float32 time_dilation_factor +float32[] hold_partial_pose_vec_weight +float32[] grasp_partial_pose_vec_weight +float32[] retract_partial_pose_vec_weight +string[] disable_collision_links +geometry_msgs/Pose grasp_offset_pose +geometry_msgs/Pose retract_offset_pose +bool update_esdf +bool clear_esdf +bool plan_cspace +bool plan_pose +bool visualize_trajectory +bool execute_trajectory +bool visualize_world +bool use_current_state +bool use_planning_scene +bool hold_partial_pose +bool plan_grasp +bool plan_approach_to_grasp +bool plan_grasp_to_retract +bool grasp_approach_constraint_in_goal_frame +bool retract_constraint_in_goal_frame +--- +bool success +string message +int32 goal_index +moveit_msgs/MoveItErrorCodes error_code +moveit_msgs/RobotTrajectory[] planned_trajectory +float32 planning_time +--- +string status diff --git a/isaac_ros_cumotion_interfaces/action/UpdateLinkSpheres.action b/isaac_ros_cumotion_interfaces/action/UpdateLinkSpheres.action new file mode 100644 index 0000000..48c48ac --- /dev/null +++ b/isaac_ros_cumotion_interfaces/action/UpdateLinkSpheres.action @@ -0,0 +1,17 @@ +# Use this action to update the robot's kinematic model by attaching an object and defining its collision geometry with spheres. + +# Goal: +# Set to true to attach the object, false to detach it. +bool attach_object +# Flattened array of spheres defining the collision geometry. Each sphere is defined by x, y, z coordinates and radius. +float32[] flattened_sphere_arr +# Name of the object's associated frame. +string object_link_name +--- +# Result: +# Description of the final result (e.g., success or failure). +string outcome +--- +# Feedback: +# Real-time status update during the action's execution. +string status \ No newline at end of file diff --git a/isaac_ros_cumotion_interfaces/package.xml b/isaac_ros_cumotion_interfaces/package.xml new file mode 100644 index 0000000..cb06416 --- /dev/null +++ b/isaac_ros_cumotion_interfaces/package.xml @@ -0,0 +1,52 @@ + + + + + + + isaac_ros_cumotion_interfaces + 3.2.0 + Interfaces for Isaac ROS Cumotion + Isaac ROS Maintainers + Apache-2.0 + https://developer.nvidia.com/isaac-ros-gems/ + Animesh Singhal + Swapnesh Wani + + geometry_msgs + moveit_msgs + sensor_msgs + visualization_msgs + + ament_cmake + + isaac_ros_common + rosidl_default_generators + + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/isaac_ros_cumotion_moveit/package.xml b/isaac_ros_cumotion_moveit/package.xml index 91146e3..59b8e3a 100644 --- a/isaac_ros_cumotion_moveit/package.xml +++ b/isaac_ros_cumotion_moveit/package.xml @@ -2,7 +2,7 @@ isaac_ros_cumotion_moveit - 3.1.0 + 3.2.0 Package adds a cuMotion planner plugin to MoveIt Isaac ROS Maintainers Apache-2.0 @@ -10,6 +10,7 @@ Balakumar Sundaralingam ament_cmake + isaac_ros_common isaac_ros_cumotion moveit_common diff --git a/isaac_ros_cumotion_object_attachment/isaac_ros_cumotion_object_attachment/__init__.py b/isaac_ros_cumotion_object_attachment/isaac_ros_cumotion_object_attachment/__init__.py new file mode 100644 index 0000000..34d9bfe --- /dev/null +++ b/isaac_ros_cumotion_object_attachment/isaac_ros_cumotion_object_attachment/__init__.py @@ -0,0 +1,16 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/isaac_ros_cumotion_object_attachment/isaac_ros_cumotion_object_attachment/attach_object_client.py b/isaac_ros_cumotion_object_attachment/isaac_ros_cumotion_object_attachment/attach_object_client.py new file mode 100644 index 0000000..28589cc --- /dev/null +++ b/isaac_ros_cumotion_object_attachment/isaac_ros_cumotion_object_attachment/attach_object_client.py @@ -0,0 +1,192 @@ +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual +# property and proprietary rights in and to this material, related +# documentation and any modifications thereto. Any use, reproduction, +# disclosure or distribution of this material and related documentation +# without an express license agreement from NVIDIA CORPORATION or +# its affiliates is strictly prohibited. + +from isaac_manipulator_ros_python_utils.launch_utils import ( + extract_pose_from_parameter, + extract_vector3_from_parameter, +) +from isaac_manipulator_ros_python_utils.types import ( + AttachState, Mode, ObjectAttachmentShape +) +from isaac_ros_cumotion_interfaces.action import AttachObject + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.task import Future +from visualization_msgs.msg import Marker + + +class AttachObjectClient(Node): + """ + Client node to manage object attachment and detachment via ROS 2 actions. + + This client can operate in ONCE or CYCLE mode. In ONCE mode, it sends a single + attach or detach goal. In CYCLE mode, it alternates between attaching and detaching + the object at regular intervals. + + mode (Mode): The mode of operation, either ONCE (executes the action once) + or CYCLE (continuously alternates between attach and detach). + attach_object (AttachState): The initial state, whether to attach or detach the object. + object_shape (ObjectAttachmentShape): Shape of the object (SPHERE, CUBOID, CUSTOM_MESH). + """ + + def __init__(self) -> None: + """Initialize the AttachObjectClient node.""" + super().__init__('attach_object_client') + self.get_logger().info('Started client...') + + self.declare_parameter('object_attachment_attach_object', + AttachState.ATTACH.value) + self.declare_parameter('object_attachment_mesh_resource', + '/workspaces/isaac_ros-dev/temp/nontextured.stl') + self.declare_parameter('object_attachment_mode', + Mode.ONCE.value) + self.declare_parameter('object_attachment_object_shape', + ObjectAttachmentShape.SPHERE.value) + self.declare_parameter('object_attachment_fallback_radius', 0.15) + + # translation: x, y, z and then orientation x, y, z, w + self.declare_parameter('object_attachment_object_pose', + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) + self.declare_parameter('object_attachment_object_scale', + [0.09, 0.185, 0.035]) + + self.attach_object = AttachState( + self.get_parameter('object_attachment_attach_object').value) + self.mesh_resource = self.get_parameter( + 'object_attachment_mesh_resource').get_parameter_value().string_value + self.mode = Mode(self.get_parameter( + 'object_attachment_mode').value) + self.object_shape = ObjectAttachmentShape( + self.get_parameter('object_attachment_object_shape').value) + self.fallback_radius = self.get_parameter( + 'object_attachment_fallback_radius').get_parameter_value().double_value + self._attach_object_client = ActionClient( + self, AttachObject, 'attach_object') + self.object_pose = extract_pose_from_parameter( + self.get_parameter('object_attachment_object_pose' + ).get_parameter_value().double_array_value) + self.object_scale = extract_vector3_from_parameter( + self.get_parameter('object_attachment_object_scale' + ).get_parameter_value().double_array_value) + + self._initialized = True + self.active_goal = False # Track if there's an active goal + self.goal_handle = None # Handle to manage the active goal + self.goal_sent = False # Track if the goal has been sent (for 'once' mode) + self.cycle_counter = 0 # Counter for the 'cycle' mode + + # Set timer for goal sending + self.timer = self.create_timer( + 4.00, self.send_goal_callback + ) + + def create_marker(self) -> Marker: + """ + Create a Marker based on the object type and other parameters. + + Returns + ------- + Marker: The marker object that defines attachment process. + + """ + marker = Marker() + marker.pose = self.object_pose + marker.scale = self.object_scale + + # Handle different object types + if self.object_shape == ObjectAttachmentShape.SPHERE: + marker.type = Marker.SPHERE + elif self.object_shape == ObjectAttachmentShape.CUBOID: + marker.type = Marker.CUBE + elif self.object_shape == ObjectAttachmentShape.CUSTOM_MESH: + marker.type = Marker.MESH_RESOURCE + marker.mesh_resource = self.mesh_resource + else: + self.get_logger().error('Unknown object type') + + return marker + + def send_goal_callback(self) -> None: + """ + Send a goal based on the current mode. + + In ONCE mode, sends a goal if it hasn't been sent already. In CYCLE mode, + alternates between attach and detach states. + """ + if self.mode == Mode.ONCE and not self.goal_sent: + self.send_goal() + self.goal_sent = True + elif self.mode == Mode.CYCLE: + if not self.active_goal: + self.attach_object = ( + AttachState.ATTACH if self.cycle_counter % 2 == 0 else AttachState.DETACH + ) + self.send_goal() + self.cycle_counter += 1 # Increment the counter + + def send_goal(self) -> None: + """ + Construct and send a goal to attach or detach the object. + + Waits for the action server, then sends the goal with a feedback callback. + """ + goal_msg = AttachObject.Goal() + goal_msg.attach_object = self.attach_object.value + goal_msg.fallback_radius = self.fallback_radius + goal_msg.object_config = self.create_marker() + self._attach_object_client.wait_for_server() + self.get_logger().info('Sending new action call...') + self._send_goal_future = self._attach_object_client.send_goal_async( + goal_msg, feedback_callback=self.feedback_callback) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future: Future) -> None: + """ + Handle the response from the action server when a goal is sent. + + If the goal is accepted, track it and set up the result callback. + """ + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().error('Goal Rejected') + return + self.goal_handle = goal_handle # Store the goal handle + self.active_goal = True # Mark the goal as active + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def feedback_callback(self, feedback_msg: AttachObject.Feedback) -> None: + """ + Handle feedback from the action server. + + Logs the status of the ongoing goal. + """ + self.get_logger().info(f'Feedback: {feedback_msg.feedback.status}') + + def get_result_callback(self, future: Future) -> None: + """ + Handle the result from the action server. + + Logs the final outcome of the action and marks the goal as complete. + """ + result = future.result().result + self.get_logger().info(f'Final Outcome: {result.outcome}') + self.active_goal = False # Mark the goal as complete + + +def main(args=None) -> None: + rclpy.init(args=args) + attach_object_client = AttachObjectClient() + rclpy.spin(attach_object_client) + + +if __name__ == '__main__': + main() diff --git a/isaac_ros_cumotion_object_attachment/isaac_ros_cumotion_object_attachment/attach_object_server.py b/isaac_ros_cumotion_object_attachment/isaac_ros_cumotion_object_attachment/attach_object_server.py new file mode 100644 index 0000000..145d527 --- /dev/null +++ b/isaac_ros_cumotion_object_attachment/isaac_ros_cumotion_object_attachment/attach_object_server.py @@ -0,0 +1,2406 @@ +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual +# property and proprietary rights in and to this material, related +# documentation and any modifications thereto. Any use, reproduction, +# disclosure or distribution of this material and related documentation +# without an express license agreement from NVIDIA CORPORATION or +# its affiliates is strictly prohibited. + +from copy import deepcopy +from enum import Enum +import os +import struct +import threading +import time +import traceback +from typing import Dict, List, Tuple, Union + +from action_msgs.msg import GoalStatus +import cupy as cp +from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel +from curobo.geom.types import Cuboid as CuCuboid +from curobo.geom.types import Mesh as CuMesh +from curobo.geom.types import Obstacle as CuObstacle +from curobo.types.base import TensorDeviceType +from curobo.types.camera import CameraObservation +from curobo.types.math import Pose as CuPose +from curobo.types.robot import RobotConfig +from curobo.types.state import JointState as CuJointState +from cv_bridge import CvBridge +from geometry_msgs.msg import Point, PointStamped +from geometry_msgs.msg import Pose, Vector3 +from hdbscan import HDBSCAN as cpu_HDBSCAN +from isaac_ros_common.qos import add_qos_parameter +from isaac_ros_cumotion.update_kinematics import get_robot_config +from isaac_ros_cumotion.util import get_spheres_marker +from isaac_ros_cumotion_interfaces.action import AttachObject +from isaac_ros_cumotion_interfaces.action import UpdateLinkSpheres +from message_filters import ApproximateTimeSynchronizer, Subscriber +import numpy as np +from nvblox_msgs.srv import EsdfAndGradients +import rclpy +from rclpy.action import ActionClient, ActionServer, GoalResponse +from rclpy.action.server import ServerGoalHandle +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from rclpy.publisher import Publisher +from rclpy.task import Future +from scipy.spatial.transform import Rotation as R +from sensor_msgs.msg import (CameraInfo, Image, JointState, PointCloud2, + PointField) +from sensor_msgs_py import point_cloud2 +from std_msgs.msg import Header +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +import torch +import trimesh +from visualization_msgs.msg import Marker, MarkerArray + + +torch.backends.cuda.matmul.allow_tf32 = True +torch.backends.cudnn.allow_tf32 = True + + +class ObjectState(Enum): + """ + Enum representing the state of an object. + + Values: + ATTACHED (str): The object is currently attached. + DETACHED (str): The object is currently detached. + ATTACHED_FALLBACK (str): The object has been approximated by a fallback sphere. + """ + + ATTACHED = 'attached' + DETACHED = 'detached' + ATTACHED_FALLBACK = 'attached (fallback)' + + +class AttachObjectServer(Node): + """ + Node that handles attaching or detaching an object based on a given action goal. + + It processes a depth image, with the robot segmented out, and the joint state. The goal is to + extract the object's point cloud, approximate it with a mesh, and generate object spheres using + forward kinematics. These spheres are then sent to other nodes to update their robot model. + """ + + def __init__(self): + + # Constructor of the parent class + super().__init__('object_attachment_node') + + # Initialize parameters + + self.declare_parameter('robot', 'ur5e_robotiq_2f_140.xrdf') + self.declare_parameter('urdf_path', rclpy.Parameter.Type.STRING) + + self.declare_parameter('cuda_device', 0) + self.declare_parameter('time_sync_slop', 0.1) + self.declare_parameter('filter_depth_buffer_time', 0.1) + self.declare_parameter('tf_lookup_duration', 5.0) + self.declare_parameter('joint_states_topic', '/joint_states') + self.declare_parameter('depth_image_topics', [ + '/cumotion/camera_1/world_depth']) + self.declare_parameter('depth_camera_infos', [ + '/camera_1/aligned_depth_to_color/camera_info']) + self.declare_parameter('object_link_name', 'attached_object') + self.declare_parameter('object_attachment_gripper_frame_name', 'grasp_frame') + self.declare_parameter( + 'action_names', rclpy.Parameter.Type.STRING_ARRAY) + self.declare_parameter('search_radius', 0.2) + self.declare_parameter('surface_sphere_radius', 0.01) + + # Declare clustering parameters + self.declare_parameter('clustering_bypass_clustering', False) + self.declare_parameter('clustering_hdbscan_min_samples', 20) + self.declare_parameter('clustering_hdbscan_min_cluster_size', 30) + self.declare_parameter( + 'clustering_hdbscan_cluster_selection_epsilon', 0.5) + self.declare_parameter('clustering_num_top_clusters_to_select', 3) + self.declare_parameter('clustering_group_clusters', False) + self.declare_parameter('clustering_min_points', 100) + + # Get frame information from URDF that object attachment adds the spheres w.r.t to + self.declare_parameter('object_attachment_grasp_frame_name', 'grasp_frame') + # Number of spheres to add for CUBOID or MESH approach + self.declare_parameter('object_attachment_n_spheres', 100) + + # Nvblox parameters for object clearing + # update_esdf_on_request determines if the esdf should be updated upon service request + self.declare_parameter('update_esdf_on_request', True) + self.declare_parameter('trigger_aabb_object_clearing', False) + + # object_esdf_clearing_padding adds padding to the dims of the AABB / radius of the sphere + self.declare_parameter('object_esdf_clearing_padding', [0.05, 0.05, 0.05]) + + # QOS settings + depth_qos = add_qos_parameter(self, 'DEFAULT', 'depth_qos') + depth_info_qos = add_qos_parameter(self, 'DEFAULT', 'depth_info_qos') + + # Retrieve parameters + self.__robot_file = self.get_parameter( + 'robot').get_parameter_value().string_value + + try: + self.__urdf_path = self.get_parameter('urdf_path') + self.__urdf_path = self.__urdf_path.get_parameter_value().string_value + if self.__urdf_path == '': + self.__urdf_path = None + except rclpy.exceptions.ParameterUninitializedException: + self.__urdf_path = None + + self.__cuda_device_id = self.get_parameter( + 'cuda_device').get_parameter_value().integer_value + self.__time_sync_slop = self.get_parameter( + 'time_sync_slop').get_parameter_value().double_value + self.__filter_depth_buffer_time = self.get_parameter( + 'filter_depth_buffer_time').get_parameter_value().double_value + self.__tf_lookup_duration = self.get_parameter( + 'tf_lookup_duration').get_parameter_value().double_value + self.__joint_states_topic = self.get_parameter( + 'joint_states_topic').get_parameter_value().string_value + self.__depth_image_topics = self.get_parameter( + 'depth_image_topics').get_parameter_value().string_array_value + self.__depth_camera_infos = self.get_parameter( + 'depth_camera_infos').get_parameter_value().string_array_value + self.__object_link_name = self.get_parameter( + 'object_link_name').get_parameter_value().string_value + self.__gripper_frame_name = self.get_parameter( + 'object_attachment_gripper_frame_name').get_parameter_value().string_value + self.__action_names = list(self.get_parameter( + 'action_names').get_parameter_value().string_array_value) + self.__search_radius = self.get_parameter( + 'search_radius').get_parameter_value().double_value + self.__surface_sphere_radius = self.get_parameter( + 'surface_sphere_radius').get_parameter_value().double_value + self.__object_attachment_n_spheres = self.get_parameter( + 'object_attachment_n_spheres').get_parameter_value().integer_value + + # Retrieve clustering parameters + self.__bypass_clustering = self.get_parameter( + 'clustering_bypass_clustering').get_parameter_value().bool_value + self.__hdbscan_min_samples = self.get_parameter( + 'clustering_hdbscan_min_samples').get_parameter_value().integer_value + self.__hdbscan_min_cluster_size = self.get_parameter( + 'clustering_hdbscan_min_cluster_size').get_parameter_value().integer_value + self.__hdbscan_cluster_selection_epsilon = self.get_parameter( + 'clustering_hdbscan_cluster_selection_epsilon').get_parameter_value().double_value + self.__num_top_clusters_to_select = self.get_parameter( + 'clustering_num_top_clusters_to_select').get_parameter_value().integer_value + self.__group_clusters = self.get_parameter( + 'clustering_group_clusters').get_parameter_value().bool_value + self.__min_points = self.get_parameter( + 'clustering_min_points').get_parameter_value().integer_value + + # Nvblox object clearing parameters + self.__trigger_aabb_object_clearing = ( + self.get_parameter('trigger_aabb_object_clearing').get_parameter_value().bool_value + ) + self.__update_esdf_on_request = ( + self.get_parameter('update_esdf_on_request').get_parameter_value().bool_value + ) + self._object_esdf_clearing_padding = np.asarray(self.get_parameter( + 'object_esdf_clearing_padding').get_parameter_value().double_array_value) + + # Validate topic lengths + self.__num_cameras = len(self.__depth_image_topics) + if len(self.__depth_camera_infos) != self.__num_cameras: + self.get_logger().error( + 'Number of topics in depth_camera_infos does not match depth_image_topics') + + self.__tensor_args = TensorDeviceType( + device=torch.device('cuda', self.__cuda_device_id)) + + # Create subscribers for depth image and robot joint state: + subscribers = [Subscriber(self, Image, topic, qos_profile=depth_qos) + for topic in self.__depth_image_topics] + subscribers.append(Subscriber( + self, JointState, self.__joint_states_topic)) + + # Subscribe to topics with sync: + self.__approx_time_sync = ApproximateTimeSynchronizer( + tuple(subscribers), queue_size=100, slop=self.__time_sync_slop) + self.__approx_time_sync.registerCallback( + self.process_depth_and_joint_state) + + # Create subscriber for camera info + self.__info_subscribers = [] + for idx in range(self.__num_cameras): + self.__info_subscribers.append( + self.create_subscription( + CameraInfo, self.__depth_camera_infos[idx], + lambda msg, index=idx: self.camera_info_cb(msg, index), depth_info_qos) + ) + + # Create publishers: + self.__object_origin_publisher = self.create_publisher( + PointStamped, 'object_origin_frame', 10) + self.__nearby_points_publisher = self.create_publisher( + PointCloud2, 'nearby_point_cloud', 100) + self.__clustered_points_publisher = self.create_publisher( + PointCloud2, 'clustered_point_cloud', 100) + self.__object_cloud_publisher = self.create_publisher( + PointCloud2, 'object_point_cloud', 100) + self.__robot_sphere_markers_publisher = self.create_publisher( + MarkerArray, 'robot_sphere_markers', 10) + + self.__tf_buffer = Buffer( + cache_time=rclpy.duration.Duration(seconds=60.0)) + self.__tf_listener = TransformListener(self.__tf_buffer, self) + self.__br = CvBridge() + + # Create buffers to store data: + self.__depth_buffers = None + self.__depth_intrinsics = [None for x in range(self.__num_cameras)] + self.__robot_pose_camera = [None for x in range(self.__num_cameras)] + self.__depth_encoding = None + + self.__js_buffer = None + self.__timestamp = None + self.__camera_headers = [] + self.__lock = threading.Lock() + + self.__robot_pose_cameras = None + self.__object_spheres = None + self.__object_mesh = None + + robot_config = get_robot_config( + robot_file=self.__robot_file, + urdf_file_path=self.__urdf_path, + logger=self.get_logger(), + object_link_name=self.__object_link_name, + ) + + # Extracting robot's base link name + self.__cfg_base_link = robot_config['robot_cfg']['kinematics']['base_link'] + + # Creating an instance of robot kinematics using config file: + robot_cfg = RobotConfig.from_dict( + robot_config, self.__tensor_args) + self.__kin_model = CudaRobotModel(robot_cfg.kinematics) + + # Maintain the state of the object attachment + self.__object_state = ObjectState.DETACHED + + # Defining action server + action_server_cb_grp = MutuallyExclusiveCallbackGroup() + self.__action_server = ActionServer( + self, + AttachObject, + 'attach_object', + self.attach_object_server_execute_callback, + goal_callback=self.attach_object_server_goal_callback, + callback_group=action_server_cb_grp) + + self.__cpu_hdbscan = cpu_HDBSCAN( + min_samples=self.__hdbscan_min_samples, + min_cluster_size=self.__hdbscan_min_cluster_size, + cluster_selection_epsilon=self.__hdbscan_cluster_selection_epsilon + ) + + self.__att_obj_srv_fb_msg = AttachObject.Feedback() + self.__att_obj_result = AttachObject.Result() + + # Dictionary to store action clients and their callback groups + self.__action_clients = {} + self.__callback_groups = {} + for action_name in self.__action_names: + callback_group = MutuallyExclusiveCallbackGroup() + self.__callback_groups[action_name] = callback_group + self.__action_clients[action_name] = ActionClient( + self, + UpdateLinkSpheres, + action_name, + callback_group=callback_group + ) + + # Create esdf action client + if self.__trigger_aabb_object_clearing: + esdf_service_name = 'nvblox_node/get_esdf_and_gradient' + esdf_service_cb_group = MutuallyExclusiveCallbackGroup() + self.__esdf_client = self.create_client( + EsdfAndGradients, esdf_service_name, callback_group=esdf_service_cb_group + ) + while not self.__esdf_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info( + f'Service({esdf_service_name}) not available, waiting again...' + ) + self.__esdf_req = EsdfAndGradients.Request() + + self.get_logger().info( + f'Node initialized with {self.__num_cameras} cameras') + + def attach_object_server_goal_callback( + self, + attachment_goal: AttachObject.Goal + ) -> GoalResponse: + """ + Validate the incoming goal to attach or detach an object. + + Ensure goal data types match the action definition. Reject if the object is already in + the requested state (attached or detached). Accept the goal if valid, otherwise reject. + """ + if not self.validate_goal(attachment_goal): + return GoalResponse.REJECT + + if not self.check_object_state(attachment_goal.attach_object): + return GoalResponse.REJECT + + return GoalResponse.ACCEPT + + def validate_goal( + self, + attachment_goal: AttachObject.Goal + ) -> bool: + """ + Validate if the goal type matches the action definition. + + Args + ---- + attachment_goal (AttachObject.Goal): The goal sent by the action client. + + Returns + ------- + bool: True if the goal is valid; otherwise, False. + + """ + # Validate the 'attach_object' field + if not isinstance(attachment_goal.attach_object, bool): + self.get_logger().error( + 'Value of attach_object sent over action call is invalid. Expecting bool value.' + ) + return False + + # Validate the 'fallback_radius' field + if not isinstance(attachment_goal.fallback_radius, float): + self.get_logger().error( + 'Value of fallback_radius sent over action call is invalid. Expecting float value.' + ) + return False + + # Validate the 'object_config' field + if not isinstance(attachment_goal.object_config, Marker): + self.get_logger().error( + 'Object configuration is invalid. Expecting Marker type.' + ) + return False + + # Validate the shape of the object (SPHERE, CUBE, MESH_RESOURCE) + if attachment_goal.object_config.type not in [ + Marker.SPHERE, Marker.CUBE, Marker.MESH_RESOURCE]: + self.get_logger().error( + 'Object type is invalid. Expected one of SPHERE, CUBE, or MESH_RESOURCE.' + ) + return False + + # For CUBE and MESH_RESOURCE types, validate pose and scale + if attachment_goal.object_config.type in [Marker.CUBE, Marker.MESH_RESOURCE]: + # Validate the object pose (Pose type) + if not isinstance(attachment_goal.object_config.pose, Pose): + self.get_logger().error( + 'Object pose is invalid. Expecting Pose type for CUBE or MESH_RESOURCE.' + ) + return False + + # Validate the object scale (Vector3 type) + if not isinstance(attachment_goal.object_config.scale, Vector3): + self.get_logger().error( + 'Object scale is invalid. Expecting Vector3 type for CUBE or MESH_RESOURCE.' + ) + return False + + # Additional validation for custom meshes: check if mesh_resource is set and is a string + if attachment_goal.object_config.type == Marker.MESH_RESOURCE: + if not isinstance(attachment_goal.object_config.mesh_resource, str): + self.get_logger().error( + 'Mesh resource is invalid. Expecting a string value for MESH_RESOURCE.' + ) + return False + if not attachment_goal.object_config.mesh_resource: + self.get_logger().error( + 'Mesh resource is missing for custom mesh object.' + ) + return False + if not os.path.isfile(attachment_goal.object_config.mesh_resource): + self.get_logger().error(f'Mesh resource file does not exist: ' + f'{attachment_goal.object_config.mesh_resource}') + return False + + # All validations passed + return True + + def check_object_state( + self, + attach_object: bool + ) -> bool: + """ + Prevent redundant attachment or detachment. + + Args + ---- + attach_object (bool): True to attach, False to detach. + + Returns + ------- + bool: True if the operation can proceed; otherwise, False. + + """ + if self.__object_state == ObjectState.DETACHED: + if attach_object: + return True + else: + self.get_logger().error( + 'Goal to detach object received, but the object is already detached.') + return False + + elif self.__object_state in [ObjectState.ATTACHED, ObjectState.ATTACHED_FALLBACK]: + if attach_object: + self.get_logger().error( + 'Detach the current object before attempting to attach the new one.') + return False + else: + return True + + else: + self.get_logger().error( + f'Unexpected object state: {self.__object_state}') + return False + + def attach_object_server_execute_callback( + self, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> AttachObject.Result: + """ + On goal acceptance, attach or detach the object as commanded. + + If attaching: Obtain object point cloud, generate spheres, and send to other nodes. + If this fails, use a fallback sphere. Report success or failure. + + If detaching: Clear sphere buffers linked to the object and notify other nodes. + Report success or abort based on the outcome. + + Args + ---- + att_obj_srv_goal_handle (ServerGoalHandle): The goal handle that manages the goal. + + Returns + ------- + AttachObject.Result: The result indicating success or failure of the operation. + + """ + # Check whether to attach or detach object from goal + attach_object = att_obj_srv_goal_handle.request.attach_object + + # Check whether to attach or detach object from goal + self.__attached_object_config = att_obj_srv_goal_handle.request.object_config + + if attach_object: + self.__att_obj_srv_fb_msg.status = 'Executing action call to attach object' + else: + self.__att_obj_srv_fb_msg.status = 'Executing action call to detach object' + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + # Initialize timing variables + start_time = time.time() + + try: + if attach_object: + self.handle_attachment(att_obj_srv_goal_handle) + if self.__object_state != ObjectState.ATTACHED: + raise RuntimeError('Failed to attach the object.') + self.__att_obj_result.outcome = 'Object attached' + else: + self.handle_detachment(att_obj_srv_goal_handle) + if self.__object_state != ObjectState.DETACHED: + raise RuntimeError('Failed to detach the object.') + self.__att_obj_result.outcome = 'Object Detached' + + att_obj_srv_goal_handle.succeed() + + except Exception as e: + + self.__att_obj_srv_fb_msg.status = f'Error during goal execution: {str(e)}' + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + # Handling fallback if attachment fails + if attach_object: + self.__att_obj_srv_fb_msg.status = ( + 'Attempting to attach sphere of radius=fallback_radius' + ) + att_obj_srv_goal_handle.publish_feedback( + self.__att_obj_srv_fb_msg) + self.handle_fallback( + att_obj_srv_goal_handle) + if self.__object_state == ObjectState.ATTACHED_FALLBACK: + self.__att_obj_result.outcome = 'Fallback object attached' + att_obj_srv_goal_handle.succeed() + else: + self.__att_obj_result.outcome = 'Fallback object attachment also failed!' + att_obj_srv_goal_handle.abort() + else: + self.__att_obj_result.outcome = 'Detachment failed' + att_obj_srv_goal_handle.abort() + + finally: + # Calculate total execution time + self.__att_obj_srv_fb_msg.status = ( + f'Total node time: {time.time() - start_time}') + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + return self.__att_obj_result + + def handle_attachment( + self, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> None: + """ + Attempt to attach the object. + + If depth image or joint state data is unavailable, retry a few times. If still unavailable, + exit without updating the object state to attached. + + If data is available, generate object spheres from the point cloud. On error, keep the + object detached. If successful, attempt to sync link spheres with other nodes. + If sync succeeds, update the object state to attached. + """ + max_retries = 5 # Maximum number of retries + retries = 0 + success = False # Initialize success before the loop + + # Keep trying to attach in case of data unavailability + while retries < max_retries: + + if retries > 0: + self.__att_obj_srv_fb_msg.status = ( + f'Attempt #{retries + 1} to attach object...') + att_obj_srv_goal_handle.publish_feedback( + self.__att_obj_srv_fb_msg) + + success, error = self.attach_object(att_obj_srv_goal_handle) + + if success and self.__object_spheres is not None: + break # Exit loop if object attachment successfull + elif error is not None: + self.__att_obj_srv_fb_msg.status = ( + f'Attachment failed due to an error: {error}') + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + break # Exit loop if error in sphere generation + else: + self.__att_obj_srv_fb_msg.status = ( + 'Subscriber(s) didnt recieve data on topics (cam, joint, tf)...') + att_obj_srv_goal_handle.publish_feedback( + self.__att_obj_srv_fb_msg) + retries += 1 + time.sleep(0.5) # Wait 0.5s before retrying + + if not success: # Only check if success is False, which covers both cases + self.__att_obj_srv_fb_msg.status = ( + 'Attachment failed after maximum retries or due to a critical error.') + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + else: + sync_success = self.sync_object_link_spheres_across_nodes( + att_obj_srv_goal_handle.request.attach_object, att_obj_srv_goal_handle) + if sync_success: + self.__object_state = ObjectState.ATTACHED + self.update_esdf_voxel_grid_for_aabb_clearing() + + def update_esdf_voxel_grid_for_aabb_clearing(self) -> None: + """ + Update the ESDF voxel by computing the AABBs to clear. + + It sends a request to clear them. + """ + if not self.__trigger_aabb_object_clearing: + return + + self.get_logger().info('Calling ESDF service') + + objects_to_clear = self.calculate_aabbs_and_spheres_to_clear() + aabbs_to_clear_min_m = objects_to_clear[0] + aabbs_to_clear_size_m = objects_to_clear[1] + spheres_to_clear_center_m = objects_to_clear[2] + spheres_to_clear_radius_m = objects_to_clear[3] + + esdf_future = self.send_esdf_request( + aabbs_to_clear_min_m, aabbs_to_clear_size_m, + spheres_to_clear_center_m, spheres_to_clear_radius_m) + while not esdf_future.done(): + time.sleep(0.001) + response = esdf_future.result() + if not response.success: + self.get_logger().error('ESDF request failed! Not clearing object.') + + def calculate_aabbs_and_spheres_to_clear(self) -> List[List]: + """ + Compute the AABBs to clear by getting the local object frame to world transform. + + Then calculating the vertices of the object. Finally, it transforms the vertices into + world frame and computes the AABB with padding. If fallback was reached, it instead + clears the fallback sphere with padding. + """ + world_T_object = self.calculate_world_T_object() + if world_T_object is None: + return [], [], [], [] + + # Handle fallback case; simply clear the radius inside the fallback sphere + if self.__object_state == ObjectState.ATTACHED_FALLBACK: + centre = self.__object_spheres[:3] + homogeneous_centre = np.append(centre, 1) + world_centre = np.matmul(world_T_object, homogeneous_centre)[:3] + radius = self.__object_spheres[3] + self._object_esdf_clearing_padding[0] + world_centre = [Point(x=world_centre[0], y=world_centre[1], z=world_centre[2])] + return [], [], world_centre, [radius] + + # Calculate the vertices in world frame depending on the marker type + if self.__attached_object_config.type == Marker.CUBE: + vertices = self.calculate_cuboid_vertices() + homogeneous_vertices = np.hstack([vertices, np.ones((vertices.shape[0], 1))]) + world_vertices = np.matmul(world_T_object, homogeneous_vertices.T).T[:, :3] + elif self.__attached_object_config.type == Marker.MESH_RESOURCE: + mesh = trimesh.load_mesh(self.__attached_object_config.mesh_resource) + world_vertices = trimesh.transform_points(mesh.vertices, world_T_object) + elif self.__attached_object_config.type == Marker.SPHERE: + mesh = self.__object_mesh.get_trimesh_mesh() + world_vertices = mesh.vertices + + min_corner = np.min(world_vertices, axis=0) - self._object_esdf_clearing_padding / 2 + max_corner = np.max(world_vertices, axis=0) + self._object_esdf_clearing_padding / 2 + + aabb_size = np.abs(max_corner - min_corner).tolist() + min_corner = min_corner.tolist() + aabb_min = [Point(x=min_corner[0], y=min_corner[1], z=min_corner[2])] + aabb_size = [Vector3(x=aabb_size[0], y=aabb_size[1], z=aabb_size[2])] + return aabb_min, aabb_size, [], [] + + def calculate_world_T_object(self) -> np.ndarray | None: + """Compute the transform to go from local object frame to world frame.""" + grasp_pose_object = self.__attached_object_config.pose + try: + world_pose_grasp = self.__tf_buffer.lookup_transform( + 'world', + 'grasp_frame', + rclpy.time.Time() + ) + except Exception as ex: + self.get_logger.error(f'Could not transform world to grasp_frame: {ex}') + return None + + grasp_T_object = np.eye(4) + grasp_T_object[:3, :3] = R.from_quat([ + grasp_pose_object.orientation.x, + grasp_pose_object.orientation.y, + grasp_pose_object.orientation.z, + grasp_pose_object.orientation.w, + ]).as_matrix() + grasp_T_object[:3, 3] = np.asarray([ + grasp_pose_object.position.x, + grasp_pose_object.position.y, + grasp_pose_object.position.z, + ]) + world_T_grasp = np.eye(4) + world_T_grasp[:3, :3] = R.from_quat([ + world_pose_grasp.transform.rotation.x, + world_pose_grasp.transform.rotation.y, + world_pose_grasp.transform.rotation.z, + world_pose_grasp.transform.rotation.w, + ]).as_matrix() + world_T_grasp[:3, 3] = np.asarray([ + world_pose_grasp.transform.translation.x, + world_pose_grasp.transform.translation.y, + world_pose_grasp.transform.translation.z, + ]) + world_T_object = np.matmul(world_T_grasp, grasp_T_object) + return world_T_object + + def calculate_cuboid_vertices(self) -> np.ndarray: + """ + Calculate the vertices required for the cuboid approach. + + Manually computes them and then offseting them so that it's centered + at (0, 0, 0). + """ + width = self.__attached_object_config.scale.x + height = self.__attached_object_config.scale.y + depth = self.__attached_object_config.scale.z + # Manually create the vertices with the lower left corner + # being (0, 0, 0) + vertices = np.array([ + [0, 0, 0], + [0, 0, depth], + [0, height, 0], + [0, height, depth], + [width, 0, 0], + [width, 0, depth], + [width, height, 0], + [width, height, depth], + ]) + # Offset the cuboid so that centre is (0, 0, 0) + for i in range(len(vertices)): + vertices[i][0] -= width / 2 + vertices[i][1] -= height / 2 + vertices[i][2] -= depth / 2 + return vertices + + def send_esdf_request(self, aabbs_to_clear_min_m: List[Point], + aabbs_to_clear_size_m: List[Vector3], + spheres_to_clear_center_m: List[Point], + spheres_to_clear_radius_m: List[float]) -> Future: + """ + Send the AABBs and spheres to clear to the ESDF client. + + Args + ---- + aabbs_to_clear_min_m (List[Point]): List of aabb minimum corners to clear + aabbs_to_clear_size_m (List[Vector3]): List of aabb sizes to clear + spheres_to_clear_center_m (List[Point]): List of sphere centers to clear + spheres_to_clear_radius_m (List[float]): List of sphere radii to clear + + Returns + ------- + Future: Future of the request + + """ + self.__esdf_req.visualize_esdf = False + self.__esdf_req.update_esdf = self.__update_esdf_on_request + self.__esdf_req.frame_id = self.__cfg_base_link + + self.__esdf_req.aabbs_to_clear_min_m = aabbs_to_clear_min_m + self.__esdf_req.aabbs_to_clear_size_m = aabbs_to_clear_size_m + self.__esdf_req.spheres_to_clear_center_m = spheres_to_clear_center_m + self.__esdf_req.spheres_to_clear_radius_m = spheres_to_clear_radius_m + + self.get_logger().info( + '[Object Attachment]: Sending ESDF Update Request for AABB clearing' + ) + + esdf_future = self.__esdf_client.call_async(self.__esdf_req) + + return esdf_future + + def handle_fallback( + self, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> None: + """ + Use a fallback radius sphere for the object. + + Attempt to sync link spheres with other nodes. If sync succeeds, mark as attached; + otherwise, keep detached. + """ + self.__att_obj_srv_fb_msg.status = ( + 'Attaching pre-defined collision sphere...') + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + fallback_radius = att_obj_srv_goal_handle.request.fallback_radius + + # Initialize a sphere at the object frame's origin with fallback_radius + self.__object_spheres = np.array([0.0, 0.0, 0.0, fallback_radius]) + + sync_success = self.sync_object_link_spheres_across_nodes( + True, att_obj_srv_goal_handle) + if sync_success: + self.__object_state = ObjectState.ATTACHED_FALLBACK + self.update_esdf_voxel_grid_for_aabb_clearing() + + def handle_detachment( + self, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> None: + """ + Clear sphere buffers linked to the object. + + Attempt to sync link spheres with other nodes. If sync succeeds, mark as detached; + otherwise, keep attached. + """ + detachment_time = time.time() + try: + self.__kin_model.kinematics_config.detach_object( + link_name=self.__object_link_name) + + self.__att_obj_srv_fb_msg.status = ( + f'Object detached in {time.time()-detachment_time}s.') + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + sync_success = self.sync_object_link_spheres_across_nodes( + False, att_obj_srv_goal_handle) + if sync_success: + self.__object_state = ObjectState.DETACHED + + except Exception as e: + self.__att_obj_srv_fb_msg.status = ( + f'Error in detachment: {str(e)}') + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + def sync_object_link_spheres_across_nodes( + self, + attach_object: bool, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> bool: + """ + Synchronize object link spheres across nodes via action calls. + + For attachment: + - Send a flattened list of spheres and the associated link to other nodes. + + For detachment: + - Notify nodes to clear the sphere buffer for the link. + + Use an action client to send the update goal and wait for completion. + Return True if all nodes successfully update; otherwise, return False. + + Args + ---- + attach_object (bool): True to attach, False to detach. + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the goal + sent to the server. + + Returns + ------- + bool: True if synchronization succeeds; False otherwise. + + """ + update_spheres_goal_msg = UpdateLinkSpheres.Goal() + update_spheres_goal_msg.attach_object = attach_object + update_spheres_goal_msg.object_link_name = self.__object_link_name + + if attach_object: + update_spheres_goal_msg.flattened_sphere_arr = self.__object_spheres.flatten().tolist() + + update_spheres_goal_futures = [] + for action_name, action_client in self.__action_clients.items(): + update_spheres_future = self.send_goal_to_update_spheres( + action_client, update_spheres_goal_msg, action_name, att_obj_srv_goal_handle) + update_spheres_goal_futures.append( + (action_name, update_spheres_future)) + + # Wait for the completion of goal at all dependent action servers + all_succeeded = True + while update_spheres_goal_futures: + for action_name, update_spheres_future in update_spheres_goal_futures[:]: + if update_spheres_future.done(): + result = update_spheres_future.result() + if not result.accepted: + all_succeeded = False + if result.status == GoalStatus.STATUS_ABORTED: + all_succeeded = False + update_spheres_goal_futures.remove( + (action_name, update_spheres_future)) + + time.sleep(0.01) + + return all_succeeded + + def send_goal_to_update_spheres( + self, + action_client: ActionClient, + update_spheres_goal_msg: UpdateLinkSpheres.Goal, + action_name: str, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> Future: + """ + Send a goal to update link spheres when the server is ready. + + Set up feedback and response callbacks to track progress and the outcome of the goal. + + Args + ---- + action_client (ActionClient): Client used to communicate with the action server. + update_spheres_goal_msg (UpdateLinkSpheres.Goal): Goal message to update link spheres. + action_name (str): Name of the action being executed. + att_obj_srv_goal_handle (ServerGoalHandle): Represents the ongoing goal to attach or + detach the object, managing its state separately from the update link spheres + action. + + Returns + ------- + Future: Represents the asynchronous result of the action, including whether the goal + to update spheres was accepted, rejected, or completed successfully. + + """ + action_client.wait_for_server() + send_goal_future = action_client.send_goal_async( + update_spheres_goal_msg, + feedback_callback=( + lambda updt_spheres_fb_msg: self.update_spheres_feedback_callback( + updt_spheres_fb_msg, action_name, att_obj_srv_goal_handle + )) + ) + send_goal_future.add_done_callback( + lambda update_spheres_future: self.update_spheres_response_callback( + update_spheres_future, action_name, att_obj_srv_goal_handle) + ) + + return send_goal_future + + def update_spheres_feedback_callback( + self, + updt_spheres_fb_msg: UpdateLinkSpheres.Feedback, + action_name: str, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> None: + """ + Handle feedback from the update spheres action. + + Updates the status message based on the received feedback and publishes it + to the original goal (attach/detach) client. + + Args + ---- + updt_spheres_fb_msg (UpdateLinkSpheres.Feedback): Feedback message from the + update spheres action. + action_name (str): Name of the action sending the feedback. + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the original + attach/detach goal, to which the feedback is published. + + Returns + ------- + None + + """ + self.__att_obj_srv_fb_msg.status = ( + f'Feedback for {action_name}: {updt_spheres_fb_msg.feedback.status}') + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + def update_spheres_response_callback( + self, + update_spheres_future: Future, + action_name: str, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> None: + """ + Handle the response from the update spheres action. + + Checks if the goal was accepted or rejected and publishes feedback accordingly. + If accepted, sets up a callback to handle the final result of the update action. + + Args + ---- + update_spheres_future (Future): Future object holding the result of the goal request. + action_name (str): Name of the action sending the response. + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the + original attach/detach goal, ensuring the correct goal receives feedback. + + Returns + ------- + None + + """ + update_spheres_goal_handle = update_spheres_future.result() + if not update_spheres_goal_handle.accepted: + self.__att_obj_srv_fb_msg.status = ( + f'Goal for {action_name} rejected.') + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + return + + self.__att_obj_srv_fb_msg.status = ( + f'Goal for {action_name} accepted.') + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + self.__get_result_future = update_spheres_goal_handle.get_result_async() + self.__get_result_future.add_done_callback( + lambda update_spheres_future: self.update_spheres_result_callback( + update_spheres_future, action_name, att_obj_srv_goal_handle) + ) + + def update_spheres_result_callback( + self, + update_spheres_future: Future, + action_name: str, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> None: + """ + Handle the final result of the update spheres action. + + Extracts the result and publishes a status message indicating success or failure. + + Args + ---- + update_spheres_future (Future): Future containing the result of the update + spheres action. + action_name (str): Name of the action for which the result is being handled. + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the original + attach/detach goal, ensuring feedback is sent to the correct goal. + + Returns + ------- + None + + """ + result = update_spheres_future.result().result + self.__att_obj_srv_fb_msg.status = ( + f'Final result for {action_name} action: Success: {result.outcome}') + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + def filter_depth_buffers(self) -> None: + """ + Filter the depth buffers by as much as time slop. + + This makes sure object attachment always filters based on latest depth information. + """ + current_time = self.get_clock().now() # Get current time + time_threshold = current_time - rclpy.duration.Duration( + seconds=self.__filter_depth_buffer_time) + time_threshold_msg = time_threshold.to_msg() + + filtered_depth_buffers = [] + filtered_depth_timestamps = [] + + for i, timestamp in enumerate(self.__depth_timestamps): + time_in_seconds_one = timestamp.sec + timestamp.nanosec * 1e-9 + time_in_seconds_two = time_threshold_msg.sec + time_threshold_msg.nanosec * 1e-9 + if time_in_seconds_one > time_in_seconds_two: + filtered_depth_buffers.append(self.__depth_buffers[i]) + filtered_depth_timestamps.append(timestamp) + + self.__depth_buffers = filtered_depth_buffers + self.__depth_timestamps = filtered_depth_timestamps + + def process_depth_and_joint_state( + self, + *msgs: Tuple[Union[Image, JointState]] + ) -> None: + """ + Process depth and joint state messages. + + Extracts and stores depth buffers, encoding, and camera headers from Image messages. + For JointState messages, stores joint names, positions, and the timestamp. + + Args + ---- + *msgs (Tuple[Union[Image, JointState]]): Variable-length tuple of Image and JointState + messages to process. + + Returns + ------- + None + + """ + self.__depth_buffers = [] + self.__depth_encoding = [] + self.__camera_headers = [] + self.__depth_timestamps = [] + for msg in msgs: + if (isinstance(msg, Image)): + img = self.__br.imgmsg_to_cv2(msg) + if msg.encoding == '32FC1': + img = 1000.0 * img + self.__depth_buffers.append(img) + self.__camera_headers.append(msg.header) + self.__depth_encoding.append(msg.encoding) + self.__depth_timestamps.append(msg.header.stamp) # Store timestamp + if (isinstance(msg, JointState)): + self.__js_buffer = {'joint_names': msg.name, + 'position': msg.position} + self.__timestamp = msg.header.stamp + + def camera_info_cb( + self, + msg: CameraInfo, + idx: int + ) -> None: + """ + Handle incoming CameraInfo messages. + + Updates the depth intrinsics for the specified camera index. + + Args + ---- + msg (CameraInfo): The CameraInfo message containing intrinsic parameters. + idx (int): Index of the camera to update. + + Returns + ------- + None + + """ + self.__depth_intrinsics[idx] = msg.k + + def attach_object( + self, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> Tuple[bool, Union[str, None]]: + """ + Attach an object to the robot based on the object's geometry type. + + The method ensures that valid data from sensors (subscribers) is available, retrieves + camera transforms, and processes object-specific data for proper attachment. Depending + on the shape of the object, the function performs different computations, such as handling + cuboids, custom meshes, and spheres using depth images and joint state data. The final + outcome is the creation and publication of robot sphere markers and collision spheres. + + Args + ---- + att_obj_srv_goal_handle (ServerGoalHandle): The goal handle containing necessary data + for attaching the object and managing the feedback communication with the client. + + Returns + ------- + Tuple[bool, Union[str, None]]: A tuple containing: + - bool: True if the object was successfully attached; otherwise, False. + - Union[str, None]: Error message if an exception occurred during the process, + otherwise None. + + """ + # Validate necessary data from subscribers + if not self.has_valid_subscriber_data(): + return False, None + + # Handle camera transforms + if not self.retrieve_camera_transforms(att_obj_srv_goal_handle): + return False, None + + try: + attached_object_shape = self.__attached_object_config.type + # Lock to prevent concurrent data modification during read and copy. + with self.__lock: + error_msg = 'No depth images in the buffer, please check the time sync slop ' \ + 'parameter. It could be that joint states and images could not ' \ + 'be synced together.' + if attached_object_shape == Marker.SPHERE: + self.filter_depth_buffers() + error_msg += f' We filter the depth to only get depth images from the past ' \ + f'{self.__filter_depth_buffer_time} seconds' + + if len(self.__depth_buffers) == 0: + self.__att_obj_srv_fb_msg.status = error_msg + self.get_logger().error(self.__att_obj_srv_fb_msg.status) + return False, None + + depth_image = np.copy(np.stack((self.__depth_buffers))) + intrinsics = np.copy(np.stack(self.__depth_intrinsics)) + js = np.copy(self.__js_buffer['position']) + j_names = deepcopy(self.__js_buffer['joint_names']) + + # Reset the timestamp and camera headers for the next cycle. + self.__timestamp = None + self.__camera_headers = [] + + object_frame_origin, joint_states = self.forward_kinematics_computations( + joint_positions=js, + joint_names=j_names, + att_obj_srv_goal_handle=att_obj_srv_goal_handle + ) + + if attached_object_shape in (Marker.CUBE, Marker.MESH_RESOURCE): + attached_object_frame_sphere_tensor = self.get_spheres_in_attached_object_frame() + self.attach_object_collision_spheres( + spheres_in_attached_object_frame=attached_object_frame_sphere_tensor + ) + elif attached_object_shape == Marker.SPHERE: + self.attach_object_collision_spheres_from_point_cloud( + att_obj_srv_goal_handle=att_obj_srv_goal_handle, + depth_image=depth_image, + intrinsics=intrinsics, + object_frame_origin=object_frame_origin, + joint_states=joint_states) + + if self.__robot_sphere_markers_publisher.get_subscription_count() > 0: + self.publish_robot_spheres( + joint_states=joint_states, + att_obj_srv_goal_handle=att_obj_srv_goal_handle + ) + + return True, None # Indicate success + + except Exception as e: + + self.__att_obj_srv_fb_msg.status = f'{traceback.format_exc()}' + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + return False, str(e) + + def attach_object_collision_spheres_from_point_cloud( + self, att_obj_srv_goal_handle: ServerGoalHandle, + depth_image: np.ndarray, + intrinsics: CameraInfo, + object_frame_origin: Pose, + joint_states: JointState + ): + """ + Generate spheres for object by processing the depth image. + + It takes as input the depth image, intrinsics and joint states as input to construct the + collision spheres which are attached to the object_frame on the robot. + """ + points_wrt_robot_base = self.get_point_cloud_from_depth( + depth_image=depth_image, + intrinsics=intrinsics, + att_obj_srv_goal_handle=att_obj_srv_goal_handle + ) + + points_near_object_frame_origin = self.get_points_around_center( + all_points=points_wrt_robot_base, + center=object_frame_origin, + att_obj_srv_goal_handle=att_obj_srv_goal_handle + ) + + object_point_cloud = self.get_object_point_cloud( + point_cloud=points_near_object_frame_origin, + object_frame_origin=object_frame_origin, + bypass_clustering=self.__bypass_clustering, + num_top_clusters_to_select=self.__num_top_clusters_to_select, + group_clusters=self.__group_clusters, + min_points=self.__min_points, + att_obj_srv_goal_handle=att_obj_srv_goal_handle + ) + + object_mesh = self.get_object_mesh( + object_point_cloud=object_point_cloud, + att_obj_srv_goal_handle=att_obj_srv_goal_handle + ) + self.__object_mesh = object_mesh + self.generate_object_collision_spheres_from_mesh( + joint_states=joint_states, + object_mesh=object_mesh, + att_obj_srv_goal_handle=att_obj_srv_goal_handle + ) + + def has_valid_subscriber_data( + self + ) -> bool: + """ + Check if the necessary subscriber data is available. + + Verifies that all depth intrinsics are valid, camera headers are present, + and a timestamp exists. + + Returns + ------- + bool: True if all necessary subscriber data is available; otherwise, False. + + """ + if ( + not all(isinstance(intrinsic, np.ndarray) + for intrinsic in self.__depth_intrinsics) + or len(self.__camera_headers) == 0 + or self.__timestamp is None + ): + self.get_logger().error('Could not find valid subscriber data to attach object') + return False + return True + + def retrieve_camera_transforms( + self, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> bool: + """ + Read and process camera transforms. + + If camera transforms are not yet available, attempts to retrieve them. Publishes feedback + based on success or failure. + + Args + ---- + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the original + attach/detach goal, ensuring feedback is sent correctly. + + Returns + ------- + bool: True if transforms were successfully retrieved for all cameras; otherwise, False. + + """ + if self.__robot_pose_cameras is None: + with self.__lock: + camera_headers = deepcopy(self.__camera_headers) + + for i in range(self.__num_cameras): + if self.__robot_pose_camera[i] is None: + if not self.lookup_camera_transform(i, camera_headers[i], + att_obj_srv_goal_handle): + continue + + if None not in self.__robot_pose_camera: + self.__robot_pose_cameras = CuPose.cat( + self.__robot_pose_camera) + self.__att_obj_srv_fb_msg.status = 'Received TF from cameras to robot' + att_obj_srv_goal_handle.publish_feedback( + self.__att_obj_srv_fb_msg) + else: + self.__att_obj_srv_fb_msg.status = 'Failed to retrieve transform for any camera' + self.get_logger().error(self.__att_obj_srv_fb_msg.status) + att_obj_srv_goal_handle.publish_feedback( + self.__att_obj_srv_fb_msg) + return False + + return True + + def lookup_camera_transform( + self, + index: int, + camera_header: Header, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> bool: + """ + Attempt to look up and store a camera transform. + + Tries to retrieve the transform between the camera and the robot base link. On success, + stores the transform; on failure, logs an error and publishes feedback. + + Args + ---- + index (int): Index of the camera for which the transform is being looked up. + camera_header (Header): The header of the camera containing the frame ID. + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the original + attach/detach goal, ensuring feedback is sent correctly. + + Returns + ------- + bool: True if the transform was successfully retrieved; otherwise, False. + + """ + try: + t = self.__tf_buffer.lookup_transform( + self.__cfg_base_link, + camera_header.frame_id, + self.__timestamp, + rclpy.duration.Duration(seconds=self.__tf_lookup_duration), + ) + self.__robot_pose_camera[index] = CuPose.from_list( + [ + t.transform.translation.x, + t.transform.translation.y, + t.transform.translation.z, + t.transform.rotation.w, + t.transform.rotation.x, + t.transform.rotation.y, + t.transform.rotation.z, + ] + ) + return True + except TransformException as ex: + self.__att_obj_srv_fb_msg.status = ( + f'Could not transform {camera_header.frame_id} to {self.__cfg_base_link}: {ex}' + ) + self.get_logger().error(self.__att_obj_srv_fb_msg.status) + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + return False + + def get_spheres_in_attached_object_frame( + self, + ) -> torch.Tensor: + """ + Generate and return the collision spheres in the attached object's frame of reference. + + Calculates the position of collision spheres relative to the attached object's + frame, taking into account the transformations between the detected object, gripper, and + attached object frames. The function also concatenates the computed sphere positions and + their radii into a tensor for further processing. + + Args + ---- + None + + Returns + ------- + torch.Tensor: A tensor containing the sphere centers and radii, with the sphere + positions transformed to the attached object's frame. + + """ + grasp_pose = self.__attached_object_config.pose + gripper_frame_pose_object_frame = [grasp_pose.position.x, + grasp_pose.position.y, + grasp_pose.position.z, + grasp_pose.orientation.w, + grasp_pose.orientation.x, + grasp_pose.orientation.y, + grasp_pose.orientation.z] + + attached_object_frame_pose_gripper_frame = ( + self.get_gripper_to_attached_object_frame_transform() + ) + + detected_object_frame_position_sphere_centers, radii_spheres = ( + self.get_collision_spheres_in_detected_object_frame( + pose=gripper_frame_pose_object_frame + ) + ) + + attached_object_frame_position_sphere_centers = ( + attached_object_frame_pose_gripper_frame.transform_points( + points=detected_object_frame_position_sphere_centers + ) + ) + + attached_object_frame_sphere_tensor = torch.cat( + (attached_object_frame_position_sphere_centers, radii_spheres), + dim=1 + ) + + return attached_object_frame_sphere_tensor + + def get_gripper_to_attached_object_frame_transform( + self + ) -> CuPose: + """ + Compute the transform from the gripper frame to the attached object's frame. + + Retrieves the poses of the gripper and attached object frames relative to the + robot base frame using the robot's kinematic model based on default joint positions. + The relative pose of the gripper frame relative to the attached object frame is then + calculated and returned. + + Args + ---- + None + + Returns + ------- + CuPose: The transformation matrix that defines the pose of the gripper frame relative + to the attached object frame. + + """ + object_frame = self.__object_link_name + gripper_frame = self.__gripper_frame_name + + # Type hint | self.__kin_model: CudaRobotModel + default_joint_positions = self.__kin_model.retract_config + + # Type hint | robot_state: CudaRobotModelState + robot_state = self.__kin_model.compute_kinematics_from_joint_position( + joint_position=default_joint_positions + ) + + link_poses = robot_state.link_poses + robot_base_frame_pose_object_frame = link_poses[object_frame] + robot_base_frame_pose_gripper_frame = link_poses[gripper_frame] + + object_frame_pose_robot_base_frame = robot_base_frame_pose_object_frame.inverse() + + object_frame_pose_gripper_frame = object_frame_pose_robot_base_frame.multiply( + robot_base_frame_pose_gripper_frame + ) + + return object_frame_pose_gripper_frame + + def get_detected_object_to_gripper_frame_transform( + self + ) -> CuPose: + """ + Compute the transform from the detected object's frame to the gripper frame. + + Retrieves the grasp pose, which defines the pose of the detected object's frame + relative to the gripper frame. The position and orientation are extracted from + the grasp pose and used to construct a CuPose object. + + Args + ---- + None + + Returns + ------- + CuPose: The transformation matrix that defines the pose of the detected object frame + relative to the gripper frame. + + """ + grasp_pose = self.__attached_object_config.pose + + gripper_frame_pose_detected_object_frame = CuPose( + position=torch.tensor( + [grasp_pose.position.x, + grasp_pose.position.y, + grasp_pose.position.z], + device=torch.device('cuda', self.__cuda_device_id)), + quaternion=torch.tensor( + [grasp_pose.orientation.x, + grasp_pose.orientation.y, + grasp_pose.orientation.z, + grasp_pose.orientation.w], + device=torch.device('cuda', self.__cuda_device_id)) + ) + + return gripper_frame_pose_detected_object_frame + + def get_collision_spheres_in_detected_object_frame( + self, + pose: List[float] + ) -> Tuple[torch.Tensor, List[float]]: + """ + Compute collision spheres in the detected object's frame. + + Depending on the shape of the attached object (cuboid or custom mesh), creates + an appropriate object representation using the provided pose. Then generates + collision spheres within the object's frame by invoking + `generate_spheres_in_object_frame`. Returns the positions of the spheres' centers + and their radii. + + Args + ---- + pose (List[float]): The pose of the detected object. + + Returns + ------- + Tuple[torch.Tensor, List[float]]: A tuple containing the positions of the spheres' + centers in the object frame and the radii of the spheres. + + """ + attached_object_shape = self.__attached_object_config.type + + if attached_object_shape == Marker.CUBE: + + dims_array = [self.__attached_object_config.scale.x, + self.__attached_object_config.scale.y, + self.__attached_object_config.scale.z] + + attached_object = CuCuboid( + name='blind_cuboid', + pose=pose, + dims=dims_array + ) + + if attached_object_shape == Marker.MESH_RESOURCE: + + attached_object = CuMesh( + name='mesh_object_for_object_attachment', + pose=pose, + file_path=self.__attached_object_config.mesh_resource + ) + + object_frame_position_sphere_centers, radii_spheres = ( + self.generate_spheres_in_object_frame( + attached_object=attached_object + ) + ) + + return object_frame_position_sphere_centers, radii_spheres + + def generate_spheres_in_object_frame( + self, + attached_object: CuObstacle + ) -> Tuple[torch.Tensor, torch.Tensor]: + """ + Generate collision spheres within the object's frame. + + Uses the provided attached object to generate collision spheres. The positions of the + spheres' centers and their radii are collected and returned as tensors. + + Args + ---- + attached_object (CuObstacle): The object for which to generate bounding spheres. + + Returns + ------- + Tuple[torch.Tensor, torch.Tensor]: A tuple containing: + - torch.Tensor: Tensor of shape (n_spheres, 3) representing the positions of the + spheres' centers in the object's frame. + - torch.Tensor: Tensor of shape (n_spheres, 1) representing the radii of spheres. + + """ + cu_spheres = attached_object.get_bounding_spheres( + n_spheres=self.__object_attachment_n_spheres, + surface_sphere_radius=self.__surface_sphere_radius + ) + + object_frame_position_sphere_centers = torch.tensor( + [cu_sphere.pose[:3] for cu_sphere in cu_spheres], + dtype=torch.float32, + device=torch.device('cuda', self.__cuda_device_id) + ) + radii_spheres = torch.tensor( + [cu_sphere.radius for cu_sphere in cu_spheres], + dtype=torch.float32, + device=torch.device('cuda', self.__cuda_device_id) + ) + + radii_spheres = radii_spheres.view(-1, 1) + + return object_frame_position_sphere_centers, radii_spheres + + def attach_object_collision_spheres( + self, + spheres_in_attached_object_frame: torch.Tensor + ) -> None: + """ + Update the kinematic model with collision spheres in the attached object's frame. + + This method takes the provided collision spheres' positions and radii in the attached + object's frame and updates the kinematic configuration to reflect these spheres. + + Args + ---- + spheres_in_attached_object_frame (torch.Tensor): A tensor containing the positions + and radii of the collision spheres in the attached object's frame. + + Returns + ------- + None + + """ + self.__kin_model.kinematics_config.update_link_spheres( + link_name=self.__object_link_name, + sphere_position_radius=spheres_in_attached_object_frame + ) + + link_spheres_object_frame = self.__kin_model.kinematics_config.get_link_spheres( + link_name=self.__object_link_name) + + self.__object_spheres = link_spheres_object_frame.cpu().numpy() + + def get_point_cloud_from_depth( + self, + depth_image: torch.Tensor, + intrinsics: torch.Tensor, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> torch.Tensor: + """ + Generate a 3D point cloud from the segmented depth image using camera intrinsics. + + Converts the depth image into a point cloud in the robot's base frame by leveraging the + camera intrinsics & robot's pose. The point cloud is extracted using the camera observation + model and transformed into the robot base frame. + + Args + ---- + depth_image (torch.Tensor): A tensor representing the segmented depth image. + intrinsics (torch.Tensor): Camera intrinsics used for projecting the depth image. + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the original + attach/detach goal, to which the feedback is published. + + Returns + ------- + torch.Tensor: A 3D point cloud in the robot's base frame. + + """ + time_start = time.time() + + depth_image = self.__tensor_args.to_device( + depth_image.astype(np.float32)) + depth_image = depth_image.view( + self.__num_cameras, depth_image.shape[-2], depth_image.shape[-1]) + + intrinsics = ( + self.__tensor_args.to_device( + intrinsics).view(self.__num_cameras, 3, 3) + ) + + cam_obs = CameraObservation( + depth_image=depth_image, + pose=self.__robot_pose_cameras, + intrinsics=intrinsics + ) + + point_cloud = cam_obs.get_pointcloud() + + # robot_base_points: point wrt robot base frame + points_wrt_robot_base = ( + self.__robot_pose_cameras.batch_transform_points(point_cloud) + ) + + points_wrt_robot_base = points_wrt_robot_base.contiguous().view(-1, 3) + + self.__att_obj_srv_fb_msg.status = ( + f'Extracted point cloud from segmented depth image in {time.time() - time_start}s.' + ) + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + return points_wrt_robot_base + + def forward_kinematics_computations( + self, + joint_positions: np.ndarray, + joint_names: list[str], + att_obj_srv_goal_handle: ServerGoalHandle + ) -> tuple[torch.Tensor, JointState]: + """ + Compute the forward kinematics for the manipulator and obtain the object frame origin. + + Uses the joint positions and joint names to compute the forward kinematics of the + manipulator and determine the position of the object frame origin. Optionally publishes + the object position if a subscriber is active. + + Args + ---- + joint_positions (np.ndarray): The angular positions of the manipulator's joints. + joint_names (list): The names of the joints corresponding to the positions. + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the original + attach/detach goal, to which the feedback is published. + + Returns + ------- + tuple[torch.Tensor, JointState]: A tuple containing the object's frame origin as a + tensor and the current joint states. + + """ + start_time = time.time() + + joint_states = CuJointState.from_numpy( + position=joint_positions, + joint_names=joint_names, + tensor_args=self.__tensor_args).unsqueeze(0) + + active_jnames = self.__kin_model.joint_names + joint_states = joint_states.get_ordered_joint_state(active_jnames) + out = self.__kin_model.get_state(joint_states.position) + object_frame_origin = out.link_poses[self.__object_link_name].position + + self.__att_obj_srv_fb_msg.status = ( + f'Obtained object position using forward kinematics in {time.time() - start_time}s.' + ) + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + if self.__object_origin_publisher.get_subscription_count() > 0: + self.publish_point(object_frame_origin) + + return object_frame_origin, joint_states + + def get_points_around_center( + self, + all_points: torch.Tensor, + center: torch.Tensor, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> torch.Tensor: + """ + Extract points within a specified radius around the object center. + + Computes the Euclidean distance between each point and the object center and selects points + within a defined search radius. Optionally publishes the nearby points if a subscriber + is active. + + Args + ---- + all_points (torch.Tensor): The entire point cloud from which points are filtered. + center (torch.Tensor): The object frame's origin used to calculate distances. + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the original + attach/detach goal, to which the feedback is published. + + Returns + ------- + torch.Tensor: A tensor of points located near the object center. + + """ + start_time = time.time() + + distances = torch.norm(all_points[:, :] - center, dim=1) + indices = distances <= self.__search_radius + nearby_points = all_points[indices] + + self.__att_obj_srv_fb_msg.status = ( + f'Obtained points near object center in {time.time()-start_time}s' + ) + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + if self.__nearby_points_publisher.get_subscription_count() > 0: + self.publish_point_cloud( + nearby_points, + self.__nearby_points_publisher + ) + + return nearby_points + + def get_object_point_cloud( + self, + point_cloud: torch.Tensor, + object_frame_origin: torch.Tensor, + bypass_clustering: bool, + num_top_clusters_to_select: int, + group_clusters: bool, + min_points: int, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> torch.Tensor: + """ + Obtain the object point cloud either by clustering or bypassing clustering. + + If clustering is enabled, function applies clustering algorithm to the input point cloud to + detect clusters. The most relevant cluster(s) are selected using a heuristic. If clustering + is bypassed, the input point cloud is used directly. The object point cloud can optionally + be published if a subscriber is active. + + Args + ---- + point_cloud (torch.Tensor): The point cloud representing the scene expressed with + respect to robot base frame. + bypass_clustering (bool): Whether to bypass clustering & use the point cloud directly. + min_samples (int): Minimum samples for HDBSCAN clustering. + min_cluster_size (int): Minimum cluster size for HDBSCAN. + cluster_selection_epsilon (float): Epsilon value for cluster selection in HDBSCAN. + object_frame_origin (torch.Tensor): The reference point (object frame origin) for + cluster selection. + num_top_clusters_to_select (int): The number of top/largest clusters to select. + group_clusters (bool): Whether to group multiple clusters into one. + min_points (int): Minimum number of points required for a cluster to be valid. + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the original + attach/detach goal, to which the feedback is published. + + Returns + ------- + torch.Tensor: The filtered point cloud representing the object. + + """ + if bypass_clustering: + object_point_cloud = point_cloud + else: + start_time = time.time() + + # The CPU implementation can be switched to a GPU-based cuML approach once Jetson's + # binary support is restored. Since cuML uses CuPy arrays (which are GPU-accelerated), + # downstream calculations remain in CuPy for efficiency. This avoids replacing CuPy + # with torch.tensor and prevents future rework when cuML is reintroduced. + points_cupy = cp.asarray(point_cloud) + points_cpu = point_cloud.cpu().numpy() + labels_cpu = self.__cpu_hdbscan.fit_predict(points_cpu) + labels_cupy = cp.asarray(labels_cpu) + + self.__att_obj_srv_fb_msg.status = ( + f'CPU HDBSCAN clustering completed in {time.time()-start_time}s') + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + object_point_cloud = self.cluster_selection_heuristic( + points=points_cupy, + labels=labels_cupy, + ref_point=object_frame_origin, + num_top_clusters_to_select=num_top_clusters_to_select, + group_clusters=group_clusters, + min_points=min_points, + att_obj_srv_goal_handle=att_obj_srv_goal_handle + ) + + if self.__object_cloud_publisher.get_subscription_count() > 0: + self.publish_point_cloud( + object_point_cloud, self.__object_cloud_publisher) + + return object_point_cloud + + def cluster_selection_heuristic( + self, + points: cp.ndarray, + labels: cp.ndarray, + ref_point: torch.Tensor, + num_top_clusters_to_select: int, + group_clusters: bool, + min_points: int, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> torch.Tensor: + """ + Apply a heuristic to filter, select, and group clusters based on distance and size. + + Filters the clusters based on a minimum number of points, calculates their distance from + a reference point, and selects the largest clusters. Optionally groups the selected + clusters into one point cloud. + + Args + ---- + points (cp.ndarray): An array of points (XYZ coordinates). + labels (cp.ndarray): An array of cluster labels corresponding to the points. + ref_point (torch.Tensor): A tensor representing the reference point. + num_top_clusters_to_select (int): The number of top largest clusters to select. + group_clusters (bool): Whether to group the selected clusters into a single cluster. + min_points (int): The minimum number of points required for a cluster to be valid. + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the original + attach/detach goal, to which the feedback is published. + + Returns + ------- + torch.Tensor: A tensor representing the selected or grouped clusters. + + """ + total_start_time = time.time() + start_time = time.time() + + filtered_clusters = self.get_filtered_labeled_clusters( + points, labels, min_points) + + self.__att_obj_srv_fb_msg.status = ( + f' Heuristic Step-1 | Got Filtered labeled clusters in {time.time()-start_time}s' + ) + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + # Publish the filtered clusters as a point cloud + if self.__clustered_points_publisher.get_subscription_count() > 0: + publish_time = time.time() + self.publish_all_clusters(filtered_clusters) + self.__att_obj_srv_fb_msg.status = ( + f' Published all clusters in {time.time()-publish_time}s' + ) + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + if not filtered_clusters: + self.__att_obj_srv_fb_msg.status = 'No clusters found after filtering' + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + return None + + start_time = time.time() + + cluster_distances = self.calculate_cluster_distances( + filtered_clusters, ref_point) + + self.__att_obj_srv_fb_msg.status = ( + ' Heuristic Step-2 | Calculated cluster distanced from object frame' + f'in {time.time()-start_time}s' + ) + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + start_time = time.time() + + selected_clusters = self.sort_and_select_clusters( + cluster_distances, filtered_clusters, num_top_clusters_to_select) + + self.__att_obj_srv_fb_msg.status = ( + f' Heuristic Step-3 | Sort and select clusters in {time.time()-start_time}s' + ) + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + # If group_clusters is True, combine the top clusters into one + if group_clusters: + object_cluster = cp.concatenate(selected_clusters, axis=0) + # Otherwise, return the largest cluster + else: + object_cluster = max( + selected_clusters, key=lambda cluster: cluster.shape[0]) + + object_point_cloud = torch.as_tensor(object_cluster, device='cuda:0') + + self.__att_obj_srv_fb_msg.status = ( + 'Total Heuristic time | Selected cluster as per heuristic' + f'in {time.time()-total_start_time}s' + ) + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + return object_point_cloud + + def get_filtered_labeled_clusters( + self, + points: cp.ndarray, + labels: cp.ndarray, + min_points: int + ) -> Dict[int, cp.ndarray]: + """ + Filter clusters based on a minimum number of points. + + Filters out clusters that have fewer points than the specified threshold. + + Args + ---- + points (cp.ndarray): An array of points (XYZ coordinates). + labels (cp.ndarray): An array of cluster labels corresponding to the points. + min_points (int): The minimum number of points required for a cluster to be valid. + + Returns + ------- + Dict[int, cp.ndarray]: A dictionary mapping valid cluster labels (int) to their + respective points (XYZ coordinates). + + """ + unique_labels = cp.unique(labels) + + clusters = {} + for label in unique_labels: + label = int(label) # Ensure label is a scalar integer + if label != -1: # Ignore noise points + cluster_points = points[labels == label] + if cluster_points.shape[0] > min_points: + clusters[label] = cluster_points + + return clusters + + def calculate_cluster_distances( + self, + clusters: Dict[int, cp.ndarray], + ref_point: torch.Tensor + ) -> Dict[int, float]: + """ + Calculate the distance from a reference point to each cluster. + + The distance is calculated by finding the Euclidean distance between the reference point + and the mean of each cluster. + + Args + ---- + clusters (Dict[int, cp.ndarray]): A dictionary mapping cluster labels (int) to arrays + of points (XYZ coordinates). + ref_point (torch.Tensor): A tensor representing the reference point in space. + + Returns + ------- + Dict[int, float]: A dictionary mapping each cluster label (int) to its distance from + the reference point. + + """ + ref_point_gpu = cp.asarray(ref_point.flatten()) + cluster_distances = {} + for label, cluster in clusters.items(): + cluster_mean = cp.mean(cluster, axis=0) + distance = cp.linalg.norm(cluster_mean - ref_point_gpu) + cluster_distances[label] = distance + + return cluster_distances + + def sort_and_select_clusters( + self, + cluster_distances: Dict[int, float], + clusters: Dict[int, cp.ndarray], + num_top_clusters: int + ) -> List[cp.ndarray]: + """ + Sort clusters by their distance from a reference point and select the largest N clusters. + + The clusters are first sorted based on their distance to the reference point, and then the + top N clusters in terms of number of points are selected. + + Args + ---- + cluster_distances (Dict[int, float]): A dictionary mapping cluster labels (int) to + their calculated distance from a reference point. + clusters (Dict[int, cp.ndarray]): A dictionary mapping each cluster label (int) to an + array of points (XYZ coordinates). + num_top_clusters (int): The number of top clusters to select based on distance. + + Returns + ------- + List[cp.ndarray]: A list of the top N clusters, where each cluster is represented by an + array of points (XYZ coordinates). + + """ + sorted_cluster_distances = sorted( + cluster_distances.items(), key=lambda item: item[1]) + selected_top_clusters = sorted_cluster_distances[:num_top_clusters] + selected_clusters = [clusters[label] + for label, _ in selected_top_clusters] + + return selected_clusters + + def publish_all_clusters( + self, + filtered_clusters: Dict[int, cp.ndarray] + ) -> None: + """ + Publish colored point clouds for each cluster using a ROS PointCloud2 message. + + Initializes the header with the current timestamp and sets the frame ID to the base link + of the manipulator. The function sets up the point fields for XYZ and RGB values, where + the XYZ values represent the positions of points in each cluster and RGB values represent + the color assigned to each cluster. + + The colors are assigned based on the order in which clusters are processed. + + Args + ---- + filtered_clusters (Dict[int, cp.ndarray]): A dictionary mapping each cluster label + (int) to an array of XYZ coordinates representing the points in the cluster. + + Returns + ------- + None + + """ + if not filtered_clusters: + return + + header = Header() + header.stamp = self.get_clock().now().to_msg() + header.frame_id = self.__cfg_base_link + + fields = [ + PointField(name='x', offset=0, + datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, + datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, + datatype=PointField.FLOAT32, count=1), + PointField(name='rgb', offset=12, + datatype=PointField.UINT32, count=1), + ] + + unique_labels = list(filtered_clusters.keys()) + color_map = self.get_cluster_colors(len(unique_labels)) + cluster_colors = {label: color_map[i] + for i, label in enumerate(unique_labels)} + + points_with_color = [] + + # Convert all cupy arrays to numpy arrays and then to lists outside the loop + filtered_clusters_list = {label: points.get().tolist() + for label, points in filtered_clusters.items()} + + for label, points_list in filtered_clusters_list.items(): + rgb = cluster_colors[label] + r, g, b = rgb + rgb_int = struct.unpack('I', struct.pack('BBBB', b, g, r, 255))[0] + for point in points_list: + x, y, z = point + points_with_color.append([x, y, z, rgb_int]) + + cloud_data = point_cloud2.create_cloud( + header, fields, points_with_color) + self.__clustered_points_publisher.publish(cloud_data) + + def get_cluster_colors( + self, + num_clusters: int + ) -> Dict[int, tuple]: + """ + Generate a color map for clusters, assigning each cluster a unique RGB color. + + Creates a dictionary where each cluster (represented by an integer) is mapped to an RGB + tuple. Predefined colors (red, green, blue, yellow, etc.) are used for the first few + clusters, and random colors are generated for additional clusters if the number of clusters + exceeds the predefined colors list. + + Args + ---- + num_clusters (int): The number of clusters to be colored. If more than the available + predefined colors, random colors are generated for the additional clusters. + + Returns + ------- + Dict[int, tuple]: A dictionary mapping each cluster to an RGB color. + + """ + fixed_colors = [ + (255, 0, 0), # Red + (0, 255, 0), # Green + (0, 0, 255), # Blue + (255, 255, 0), # Yellow + (0, 255, 255) # Cyan + ] + + color_map = {} + for i in range(min(num_clusters, len(fixed_colors))): + color_map[i] = fixed_colors[i] + + np.random.seed(42) # For reproducibility + used_colors = set(fixed_colors) + for i in range(len(fixed_colors), num_clusters): + while True: + random_color = (np.random.randint(0, 255), np.random.randint( + 0, 255), np.random.randint(0, 255)) + if random_color not in used_colors: + color_map[i] = random_color + used_colors.add(random_color) + break + + return color_map + + def publish_point( + self, + object_frame_origin_tensor: torch.Tensor + ) -> None: + """ + Publish the origin point of the object frame using a ROS PointStamped message. + + Takes the object's origin point, expressed in the robot's base link frame, and publishes it + using a ROS `PointStamped` message. The point is initialized with the necessary headers + (including the timestamp) and then published. + + Args + ---- + object_frame_origin_tensor (torch.Tensor): A 3D point representing the origin of the + object frame, expressed in the robot's base link frame. + + Returns + ------- + None + + """ + self.point = object_frame_origin_tensor.cpu().numpy().flatten() + + point_msg = PointStamped() + point_msg.header = Header() + point_msg.header.stamp = self.get_clock().now().to_msg() + point_msg.header.frame_id = self.__cfg_base_link + point_msg.point.x = float(self.point[0]) + point_msg.point.y = float(self.point[1]) + point_msg.point.z = float(self.point[2]) + + self.__object_origin_publisher.publish(point_msg) + + def publish_point_cloud( + self, + point_cloud: torch.Tensor, + publisher: Publisher + ) -> None: + """ + Publish a point cloud using a ROS PointCloud2 message. + + Converts the input point cloud (a tensor containing x, y, z coordinates) into a + ROS `PointCloud2` message. It attaches a timestamp and sets the frame ID to the + robot's base link. The point cloud data is then published via the provided publisher. + + Args + ---- + point_cloud (torch.Tensor): A tensor representing the 3D point cloud with x, y, z + coordinates. + publisher (Publisher): A ROS publisher object used to publish `PointCloud2` message. + + Returns + ------- + None + + """ + # Initialize PointCloud2 message + point_cloud_msg = PointCloud2() + point_cloud_msg.header.stamp = self.get_clock().now().to_msg() + point_cloud_msg.header.frame_id = self.__cfg_base_link + point_cloud_msg.height = 1 + point_cloud_msg.width = point_cloud.shape[0] + + # Define PointField layout + point_cloud_msg.fields = [ + PointField(name='x', offset=0, + datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, + datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, + datatype=PointField.FLOAT32, count=1), + ] + point_cloud_msg.is_bigendian = False + point_cloud_msg.point_step = 12 # 3 fields * 4 bytes per field + point_cloud_msg.row_step = point_cloud_msg.point_step * point_cloud_msg.width + point_cloud_msg.is_dense = False + + # Serialize point cloud data + point_cloud_msg.data = b''.join( + struct.pack('fff', *point) for point in point_cloud.cpu().numpy() + ) + + # Publish the point cloud + publisher.publish(point_cloud_msg) + + def get_object_mesh( + self, + object_point_cloud: torch.Tensor, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> CuMesh: + """ + Generate a 3D mesh from the object's point cloud using the CuMesh class. + + Converts the input point cloud (a torch tensor) into a numpy array and uses the + `from_point_cloud` method of the `CuMesh` class from `curobo.geom.types` to generate a + 3D mesh. The mesh is created using the marching cubes algorithm, which approximates the + object's surface by forming a convex hull around the point cloud. + + Args + ---- + object_point_cloud (torch.Tensor): A tensor representing the 3D object point cloud, + expressed with respect to robot base frame + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the original + attach/detach goal, to which the feedback is published. + + Returns + ------- + CuMesh: A mesh object of type `CuMesh`, representing the object's surface generated + from the point cloud data. + + """ + start_time = time.time() + + object_point_cloud_np = object_point_cloud.cpu().numpy() + + object_mesh = CuMesh.from_pointcloud(pointcloud=object_point_cloud_np) + + self.__att_obj_srv_fb_msg.status = f'Generated object mesh in {time.time()-start_time}s' + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + return object_mesh + + def generate_object_collision_spheres_from_mesh( + self, + joint_states: JointState, + object_mesh: CuMesh, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> None: + """ + Attach an external object to the robot and generate collision spheres. + + Uses the robot's forward kinematics model to attach the external object (in the form of a + mesh) to a specified link. Then retrieves the collision sphere information for the link, + which includes the coordinates (x, y, z) and radii for each sphere in the object frame. + + Args + ---- + joint_states (JointState): Contains the state information (e.g., positions, velocities, + accelerations) for each joint in the configuration space of the manipulator. + object_mesh (CuMesh): The mesh representing the external object to be attached. All + coordinates of the mesh vertices are expressed with respect to robot base frame. + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the original + attach/detach goal, to which the feedback is published. + + Returns + ------- + None + + """ + start_time = time.time() + + self.__kin_model.attach_external_objects_to_robot( + joint_state=joint_states, + external_objects=[object_mesh], + surface_sphere_radius=self.__surface_sphere_radius, + link_name=self.__object_link_name + ) + + link_spheres_object_frame = self.__kin_model.kinematics_config.get_link_spheres( + link_name=self.__object_link_name) + + self.__object_spheres = link_spheres_object_frame.cpu().numpy() + + self.__att_obj_srv_fb_msg.status = ( + f'Generated object collision spheres in {time.time()-start_time}s' + ) + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + def publish_robot_spheres( + self, + joint_states: JointState, + att_obj_srv_goal_handle: ServerGoalHandle + ) -> None: + """ + Publish the robot's collision spheres based on joint state information. + + Uses the forward kinematics model and the state information from `JointState` (e.g., + positions, velocities, accelerations) to obtain robot's collision spheres, approximating + its geometry in the robot base frame. These spheres are published using an API as a marker + array, which can be visualized in RViz. + + Args + ---- + joint_states (JointState): Contains the state information (e.g., positions, velocities, + accelerations) for each joint in the manipulator. + att_obj_srv_goal_handle (ServerGoalHandle): Manages the state of the original + attach/detach goal, to which the feedback is published. + + Returns + ------- + None + + """ + start_time = time.time() + + link_pos, link_quat, _, _, _, _, robot_spheres_robot_base_frame = ( + self.__kin_model.forward( + q=joint_states.position, + link_name=self.__object_link_name + ) + ) + + m_arr = get_spheres_marker( + robot_spheres=robot_spheres_robot_base_frame.cpu().numpy().squeeze(0), + base_frame=self.__cfg_base_link, + time=self.get_clock().now().to_msg(), + rgb=[1.0, 0.0, 0.0, 1.0], + ) + + self.__robot_sphere_markers_publisher.publish(m_arr) + + self.__att_obj_srv_fb_msg.status = f'Published object spheres in {time.time()-start_time}s' + att_obj_srv_goal_handle.publish_feedback(self.__att_obj_srv_fb_msg) + + +def main(args=None): + + rclpy.init(args=args) + attach_object_server = AttachObjectServer() + executor = MultiThreadedExecutor() + executor.add_node(attach_object_server) + try: + executor.spin() + except KeyboardInterrupt: + attach_object_server.get_logger().info('KeyboardInterrupt, shutting down.\n') + attach_object_server.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/isaac_ros_cumotion_object_attachment/launch/cumotion_bringup.launch.py b/isaac_ros_cumotion_object_attachment/launch/cumotion_bringup.launch.py new file mode 100644 index 0000000..1f64f63 --- /dev/null +++ b/isaac_ros_cumotion_object_attachment/launch/cumotion_bringup.launch.py @@ -0,0 +1,283 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import SetParameter + + +def generate_launch_description(): + + # The 'robot' argument can accept: + # - XRDF filename + # - YAML filename + # - Absolute paths for XRDF or YAML files + + # Default URDF file path (currently absolute, should be updated as needed). + default_urdf_file_path = os.path.join( + get_package_share_directory('isaac_ros_cumotion_robot_description'), + 'urdf', + 'ur5e_robotiq_2f_140.urdf' + ) + + # Declare launch arguments with full paths + launch_args = [ + DeclareLaunchArgument( + 'robot', + default_value='ur5e_robotiq_2f_140.xrdf', + description='Robot file (XRDF or YAML)' + ), + DeclareLaunchArgument( + 'urdf_file_path', + default_value=default_urdf_file_path, + description='Full path to the URDF file' + ), + DeclareLaunchArgument( + 'yml_file_path', + default_value='', + description='Path to the YAML file containing robot configurations' + ), + DeclareLaunchArgument( + 'joint_states_topic', + default_value='/joint_states', + description='Joint states topic' + ), + DeclareLaunchArgument( + 'depth_camera_info_topics', + default_value="['/camera_1/color/camera_info']", + description='Depth camera info topic' + ), + DeclareLaunchArgument( + 'depth_image_topics', + default_value="['/camera_1/aligned_depth_to_color/image_raw']", + description='Depth image topic for robot segmenter' + ), + DeclareLaunchArgument( + 'object_link_name', + default_value='attached_object', + description='Object link name for object attachment' + ), + DeclareLaunchArgument( + 'search_radius', + default_value='0.2', + description='Search radius for object attachment' + ), + DeclareLaunchArgument( + 'surface_sphere_radius', + default_value='0.01', + description='Radius for object surface collision spheres' + ), + DeclareLaunchArgument( + 'update_link_sphere_server_segmenter', + default_value='segmenter_attach_object', + description='Update link sphere server for robot segmenter' + ), + DeclareLaunchArgument( + 'update_link_sphere_server_planner', + default_value='planner_attach_object', + description='Update link sphere server for cumotion planner' + ), + DeclareLaunchArgument( + 'clustering_bypass', + default_value='False', + description='Whether to bypass clustering' + ), + DeclareLaunchArgument( + 'update_esdf_on_request', + default_value='False', + description='Whether object attachment should request an updated ESDF from nvblox ' + 'as part of the service call' + ), + DeclareLaunchArgument( + 'action_names', + default_value="['segmenter_attach_object', 'planner_attach_object']", + description='List of action names for the object attachment' + ), + DeclareLaunchArgument( + 'time_sync_slop', + default_value='0.1', + description='Time synchronization slop' + ), + DeclareLaunchArgument( + 'distance_threshold', + default_value='0.02', + description='Distance threshold for segmentation' + ), + DeclareLaunchArgument( + 'clustering_hdbscan_min_samples', + default_value='20', + description='HDBSCAN min samples for clustering' + ), + DeclareLaunchArgument( + 'clustering_hdbscan_min_cluster_size', + default_value='30', + description='HDBSCAN min cluster size for clustering' + ), + DeclareLaunchArgument( + 'clustering_hdbscan_cluster_selection_epsilon', + default_value='0.5', + description='HDBSCAN cluster selection epsilon' + ), + DeclareLaunchArgument( + 'clustering_num_top_clusters_to_select', + default_value='3', + description='Number of top clusters to select' + ), + DeclareLaunchArgument( + 'clustering_group_clusters', + default_value='False', + description='Whether to group clusters' + ), + DeclareLaunchArgument( + 'clustering_min_points', + default_value='100', + description='Minimum points for clustering' + ), + ] + + # LaunchConfiguration objects to pass to the launch files + robot = LaunchConfiguration('robot') + urdf_path = LaunchConfiguration('urdf_file_path') + yml_file_path = LaunchConfiguration('yml_file_path') + joint_states_topic = LaunchConfiguration('joint_states_topic') + depth_camera_info_topics = LaunchConfiguration('depth_camera_info_topics') + depth_image_topics = LaunchConfiguration('depth_image_topics') + object_link_name = LaunchConfiguration('object_link_name') + search_radius = LaunchConfiguration('search_radius') + surface_sphere_radius = LaunchConfiguration('surface_sphere_radius') + update_esdf_on_request = LaunchConfiguration('update_esdf_on_request') + update_link_sphere_server_segmenter = LaunchConfiguration( + 'update_link_sphere_server_segmenter') + update_link_sphere_server_planner = LaunchConfiguration( + 'update_link_sphere_server_planner') + clustering_bypass = LaunchConfiguration('clustering_bypass') + action_names = LaunchConfiguration('action_names') + time_sync_slop = LaunchConfiguration('time_sync_slop') + distance_threshold = LaunchConfiguration('distance_threshold') + clustering_hdbscan_min_samples = LaunchConfiguration( + 'clustering_hdbscan_min_samples') + clustering_hdbscan_min_cluster_size = LaunchConfiguration( + 'clustering_hdbscan_min_cluster_size') + clustering_hdbscan_cluster_selection_epsilon = LaunchConfiguration( + 'clustering_hdbscan_cluster_selection_epsilon') + clustering_num_top_clusters_to_select = LaunchConfiguration( + 'clustering_num_top_clusters_to_select') + clustering_group_clusters = LaunchConfiguration( + 'clustering_group_clusters') + clustering_min_points = LaunchConfiguration('clustering_min_points') + + # Shared world depth topic as a string array + world_depth_topic = "['/cumotion/camera_1/world_depth']" + + # Paths to the launch files + cumotion_launch_path = os.path.join( + get_package_share_directory('isaac_ros_cumotion'), + 'launch', + 'isaac_ros_cumotion.launch.py') + + robot_segmenter_launch_path = os.path.join( + get_package_share_directory('isaac_ros_cumotion'), + 'launch', + 'robot_segmentation.launch.py') + + object_attachment_launch_path = os.path.join( + get_package_share_directory('isaac_ros_cumotion_object_attachment'), + 'launch', + 'object_attachment.launch.py') + + # Include the launch files with updated arguments + cumotion_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(cumotion_launch_path), + launch_arguments={ + 'cumotion_planner.robot': robot, + 'cumotion_planner.urdf_path': urdf_path, + 'cumotion_planner.yml_file_path': yml_file_path, + 'cumotion_planner.update_link_sphere_server': + update_link_sphere_server_planner, + }.items() + ) + + robot_segmenter_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(robot_segmenter_launch_path), + launch_arguments={ + 'robot_segmenter.robot': robot, + 'robot_segmenter.urdf_path': urdf_path, + 'cumotion_planner.yml_file_path': yml_file_path, + 'robot_segmenter.depth_image_topics': depth_image_topics, + 'robot_segmenter.depth_camera_infos': depth_camera_info_topics, + 'robot_segmenter.joint_states_topic': joint_states_topic, + 'robot_segmenter.time_sync_slop': time_sync_slop, + 'robot_segmenter.distance_threshold': distance_threshold, + 'robot_segmenter.update_link_sphere_server': + update_link_sphere_server_segmenter, + 'robot_segmenter.world_depth_publish_topics': world_depth_topic, + 'robot_segmenter.depth_qos': 'SENSOR_DATA', + 'robot_segmenter.depth_info_qos': 'SENSOR_DATA', + 'robot_segmenter.mask_qos': 'SENSOR_DATA', + 'robot_segmenter.world_depth_qos': 'SENSOR_DATA', + + }.items() + ) + + object_attachment_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(object_attachment_launch_path), + launch_arguments={ + 'object_attachment.robot': robot, + 'object_attachment.urdf_path': urdf_path, + 'object_attachment.time_sync_slop': time_sync_slop, + 'object_attachment.joint_states_topic': joint_states_topic, + 'object_attachment.depth_image_topics': world_depth_topic, + 'object_attachment.depth_camera_infos': depth_camera_info_topics, + 'object_attachment.object_link_name': object_link_name, + 'object_attachment.action_names': action_names, + 'object_attachment.search_radius': search_radius, + 'object_attachment.surface_sphere_radius': surface_sphere_radius, + 'object_attachment.update_esdf_on_request': update_esdf_on_request, + 'object_attachment.clustering_bypass_clustering': + clustering_bypass, + 'object_attachment.clustering_hdbscan_min_samples': + clustering_hdbscan_min_samples, + 'object_attachment.clustering_hdbscan_min_cluster_size': + clustering_hdbscan_min_cluster_size, + 'object_attachment.clustering_hdbscan_cluster_selection_epsilon': + clustering_hdbscan_cluster_selection_epsilon, + 'object_attachment.clustering_num_top_clusters_to_select': + clustering_num_top_clusters_to_select, + 'object_attachment.clustering_group_clusters': + clustering_group_clusters, + 'object_attachment.clustering_min_points': + clustering_min_points, + 'object_attachment.depth_qos': 'SENSOR_DATA', + 'object_attachment.depth_info_qos': 'SENSOR_DATA', + }.items() + ) + # Add sim time parameter as we always run this launch file with a ROSbag which needs this + # parameter so that object attachment can filter depth images correctly. + use_sim_time_param = [SetParameter(name='use_sim_time', value=True)] + # Return the LaunchDescription with all included launch files + return LaunchDescription(launch_args + use_sim_time_param + [ + cumotion_launch, + robot_segmenter_launch, + object_attachment_launch + ]) diff --git a/isaac_ros_cumotion_object_attachment/launch/object_attachment.launch.py b/isaac_ros_cumotion_object_attachment/launch/object_attachment.launch.py new file mode 100755 index 0000000..62ab62d --- /dev/null +++ b/isaac_ros_cumotion_object_attachment/launch/object_attachment.launch.py @@ -0,0 +1,66 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES', +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +import yaml + + +def read_params(pkg_name, params_dir, params_file_name): + params_file = os.path.join( + get_package_share_directory(pkg_name), params_dir, params_file_name) + return yaml.safe_load(open(params_file, 'r')) + + +def launch_args_from_params(pkg_name, params_dir, params_file_name, prefix: str = None): + launch_args = [] + launch_configs = {} + params = read_params(pkg_name, params_dir, params_file_name) + + for param, value in params['/**']['ros__parameters'].items(): + if value is not None: + arg_name = param if prefix is None else f'{prefix}.{param}' + launch_args.append(DeclareLaunchArgument( + name=arg_name, default_value=str(value))) + launch_configs[param] = LaunchConfiguration(arg_name) + + return launch_args, launch_configs + + +def generate_launch_description(): + """Launch file to bring up object attachment server node.""" + launch_args, launch_configs = launch_args_from_params( + 'isaac_ros_cumotion_object_attachment', + 'params', 'object_attachment_params.yaml', 'object_attachment') + + attach_object_server_node = Node( + package='isaac_ros_cumotion_object_attachment', + namespace='', + executable='attach_object_server_node', + name='object_attachment', + parameters=[launch_configs], + output='screen', + ) + + return LaunchDescription(launch_args + [attach_object_server_node]) diff --git a/isaac_ros_cumotion_object_attachment/package.xml b/isaac_ros_cumotion_object_attachment/package.xml new file mode 100644 index 0000000..3de7295 --- /dev/null +++ b/isaac_ros_cumotion_object_attachment/package.xml @@ -0,0 +1,54 @@ + + + + + + + isaac_ros_cumotion_object_attachment + 3.2.0 + Package attaches/detaches a grasped object to robot's collision geometry + + Isaac ROS Maintainers + Apache-2.0 + https://developer.nvidia.com/isaac-ros-gems/ + Animesh Singhal + + curobo_core + isaac_ros_cumotion + isaac_ros_cumotion_interfaces + isaac_manipulator_ros_python_utils + isaac_ros_cumotion_robot_description + isaac_ros_launch_utils + rclpy + + ament_cmake + + action_msgs + rosidl_default_generators + isaac_ros_common + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/isaac_ros_cumotion_object_attachment/params/object_attachment_params.yaml b/isaac_ros_cumotion_object_attachment/params/object_attachment_params.yaml new file mode 100755 index 0000000..5a45c40 --- /dev/null +++ b/isaac_ros_cumotion_object_attachment/params/object_attachment_params.yaml @@ -0,0 +1,48 @@ +/**: + ros__parameters: + + # Robot info + robot: '' + urdf_path: '' + + # Device configuration + cuda_device: 0 + + # Time and synchronization params + time_sync_slop: 0.1 + filter_depth_buffer_time: 0.07 + tf_lookup_duration: 5.0 + + # Topic names + joint_states_topic: /joint_states + depth_image_topics: [/depth_image] + depth_camera_infos: [/rgb/camera_info] + + # Object attachment params + object_link_name: 'attached_object' + action_names: ['segmenter_attach_object', 'planner_attach_object'] + search_radius: 0.2 + surface_sphere_radius: 0.01 + object_attachment_gripper_frame_name: 'grasp_frame' + object_attachment_n_spheres: 100 + + # Clustering params + clustering_bypass_clustering: False + clustering_hdbscan_min_samples: 20 + clustering_hdbscan_min_cluster_size: 30 + clustering_hdbscan_cluster_selection_epsilon: 0.5 + clustering_num_top_clusters_to_select: 3 + clustering_group_clusters: False + clustering_min_points: 100 + + # QOS + depth_qos: 'DEFAULT' + depth_info_qos: 'DEFAULT' + + # Sphere radius + surface_sphere_radius: 0.01 + + # nvblox params + update_esdf_on_request: False + trigger_aabb_object_clearing: False + object_esdf_clearing_padding: [0.05, 0.05, 0.05] diff --git a/isaac_ros_cumotion_object_attachment/resource/isaac_ros_cumotion_object_attachment b/isaac_ros_cumotion_object_attachment/resource/isaac_ros_cumotion_object_attachment new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_cumotion_object_attachment/setup.cfg b/isaac_ros_cumotion_object_attachment/setup.cfg new file mode 100644 index 0000000..7de4640 --- /dev/null +++ b/isaac_ros_cumotion_object_attachment/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/isaac_ros_cumotion_object_attachment +[install] +install_scripts=$base/lib/isaac_ros_cumotion_object_attachment diff --git a/isaac_ros_cumotion_object_attachment/setup.py b/isaac_ros_cumotion_object_attachment/setup.py new file mode 100644 index 0000000..f0c65ab --- /dev/null +++ b/isaac_ros_cumotion_object_attachment/setup.py @@ -0,0 +1,58 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'isaac_ros_cumotion_object_attachment' + +setup( + name=package_name, + version='3.2.0', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ( + os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*launch.[pxy][yma]*')), + ), + ( + os.path.join('share', package_name, 'params'), + glob(os.path.join('params', '*.[yma]*')), + ), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Isaac ROS Maintainers', + maintainer_email='isaac-ros-maintainers@nvidia.com', + author='Animesh Singhal', + description='Package attaches/detaches a grasped object to robot collision geometry', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'attach_object_server_node = ' + 'isaac_ros_cumotion_object_attachment.attach_object_server:main', + + 'attach_object_client_node = ' + 'isaac_ros_cumotion_object_attachment.attach_object_client:main', + ], + }, +) diff --git a/isaac_ros_cumotion_object_attachment/test/test_copyright.py b/isaac_ros_cumotion_object_attachment/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/isaac_ros_cumotion_object_attachment/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/isaac_ros_cumotion_object_attachment/test/test_flake8.py b/isaac_ros_cumotion_object_attachment/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/isaac_ros_cumotion_object_attachment/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/isaac_ros_cumotion_object_attachment/test/test_pep257.py b/isaac_ros_cumotion_object_attachment/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/isaac_ros_cumotion_object_attachment/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/isaac_ros_cumotion_python_utils/isaac_ros_cumotion_python_utils/__init__.py b/isaac_ros_cumotion_python_utils/isaac_ros_cumotion_python_utils/__init__.py new file mode 100644 index 0000000..34d9bfe --- /dev/null +++ b/isaac_ros_cumotion_python_utils/isaac_ros_cumotion_python_utils/__init__.py @@ -0,0 +1,16 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/isaac_ros_cumotion_python_utils/isaac_ros_cumotion_python_utils/utils.py b/isaac_ros_cumotion_python_utils/isaac_ros_cumotion_python_utils/utils.py new file mode 100644 index 0000000..7f33df5 --- /dev/null +++ b/isaac_ros_cumotion_python_utils/isaac_ros_cumotion_python_utils/utils.py @@ -0,0 +1,102 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import math +import numpy as np +import numpy.typing as npt +from typing import List, Tuple +import yaml + + +def load_grid_corners_from_workspace_file( + workspace_file_path: str) -> Tuple[npt.NDArray, npt.NDArray]: + """Load a workspace file and return the min / max corners specified. + + All voxels that are fully or partially contained in the bounding box + defined by the min / max corners are part of the workspace. + + See the following file for an example: + - isaac_manipulator_bringup/config/nvblox/workspace_bounds/zurich_test_bench.yaml + + Args: + workspace_file_path (str): The absolute path to the workspace file. + + Returns: + Tuple[npt.NDArray, npt.NDArray]: A tuple of two 3d vectors specifying the + min and max corners of the workspace. + """ + with open(workspace_file_path) as config_file: + config_dict = yaml.safe_load(config_file)['/**']['ros__parameters']['static_mapper'] + min_corner = np.array([ + config_dict['workspace_bounds_min_corner_x_m'], + config_dict['workspace_bounds_min_corner_y_m'], + config_dict['workspace_bounds_min_height_m'] + ]) + max_corner = np.array([ + config_dict['workspace_bounds_max_corner_x_m'], + config_dict['workspace_bounds_max_corner_y_m'], + config_dict['workspace_bounds_max_height_m'] + ]) + return min_corner, max_corner + + +def get_grid_center(min_corner: List[float], grid_size: List[float]) -> List[float]: + """Get the grid center from the minimum corner and grid size.""" + return (np.array(min_corner) + 0.5 * np.array(grid_size)).tolist() + + +def get_grid_min_corner(grid_center: List[float], grid_size: List[float]) -> List[float]: + """Get the grid minimum corner from the grid center and the grid size.""" + return (np.array(grid_center) - 0.5 * np.array(grid_size)).tolist() + + +def get_grid_size( + min_corner: List[float], + max_corner: List[float], + voxel_size: float, + ) -> List[float]: + """Get the grid size from corners and voxel size""" + inv_voxel_size = 1.0 / voxel_size + min_in_vox = [robust_floor(x * inv_voxel_size) for x in min_corner] + max_in_vox = [robust_floor(x * inv_voxel_size) for x in max_corner] + dims_in_vox = [max_in_vox[i] - min_in_vox[i] for i in range(len(min_in_vox))] + + dims_in_meters = [x * voxel_size for x in dims_in_vox] + + return dims_in_meters + + +def is_grid_valid(grid_size: List[float], voxel_size: float) -> bool: + """Check if the grid is valid. + + Currently the only restriction is that there should be at least 1 voxel per dimension. + + Returns: + bool: Whether the grid is valid. + """ + num_voxels_per_dimension = np.array(grid_size) / voxel_size + num_voxels_per_dim_is_zero = (num_voxels_per_dimension <= 0).any() + return num_voxels_per_dim_is_zero + + +def robust_floor(x: float, threshold: float = 1e-04) -> int: + """Floor float to integer when within threshold to account for floating point precision.""" + nearest_int = round(x) + if (abs(x - nearest_int) < threshold): + return nearest_int + else: + return int(math.floor(x)) diff --git a/isaac_ros_cumotion_python_utils/package.xml b/isaac_ros_cumotion_python_utils/package.xml new file mode 100644 index 0000000..b4594f2 --- /dev/null +++ b/isaac_ros_cumotion_python_utils/package.xml @@ -0,0 +1,16 @@ + + + + isaac_ros_cumotion_python_utils + 3.2.0 + Python utilities used across Isaac ROS cuMotion + Isaac ROS Maintainers + NVIDIA Isaac ROS Software License + https://developer.nvidia.com/isaac-ros-gems/ + Remo Steiner + isaac_ros_common + + + ament_python + + diff --git a/isaac_ros_cumotion_python_utils/resource/isaac_ros_cumotion_python_utils b/isaac_ros_cumotion_python_utils/resource/isaac_ros_cumotion_python_utils new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_cumotion_python_utils/setup.cfg b/isaac_ros_cumotion_python_utils/setup.cfg new file mode 100644 index 0000000..d6fdb98 --- /dev/null +++ b/isaac_ros_cumotion_python_utils/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/isaac_ros_cumotion_python_utils +[install] +install_scripts=$base/lib/isaac_ros_cumotion_python_utils diff --git a/isaac_ros_cumotion_python_utils/setup.py b/isaac_ros_cumotion_python_utils/setup.py new file mode 100644 index 0000000..972cea7 --- /dev/null +++ b/isaac_ros_cumotion_python_utils/setup.py @@ -0,0 +1,65 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import importlib.util +from pathlib import Path +import sys + +from ament_index_python.packages import get_resource +from setuptools import setup + +ISAAC_ROS_COMMON_PATH = get_resource( + 'isaac_ros_common_scripts_path', + 'isaac_ros_common' +)[0] + +ISAAC_ROS_COMMON_VERSION_INFO = Path(ISAAC_ROS_COMMON_PATH) / 'isaac_ros_common-version-info.py' + +spec = importlib.util.spec_from_file_location( + 'isaac_ros_common_version_info', + ISAAC_ROS_COMMON_VERSION_INFO +) + +isaac_ros_common_version_info = importlib.util.module_from_spec(spec) +sys.modules['isaac_ros_common_version_info'] = isaac_ros_common_version_info +spec.loader.exec_module(isaac_ros_common_version_info) + +from isaac_ros_common_version_info import GenerateVersionInfoCommand # noqa: E402, I100 + +package_name = 'isaac_ros_cumotion_python_utils' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Isaac ROS Maintainers', + maintainer_email='isaac-ros-maintainers@nvidia.com', + description='Python utilities used across Isaac ROS cuMotion', + license='Apache-2.0', + tests_require=[], + entry_points={}, + cmdclass={ + 'build_py': GenerateVersionInfoCommand, + }, +) diff --git a/isaac_ros_cumotion_robot_description/package.xml b/isaac_ros_cumotion_robot_description/package.xml index 6714fc7..95198da 100644 --- a/isaac_ros_cumotion_robot_description/package.xml +++ b/isaac_ros_cumotion_robot_description/package.xml @@ -2,10 +2,11 @@ isaac_ros_cumotion_robot_description - 3.1.0 + 3.2.0 Package containing XRDF (extended robot description format) files for various robots Isaac ROS Maintainers Apache-2.0 + isaac_ros_common https://developer.nvidia.com/isaac-ros-gems/ ament_copyright diff --git a/isaac_ros_cumotion_robot_description/setup.py b/isaac_ros_cumotion_robot_description/setup.py index 500ed18..1bc6dc0 100644 --- a/isaac_ros_cumotion_robot_description/setup.py +++ b/isaac_ros_cumotion_robot_description/setup.py @@ -33,6 +33,10 @@ os.path.join('share', package_name, 'xrdf'), glob(os.path.join('xrdf', '*.xrdf')), ), + ( + os.path.join('share', package_name, 'urdf'), + glob(os.path.join('urdf', '*.urdf')), + ), ], install_requires=['setuptools'], zip_safe=True, diff --git a/isaac_ros_cumotion_robot_description/urdf/ur10e_robotiq_2f_140.urdf b/isaac_ros_cumotion_robot_description/urdf/ur10e_robotiq_2f_140.urdf new file mode 100644 index 0000000..96d91ac --- /dev/null +++ b/isaac_ros_cumotion_robot_description/urdf/ur10e_robotiq_2f_140.urdf @@ -0,0 +1,705 @@ + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/isaac_ros_cumotion_robot_description/urdf/ur5e_robotiq_2f_140.urdf b/isaac_ros_cumotion_robot_description/urdf/ur5e_robotiq_2f_140.urdf new file mode 100644 index 0000000..454edc9 --- /dev/null +++ b/isaac_ros_cumotion_robot_description/urdf/ur5e_robotiq_2f_140.urdf @@ -0,0 +1,704 @@ + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/isaac_ros_cumotion_robot_description/urdf/ur5e_robotiq_2f_85.urdf b/isaac_ros_cumotion_robot_description/urdf/ur5e_robotiq_2f_85.urdf new file mode 100644 index 0000000..d113a04 --- /dev/null +++ b/isaac_ros_cumotion_robot_description/urdf/ur5e_robotiq_2f_85.urdf @@ -0,0 +1,830 @@ + + + + + + + + + + + + + + + ur_robot_driver/URPositionHardwareInterface + 192.56.1.2 + /opt/ros/humble/share/ur_client_library/resources/external_control.urscript + /opt/ros/humble/share/ur_robot_driver/resources/rtde_output_recipe.txt + /opt/ros/humble/share/ur_robot_driver/resources/rtde_input_recipe.txt + False + 50001 + 50002 + 0.0.0.0 + 50004 + 50003 + + True + 2000 + 0.03 + False + calib_12788084448423163542 + 0 + 0 + 115200 + 1 + 1.5 + 3.5 + /tmp/ttyUR + 54321 + 2 + + + + + + + 0.0 + + + + + + + + + + -1.57 + + + + + + + + + + 0.0 + + + + + + + + + + -1.57 + + + + + + + + + + 0.0 + + + + + + + + + + 0.0 + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + false + robotiq_driver/RobotiqGripperHardwareInterface + 0.7929 + /tmp/ttyUR + 1.0 + 0.5 + + + + + + + 0.7929 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100000.0 + 100000.0 + + + + + 1e+5 + 1 + 0 + 0.2 + 0.002 + 0 + + + + + + + + + + + + + + + + + + + + + + + + 100000.0 + 100000.0 + + + + + 1e+5 + 1 + 0 + 0.2 + 0.002 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/isaac_ros_cumotion_robot_description/xrdf/ur10e_robotiq_2f_140.xrdf b/isaac_ros_cumotion_robot_description/xrdf/ur10e_robotiq_2f_140.xrdf new file mode 100644 index 0000000..d7f7aca --- /dev/null +++ b/isaac_ros_cumotion_robot_description/xrdf/ur10e_robotiq_2f_140.xrdf @@ -0,0 +1,328 @@ +format: xrdf +format_version: 1.0 + +modifiers: + - set_base_frame: "base_link" + - add_frame: + frame_name: "attached_object" + parent_frame_name: "grasp_frame" + joint_name: "attach_joint" + joint_type: FIXED + fixed_transform: + position: [0.0, 0.0, 0.0] + orientation: {w: 1.0, xyz: [0.0, 0.0, 0.0]} + +default_joint_positions: + shoulder_pan_joint: 0.0 + shoulder_lift_joint: -2.2 + elbow_joint: 1.0 + wrist_1_joint: -1.383 + wrist_2_joint: -1.57 + wrist_3_joint: 0.00 + +cspace: + joint_names: + - "shoulder_pan_joint" + - "shoulder_lift_joint" + - "elbow_joint" + - "wrist_1_joint" + - "wrist_2_joint" + - "wrist_3_joint" + acceleration_limits: [12.0, 12.0, 12.0, 12.0, 12.0, 12.0] + jerk_limits: [500.0, 500.0, 500.0, 500.0, 500.0, 500.0] + +tool_frames: ["grasp_frame"] + +collision: + geometry: "ur_robotiq_2f_140_collision_spheres" + buffer_distance: + shoulder_link: 0.005 + upper_arm_link: 0.005 + forearm_link: 0.005 + wrist_1_link: 0.005 + wrist_2_link: 0.005 + wrist_3_link: 0.005 + tool0: 0.005 + robotiq_base_link: 0.005 + left_outer_finger: 0.005 + left_inner_finger_pad: 0.005 + left_inner_knuckle: 0.005 + left_inner_finger: 0.005 + left_outer_knuckle: 0.005 + right_outer_finger: 0.005 + right_inner_finger_pad: 0.005 + right_inner_knuckle: 0.005 + right_inner_finger: 0.005 + attached_object: 0.005 + +self_collision: + geometry: "ur_robotiq_2f_140_collision_spheres" + buffer_distance: + shoulder_link: 0.01 + tool0: 0.02 + ignore: + upper_arm_link: ["forearm_link", "shoulder_link"] + forearm_link: ["wrist_1_link", "wrist_2_link", "wrist_3_link"] + wrist_1_link: ["wrist_2_link","wrist_3_link","tool0", "robotiq_base_link"] + wrist_2_link: ["wrist_3_link", "tool0", "robotiq_base_link"] + wrist_3_link: ["tool0", "robotiq_base_link", "left_outer_finger"] + + tool0: ['robotiq_base_link', 'left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'] + + robotiq_base_link: ['left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'] + + left_outer_finger: ['robotiq_base_link', + 'left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle', + 'tool0', 'wrist_3_link'] + + left_inner_finger_pad: [ + 'robotiq_base_link', + 'left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle', + 'tool0', 'wrist_3_link'] + + left_inner_knuckle: ['robotiq_base_link', + 'left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle', + 'tool0', 'wrist_3_link'] + + left_inner_finger: ['robotiq_base_link', + 'left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle', + 'tool0', 'wrist_3_link'] + + left_outer_knuckle: [ + 'robotiq_base_link', + 'left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle', + 'tool0', 'wrist_3_link'] + + right_outer_finger: ['robotiq_base_link', + 'left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle', + 'tool0', 'wrist_3_link'] + + right_inner_finger_pad: [ + 'robotiq_base_link', + 'left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle', + 'tool0', 'wrist_3_link'] + + right_inner_knuckle: [ 'robotiq_base_link', + 'left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle', + 'tool0', 'wrist_3_link'] + + right_inner_finger: ['robotiq_base_link', + 'left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle', + 'tool0', 'wrist_3_link'] + + right_outer_knuckle: [ + 'robotiq_base_link', + 'left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle', + 'tool0', 'wrist_3_link'] + + attached_object: [ + 'wrist_2_link', 'wrist_3_link', 'tool0', 'robotiq_base_link', + 'left_outer_finger', 'left_inner_finger_pad', 'left_inner_knuckle', + 'left_inner_finger', 'left_outer_knuckle', 'right_outer_finger', + 'right_inner_finger_pad', 'right_inner_knuckle', 'right_inner_finger', + 'right_outer_knuckle'] + +geometry: + ur_robotiq_2f_140_collision_spheres: + spheres: + shoulder_link: + - center: [0.0, 0.0, 0.01] + radius: 0.085 + - center: [0.003, -0.022, -0.009] + radius: 0.082 + + upper_arm_link: + - center: [0.008, -0.001, 0.184] + radius: 0.077 + - center: [-0.031, 0.0, 0.176] + radius: 0.084 + - center: [-0.068, -0.005, 0.179] + radius: 0.079 + - center: [-0.14, -0.001, 0.177] + radius: 0.064 + - center: [-0.18, -0.001, 0.177] + radius: 0.064 + - center: [-0.224, -0.0, 0.176] + radius: 0.064 + - center: [-0.284, -0.0, 0.176] + radius: 0.064 + - center: [-0.309, 0.002, 0.176] + radius: 0.063 + - center: [-0.370, 0.002, 0.176] + radius: 0.063 + - center: [-0.418, -0.0, 0.176] + radius: 0.064 + - center: [-0.480, -0.0, 0.176] + radius: 0.064 + - center: [-0.516, 0.0, 0.176] + radius: 0.064 + - center: [-0.550, 0.0, 0.176] + radius: 0.064 + - center: [-0.600, 0.0, 0.177] + radius: 0.080 + + forearm_link: + - center: [-0.0, 0.005, 0.119] + radius: 0.06 + - center: [-0.017, 0.007, 0.053] + radius: 0.057 + - center: [-0.056, -0.0, 0.04] + radius: 0.067 + - center: [-0.106, 0.0, 0.044] + radius: 0.067 + - center: [-0.182, -0.0, 0.038] + radius: 0.065 + - center: [-0.256, 0.0, 0.024] + radius: 0.064 + - center: [-0.317, -0.0, 0.024] + radius: 0.062 + - center: [-0.378, -0.0, 0.025] + radius: 0.061 + - center: [-0.429, 0.0, 0.029] + radius: 0.059 + - center: [-0.475, -0.0, 0.029] + radius: 0.059 + - center: [-0.520, -0.001, 0.029] + radius: 0.058 + - center: [-0.566, 0.0, 0.056] + radius: 0.057 + - center: [-0.565, -0.001, 0.029] + radius: 0.057 + - center: [-0.565, 0.010, 0.029] + radius: 0.057 + + wrist_1_link: + - center: [0.0, 0.005, -0.007] + radius: 0.056 + - center: [-0.001, -0.02, 0.0] + radius: 0.055 + + wrist_2_link: + - center: [-0.0, 0.001, -0.0] + radius: 0.056 + - center: [-0.0, 0.021, 0.0] + radius: 0.055 + - center: [-0.004, -0.011, -0.011] + radius: 0.053 + + wrist_3_link: + - center: [-0.016, 0.002, -0.025] + radius: 0.034 + - center: [0.016, -0.011, -0.024] + radius: 0.034 + - center: [0.009, 0.018, -0.025] + radius: 0.034 + + tool0: + - center: [0, 0, 0.12] + radius: -0.01 + + robotiq_base_link: + - center: [0.0, -0.0, 0.04] + radius: 0.035 + - center: [0.0, -0.0, 0.02] + radius: 0.035 + + left_outer_finger: + - center: [0.0, -0.01, 0.0] + radius: 0.01 + - center: [0.0, 0.015, -0.01] + radius: 0.01 + + left_inner_finger_pad: + - center: [0.0, -0.0, 0.005] + radius: 0.01 + - center: [0.0, 0.02, 0.005] + radius: 0.01 + - center: [0.0, -0.02, 0.005] + radius: 0.01 + + left_inner_knuckle: + - center: [0.0, 0.04, -0.0] + radius: 0.01 + - center: [0.0, 0.06, -0.0] + radius: 0.01 + - center: [0.0, 0.08, -0.0] + radius: 0.01 + + left_inner_finger: + - center: [0.0, -0.0, -0.0] + radius: 0.01 + + left_outer_knuckle: + - center: [0.0, 0.055, 0.01] + radius: 0.01 + - center: [0.0, 0.08, 0.005] + radius: 0.01 + + right_outer_finger: + - center: [0.0, -0.01, 0.0] + radius: 0.01 + - center: [0.0, 0.015, -0.01] + radius: 0.01 + + right_inner_finger_pad: + - center: [0.0, -0.0, 0.005] + radius: 0.01 + - center: [0.0, 0.02, 0.005] + radius: 0.01 + - center: [0.0, -0.02, 0.005] + radius: 0.01 + + right_inner_knuckle: + - center: [0.0, 0.04, -0.0] + radius: 0.01 + - center: [0.0, 0.06, -0.0] + radius: 0.01 + - center: [0.0, 0.08, -0.0] + radius: 0.01 + + right_inner_finger: + - center: [0.0, -0.0, -0.0] + radius: 0.01 + + right_outer_knuckle: + - center: [0.0, 0.055, 0.01] + radius: 0.01 + - center: [0.0, 0.08, 0.005] + radius: 0.01 + + attached_object: + - center: [0.0, 0.08, 0.005] + radius: -100.0 \ No newline at end of file diff --git a/isaac_ros_cumotion_robot_description/xrdf/ur5e_robotiq_2f_140.xrdf b/isaac_ros_cumotion_robot_description/xrdf/ur5e_robotiq_2f_140.xrdf new file mode 100644 index 0000000..8ed97d5 --- /dev/null +++ b/isaac_ros_cumotion_robot_description/xrdf/ur5e_robotiq_2f_140.xrdf @@ -0,0 +1,270 @@ +format: xrdf +format_version: 1.0 + +modifiers: + - set_base_frame: "base_link" + - add_frame: + frame_name: "attached_object" + parent_frame_name: "grasp_frame" + joint_name: "attach_joint" + joint_type: FIXED + fixed_transform: + position: [0.0, 0.0, 0.0] + orientation: {w: 1.0, xyz: [0.0, 0.0, 0.0]} + +default_joint_positions: + shoulder_pan_joint: 0.0 + shoulder_lift_joint: -2.2 + elbow_joint: 1.0 + wrist_1_joint: -1.383 + wrist_2_joint: -1.57 + wrist_3_joint: 0.00 + +cspace: + joint_names: + - "shoulder_pan_joint" + - "shoulder_lift_joint" + - "elbow_joint" + - "wrist_1_joint" + - "wrist_2_joint" + - "wrist_3_joint" + acceleration_limits: [12.0, 12.0, 12.0, 12.0, 12.0, 12.0] + jerk_limits: [500.0, 500.0, 500.0, 500.0, 500.0, 500.0] + +tool_frames: ["grasp_frame"] + +collision: + geometry: "ur5e_robotiq_2f_140_collision_spheres" + buffer_distance: + shoulder_link: 0.005 + upper_arm_link: 0.005 + forearm_link: 0.005 + wrist_1_link: 0.005 + wrist_2_link: 0.005 + wrist_3_link: 0.005 + tool0: 0.005 + robotiq_base_link: 0.005 + left_outer_finger: 0.005 + left_inner_finger_pad: 0.005 + left_inner_knuckle: 0.005 + left_inner_finger: 0.005 + left_outer_knuckle: 0.005 + right_outer_finger: 0.005 + right_inner_finger_pad: 0.005 + right_inner_knuckle: 0.005 + right_inner_finger: 0.005 + attached_object: 0.005 + +self_collision: + geometry: "ur5e_robotiq_2f_140_collision_spheres" + buffer_distance: + shoulder_link: 0.01 + tool0: 0.02 + ignore: + upper_arm_link: ["forearm_link", "shoulder_link"] + forearm_link: ["wrist_1_link"] + wrist_1_link: ["wrist_2_link","wrist_3_link"] + wrist_2_link: ["wrist_3_link", "tool0"] + wrist_3_link: ["tool0"] + + upper_arm_link: ["forearm_link", "shoulder_link"] + forearm_link: ["wrist_1_link", "wrist_2_link", "wrist_3_link"] + wrist_1_link: ["wrist_2_link","wrist_3_link","tool0", "robotiq_base_link"] + wrist_2_link: ["wrist_3_link", "tool0", "robotiq_base_link"] + wrist_3_link: ["tool0", "robotiq_base_link"] + + tool0: ['robotiq_base_link', 'left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'] + + robotiq_base_link: ['left_outer_finger', 'left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'] + + left_outer_finger: ['left_inner_finger_pad', + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'] + + left_inner_finger_pad: [ + 'left_inner_knuckle', 'left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'] + + left_inner_knuckle: ['left_inner_finger', 'left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'] + + left_inner_finger: ['left_outer_knuckle', + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'] + + left_outer_knuckle: [ + 'right_outer_finger', 'right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'] + + right_outer_finger: ['right_inner_finger_pad', + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'] + + right_inner_finger_pad: [ + 'right_inner_knuckle', 'right_inner_finger', 'right_outer_knuckle'] + + right_inner_knuckle: [ 'right_inner_finger', 'right_outer_knuckle'] + + right_inner_finger: [ 'right_outer_knuckle'] + + attached_object: [ + 'wrist_2_link', 'wrist_3_link', 'tool0', 'robotiq_base_link', + 'left_outer_finger', 'left_inner_finger_pad', 'left_inner_knuckle', + 'left_inner_finger', 'left_outer_knuckle', 'right_outer_finger', + 'right_inner_finger_pad', 'right_inner_knuckle', 'right_inner_finger', + 'right_outer_knuckle'] + +geometry: + ur5e_robotiq_2f_140_collision_spheres: + spheres: + shoulder_link: + - center: [0.0, 0.0, 0.0] + radius: 0.1 + + upper_arm_link: + - center: [-0.416, -0.0, 0.143] + radius: 0.078 + - center: [-0.015, 0.0, 0.134] + radius: 0.077 + - center: [-0.14, 0.0, 0.138] + radius: 0.062 + - center: [-0.285, -0.001, 0.139] + radius: 0.061 + - center: [-0.376, 0.001, 0.138] + radius: 0.077 + - center: [-0.222, 0.001, 0.139] + radius: 0.061 + - center: [-0.055, 0.008, 0.14] + radius: 0.07 + - center: [-0.001, -0.002, 0.143] + radius: 0.08 + + forearm_link: + - center: [-0.01, 0.002, 0.031] + radius: 0.072 + - center: [-0.387, 0.0, 0.014] + radius: 0.057 + - center: [-0.121, -0.0, 0.006] + radius: 0.057 + - center: [-0.206, 0.001, 0.007] + radius: 0.057 + - center: [-0.312, -0.001, 0.006] + radius: 0.056 + - center: [-0.057, 0.003, 0.008] + radius: 0.065 + - center: [-0.266, 0.0, 0.006] + radius: 0.057 + - center: [-0.397, -0.001, -0.018] + radius: 0.052 + - center: [-0.164, -0.0, 0.007] + radius: 0.057 + + wrist_1_link: + - center: [-0.0, 0.0, -0.009] + radius: 0.047 + - center: [-0.0, 0.0, -0.052] + radius: 0.047 + - center: [-0.002, 0.027, -0.001] + radius: 0.045 + - center: [0.001, -0.01, 0.0] + radius: 0.046 + + wrist_2_link: + - center: [0.0, -0.01, -0.001] + radius: 0.047 + - center: [0.0, 0.008, -0.001] + radius: 0.047 + - center: [0.001, -0.001, -0.036] + radius: 0.047 + - center: [0.001, -0.03, -0.0] + radius: 0.047 + + wrist_3_link: + - center: [0.001, 0.001, -0.029] + radius: 0.043 + + tool0: + - center: [0.001, 0.001, 0.05] + radius: -0.01 #0.05 + + robotiq_base_link: + - center: [0.0, -0.0, 0.04] + radius: 0.035 + - center: [0.0, -0.0, 0.02] + radius: 0.035 + + left_outer_finger: + - center: [0.0, -0.01, 0.0] + radius: 0.01 + - center: [0.0, 0.015, -0.01] + radius: 0.01 + + left_inner_finger_pad: + - center: [0.0, -0.0, 0.005] + radius: 0.01 + - center: [0.0, 0.02, 0.005] + radius: 0.01 + - center: [0.0, -0.02, 0.005] + radius: 0.01 + + left_inner_knuckle: + - center: [0.0, 0.04, -0.0] + radius: 0.01 + - center: [0.0, 0.06, -0.0] + radius: 0.01 + - center: [0.0, 0.08, -0.0] + radius: 0.01 + + left_inner_finger: + - center: [0.0, -0.0, -0.0] + radius: 0.01 + + left_outer_knuckle: + - center: [0.0, 0.055, 0.01] + radius: 0.01 + - center: [0.0, 0.08, 0.005] + radius: 0.01 + + right_outer_finger: + - center: [0.0, -0.01, 0.0] + radius: 0.01 + - center: [0.0, 0.015, -0.01] + radius: 0.01 + + right_inner_finger_pad: + - center: [0.0, -0.0, 0.005] + radius: 0.01 + - center: [0.0, 0.02, 0.005] + radius: 0.01 + - center: [0.0, -0.02, 0.005] + radius: 0.01 + + right_inner_knuckle: + - center: [0.0, 0.04, -0.0] + radius: 0.01 + - center: [0.0, 0.06, -0.0] + radius: 0.01 + - center: [0.0, 0.08, -0.0] + radius: 0.01 + + right_inner_finger: + - center: [0.0, -0.0, -0.0] + radius: 0.01 + + right_outer_knuckle: + - center: [0.0, 0.055, 0.01] + radius: 0.01 + - center: [0.0, 0.08, 0.005] + radius: 0.01 + + attached_object: + - center: [0.0, 0.08, 0.005] + radius: -100.0 \ No newline at end of file diff --git a/isaac_ros_cumotion_robot_description/xrdf/ur5e_robotiq_2f_85.xrdf b/isaac_ros_cumotion_robot_description/xrdf/ur5e_robotiq_2f_85.xrdf new file mode 100644 index 0000000..5c5d474 --- /dev/null +++ b/isaac_ros_cumotion_robot_description/xrdf/ur5e_robotiq_2f_85.xrdf @@ -0,0 +1,246 @@ +format: xrdf +format_version: 1.0 + +modifiers: + - set_base_frame: "base_link" + - add_frame: + frame_name: "attached_object" + parent_frame_name: "grasp_frame" + joint_name: "attach_joint" + joint_type: FIXED + fixed_transform: + position: [0.0, 0.0, 0.0] + orientation: {w: 1.0, xyz: [0.0, 0.0, 0.0]} + +default_joint_positions: + shoulder_pan_joint: 0.0 + shoulder_lift_joint: -2.2 + elbow_joint: 1.0 + wrist_1_joint: -1.383 + wrist_2_joint: -1.57 + wrist_3_joint: 0.00 + +cspace: + joint_names: + - "shoulder_pan_joint" + - "shoulder_lift_joint" + - "elbow_joint" + - "wrist_1_joint" + - "wrist_2_joint" + - "wrist_3_joint" + acceleration_limits: [12.0, 12.0, 12.0, 12.0, 12.0, 12.0] + jerk_limits: [500.0, 500.0, 500.0, 500.0, 500.0, 500.0] + +tool_frames: ["grasp_frame"] + +collision: + geometry: "ur5e_robotiq_2f_85_collision_spheres" + buffer_distance: + shoulder_link: 0.005 + upper_arm_link: 0.005 + forearm_link: 0.005 + wrist_1_link: 0.005 + wrist_2_link: 0.005 + wrist_3_link: 0.005 + tool0: 0.005 + robotiq_85_base_link: 0.005 + robotiq_85_left_finger_link: 0.005 + robotiq_85_left_finger_tip_link: 0.005 + robotiq_85_left_inner_knuckle_link: 0.005 + robotiq_85_left_knuckle_link: 0.005 + robotiq_85_right_finger_link: 0.005 + robotiq_85_right_finger_tip_link: 0.005 + robotiq_85_right_inner_knuckle_link: 0.005 + robotiq_85_right_knuckle_link: 0.005 + attached_object: 0.005 + +self_collision: + geometry: "ur5e_robotiq_2f_85_collision_spheres" + buffer_distance: + shoulder_link: 0.01 + tool0: 0.02 + ignore: + upper_arm_link: ["forearm_link", "shoulder_link"] + forearm_link: ["wrist_1_link", "wrist_2_link", "wrist_3_link"] + wrist_1_link: ["wrist_2_link","wrist_3_link","tool0", "robotiq_85_base_link"] + wrist_2_link: ["wrist_3_link", "tool0", "robotiq_85_base_link"] + wrist_3_link: ["tool0", "robotiq_85_base_link"] + + tool0: ['robotiq_85_base_link', 'robotiq_85_left_finger_link', 'robotiq_85_left_finger_tip_link', + 'robotiq_85_left_inner_knuckle_link', 'robotiq_85_left_knuckle_link', + 'robotiq_85_right_finger_link', 'robotiq_85_right_finger_tip_link', + 'robotiq_85_right_inner_knuckle_link', 'robotiq_85_right_knuckle_link'] + + robotiq_85_base_link: ['robotiq_85_left_finger_link', 'robotiq_85_left_finger_tip_link', + 'robotiq_85_left_inner_knuckle_link', 'robotiq_85_left_knuckle_link', + 'robotiq_85_right_finger_link', 'robotiq_85_right_finger_tip_link', + 'robotiq_85_right_inner_knuckle_link', 'robotiq_85_right_knuckle_link'] + + robotiq_85_left_finger_link: ['robotiq_85_left_finger_tip_link', + 'robotiq_85_left_inner_knuckle_link', 'robotiq_85_left_knuckle_link', + 'robotiq_85_right_finger_link', 'robotiq_85_right_finger_tip_link', + 'robotiq_85_right_inner_knuckle_link', 'robotiq_85_right_knuckle_link'] + + robotiq_85_left_finger_tip_link: ['robotiq_85_left_inner_knuckle_link', 'robotiq_85_left_knuckle_link', + 'robotiq_85_right_finger_link', 'robotiq_85_right_finger_tip_link', + 'robotiq_85_right_inner_knuckle_link', 'robotiq_85_right_knuckle_link'] + + robotiq_85_left_inner_knuckle_link: ['robotiq_85_left_knuckle_link', + 'robotiq_85_right_finger_link', 'robotiq_85_right_finger_tip_link', + 'robotiq_85_right_inner_knuckle_link', 'robotiq_85_right_knuckle_link'] + + robotiq_85_left_knuckle_link: ['robotiq_85_right_finger_link', 'robotiq_85_right_finger_tip_link', + 'robotiq_85_right_inner_knuckle_link', 'robotiq_85_right_knuckle_link'] + + robotiq_85_right_finger_link: ['robotiq_85_right_finger_tip_link', + 'robotiq_85_right_inner_knuckle_link', 'robotiq_85_right_knuckle_link'] + + robotiq_85_right_finger_tip_link: ['robotiq_85_right_inner_knuckle_link', + 'robotiq_85_right_knuckle_link'] + + robotiq_85_right_inner_knuckle_link: ['robotiq_85_right_knuckle_link'] + + attached_object: [ + 'wrist_2_link', 'wrist_3_link', 'tool0', 'robotiq_85_base_link', + 'robotiq_85_left_finger_link', 'robotiq_85_left_finger_tip_link', 'robotiq_85_left_inner_knuckle_link', + 'robotiq_85_left_knuckle_link', 'robotiq_85_right_finger_link', + 'robotiq_85_right_finger_tip_link', 'robotiq_85_right_inner_knuckle_link', 'robotiq_85_right_knuckle_link'] + +geometry: + ur5e_robotiq_2f_85_collision_spheres: + spheres: + shoulder_link: + - center: [0.0, 0.0, 0.0] + radius: 0.1 + + upper_arm_link: + - center: [-0.416, -0.0, 0.143] + radius: 0.078 + - center: [-0.015, 0.0, 0.134] + radius: 0.077 + - center: [-0.14, 0.0, 0.138] + radius: 0.062 + - center: [-0.285, -0.001, 0.139] + radius: 0.061 + - center: [-0.376, 0.001, 0.138] + radius: 0.077 + - center: [-0.222, 0.001, 0.139] + radius: 0.061 + - center: [-0.055, 0.008, 0.14] + radius: 0.07 + - center: [-0.001, -0.002, 0.143] + radius: 0.08 + + forearm_link: + - center: [-0.01, 0.002, 0.031] + radius: 0.072 + - center: [-0.387, 0.0, 0.014] + radius: 0.057 + - center: [-0.121, -0.0, 0.006] + radius: 0.057 + - center: [-0.206, 0.001, 0.007] + radius: 0.057 + - center: [-0.312, -0.001, 0.006] + radius: 0.056 + - center: [-0.057, 0.003, 0.008] + radius: 0.065 + - center: [-0.266, 0.0, 0.006] + radius: 0.057 + - center: [-0.397, -0.001, -0.018] + radius: 0.052 + - center: [-0.164, -0.0, 0.007] + radius: 0.057 + + wrist_1_link: + - center: [-0.0, 0.0, -0.009] + radius: 0.047 + - center: [-0.0, 0.0, -0.052] + radius: 0.047 + - center: [-0.002, 0.027, -0.001] + radius: 0.045 + - center: [0.001, -0.01, 0.0] + radius: 0.046 + + wrist_2_link: + - center: [0.0, -0.01, -0.001] + radius: 0.047 + - center: [0.0, 0.008, -0.001] + radius: 0.047 + - center: [0.001, -0.001, -0.036] + radius: 0.047 + - center: [0.001, -0.03, -0.0] + radius: 0.047 + + wrist_3_link: + - center: [0.001, 0.001, -0.029] + radius: 0.043 + + tool0: + - center: [0.001, 0.001, 0.05] + radius: -0.01 + + robotiq_85_base_link: + - center: [0.0, 0.0, 0.04] + radius: 0.035 + - center: [0.0, 0.0, 0.02] + radius: 0.035 + + robotiq_85_left_finger_link: + - center: [0.0, 0.0, 0.01] + radius: 0.012 + - center: [0.0, 0.0, 0.03] + radius: 0.012 + - center: [0.0, 0.0, 0.05] + radius: 0.012 + + robotiq_85_left_finger_tip_link: + - center: [-0.025, 0.0, 0.01] + radius: 0.01 + - center: [-0.025, 0.0, 0.03] + radius: 0.01 + - center: [-0.025, 0.0, 0.05] + radius: 0.01 + + robotiq_85_left_inner_knuckle_link: + - center: [0.0, 0.0, 0.0] + radius: 0.012 + - center: [0.005, 0.0, 0.012] + radius: 0.012 + - center: [0.015, 0.0, 0.025] + radius: 0.012 + + robotiq_85_left_knuckle_link: + - center: [0.0, 0.0, 0.0] + radius: 0.01 + + robotiq_85_right_finger_link: + - center: [0.0, 0.0, 0.01] + radius: 0.012 + - center: [0.0, 0.0, 0.03] + radius: 0.012 + - center: [0.0, 0.0, 0.05] + radius: 0.012 + + robotiq_85_right_finger_tip_link: + - center: [0.025, 0.0, 0.01] + radius: 0.01 + - center: [0.025, 0.0, 0.03] + radius: 0.01 + - center: [0.025, 0.0, 0.05] + radius: 0.01 + + robotiq_85_right_inner_knuckle_link: + - center: [0.0, 0.0, 0.0] + radius: 0.012 + - center: [-0.005, 0.0, 0.012] + radius: 0.012 + - center: [-0.015, 0.0, 0.025] + radius: 0.012 + + robotiq_85_right_knuckle_link: + - center: [0.0, -0.0, -0.0] + radius: 0.01 + + attached_object: + - center: [0.0, 0.08, 0.005] + radius: -100.0 \ No newline at end of file diff --git a/isaac_ros_esdf_visualizer/isaac_ros_esdf_visualizer/esdf_visualizer.py b/isaac_ros_esdf_visualizer/isaac_ros_esdf_visualizer/esdf_visualizer.py index 9489852..ddf828d 100644 --- a/isaac_ros_esdf_visualizer/isaac_ros_esdf_visualizer/esdf_visualizer.py +++ b/isaac_ros_esdf_visualizer/isaac_ros_esdf_visualizer/esdf_visualizer.py @@ -15,6 +15,8 @@ # # SPDX-License-Identifier: Apache-2.0 +import os + from curobo.geom.sdf.world import CollisionCheckerType from curobo.geom.sdf.world import WorldCollisionConfig from curobo.geom.sdf.world_voxel import WorldVoxelCollision @@ -24,12 +26,14 @@ from curobo.types.base import TensorDeviceType from geometry_msgs.msg import Point from geometry_msgs.msg import Vector3 +from isaac_ros_cumotion_python_utils.utils import \ + get_grid_center, get_grid_min_corner, get_grid_size, is_grid_valid, \ + load_grid_corners_from_workspace_file import numpy as np from nvblox_msgs.srv import EsdfAndGradients import rclpy from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.node import Node -from std_msgs.msg import ColorRGBA import torch from visualization_msgs.msg import Marker @@ -39,15 +43,29 @@ class ESDFVisualizer(Node): def __init__(self): super().__init__('esdf_visualizer') - self.declare_parameter('voxel_dims', [1.25, 1.8, 1.8]) - self.declare_parameter('grid_position', [0.0, 0.0, 0.0]) - - self.declare_parameter('voxel_size', 0.05) - self.declare_parameter('publish_voxel_size', 0.025) - self.declare_parameter('max_publish_voxels', 50000) + # The grid_center_m and grid_size_m parameters are loaded from the workspace file + # if the workspace_file_path is set and valid. + self.declare_parameter('workspace_file_path', '') + self.declare_parameter('grid_center_m', [0.0, 0.0, 0.0]) + self.declare_parameter('grid_size_m', [2.0, 2.0, 2.0]) + self.declare_parameter('update_esdf_on_request', True) + self.declare_parameter('use_aabb_on_request', True) + + # Parameters for clearing shapes in the map. + self.declare_parameter('clear_shapes_on_request', False) + self.declare_parameter('clear_shapes_subsampling_factor', 2) + self.declare_parameter('aabbs_to_clear_min_m', [0.7, 0.6, -0.1]) + self.declare_parameter('aabbs_to_clear_size_m', [0.2, 0.2, 0.9]) + self.declare_parameter('spheres_to_clear_center_m', [1.5, 0.5, 0.0]) + self.declare_parameter('spheres_to_clear_radius_m', 0.2) + + self.declare_parameter('voxel_size', 0.01) + self.declare_parameter('publish_voxel_size', 0.01) + self.declare_parameter('max_publish_voxels', 500000) self.declare_parameter('esdf_service_name', '/nvblox_node/get_esdf_and_gradient') self.declare_parameter('robot_base_frame', 'base_link') + self.declare_parameter('esdf_service_call_period_secs', 0.01) self.__esdf_future = None # Voxel publisher self.__voxel_pub = self.create_publisher(Marker, '/curobo/voxels', 10) @@ -64,18 +82,26 @@ def __init__(self): self.__esdf_req = EsdfAndGradients.Request() # Timer for calling the ESDF service - timer_period = 0.01 timer_cb_group = MutuallyExclusiveCallbackGroup() self.timer = self.create_timer( - timer_period, self.timer_callback, callback_group=timer_cb_group + self.get_parameter('esdf_service_call_period_secs').get_parameter_value().double_value, + self.timer_callback, callback_group=timer_cb_group ) - self.__voxel_dims = ( - self.get_parameter('voxel_dims').get_parameter_value().double_array_value + self.__workspace_file_path = ( + self.get_parameter('workspace_file_path').get_parameter_value().string_value ) - - self.__grid_position = ( - self.get_parameter('grid_position').get_parameter_value().double_array_value + self.__grid_size_m = ( + self.get_parameter('grid_size_m').get_parameter_value().double_array_value + ) + self.__update_esdf_on_request = ( + self.get_parameter('update_esdf_on_request').get_parameter_value().bool_value + ) + self.__use_aabb_on_request = ( + self.get_parameter('use_aabb_on_request').get_parameter_value().bool_value + ) + self.__grid_center_m = ( + self.get_parameter('grid_center_m').get_parameter_value().double_array_value ) self.__voxel_size = self.get_parameter('voxel_size').get_parameter_value().double_value self.__publish_voxel_size = ( @@ -84,16 +110,47 @@ def __init__(self): self.__max_publish_voxels = ( self.get_parameter('max_publish_voxels').get_parameter_value().integer_value ) + self.__clear_shapes_on_request = ( + self.get_parameter('clear_shapes_on_request').get_parameter_value().bool_value) + self.__clear_shapes_subsampling_factor = ( + self.get_parameter( + 'clear_shapes_subsampling_factor').get_parameter_value().integer_value) + self.__aabbs_to_clear_min_m = ( + self.get_parameter('aabbs_to_clear_min_m').get_parameter_value().double_array_value) + self.__aabbs_to_clear_size_m = ( + self.get_parameter('aabbs_to_clear_size_m').get_parameter_value().double_array_value) + self.__spheres_to_clear_center_m = ( + self.get_parameter( + 'spheres_to_clear_center_m').get_parameter_value().double_array_value) + self.__spheres_to_clear_radius_m = ( + self.get_parameter('spheres_to_clear_radius_m').get_parameter_value().double_value) + + # Setup the grid position and dimension. + if os.path.exists(self.__workspace_file_path): + self.get_logger().info( + f'Loading grid center and dims from workspace file: {self.__workspace_file_path}.') + min_corner, max_corner = load_grid_corners_from_workspace_file( + self.__workspace_file_path) + self.__grid_size_m = get_grid_size(min_corner, max_corner, self.__voxel_size) + self.__grid_center_m = get_grid_center(min_corner, self.__grid_size_m) + else: + self.get_logger().info( + 'Loading grid position and dims from grid_center_m and grid_size_m parameters.') + + if is_grid_valid(self.__grid_size_m, self.__voxel_size): + self.get_logger().fatal('Number of voxels should be at least 1 in every dimension.') + raise SystemExit + # Init WorldVoxelCollision world_cfg = WorldConfig.from_dict( { 'voxel': { 'world_voxel': { - 'dims': self.__voxel_dims, + 'dims': self.__grid_size_m, 'pose': [ - self.__grid_position[0], - self.__grid_position[1], - self.__grid_position[2], + self.__grid_center_m[0], + self.__grid_center_m[1], + self.__grid_center_m[2], 1, 0, 0, @@ -121,47 +178,68 @@ def __init__(self): self.get_parameter('robot_base_frame').get_parameter_value().string_value ) self.__tensor_args = tensor_args - - esdf_grid = CuVoxelGrid( - name='world_voxel', - dims=self.__voxel_dims, - pose=[ - self.__grid_position[0], - self.__grid_position[1], - self.__grid_position[2], - 1, - 0, - 0, - 0, - ], - voxel_size=self.__voxel_size, - feature_dtype=torch.float32, - ) - self.__grid_shape, _, _ = esdf_grid.get_grid_shape() + self.__clear_shapes_counter = 0 + self.__cumotion_grid_shape = self.__world_collision.get_voxel_grid( + 'world_voxel').get_grid_shape()[0] def timer_callback(self): if self.__esdf_future is None: self.get_logger().info('Calling ESDF service') - # This is half of x,y and z dims + + # Get the AABB + min_corner = get_grid_min_corner(self.__grid_center_m, self.__grid_size_m) aabb_min = Point() - aabb_min.x = (-0.5 * self.__voxel_dims[0]) + self.__grid_position[0] - aabb_min.y = (-0.5 * self.__voxel_dims[1]) + self.__grid_position[1] - aabb_min.z = (-0.5 * self.__voxel_dims[2]) + self.__grid_position[2] - # This is a voxel size. - voxel_dims = Vector3() - voxel_dims.x = self.__voxel_dims[0] - voxel_dims.y = self.__voxel_dims[1] - voxel_dims.z = self.__voxel_dims[2] - - self.__esdf_future = self.send_request(aabb_min, voxel_dims) + aabb_min.x = min_corner[0] + aabb_min.y = min_corner[1] + aabb_min.z = min_corner[2] + aabb_size = Vector3() + aabb_size.x = self.__grid_size_m[0] + aabb_size.y = self.__grid_size_m[1] + aabb_size.z = self.__grid_size_m[2] + + # Request the esdf grid + self.__esdf_future = self.send_request(aabb_min, aabb_size) if self.__esdf_future is not None and self.__esdf_future.done(): response = self.__esdf_future.result() - self.fill_marker(response) + if response.success: + self.fill_marker(response) + else: + self.get_logger().info('ESDF request failed. Not updating the grid.') self.__esdf_future = None def send_request(self, aabb_min_m, aabb_size_m): + self.__esdf_req.visualize_esdf = True + self.__esdf_req.update_esdf = self.__update_esdf_on_request + self.__esdf_req.use_aabb = self.__use_aabb_on_request + self.__esdf_req.frame_id = self.__robot_base_frame self.__esdf_req.aabb_min_m = aabb_min_m self.__esdf_req.aabb_size_m = aabb_size_m + # Optionally clear an AABB and a sphere in the map. + if self.__clear_shapes_on_request and \ + self.__clear_shapes_counter % self.__clear_shapes_subsampling_factor == 0: + aabbs_to_clear_min_m = Point( + x=self.__aabbs_to_clear_min_m[0], + y=self.__aabbs_to_clear_min_m[1], + z=self.__aabbs_to_clear_min_m[2]) + aabbs_to_clear_size_m = Vector3( + x=self.__aabbs_to_clear_size_m[0], + y=self.__aabbs_to_clear_size_m[1], + z=self.__aabbs_to_clear_size_m[2]) + spheres_to_clear_center_m = Point( + x=self.__spheres_to_clear_center_m[0], + y=self.__spheres_to_clear_center_m[1], + z=self.__spheres_to_clear_center_m[2]) + self.__esdf_req.aabbs_to_clear_min_m = [aabbs_to_clear_min_m] + self.__esdf_req.aabbs_to_clear_size_m = [aabbs_to_clear_size_m] + self.__esdf_req.spheres_to_clear_center_m = [spheres_to_clear_center_m] + self.__esdf_req.spheres_to_clear_radius_m = [self.__spheres_to_clear_radius_m] + else: + self.__esdf_req.aabbs_to_clear_min_m = [] + self.__esdf_req.aabbs_to_clear_size_m = [] + self.__esdf_req.spheres_to_clear_center_m = [] + self.__esdf_req.spheres_to_clear_radius_m = [] + self.__clear_shapes_counter += 1 + self.get_logger().info( f'ESDF req = {self.__esdf_req.aabb_min_m}, {self.__esdf_req.aabb_size_m}' ) @@ -169,15 +247,38 @@ def send_request(self, aabb_min_m, aabb_size_m): return esdf_future def get_esdf_voxel_grid(self, esdf_data): + esdf_voxel_size = esdf_data.voxel_size_m + if abs(esdf_voxel_size - self.__voxel_size) > 1e-4: + self.get_logger().fatal( + 'Voxel size of esdf array is not equal to requested voxel_size, ' + f'{esdf_voxel_size} vs. {self.__voxel_size}') + raise SystemExit + + # Get the esdf and gradient data esdf_array = esdf_data.esdf_and_gradients array_shape = [ esdf_array.layout.dim[0].size, esdf_array.layout.dim[1].size, esdf_array.layout.dim[2].size, ] - array_data = np.array(esdf_array.data) - - array_data = self.__tensor_args.to_device(array_data) + array_data = np.array(esdf_array.data, dtype=np.float32) + array_data = torch.as_tensor(array_data) + + # Verify the grid shape + if array_shape != self.__cumotion_grid_shape: + self.get_logger().fatal( + 'Shape of received esdf voxel grid does not match the cumotion grid shape, ' + f'{array_shape} vs. {self.__cumotion_grid_shape}') + raise SystemExit + + # Get the origin of the grid + grid_origin = [ + esdf_data.origin_m.x, + esdf_data.origin_m.y, + esdf_data.origin_m.z, + ] + # The grid position is defined as the center point of the grid. + grid_center_m = get_grid_center(grid_origin, self.__grid_size_m) # Array data is reshaped to x y z channels array_data = array_data.view(array_shape[0], array_shape[1], array_shape[2]).contiguous() @@ -185,27 +286,20 @@ def get_esdf_voxel_grid(self, esdf_data): # Array is squeezed to 1 dimension array_data = array_data.reshape(-1, 1) - # nvblox uses negative distance inside obstacles, cuRobo needs the opposite: - array_data = -1 * array_data + # nvblox assigns a value of -1000.0 for unobserved voxels, making it positive + array_data[array_data < -999.9] = 1000.0 - # nvblox assigns a value of -1000.0 for unobserved voxels, making - array_data[array_data >= 1000.0] = -1000.0 + # nvblox uses negative distance inside obstacles, cuRobo needs the opposite: + array_data = -1.0 * array_data - # nvblox distance are at origin of each voxel, cuRobo's esdf needs it to be at faces - array_data = array_data + 0.5 * self.__voxel_size + # nvblox treats surface voxels as distance = 0.0, while cuRobo treats + # distance = 0.0 as not in collision. Adding an offset. + array_data += 0.5 * self.__voxel_size esdf_grid = CuVoxelGrid( name='world_voxel', - dims=self.__voxel_dims, - pose=[ - self.__grid_position[0], - self.__grid_position[1], - self.__grid_position[2], - 1, - 0.0, - 0.0, - 0, - ], # x, y, z, qw, qx, qy, qz + dims=self.__grid_size_m, + pose=grid_center_m + [1, 0.0, 0.0, 0.0], # x, y, z, qw, qx, qy, qz voxel_size=self.__voxel_size, feature_dtype=torch.float32, feature_tensor=array_data, @@ -217,11 +311,16 @@ def fill_marker(self, esdf_data): esdf_grid = self.get_esdf_voxel_grid(esdf_data) self.__world_collision.update_voxel_data(esdf_grid) vox_size = self.__publish_voxel_size - voxels = self.__world_collision.get_esdf_in_bounding_box( + voxels = self.__world_collision.get_occupancy_in_bounding_box( CuCuboid( name='test', - pose=[0.0, 0.0, 0.0, 1, 0, 0, 0], # x, y, z, qw, qx, qy, qz - dims=self.__voxel_dims, + pose=[ + self.__grid_center_m[0], + self.__grid_center_m[1], + self.__grid_center_m[2], + 1, 0, 0, 0 + ], # x, y, z, qw, qx, qy, qz + dims=self.__grid_size_m, ), voxel_size=vox_size, ) @@ -230,7 +329,11 @@ def fill_marker(self, esdf_data): self.publish_voxels(xyzr_tensor) def publish_voxels(self, voxels): - vox_size = 0.25 * self.__publish_voxel_size + if self.__voxel_pub.get_subscription_count() == 0: + # Nobody is listening, no need to create message and publish. + return + + vox_size = self.__publish_voxel_size # create marker: marker = Marker() @@ -240,7 +343,7 @@ def publish_voxels(self, voxels): marker.ns = 'curobo_world' marker.action = 0 marker.pose.orientation.w = 1.0 - marker.lifetime = rclpy.duration.Duration(seconds=1000.0).to_msg() + marker.lifetime = rclpy.duration.Duration(seconds=0.0).to_msg() marker.frame_locked = False marker.scale.x = vox_size marker.scale.y = vox_size @@ -248,27 +351,29 @@ def publish_voxels(self, voxels): # get only voxels that are inside surfaces: - voxels = voxels[voxels[:, 3] >= 0.0] + voxels = voxels[voxels[:, 3] > 0.0] vox = voxels.view(-1, 4).cpu().numpy() marker.points = [] - - for i in range(min(len(vox), self.__max_publish_voxels)): - + number_of_voxels_to_publish = len(vox) + if len(vox) > self.__max_publish_voxels: + self.get_logger().warn( + f'Number of voxels to publish bigger than max_publish_voxels, ' + f'{len(vox)} > {self.__max_publish_voxels}' + ) + number_of_voxels_to_publish = self.__max_publish_voxels + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + vox = vox.astype(np.float64) + for i in range(number_of_voxels_to_publish): + # Publish the markers at the center of the voxels: pt = Point() - pt.x = float(vox[i, 0]) - pt.y = float(vox[i, 1]) - pt.z = float(vox[i, 2]) - color = ColorRGBA() - d = vox[i, 3] - - rgba = [min(1.0, 1.0 - float(d)), 0.0, 0.0, 1.0] - - color.r = rgba[0] - color.g = rgba[1] - color.b = rgba[2] - color.a = rgba[3] - marker.colors.append(color) + pt.x = vox[i, 0] + pt.y = vox[i, 1] + pt.z = vox[i, 2] marker.points.append(pt) + # publish voxels: marker.header.stamp = self.get_clock().now().to_msg() @@ -290,11 +395,15 @@ def main(args=None): except KeyboardInterrupt: esdf_client.get_logger().info('Destroying ESDFVisualizer node') + except Exception as e: + esdf_client.get_logger().info(f'Shutting down due to exception of type {type(e)}: {e}') + # Destroy the node explicitly esdf_client.destroy_node() # Shutdown the ROS client library for Python - rclpy.shutdown() + if rclpy.ok(): + rclpy.shutdown() if __name__ == '__main__': diff --git a/isaac_ros_esdf_visualizer/package.xml b/isaac_ros_esdf_visualizer/package.xml index 36dda1b..559399a 100644 --- a/isaac_ros_esdf_visualizer/package.xml +++ b/isaac_ros_esdf_visualizer/package.xml @@ -2,12 +2,14 @@ isaac_ros_esdf_visualizer - 3.1.0 + 3.2.0 Package for ESDF Voxel visualizer. Isaac ROS Maintainers Apache-2.0 + isaac_ros_common curobo_core + isaac_ros_cumotion_python_utils nvblox_msgs ament_copyright diff --git a/isaac_ros_goal_setter_interfaces/CMakeLists.txt b/isaac_ros_goal_setter_interfaces/CMakeLists.txt index b95d2db..cbaeaeb 100644 --- a/isaac_ros_goal_setter_interfaces/CMakeLists.txt +++ b/isaac_ros_goal_setter_interfaces/CMakeLists.txt @@ -48,4 +48,10 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() + +# Embed versioning information into installed files +ament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common) +include("${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake") +generate_version_info(${PROJECT_NAME}) + ament_auto_package() diff --git a/isaac_ros_goal_setter_interfaces/package.xml b/isaac_ros_goal_setter_interfaces/package.xml index c9690a1..07a6da3 100644 --- a/isaac_ros_goal_setter_interfaces/package.xml +++ b/isaac_ros_goal_setter_interfaces/package.xml @@ -21,7 +21,7 @@ isaac_ros_goal_setter_interfaces - 3.1.0 + 3.2.0 Interfaces for Isaac ROS Goal Setter Isaac ROS Maintainers Apache-2.0 diff --git a/isaac_ros_moveit_goal_setter/include/isaac_ros_moveit_goal_setter/goal_setter_node.hpp b/isaac_ros_moveit_goal_setter/include/isaac_ros_moveit_goal_setter/goal_setter_node.hpp deleted file mode 100644 index c5d9111..0000000 --- a/isaac_ros_moveit_goal_setter/include/isaac_ros_moveit_goal_setter/goal_setter_node.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#ifndef ISAAC_ROS_MOVEIT_GOAL_SETTER__GOAL_SETTER_NODE_HPP_ -#define ISAAC_ROS_MOVEIT_GOAL_SETTER__GOAL_SETTER_NODE_HPP_ - -#include - -#include "isaac_ros_common/qos.hpp" -#include "isaac_ros_goal_setter_interfaces/srv/set_target_pose.hpp" -#include -#include - -namespace nvidia -{ -namespace isaac_ros -{ -namespace manipulation -{ - -class GoalSetterNode -{ -public: - GoalSetterNode(std::string name, const rclcpp::NodeOptions & options); - ~GoalSetterNode() = default; - - std::shared_ptr GetNode() const {return node_;} - - void ConfigureMoveit(); - -private: - void SetTargetPoseCallback( - const std::shared_ptr req, - std::shared_ptr res); - - const std::shared_ptr node_; - std::string planner_group_name_; - std::string planner_id_; - std::string end_effector_link_; - moveit::planning_interface::MoveGroupInterface move_group_interface_; - - rclcpp::Service::SharedPtr - set_target_pose_service_; - -}; - -} // namespace manipulation -} // namespace isaac_ros -} // namespace nvidia - - -#endif // ISAAC_ROS_MOVEIT_GOAL_SETTER__GOAL_SETTER_NODE_HPP_ diff --git a/isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/__init__.py b/isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/goal_initializer.py b/isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/goal_initializer.py new file mode 100755 index 0000000..beffac5 --- /dev/null +++ b/isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/goal_initializer.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES', +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the 'License'); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an 'AS IS' BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from geometry_msgs.msg import Pose, PoseStamped +from isaac_ros_moveit_goal_setter.move_group_client import MoveGroupClient +import numpy as np +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + + +class GoalInitializerNode(Node): + + def __init__(self): + super().__init__('goal_initializer_node') + + self._previous_goal_position = None + + self._world_frame = self.declare_parameter( + 'world_frame', 'base_link').get_parameter_value().string_value + + self._grasp_frame = self.declare_parameter( + 'grasp_frame', 'grasp_frame').get_parameter_value().string_value + + self._grasp_frame_stale_time_threshold = self.declare_parameter( + 'grasp_frame_stale_time_threshold', 30.0).get_parameter_value().double_value + + self._goal_change_position_threshold = self.declare_parameter( + 'goal_change_position_threshold', 0.1).get_parameter_value().double_value + + self._plan_timer_period = self.declare_parameter( + 'plan_timer_period', 2.0).get_parameter_value().double_value + + planner_group_name = self.declare_parameter( + 'planner_group_name', 'ur_manipulator').get_parameter_value().string_value + + pipeline_id = self.declare_parameter( + 'pipeline_id', 'isaac_ros_cumotion').get_parameter_value().string_value + + planner_id = self.declare_parameter( + 'planner_id', 'cuMotion').get_parameter_value().string_value + + ee_link = self.declare_parameter( + 'end_effector_link', 'wrist_3_link').get_parameter_value().string_value + + self.move_group_client = MoveGroupClient( + self, planner_group_name, pipeline_id, planner_id, ee_link) + + self._tf_buffer = Buffer() + self._tf_listener = TransformListener(self._tf_buffer, self) + + self.timer = self.create_timer(self._plan_timer_period, self.on_timer) + + def _transform_msg_to_pose_msg(self, tf_msg): + pose = Pose() + pose.position.x = tf_msg.translation.x + pose.position.y = tf_msg.translation.y + pose.position.z = tf_msg.translation.z + + pose.orientation.x = tf_msg.rotation.x + pose.orientation.y = tf_msg.rotation.y + pose.orientation.z = tf_msg.rotation.z + pose.orientation.w = tf_msg.rotation.w + return pose + + def on_timer(self): + + # Check if there is a valid transform between detection and grasp frame + try: + # Using rclpy.time.Time() listens to the latest TF in the buffer + world_frame_pose_grasp_frame = self._tf_buffer.lookup_transform( + self._world_frame, self._grasp_frame, rclpy.time.Time() + ) + except TransformException as ex: + self.get_logger().warning(f'Waiting for object pose transform to be available in TF, \ + between {self._world_frame} and {self._grasp_frame}. if \ + warning persists, check if object pose is detected and \ + published to tf. Message from TF: {ex}') + return + + # Send a warning if the latest grasp frame is older than grasp_frame_stale_time_threshold + stale_check_time = (self.get_clock().now() - rclpy.time.Time().from_msg( + world_frame_pose_grasp_frame.header.stamp)).nanoseconds / 1e9 + if stale_check_time > self._grasp_frame_stale_time_threshold: + self.get_logger().warn(f'A new grasp frame has not been received for \ + {self._grasp_frame_stale_time_threshold} seconds.') + + output_msg = PoseStamped() + output_msg.header.stamp = self.get_clock().now().to_msg() + output_msg.header.frame_id = self._world_frame + output_msg.pose = self._transform_msg_to_pose_msg(world_frame_pose_grasp_frame.transform) + new_goal = np.array([ + output_msg.pose.position.x, + output_msg.pose.position.y, + output_msg.pose.position.z, + ]) + if self._previous_goal_position is not None: + + goal_change_distance = np.linalg.norm(self._previous_goal_position - new_goal) + if (goal_change_distance <= self._goal_change_position_threshold): + self.get_logger().warning( + f'New goal position is within {self._goal_change_position_threshold} meters at \ + {goal_change_distance}, not setting new goal. Move goal further to set \ + new goal.') + return + + self.move_group_client.send_goal_pose(output_msg) + self._previous_goal_position = new_goal + + +def main(args=None): + + rclpy.init(args=args) + + goal_initializer_node = GoalInitializerNode() + executor = MultiThreadedExecutor() + executor.add_node(goal_initializer_node) + try: + executor.spin() + except KeyboardInterrupt: + goal_initializer_node.get_logger().info( + 'KeyboardInterrupt, shutting down.\n' + ) + goal_initializer_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/move_group_client.py b/isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/move_group_client.py new file mode 100644 index 0000000..af0f646 --- /dev/null +++ b/isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/move_group_client.py @@ -0,0 +1,120 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES', +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the 'License'); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an 'AS IS' BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from moveit_msgs.action import MoveGroup +from moveit_msgs.msg import Constraints, JointConstraint, MoveItErrorCodes, \ + OrientationConstraint, PositionConstraint +from rclpy.action import ActionClient +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +import time + + +class MoveGroupClient: + + def __init__(self, node, planner_group_name, pipeline_id, planner_id, ee_link): + self._node = node + self._planner_group_name = planner_group_name + self._pipeline_id = pipeline_id + self._planner_id = planner_id + self._end_effector_link = ee_link + + self._action_client_cb_group = MutuallyExclusiveCallbackGroup() + + self._action_client = ActionClient(node, MoveGroup, '/move_action', + callback_group=self._action_client_cb_group) + + self._result = None + + while not self._action_client.wait_for_server(timeout_sec=1.0): + self._node.get_logger().info('Server move_action not available! Waiting...') + + def _get_pose_constraints(self, pose): + constraints = Constraints() + position_constraint = PositionConstraint() + position_constraint.header.frame_id = pose.header.frame_id + position_constraint.link_name = self._end_effector_link + position_constraint.constraint_region.primitive_poses.append(pose.pose) + constraints.position_constraints.extend([position_constraint]) + + orientation_constraint = OrientationConstraint() + orientation_constraint.header.frame_id = pose.header.frame_id + orientation_constraint.link_name = self._end_effector_link + orientation_constraint.orientation = pose.pose.orientation + constraints.orientation_constraints.extend([orientation_constraint]) + return constraints + + def _get_joint_constraints(self, positions, joint_names): + constraints = Constraints() + for position, joint_name in zip(positions, joint_names): + joint_constraint = JointConstraint() + joint_constraint.joint_name = joint_name + joint_constraint.position = position + constraints.joint_constraints.append(joint_constraint) + + def send_goal_pose(self, pose, allowed_planning_time=10.0): + self._result = None + goal_msg = MoveGroup.Goal() + goal_msg.request.planner_id = self._planner_id + goal_msg.request.group_name = self._planner_group_name + goal_msg.request.goal_constraints.append(self._get_pose_constraints(pose)) + goal_msg.planning_options.plan_only = False + goal_msg.request.num_planning_attempts = 1 + goal_msg.request.allowed_planning_time = allowed_planning_time + + self._node.get_logger().debug('Sending pose target to planner.') + self._send_goal_future = self._action_client.send_goal_async(goal_msg) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + while self._result is None: + time.sleep(0.01) + + return self._result + + def send_goal_joints(self, positions, joint_names, allowed_planning_time=10.0): + self._result = None + goal_msg = MoveGroup.Goal() + goal_msg.request.planner_id = self._planner_id + goal_msg.request.group_name = self._planner_group_name + goal_msg.request.goal_constraints.append( + self._get_joint_constraints(positions, joint_names)) + goal_msg.request.num_planning_attempts = 1 + goal_msg.request.allowed_planning_time = allowed_planning_time + self._action_client.wait_for_server() + self._node.get_logger().debug('Sending joint targets to planner.') + self._send_goal_future = self._action_client.send_goal_async(goal_msg) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + while self._result is None: + time.sleep(0.01) + + return self._result + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self._node.get_logger().error('Failed to set target!') + return + self._node.get_logger().info('Planning Goal request accepted!') + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + self._result = future.result().result + if self._result.error_code.val == MoveItErrorCodes.SUCCESS: + self._node.get_logger().info('Motion planning succeeded.') + else: + self._node.get_logger().error('Planning failed!') diff --git a/isaac_ros_moveit_goal_setter/scripts/pose_to_pose.py b/isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/pose_to_pose.py similarity index 81% rename from isaac_ros_moveit_goal_setter/scripts/pose_to_pose.py rename to isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/pose_to_pose.py index 19c1970..bad8639 100755 --- a/isaac_ros_moveit_goal_setter/scripts/pose_to_pose.py +++ b/isaac_ros_moveit_goal_setter/isaac_ros_moveit_goal_setter/pose_to_pose.py @@ -17,10 +17,9 @@ # # SPDX-License-Identifier: Apache-2.0 -import time - from geometry_msgs.msg import Pose, PoseStamped -from isaac_ros_goal_setter_interfaces.srv import SetTargetPose +from isaac_ros_moveit_goal_setter.move_group_client import MoveGroupClient +from moveit_msgs.msg import MoveItErrorCodes import rclpy from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.executors import MultiThreadedExecutor @@ -46,17 +45,24 @@ def __init__(self): self._plan_timer_period = self.declare_parameter( 'plan_timer_period', 0.01).get_parameter_value().double_value - self._tf_buffer = Buffer(cache_time=rclpy.duration.Duration(seconds=60.0)) - self._tf_listener = TransformListener(self._tf_buffer, self) + planner_group_name = self.declare_parameter( + 'planner_group_name', 'ur_manipulator').get_parameter_value().string_value + + pipeline_id = self.declare_parameter( + 'pipeline_id', 'isaac_ros_cumotion').get_parameter_value().string_value + + planner_id = self.declare_parameter( + 'planner_id', 'cuMotion').get_parameter_value().string_value - self._goal_service_cb_group = MutuallyExclusiveCallbackGroup() + ee_link = self.declare_parameter( + 'end_effector_link', 'wrist_3_link').get_parameter_value().string_value - self._goal_client = self.create_client( - SetTargetPose, 'set_target_pose', callback_group=self._goal_service_cb_group) + self.move_group_client = MoveGroupClient( + self, planner_group_name, pipeline_id, planner_id, ee_link) + + self._tf_buffer = Buffer(cache_time=rclpy.duration.Duration(seconds=60.0)) + self._tf_listener = TransformListener(self._tf_buffer, self) - while not self._goal_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('Service set_target_pose not available! Waiting...') - self._goal_req = SetTargetPose.Request() self.timer = self.create_timer(self._plan_timer_period, self.on_timer) def _transform_msg_to_pose_msg(self, tf_msg): @@ -71,14 +77,6 @@ def _transform_msg_to_pose_msg(self, tf_msg): pose.orientation.w = tf_msg.rotation.w return pose - def send_goal(self, pose): - self.get_logger().debug('Sending pose target to planner.') - self._goal_req.pose = pose - self.future = self._goal_client.call_async(self._goal_req) - while not self.future.done(): - time.sleep(0.001) - return self.future.result() - def on_timer(self): # Check if there is a valid transform between world and target frame @@ -100,9 +98,8 @@ def on_timer(self): output_msg.header.frame_id = self._world_frame output_msg.pose = self._transform_msg_to_pose_msg(world_frame_pose_target_frame.transform) - response = self.send_goal(output_msg) - self.get_logger().debug(f'Goal set with response: {response}') - if response.success: + response = self.move_group_client.send_goal_pose(output_msg) + if response.error_code.val == MoveItErrorCodes.SUCCESS: self._target_frame_idx = (self._target_frame_idx + 1) % len(self._target_frames) else: self.get_logger().warning('target pose was not reachable by planner, trying again \ diff --git a/isaac_ros_moveit_goal_setter/launch/isaac_ros_goal_setter.launch.py b/isaac_ros_moveit_goal_setter/launch/isaac_ros_goal_setter.launch.py deleted file mode 100644 index 66ddae3..0000000 --- a/isaac_ros_moveit_goal_setter/launch/isaac_ros_goal_setter.launch.py +++ /dev/null @@ -1,109 +0,0 @@ -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES', -# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the 'License'); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an 'AS IS' BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 - - -import launch -from launch.actions import DeclareLaunchArgument -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def get_robot_description(): - ur_type = LaunchConfiguration('ur_type') - robot_ip = LaunchConfiguration('robot_ip') - - joint_limit_params = PathJoinSubstitution( - [FindPackageShare('ur_description'), 'config', ur_type, 'joint_limits.yaml'] - ) - kinematics_params = PathJoinSubstitution( - [FindPackageShare('ur_description'), 'config', ur_type, 'default_kinematics.yaml'] - ) - physical_params = PathJoinSubstitution( - [FindPackageShare('ur_description'), 'config', ur_type, 'physical_parameters.yaml'] - ) - visual_params = PathJoinSubstitution( - [FindPackageShare('ur_description'), 'config', ur_type, 'visual_parameters.yaml'] - ) - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', - PathJoinSubstitution([FindPackageShare('ur_description'), 'urdf', 'ur.urdf.xacro']), - ' ', 'robot_ip:=', robot_ip, - ' ', 'joint_limit_params:=', joint_limit_params, - ' ', 'kinematics_params:=', kinematics_params, - ' ', 'physical_params:=', physical_params, - ' ', 'visual_params:=', visual_params, - ' ', 'safety_limits:=true', - ' ', 'safety_pos_margin:=0.15', - ' ', 'safety_k_position:=20', - ' ', 'name:=ur', ' ', 'ur_type:=', ur_type, ' ', 'prefix:=''', - ] - ) - - robot_description = {'robot_description': robot_description_content} - return robot_description - - -def get_robot_description_semantic(): - # MoveIt Configuration - robot_description_semantic_content = Command( - [ - PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', - PathJoinSubstitution([FindPackageShare('ur_moveit_config'), 'srdf', 'ur.srdf.xacro']), - ' ', 'name:=ur', ' ', 'prefix:=""', - ] - ) - robot_description_semantic = { - 'robot_description_semantic': robot_description_semantic_content - } - return robot_description_semantic - - -def generate_launch_description(): - launch_args = [ - DeclareLaunchArgument( - 'ur_type', - description='Type/series of used UR robot.', - choices=['ur3', 'ur3e', 'ur5', 'ur5e', 'ur10', 'ur10e', 'ur16e', 'ur20'], - default_value='ur5e', - ), - DeclareLaunchArgument( - 'robot_ip', - description='IP address of the robot', - default_value='192.56.1.2', - ), - - ] - moveit_kinematics_params = PathJoinSubstitution( - [FindPackageShare('ur_moveit_config'), 'config', 'default_kinematics.yaml'] - ) - robot_description = get_robot_description() - robot_description_semantic = get_robot_description_semantic() - isaac_ros_moveit_goal_setter = Node( - package='isaac_ros_moveit_goal_setter', - executable='isaac_ros_moveit_goal_setter', - name='isaac_ros_moveit_goal_setter', - output='screen', - parameters=[ - robot_description, - robot_description_semantic, - moveit_kinematics_params - ], - ) - - return launch.LaunchDescription(launch_args + [isaac_ros_moveit_goal_setter]) diff --git a/isaac_ros_moveit_goal_setter/package.xml b/isaac_ros_moveit_goal_setter/package.xml index ae3ef18..10ba00f 100644 --- a/isaac_ros_moveit_goal_setter/package.xml +++ b/isaac_ros_moveit_goal_setter/package.xml @@ -21,7 +21,7 @@ SPDX-License-Identifier: Apache-2.0 isaac_ros_moveit_goal_setter - 3.1.0 + 3.2.0 This package sets the goal for MoveIt Isaac ROS Maintainers Apache-2.0 @@ -35,14 +35,13 @@ SPDX-License-Identifier: Apache-2.0 isaac_ros_goal_setter_interfaces geometry_msgs moveit_ros_planning_interface - rclcpp - rclcpp_components + rclpy vision_msgs ament_lint_auto ament_lint_common - ament_cmake + ament_python diff --git a/isaac_ros_moveit_goal_setter/resource/isaac_ros_moveit_goal_setter b/isaac_ros_moveit_goal_setter/resource/isaac_ros_moveit_goal_setter new file mode 100644 index 0000000..e69de29 diff --git a/isaac_ros_moveit_goal_setter/setup.cfg b/isaac_ros_moveit_goal_setter/setup.cfg new file mode 100644 index 0000000..57075c3 --- /dev/null +++ b/isaac_ros_moveit_goal_setter/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/isaac_ros_moveit_goal_setter +[install] +install_scripts=$base/lib/isaac_ros_moveit_goal_setter diff --git a/isaac_ros_moveit_goal_setter/setup.py b/isaac_ros_moveit_goal_setter/setup.py new file mode 100644 index 0000000..f855f8e --- /dev/null +++ b/isaac_ros_moveit_goal_setter/setup.py @@ -0,0 +1,47 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'isaac_ros_moveit_goal_setter' + +setup( + name=package_name, + version='3.2.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Isaac ROS Maintainers', + maintainer_email='isaac-ros-maintainers@nvidia.com', + author='Kajanan Chinniah', + description='This package sets the goal for MoveIt', + license='NVIDIA Isaac ROS Software License', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'pose_to_pose_node = isaac_ros_moveit_goal_setter.pose_to_pose:main', + 'goal_initializer_node = isaac_ros_moveit_goal_setter.goal_initializer:main' + ], + }, +) diff --git a/isaac_ros_moveit_goal_setter/src/goal_setter_node.cpp b/isaac_ros_moveit_goal_setter/src/goal_setter_node.cpp deleted file mode 100644 index feff23d..0000000 --- a/isaac_ros_moveit_goal_setter/src/goal_setter_node.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// SPDX-License-Identifier: Apache-2.0 - -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "isaac_ros_moveit_goal_setter/goal_setter_node.hpp" - -namespace nvidia -{ -namespace isaac_ros -{ -namespace manipulation -{ - -GoalSetterNode::GoalSetterNode(std::string name, const rclcpp::NodeOptions & options) -: node_{std::make_shared(name, options)}, - planner_group_name_(node_->declare_parameter( - "planner_group_name", "ur_manipulator")), - planner_id_(node_->declare_parameter("planner_id", "cuMotion")), - end_effector_link_(node_->declare_parameter("end_effector_link", "wrist_3_link")), - move_group_interface_{moveit::planning_interface::MoveGroupInterface(node_, planner_group_name_)} -{ - set_target_pose_service_ = - node_->create_service( - "set_target_pose", std::bind( - &GoalSetterNode::SetTargetPoseCallback, this, std::placeholders::_1, std::placeholders::_2)); - ConfigureMoveit(); -} - -void GoalSetterNode::ConfigureMoveit() -{ - // Initialize the move group interface - move_group_interface_.setPlannerId(planner_id_); - RCLCPP_INFO(node_->get_logger(), "Planner ID : %s", move_group_interface_.getPlannerId().c_str()); - - move_group_interface_.setEndEffectorLink(end_effector_link_); - RCLCPP_INFO(node_->get_logger(), "End Effector Link : %s", end_effector_link_.c_str()); -} - -void GoalSetterNode::SetTargetPoseCallback( - const std::shared_ptr req, - std::shared_ptr res) -{ - res->success = false; - RCLCPP_DEBUG(node_->get_logger(), "Triggered SetTargetPoseCallback"); - RCLCPP_DEBUG( - node_->get_logger(), "Pose : x=%f, y=%f, z=%f, qx=%f, qy=%f, qz=%f, qw=%f", - req->pose.pose.position.x, req->pose.pose.position.y, req->pose.pose.position.z, - req->pose.pose.orientation.x, req->pose.pose.orientation.y, req->pose.pose.orientation.z, - req->pose.pose.orientation.w); - - auto success = move_group_interface_.setPoseTarget(req->pose, end_effector_link_); - if (!success) { - RCLCPP_ERROR(node_->get_logger(), "Failed to set target pose!"); - return; - } - - auto const [status, plan] = [this] { - moveit::planning_interface::MoveGroupInterface::Plan msg; - auto const ok = static_cast(move_group_interface_.plan(msg)); - return std::make_pair(ok, msg); - }(); - - // Execute the plan - if (status) { - RCLCPP_ERROR(node_->get_logger(), "Executing!"); - move_group_interface_.execute(plan); - res->success = true; - } else { - RCLCPP_ERROR(node_->get_logger(), "Planning failed!"); - } - -} - -} // namespace manipulation -} // namespace isaac_ros -} // namespace nvidia - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto goal_setter_node = std::make_shared( - "moveit_goal_setter", - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(goal_setter_node->GetNode()); - executor.spin(); - rclcpp::shutdown(); - return 0; -}