From f0ec327e138cdc701bb595ef94d13f6855cb7b0b Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 1 Sep 2023 17:25:41 -0700 Subject: [PATCH 01/10] Added ability to have cartesian planning calls avoid collisions --- pymoveit2/moveit2.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index ef2ca26..e44b2cc 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -2192,6 +2192,12 @@ def planner_id(self) -> int: def planner_id(self, value: str): self.__move_action_goal.request.planner_id = value + def cartesian_avoid_collisions(self) -> bool: + return self.__cartesian_path_request.request.avoid_collisions + + @cartesian_avoid_collisions.setter + def cartesian_avoid_collisions(self, value: bool): + self.__cartesian_path_request.avoid_collisions = value def init_joint_state( joint_names: List[str], From f94728c479ec61f9532655fa91d1f69fccf2247e Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Wed, 6 Sep 2023 10:56:16 -0700 Subject: [PATCH 02/10] Fix bad merge --- pymoveit2/moveit2.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index e44b2cc..ec8e5fa 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -2176,6 +2176,14 @@ def allowed_planning_time(self) -> float: def allowed_planning_time(self, value: float): self.__move_action_goal.request.allowed_planning_time = value + @property + def cartesian_avoid_collisions(self) -> bool: + return self.__cartesian_path_request.request.avoid_collisions + + @cartesian_avoid_collisions.setter + def cartesian_avoid_collisions(self, value: bool): + self.__cartesian_path_request.avoid_collisions = value + @property def pipeline_id(self) -> int: return self.__move_action_goal.request.pipeline_id @@ -2192,13 +2200,6 @@ def planner_id(self) -> int: def planner_id(self, value: str): self.__move_action_goal.request.planner_id = value - def cartesian_avoid_collisions(self) -> bool: - return self.__cartesian_path_request.request.avoid_collisions - - @cartesian_avoid_collisions.setter - def cartesian_avoid_collisions(self, value: bool): - self.__cartesian_path_request.avoid_collisions = value - def init_joint_state( joint_names: List[str], joint_positions: Optional[List[str]] = None, From 43f43298a7a497b7dab67722515c600c467f8cb5 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Thu, 7 Sep 2023 11:27:18 -0700 Subject: [PATCH 03/10] Allow users to set cartesian jump threshold --- pymoveit2/moveit2.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index ec8e5fa..e968703 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -2184,6 +2184,14 @@ def cartesian_avoid_collisions(self) -> bool: def cartesian_avoid_collisions(self, value: bool): self.__cartesian_path_request.avoid_collisions = value + @property + def cartesian_jump_threshold(self) -> float: + return self.__cartesian_path_request.request.jump_threshold + + @cartesian_jump_threshold.setter + def cartesian_jump_threshold(self, value: float): + self.__cartesian_path_request.jump_threshold = value + @property def pipeline_id(self) -> int: return self.__move_action_goal.request.pipeline_id From be4992fb0b6c67e477577119a855351bfd06af6c Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Thu, 7 Sep 2023 11:28:45 -0700 Subject: [PATCH 04/10] Allow users to set cartesian max step --- pymoveit2/moveit2.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index e968703..e8a0e5b 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -537,6 +537,7 @@ def plan_async( weight_joint_position: float = 1.0, start_joint_state: Optional[Union[JointState, List[float]]] = None, cartesian: bool = False, + max_step: float = 0.0025, ) -> Optional[Future]: """ Plan motion based on previously set goals. Optional arguments can be passed in to @@ -636,7 +637,8 @@ def plan_async( future = self._plan_cartesian_path( frame_id=pose_stamped.header.frame_id if pose_stamped is not None - else frame_id + else frame_id, + max_step=max_step, ) else: # Use service From 49ed859d8e14efffdc63db53bf9091654cd9320a Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Thu, 7 Sep 2023 11:29:29 -0700 Subject: [PATCH 05/10] Pass frame_id into the cartesian planning service --- pymoveit2/moveit2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index e8a0e5b..72c14fd 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -635,10 +635,10 @@ def plan_async( # Plan trajectory asynchronously by service call if cartesian: future = self._plan_cartesian_path( + max_step=max_step, frame_id=pose_stamped.header.frame_id if pose_stamped is not None else frame_id, - max_step=max_step, ) else: # Use service From ec9622901f1d8e45ac753bfee3fda506110dcc12 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Thu, 7 Sep 2023 12:58:56 -0700 Subject: [PATCH 06/10] Add ability to specify a required fraction for a cartesian plan to succeed --- pymoveit2/moveit2.py | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 72c14fd..6c117f1 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -518,6 +518,7 @@ def plan( return self.get_trajectory(future, cartesian=cartesian) + def plan_async( self, pose: Optional[Union[PoseStamped, Pose]] = None, @@ -651,13 +652,19 @@ def plan_async( return future def get_trajectory( - self, future: Future, cartesian: bool = False + self, + future: Future, + cartesian: bool = False, + cartesian_fraction_threshold: float = 0.0, ) -> Optional[JointTrajectory]: """ Takes in a future returned by plan_async and returns the trajectory if the future is done and planning was successful, else None. + + For cartesian plans, the plan is rejected if the fraction of the path that was completed is + less than `cartesian_fraction_threshold`. """ - if not future.done(): + if (not future.done()): self._node.get_logger().warn( "Cannot get trajectory because future is not done." ) @@ -668,13 +675,20 @@ def get_trajectory( # Cartesian if cartesian: if MoveItErrorCodes.SUCCESS == res.error_code.val: - return res.solution.joint_trajectory + if res.fraction >= cartesian_fraction_threshold: + return res.solution.joint_trajectory + else: + self._node.get_logger().warn( + f"Planning failed! Cartesian planner completed {res.fraction} " + f"of the trajectory, less than the threshold {cartesian_fraction_threshold}." + ) + return None else: self._node.get_logger().warn( f"Planning failed! Error code: {res.error_code.val}." ) return None - + # Else Kinematic res = res.motion_plan_response if MoveItErrorCodes.SUCCESS == res.error_code.val: From 43ac28fe04760fcd72691feab8506c7a4994be80 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Tue, 26 Sep 2023 16:54:32 -0700 Subject: [PATCH 07/10] Allowed users to get end effector name and base link name --- pymoveit2/moveit2.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 6c117f1..05bcedb 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -2137,6 +2137,14 @@ def __init_compute_ik(self): def follow_joint_trajectory_action_client(self) -> str: return self.__follow_joint_trajectory_action_client + @property + def end_effector_name(self) -> str: + return self.__end_effector_name + + @property + def base_link_name(self) -> str: + return self.__base_link_name + @property def joint_names(self) -> List[str]: return self.__joint_names From 888ab2ce36fbcc572e4cc393bd28d148f34055c5 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 29 Sep 2023 18:03:39 -0700 Subject: [PATCH 08/10] Add revolute and prismatic jump thresholds --- pymoveit2/moveit2.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 05bcedb..ce2d3ee 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -2216,6 +2216,22 @@ def cartesian_jump_threshold(self) -> float: def cartesian_jump_threshold(self, value: float): self.__cartesian_path_request.jump_threshold = value + @property + def cartesian_prismatic_jump_threshold(self) -> float: + return self.__cartesian_path_request.request.prismatic_jump_threshold + + @cartesian_prismatic_jump_threshold.setter + def cartesian_prismatic_jump_threshold(self, value: float): + self.__cartesian_path_request.prismatic_jump_threshold = value + + @property + def cartesian_revolute_jump_threshold(self) -> float: + return self.__cartesian_path_request.request.revolute_jump_threshold + + @cartesian_revolute_jump_threshold.setter + def cartesian_revolute_jump_threshold(self, value: float): + self.__cartesian_path_request.revolute_jump_threshold = value + @property def pipeline_id(self) -> int: return self.__move_action_goal.request.pipeline_id From 5d9a236b1fd6155a2542aeb126d667c161a2afc5 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Mon, 1 Apr 2024 10:44:07 -0700 Subject: [PATCH 09/10] Formatting --- pymoveit2/moveit2.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index ce2d3ee..565a745 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -518,7 +518,6 @@ def plan( return self.get_trajectory(future, cartesian=cartesian) - def plan_async( self, pose: Optional[Union[PoseStamped, Pose]] = None, @@ -664,7 +663,7 @@ def get_trajectory( For cartesian plans, the plan is rejected if the fraction of the path that was completed is less than `cartesian_fraction_threshold`. """ - if (not future.done()): + if not future.done(): self._node.get_logger().warn( "Cannot get trajectory because future is not done." ) @@ -688,7 +687,7 @@ def get_trajectory( f"Planning failed! Error code: {res.error_code.val}." ) return None - + # Else Kinematic res = res.motion_plan_response if MoveItErrorCodes.SUCCESS == res.error_code.val: @@ -2248,6 +2247,7 @@ def planner_id(self) -> int: def planner_id(self, value: str): self.__move_action_goal.request.planner_id = value + def init_joint_state( joint_names: List[str], joint_positions: Optional[List[str]] = None, From 8d0bd53d23409eadd5feca613ace0a2a49031f11 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Wed, 3 Apr 2024 16:38:35 -0700 Subject: [PATCH 10/10] Added test cases --- examples/ex_pose_goal.py | 39 ++++++++++++++++++++++++++++++++++++--- pymoveit2/moveit2.py | 18 ++++++++++++++++-- 2 files changed, 52 insertions(+), 5 deletions(-) diff --git a/examples/ex_pose_goal.py b/examples/ex_pose_goal.py index 401fd19..fa701ad 100755 --- a/examples/ex_pose_goal.py +++ b/examples/ex_pose_goal.py @@ -25,12 +25,17 @@ def main(): # Declare parameters for position and orientation node.declare_parameter("position", [0.5, 0.0, 0.25]) node.declare_parameter("quat_xyzw", [1.0, 0.0, 0.0, 0.0]) - node.declare_parameter("cartesian", False) node.declare_parameter("synchronous", True) # If non-positive, don't cancel. Only used if synchronous is False node.declare_parameter("cancel_after_secs", 0.0) # Planner ID node.declare_parameter("planner_id", "RRTConnectkConfigDefault") + # Declare parameters for cartesian planning + node.declare_parameter("cartesian", False) + node.declare_parameter("cartesian_max_step", 0.0025) + node.declare_parameter("cartesian_fraction_threshold", 0.0) + node.declare_parameter("cartesian_jump_threshold", 0.0) + node.declare_parameter("cartesian_avoid_collisions", False) # Create callback group that allows execution of callbacks in parallel without restrictions callback_group = ReentrantCallbackGroup() @@ -62,17 +67,45 @@ def main(): # Get parameters position = node.get_parameter("position").get_parameter_value().double_array_value quat_xyzw = node.get_parameter("quat_xyzw").get_parameter_value().double_array_value - cartesian = node.get_parameter("cartesian").get_parameter_value().bool_value synchronous = node.get_parameter("synchronous").get_parameter_value().bool_value cancel_after_secs = ( node.get_parameter("cancel_after_secs").get_parameter_value().double_value ) + cartesian = node.get_parameter("cartesian").get_parameter_value().bool_value + cartesian_max_step = ( + node.get_parameter("cartesian_max_step").get_parameter_value().double_value + ) + cartesian_fraction_threshold = ( + node.get_parameter("cartesian_fraction_threshold") + .get_parameter_value() + .double_value + ) + cartesian_jump_threshold = ( + node.get_parameter("cartesian_jump_threshold") + .get_parameter_value() + .double_value + ) + cartesian_avoid_collisions = ( + node.get_parameter("cartesian_avoid_collisions") + .get_parameter_value() + .bool_value + ) + + # Set parameters for cartesian planning + moveit2.cartesian_avoid_collisions = cartesian_avoid_collisions + moveit2.cartesian_jump_threshold = cartesian_jump_threshold # Move to pose node.get_logger().info( f"Moving to {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}" ) - moveit2.move_to_pose(position=position, quat_xyzw=quat_xyzw, cartesian=cartesian) + moveit2.move_to_pose( + position=position, + quat_xyzw=quat_xyzw, + cartesian=cartesian, + cartesian_max_step=cartesian_max_step, + cartesian_fraction_threshold=cartesian_fraction_threshold, + ) if synchronous: # Note: the same functionality can be achieved by setting # `synchronous:=false` and `cancel_after_secs` to a negative value. diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py index 565a745..fdf7b1d 100644 --- a/pymoveit2/moveit2.py +++ b/pymoveit2/moveit2.py @@ -344,6 +344,8 @@ def move_to_pose( weight_position: float = 1.0, cartesian: bool = False, weight_orientation: float = 1.0, + cartesian_max_step: float = 0.0025, + cartesian_fraction_threshold: float = 0.0, ): """ Plan and execute motion based on previously set goals. Optional arguments can be @@ -428,6 +430,8 @@ def move_to_pose( weight_position=weight_position, weight_orientation=weight_orientation, cartesian=cartesian, + max_step=cartesian_max_step, + cartesian_fraction_threshold=cartesian_fraction_threshold, ) ) @@ -500,12 +504,18 @@ def plan( weight_joint_position: float = 1.0, start_joint_state: Optional[Union[JointState, List[float]]] = None, cartesian: bool = False, + max_step: float = 0.0025, + cartesian_fraction_threshold: float = 0.0, ) -> Optional[JointTrajectory]: """ Call plan_async and wait on future """ future = self.plan_async( - **{key: value for key, value in locals().items() if key != "self"} + **{ + key: value + for key, value in locals().items() + if key not in ["self", "cartesian_fraction_threshold"] + } ) if future is None: @@ -516,7 +526,11 @@ def plan( while not future.done(): rate.sleep() - return self.get_trajectory(future, cartesian=cartesian) + return self.get_trajectory( + future, + cartesian=cartesian, + cartesian_fraction_threshold=cartesian_fraction_threshold, + ) def plan_async( self,