From 7d02e7de42db7f95c6ba74076ca1da9526b0c1c8 Mon Sep 17 00:00:00 2001 From: WU MENGHUNG Date: Wed, 19 Jun 2024 21:21:34 +0900 Subject: [PATCH 1/2] attach collision objects when planning motion --- .../isaac_ros_cumotion/cumotion_planner.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py index c2dbb2d..5ef2d13 100644 --- a/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py +++ b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py @@ -547,6 +547,18 @@ def execute_callback(self, goal_handle): else: start_state = current_joint_state + # attach object to end effector: + for obj in scene.robot_state.attached_collision_objects: + cumotion_objects, supported_objects = self.get_cumotion_collision_object(obj.object) + if supported_objects: + ee_pose = self.motion_gen.compute_kinematics(start_state).ee_pose + self.motion_gen.attach_external_objects_to_robot( + start_state, + cumotion_objects, + link_name=obj.link_name, + world_objects_pose_offset=ee_pose, + ) + if len(plan_req.goal_constraints[0].joint_constraints) > 0: self.get_logger().info('Calculating goal pose from Joint target') goal_config = [ From 30fcf44fe78e352a481ab8429f1e30bc460b9405 Mon Sep 17 00:00:00 2001 From: WU MENGHUNG Date: Tue, 23 Jul 2024 09:38:40 +0000 Subject: [PATCH 2/2] merge all collision objects before calling attach_external_objects_to_robot --- .../isaac_ros_cumotion/cumotion_planner.py | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py index 5ef2d13..65e3f54 100644 --- a/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py +++ b/isaac_ros_cumotion/isaac_ros_cumotion/cumotion_planner.py @@ -548,16 +548,20 @@ def execute_callback(self, goal_handle): start_state = current_joint_state # attach object to end effector: + objs = [] for obj in scene.robot_state.attached_collision_objects: - cumotion_objects, supported_objects = self.get_cumotion_collision_object(obj.object) + collision_objects, supported_objects = self.get_cumotion_collision_object(obj.object) if supported_objects: - ee_pose = self.motion_gen.compute_kinematics(start_state).ee_pose - self.motion_gen.attach_external_objects_to_robot( - start_state, - cumotion_objects, - link_name=obj.link_name, - world_objects_pose_offset=ee_pose, - ) + objs.extend(collision_objects) + if objs: + self.get_logger().info(f'Attach object to link {obj.link_name}') + ee_pose = self.motion_gen.compute_kinematics(start_state).ee_pose + self.motion_gen.attach_external_objects_to_robot( + start_state, + objs, + link_name=obj.link_name, + world_objects_pose_offset=ee_pose, + ) if len(plan_req.goal_constraints[0].joint_constraints) > 0: self.get_logger().info('Calculating goal pose from Joint target')