From c1f66b58d9c5c761b449349ac9d7670be253b834 Mon Sep 17 00:00:00 2001 From: Jaiveer Singh Date: Fri, 31 Jan 2025 02:48:45 +0000 Subject: [PATCH] Improve input validation --- README.md | 2 +- isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py | 7 +++++++ isaac_ros_cumotion/params/isaac_ros_cumotion_params.yaml | 1 + 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index d71415a..8213138 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ NVIDIA accelerated packages for arm motion planning and control ## Overview -[Isaac ROS cuMotion](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_cumotion) provides CUDA-accelerated manipulation +Isaac ROS cuMotion provides CUDA-accelerated manipulation capabilities for robots in ROS 2. It provides two main capabilities: 1. Motion generation for robot arms via integration of [cuMotion](https://nvidia-isaac-ros.github.io/concepts/manipulation/index.html#concept-cumotion) diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py index 058d305..a7e0773 100644 --- a/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py +++ b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py @@ -696,6 +696,13 @@ def execute_callback(self, goal_handle): joint_names=self.__js_buffer['joint_names'], ) state.velocity = self.tensor_args.to_device(self.__js_buffer['velocity']).unsqueeze(0) + if state.velocity.shape != state.position.shape: + self.get_logger().error( + 'start joint position shape is ' + str(state.position.shape) + + ' start velocity shape is ' + str(state.velocity.shape) + + ', both should match. JointState was read from ' + self.__joint_states_topic + ) + return result current_joint_state = self.motion_gen.get_active_js(state) if start_state is not None and plan_req.start_state.is_diff: start_state.position += current_joint_state.position diff --git a/isaac_ros_cumotion/params/isaac_ros_cumotion_params.yaml b/isaac_ros_cumotion/params/isaac_ros_cumotion_params.yaml index 30ca15d..d06a1cf 100644 --- a/isaac_ros_cumotion/params/isaac_ros_cumotion_params.yaml +++ b/isaac_ros_cumotion/params/isaac_ros_cumotion_params.yaml @@ -9,6 +9,7 @@ num_trajopt_seeds: 6 include_trajopt_retract_seed: True num_trajopt_time_steps: 32 + joint_states_topic: "/joint_states" interpolation_dt: 0.025 collision_cache_cuboid: 20 collision_cache_mesh: 20