Merge pull request #29 from NVIDIA-ISAAC-ROS/release-3.2

Improve input validation

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)

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

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