From 498c53c6c9b03d145545cb821f8f4b3b86cbf0f9 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Thu, 20 Jul 2023 20:39:04 -0700 Subject: [PATCH 1/9] Added functions to enable/disable collisions with an object vis the allowed collision matrix --- pymoveit2/moveit2.py | 123 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 123 insertions(+) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 6d7684e..f02807a 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -1,3 +1,4 @@ +import copy import threading from enum import Enum from typing import List, Optional, Tuple, Union @@ -7,6 +8,7 @@ from moveit_msgs.action import ExecuteTrajectory, MoveGroup from moveit_msgs.msg import ( AttachedCollisionObject, + AllowedCollisionEntry, CollisionObject, Constraints, JointConstraint, @@ -15,8 +17,10 @@ PositionConstraint, ) from moveit_msgs.srv import ( + ApplyPlanningScene, GetCartesianPath, GetMotionPlan, + GetPlanningScene, GetPositionFK, GetPositionIK, ) @@ -219,6 +223,27 @@ def __init__( depth=1, ), callback_group=self._callback_group, + + # Create a service for getting the planning scene + self._get_planning_scene_service = self._node.create_client( + srv_type=GetPlanningScene, + srv_name="get_planning_scene", + qos_profile=QoSProfile( + callback_group=callback_group, + ) + self.__planning_scene = None + + # Create a service for applying the planning scene + self._apply_planning_scene_service = self._node.create_client( + srv_type=ApplyPlanningScene, + srv_name="apply_planning_scene", + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), + callback_group=callback_group, ) self.__collision_object_publisher = self._node.create_publisher( @@ -1691,6 +1716,104 @@ def detach_all_collision_objects(self): ) self.__attached_collision_object_publisher.publish(msg) + def __update_planning_scene(self) -> bool: + """ + Gets the current planning scene. Returns whether the service call was + successful. + """ + + if not self._get_planning_scene_service.service_is_ready(): + self._node.get_logger().warn( + f"Service '{self._get_planning_scene_service.srv_name}' is not yet available. Better luck next time!" + ) + return False + self.__planning_scene = self._get_planning_scene_service.call( + GetPlanningScene.Request() + ).scene + return True + + def enable_collisions(self, id: str) -> bool: + """ + Takes in the ID of an element in the planning scene. Modified the allowed + collision matrix to allow collisions between that element and all other + elements. Returns whether it succeeded. + """ + # Update the planning scene + if not self.__update_planning_scene(): + return False + allowed_collision_matrix = self.__planning_scene.allowed_collision_matrix + old_allowed_collision_matrix = copy.deepcopy(allowed_collision_matrix) + + # Destructively modify the allowed collision matrix. For every current + # link, disable collision with the object with ID. For the object with + # ID, disable collision with every current link. + allowed_collision_matrix.entry_names.append(id) + for i in range(len(allowed_collision_matrix.entry_values)): + allowed_collision_matrix.entry_values[i].enabled.append(True) + allowed_collision_matrix.entry_values.append( + AllowedCollisionEntry( + enabled=[True for _ in range(len(allowed_collision_matrix.entry_names))] + ) + ) + + # Apply the new planning scene + if not self._apply_planning_scene_service.service_is_ready(): + self._node.get_logger().warn( + f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!" + ) + return False + resp = self._apply_planning_scene_service.call( + ApplyPlanningScene.Request( + scene=self.__planning_scene + ) + ) + + # If it failed, restore the old planning scene + if not resp.success: + self.__planning_scene.allowed_collision_matrix = old_allowed_collision_matrix + + return resp.success + + def disable_collisions(self, id: str) -> bool: + """ + Takes in the ID of an element in the planning scene. Modified the allowed + collision matrix to disallow collisions between that element and all other + elements. Returns whether it succeeded. + """ + # Update the planning scene + if not self.__update_planning_scene(): + return False + allowed_collision_matrix = self.__planning_scene.allowed_collision_matrix + old_allowed_collision_matrix = copy.deepcopy(allowed_collision_matrix) + + # Destructively modify the allowed collision matrix to remove the object + # with ID. + if id in allowed_collision_matrix.entry_names: + j = allowed_collision_matrix.entry_names.index(id) + # Remove the object's row and column from the allowed collision matrix + allowed_collision_matrix.entry_names.pop(j) + allowed_collision_matrix.entry_values.pop(j) + for i in range(len(allowed_collision_matrix.entry_values)): + allowed_collision_matrix.entry_values[i].enabled.pop(j) + + # Apply the new planning scene + if not self._apply_planning_scene_service.service_is_ready(): + self._node.get_logger().warn( + f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!" + ) + return False + resp = self._apply_planning_scene_service.call( + ApplyPlanningScene.Request( + scene=self.__planning_scene + ) + ) + + # If it failed, restore the old planning scene + if not resp.success: + self.__planning_scene.allowed_collision_matrix = old_allowed_collision_matrix + + return resp.success + def __joint_state_callback(self, msg: JointState): # Update only if all relevant joints are included in the message for joint_name in self.joint_names: From 3f49a494b3d0bcb9b98a48708884a522b9e52411 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Mon, 28 Aug 2023 17:42:24 -0700 Subject: [PATCH 2/9] Clarified function name, reduced replicated code --- pymoveit2/moveit2.py | 83 ++++++++++++++------------------------------ 1 file changed, 27 insertions(+), 56 deletions(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index f02807a..215e7fd 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -1732,11 +1732,15 @@ def __update_planning_scene(self) -> bool: ).scene return True - def enable_collisions(self, id: str) -> bool: + def allow_collisions(self, id: str, allow: bool) -> bool: """ - Takes in the ID of an element in the planning scene. Modified the allowed - collision matrix to allow collisions between that element and all other - elements. Returns whether it succeeded. + Takes in the ID of an element in the planning scene. Modifies the allowed + collision matrix to (dis)allow collisions between that object and all other + object. + + If `allow` is True, a plan will succeed even if the robot collides with that object. + If `allow` is False, a plan will fail if the robot collides with that object. + Returns whether it succesfully updated the allowed collision matrix. """ # Update the planning scene if not self.__update_planning_scene(): @@ -1744,57 +1748,26 @@ def enable_collisions(self, id: str) -> bool: allowed_collision_matrix = self.__planning_scene.allowed_collision_matrix old_allowed_collision_matrix = copy.deepcopy(allowed_collision_matrix) - # Destructively modify the allowed collision matrix. For every current - # link, disable collision with the object with ID. For the object with - # ID, disable collision with every current link. - allowed_collision_matrix.entry_names.append(id) - for i in range(len(allowed_collision_matrix.entry_values)): - allowed_collision_matrix.entry_values[i].enabled.append(True) - allowed_collision_matrix.entry_values.append( - AllowedCollisionEntry( - enabled=[True for _ in range(len(allowed_collision_matrix.entry_names))] - ) - ) - - # Apply the new planning scene - if not self._apply_planning_scene_service.service_is_ready(): - self._node.get_logger().warn( - f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!" - ) - return False - resp = self._apply_planning_scene_service.call( - ApplyPlanningScene.Request( - scene=self.__planning_scene - ) - ) - - # If it failed, restore the old planning scene - if not resp.success: - self.__planning_scene.allowed_collision_matrix = old_allowed_collision_matrix - - return resp.success - - def disable_collisions(self, id: str) -> bool: - """ - Takes in the ID of an element in the planning scene. Modified the allowed - collision matrix to disallow collisions between that element and all other - elements. Returns whether it succeeded. - """ - # Update the planning scene - if not self.__update_planning_scene(): - return False - allowed_collision_matrix = self.__planning_scene.allowed_collision_matrix - old_allowed_collision_matrix = copy.deepcopy(allowed_collision_matrix) - - # Destructively modify the allowed collision matrix to remove the object - # with ID. - if id in allowed_collision_matrix.entry_names: + # Get the location in the allowed collision matrix of the object + j = None + if id not in allowed_collision_matrix.entry_names: + allowed_collision_matrix.entry_names.append(id) + else: j = allowed_collision_matrix.entry_names.index(id) - # Remove the object's row and column from the allowed collision matrix - allowed_collision_matrix.entry_names.pop(j) - allowed_collision_matrix.entry_values.pop(j) - for i in range(len(allowed_collision_matrix.entry_values)): - allowed_collision_matrix.entry_values[i].enabled.pop(j) + # For all other objects, (dis)allow collisions with the object with `id` + for i in range(len(allowed_collision_matrix.entry_values)): + if j is None: + allowed_collision_matrix.entry_values[i].enabled.append(allow) + elif i != j: + allowed_collision_matrix.entry_values[i].enabled[j] = allow + # For the object with `id`, (dis)allow collisions with all other objects + allowed_collision_entry = AllowedCollisionEntry( + enabled=[allow for _ in range(len(allowed_collision_matrix.entry_names))] + ) + if j is None: + allowed_collision_matrix.entry_values.append(allowed_collision_entry) + else: + allowed_collision_matrix.entry_values[j] = allowed_collision_entry # Apply the new planning scene if not self._apply_planning_scene_service.service_is_ready(): @@ -1923,7 +1896,6 @@ def _send_goal_async_move_action(self): ) return - self.__last_error_code = None self.__is_motion_requested = True self.__send_goal_future_move_action = self.__move_action_client.send_goal_async( goal=self.__move_action_goal, @@ -1985,7 +1957,6 @@ def _send_goal_async_execute_trajectory( ) return - self.__last_error_code = None self.__is_motion_requested = True self.__send_goal_future_execute_trajectory = ( self._execute_trajectory_action_client.send_goal_async( From ab2d3bec6e7979ff81442a9246eefda2384ebba6 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Thu, 14 Sep 2023 13:18:07 -0700 Subject: [PATCH 3/9] Made toggling collisions async --- pymoveit2/moveit2.py | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 215e7fd..4178b90 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -232,6 +232,7 @@ def __init__( callback_group=callback_group, ) self.__planning_scene = None + self.__old_allowed_collision_matrix = None # Create a service for applying the planning scene self._apply_planning_scene_service = self._node.create_client( @@ -1741,12 +1742,14 @@ def allow_collisions(self, id: str, allow: bool) -> bool: If `allow` is True, a plan will succeed even if the robot collides with that object. If `allow` is False, a plan will fail if the robot collides with that object. Returns whether it succesfully updated the allowed collision matrix. + + Returns the future of the service call. """ # Update the planning scene if not self.__update_planning_scene(): return False allowed_collision_matrix = self.__planning_scene.allowed_collision_matrix - old_allowed_collision_matrix = copy.deepcopy(allowed_collision_matrix) + self.__old_allowed_collision_matrix = copy.deepcopy(allowed_collision_matrix) # Get the location in the allowed collision matrix of the object j = None @@ -1775,15 +1778,26 @@ def allow_collisions(self, id: str, allow: bool) -> bool: f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!" ) return False - resp = self._apply_planning_scene_service.call( + return self._apply_planning_scene_service.call_async( ApplyPlanningScene.Request( scene=self.__planning_scene ) ) + def process_allow_collision_future(self, future: Future) -> bool: + """ + Return whether the allow collision service call is done and has succeeded + or not. If it failed, reset the allowed collision matrix to the old one. + """ + if not future.done(): + return False + + # Get response + resp = future.result() + # If it failed, restore the old planning scene if not resp.success: - self.__planning_scene.allowed_collision_matrix = old_allowed_collision_matrix + self.__planning_scene.allowed_collision_matrix = self.__old_allowed_collision_matrix return resp.success From 0642409d09548f869a1cd8ef287fd5d70514a733 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 3 Nov 2023 10:17:05 -0700 Subject: [PATCH 4/9] Fix type annotations for consistency --- pymoveit2/moveit2.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 4178b90..76dda34 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -1733,7 +1733,7 @@ def __update_planning_scene(self) -> bool: ).scene return True - def allow_collisions(self, id: str, allow: bool) -> bool: + def allow_collisions(self, id: str, allow: bool) -> Optional[Future]: """ Takes in the ID of an element in the planning scene. Modifies the allowed collision matrix to (dis)allow collisions between that object and all other @@ -1747,7 +1747,7 @@ def allow_collisions(self, id: str, allow: bool) -> bool: """ # Update the planning scene if not self.__update_planning_scene(): - return False + return None allowed_collision_matrix = self.__planning_scene.allowed_collision_matrix self.__old_allowed_collision_matrix = copy.deepcopy(allowed_collision_matrix) @@ -1777,7 +1777,7 @@ def allow_collisions(self, id: str, allow: bool) -> bool: self._node.get_logger().warn( f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!" ) - return False + return None return self._apply_planning_scene_service.call_async( ApplyPlanningScene.Request( scene=self.__planning_scene From 8c683f3770fc116027e85be3dd3742c24b6680d8 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Tue, 27 Feb 2024 18:48:22 -0800 Subject: [PATCH 5/9] formatting --- pymoveit2/moveit2.py | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 76dda34..0818da6 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -7,8 +7,8 @@ from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion from moveit_msgs.action import ExecuteTrajectory, MoveGroup from moveit_msgs.msg import ( - AttachedCollisionObject, AllowedCollisionEntry, + AttachedCollisionObject, CollisionObject, Constraints, JointConstraint, @@ -223,12 +223,18 @@ def __init__( depth=1, ), callback_group=self._callback_group, + ) # Create a service for getting the planning scene self._get_planning_scene_service = self._node.create_client( srv_type=GetPlanningScene, srv_name="get_planning_scene", - qos_profile=QoSProfile( + qos_profile=QoSProfile( + durability=QoSDurabilityPolicy.VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ), callback_group=callback_group, ) self.__planning_scene = None @@ -1738,10 +1744,10 @@ def allow_collisions(self, id: str, allow: bool) -> Optional[Future]: Takes in the ID of an element in the planning scene. Modifies the allowed collision matrix to (dis)allow collisions between that object and all other object. - + If `allow` is True, a plan will succeed even if the robot collides with that object. If `allow` is False, a plan will fail if the robot collides with that object. - Returns whether it succesfully updated the allowed collision matrix. + Returns whether it successfully updated the allowed collision matrix. Returns the future of the service call. """ @@ -1779,9 +1785,7 @@ def allow_collisions(self, id: str, allow: bool) -> Optional[Future]: ) return None return self._apply_planning_scene_service.call_async( - ApplyPlanningScene.Request( - scene=self.__planning_scene - ) + ApplyPlanningScene.Request(scene=self.__planning_scene) ) def process_allow_collision_future(self, future: Future) -> bool: @@ -1797,7 +1801,9 @@ def process_allow_collision_future(self, future: Future) -> bool: # If it failed, restore the old planning scene if not resp.success: - self.__planning_scene.allowed_collision_matrix = self.__old_allowed_collision_matrix + self.__planning_scene.allowed_collision_matrix = ( + self.__old_allowed_collision_matrix + ) return resp.success From 179c83ea72ec4b00d8bab2c4a4eb3097781c4529 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Tue, 27 Feb 2024 19:14:26 -0800 Subject: [PATCH 6/9] Added tests --- CMakeLists.txt | 1 + examples/ex_allow_collisions.py | 70 +++++++++++++++++++++++++++++++++ 2 files changed, 71 insertions(+) create mode 100755 examples/ex_allow_collisions.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 43d5fc3..8e195a8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,7 @@ ament_python_install_package(pymoveit2) # Install examples set(EXAMPLES_DIR examples) install(PROGRAMS + ${EXAMPLES_DIR}/ex_allow_collisions.py ${EXAMPLES_DIR}/ex_collision_mesh.py ${EXAMPLES_DIR}/ex_collision_primitive.py ${EXAMPLES_DIR}/ex_fk.py diff --git a/examples/ex_allow_collisions.py b/examples/ex_allow_collisions.py new file mode 100755 index 0000000..554b2d8 --- /dev/null +++ b/examples/ex_allow_collisions.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 +""" +Example of adding and removing a collision object with a primitive geometry. +- ros2 run pymoveit2 ex_allow_collisions.py --ros-args -p id:="sphere" -p allow:=true +- ros2 run pymoveit2 ex_allow_collisions.py --ros-args -p id:="sphere" -p allow:=false +""" + +from threading import Thread + +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node + +from pymoveit2 import MoveIt2 +from pymoveit2.robots import panda + + +def main(): + rclpy.init() + + # Create node for this example + node = Node("ex_allow_collisions") + + # Declare parameter for joint positions + node.declare_parameter( + "id", + "box", + ) + node.declare_parameter( + "allow", + True, + ) + + # Create callback group that allows execution of callbacks in parallel without restrictions + callback_group = ReentrantCallbackGroup() + + # Create MoveIt 2 interface + moveit2 = MoveIt2( + node=node, + joint_names=panda.joint_names(), + base_link_name=panda.base_link_name(), + end_effector_name=panda.end_effector_name(), + group_name=panda.MOVE_GROUP_ARM, + callback_group=callback_group, + ) + + # Spin the node in background thread(s) and wait a bit for initialization + executor = rclpy.executors.MultiThreadedExecutor(2) + executor.add_node(node) + executor_thread = Thread(target=executor.spin, daemon=True, args=()) + executor_thread.start() + node.create_rate(1.0).sleep() + + # Get parameters + object_id = node.get_parameter("id").get_parameter_value().string_value + allow = node.get_parameter("allow").get_parameter_value().bool_value + + # (Dis)allow collisions + moveit2.allow_collisions(object_id, allow) + node.get_logger().info( + f"{'Allow' if allow else 'Disallow'}ed collisions between all obejcts and '{object_id}'" + ) + + rclpy.shutdown() + executor_thread.join() + exit(0) + + +if __name__ == "__main__": + main() From 9dcdcc31402894a7a4d40fc18af2a55370f471f4 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Tue, 27 Feb 2024 19:17:47 -0800 Subject: [PATCH 7/9] Spelling --- examples/ex_allow_collisions.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/ex_allow_collisions.py b/examples/ex_allow_collisions.py index 554b2d8..0ce6710 100755 --- a/examples/ex_allow_collisions.py +++ b/examples/ex_allow_collisions.py @@ -58,7 +58,7 @@ def main(): # (Dis)allow collisions moveit2.allow_collisions(object_id, allow) node.get_logger().info( - f"{'Allow' if allow else 'Disallow'}ed collisions between all obejcts and '{object_id}'" + f"{'Allow' if allow else 'Disallow'}ed collisions between all objects and '{object_id}'" ) rclpy.shutdown() From 18caaa39e9ae363d257453547e6d5697f729e9d3 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 29 Mar 2024 09:55:46 -0700 Subject: [PATCH 8/9] Update docstring --- examples/ex_allow_collisions.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/ex_allow_collisions.py b/examples/ex_allow_collisions.py index 0ce6710..d6519a0 100755 --- a/examples/ex_allow_collisions.py +++ b/examples/ex_allow_collisions.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 """ -Example of adding and removing a collision object with a primitive geometry. +Example of (dis)allowing collisions between the robot object and an object with +the specified ID. - ros2 run pymoveit2 ex_allow_collisions.py --ros-args -p id:="sphere" -p allow:=true - ros2 run pymoveit2 ex_allow_collisions.py --ros-args -p id:="sphere" -p allow:=false """ From 534aebb2a2708f1cd1293ea2526d198be42fa689 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 29 Mar 2024 09:55:58 -0700 Subject: [PATCH 9/9] Fix unnecessary change --- pymoveit2/moveit2.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 0818da6..38566cd 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -1916,6 +1916,7 @@ def _send_goal_async_move_action(self): ) return + self.__last_error_code = None self.__is_motion_requested = True self.__send_goal_future_move_action = self.__move_action_client.send_goal_async( goal=self.__move_action_goal, @@ -1977,6 +1978,7 @@ def _send_goal_async_execute_trajectory( ) return + self.__last_error_code = None self.__is_motion_requested = True self.__send_goal_future_execute_trajectory = ( self._execute_trajectory_action_client.send_goal_async(