diff --git a/ada_feeding/ada_feeding/behaviors/move_to.py b/ada_feeding/ada_feeding/behaviors/move_to.py index ce603b80..93e1b972 100644 --- a/ada_feeding/ada_feeding/behaviors/move_to.py +++ b/ada_feeding/ada_feeding/behaviors/move_to.py @@ -85,6 +85,10 @@ def __init__( self.move_to_blackboard.register_key( key="cartesian", access=py_trees.common.Access.READ ) + # Add the ability to set a pipeline_id + self.move_to_blackboard.register_key( + key="pipeline_id", access=py_trees.common.Access.READ + ) # Add the ability to set a planner_id self.move_to_blackboard.register_key( key="planner_id", access=py_trees.common.Access.READ @@ -151,6 +155,11 @@ def initialise(self) -> None: """ self.logger.info(f"{self.name} [MoveTo::initialise()]") + # Set the pipeline_id + self.moveit2.pipeline_id = get_from_blackboard_with_default( + self.move_to_blackboard, "pipeline_id", "ompl" + ) + # Set the planner_id self.moveit2.planner_id = get_from_blackboard_with_default( self.move_to_blackboard, "planner_id", "RRTstarkConfigDefault" diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py index b46c81c6..2fc20381 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py @@ -41,6 +41,7 @@ def __init__( joint_positions: List[float], tolerance: float = 0.001, weight: float = 1.0, + pipeline_id: str = "ompl", planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, @@ -54,6 +55,7 @@ def __init__( joint_positions: The joint positions to move the robot arm to. tolerance: The tolerance for the joint positions. weight: The weight for the joint goal constraint. + pipeline_id: The pipeline ID to use for the MoveIt2 motion planner. planner_id: The planner ID to use for the MoveIt2 motion planning. allowed_planning_time: The allowed planning time for the MoveIt2 motion planner. @@ -72,6 +74,7 @@ def __init__( assert len(self.joint_positions) == 6, "Must provide 6 joint positions" self.tolerance = tolerance self.weight = weight + self.pipeline_id = pipeline_id self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor @@ -129,6 +132,12 @@ def create_move_to_tree( ) # Inputs for MoveTo + pipeline_id_key = Blackboard.separator.join( + [move_to_namespace_prefix, "pipeline_id"] + ) + self.blackboard.register_key( + key=pipeline_id_key, access=py_trees.common.Access.WRITE + ) planner_id_key = Blackboard.separator.join( [move_to_namespace_prefix, "planner_id"] ) @@ -167,6 +176,12 @@ def create_move_to_tree( self.weight, self.keys_to_not_write_to_blackboard, ) + set_to_blackboard( + self.blackboard, + pipeline_id_key, + self.pipeline_id, + self.keys_to_not_write_to_blackboard, + ) set_to_blackboard( self.blackboard, planner_id_key, diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py index 1abf9b95..75eb70a1 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py @@ -37,6 +37,7 @@ def __init__( # Optional parameters for moving to a configuration tolerance_joint: float = 0.001, weight_joint: float = 1.0, + pipeline_id: str = "ompl", planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, @@ -61,6 +62,7 @@ def __init__( joint_positions: The joint positions for the goal constraint. tolerance_joint: The tolerance for the joint goal constraint. weight_joint: The weight for the joint goal constraint. + pipeline_id: The pipeline ID to use for MoveIt2 motion planning. planner_id: The planner ID to use for MoveIt2 motion planning. allowed_planning_time: The allowed planning time for the MoveIt2 motion planner. @@ -94,6 +96,7 @@ def __init__( assert len(self.joint_positions) == 6, "Must provide 6 joint positions" self.tolerance_joint = tolerance_joint self.weight_joint = weight_joint + self.pipeline_id = pipeline_id self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor @@ -142,6 +145,7 @@ def create_move_to_tree( joint_positions=self.joint_positions, tolerance=self.tolerance_joint, weight=self.weight_joint, + pipeline_id=self.pipeline_id, planner_id=self.planner_id, allowed_planning_time=self.allowed_planning_time, max_velocity_scaling_factor=self.max_velocity_scaling_factor, diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py index 6e8145a4..fa324b20 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py @@ -41,6 +41,7 @@ def __init__( # Optional parameters for moving to a configuration tolerance_joint_goal: float = 0.001, weight_joint_goal: float = 1.0, + pipeline_id: str = "ompl", planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, @@ -64,6 +65,7 @@ def __init__( joint_positions_goal: The joint positions for the goal constraint. tolerance_joint_goal: The tolerance for the joint goal constraint. weight_joint_goal: The weight for the joint goal constraint. + pipeline_id: The pipeline ID to use for MoveIt2 motion planning. planner_id: The planner ID to use for MoveIt2 motion planning. allowed_planning_time: The allowed planning time for the MoveIt2 motion planner. @@ -97,6 +99,7 @@ def __init__( assert len(self.joint_positions_goal) == 6, "Must provide 6 joint positions" self.tolerance_joint_goal = tolerance_joint_goal self.weight_joint_goal = weight_joint_goal + self.pipeline_id = pipeline_id self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor @@ -145,6 +148,7 @@ def create_move_to_tree( joint_positions=self.joint_positions_goal, tolerance=self.tolerance_joint_goal, weight=self.weight_joint_goal, + pipeline_id=self.pipeline_id, planner_id=self.planner_id, allowed_planning_time=self.allowed_planning_time, max_velocity_scaling_factor=self.max_velocity_scaling_factor, diff --git a/ada_feeding/ada_feeding/trees/move_to_pose_tree.py b/ada_feeding/ada_feeding/trees/move_to_pose_tree.py index 1de96925..1fbfb28f 100644 --- a/ada_feeding/ada_feeding/trees/move_to_pose_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_pose_tree.py @@ -56,6 +56,7 @@ def __init__( weight_position: float = 1.0, weight_orientation: float = 1.0, cartesian: bool = False, + pipeline_id: str = "ompl", planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, @@ -78,6 +79,7 @@ def __init__( weight_position: the weight for the end effector position. weight_orientation: the weight for the end effector orientation. cartesian: whether to use cartesian path planning. + pipeline_id: the pipeline to use for path planning. planner_id: the planner to use for path planning. allowed_planning_time: the allowed planning time for path planning. max_velocity_scaling_factor: the maximum velocity scaling factor for path @@ -101,6 +103,7 @@ def __init__( self.weight_position = weight_position self.weight_orientation = weight_orientation self.cartesian = cartesian + self.pipeline_id = pipeline_id self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor @@ -224,6 +227,12 @@ def create_move_to_tree( self.blackboard.register_key( key=cartesian_key, access=py_trees.common.Access.WRITE ) + pipeline_id_key = Blackboard.separator.join( + [move_to_namespace_prefix, "pipeline_id"] + ) + self.blackboard.register_key( + key=pipeline_id_key, access=py_trees.common.Access.WRITE + ) planner_id_key = Blackboard.separator.join( [move_to_namespace_prefix, "planner_id"] ) @@ -318,6 +327,12 @@ def create_move_to_tree( self.cartesian, self.keys_to_not_write_to_blackboard, ) + set_to_blackboard( + self.blackboard, + pipeline_id_key, + self.pipeline_id, + self.keys_to_not_write_to_blackboard, + ) set_to_blackboard( self.blackboard, planner_id_key, diff --git a/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py b/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py index 5a0d9300..a533cf76 100644 --- a/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py @@ -48,6 +48,7 @@ def __init__( weight_position_goal: float = 1.0, weight_orientation_goal: float = 1.0, cartesian: bool = False, + pipeline_id: str = "ompl", planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, @@ -80,6 +81,7 @@ def __init__( weight_position_goal: the weight for the position goal. weight_orientation_goal: the weight for the orientation goal. cartesian: whether to use cartesian path planning. + pipeline_id: the pipeline ID to use for MoveIt2 motion planning. planner_id: the planner ID to use for MoveIt2 motion planning. allowed_planning_time: the allowed planning time for the MoveIt2 motion planner. @@ -116,6 +118,7 @@ def __init__( self.weight_position_goal = weight_position_goal self.weight_orientation_goal = weight_orientation_goal self.cartesian = cartesian + self.pipeline_id = pipeline_id self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor @@ -171,6 +174,7 @@ def create_move_to_tree( weight_position=self.weight_position_goal, weight_orientation=self.weight_orientation_goal, cartesian=self.cartesian, + pipeline_id=self.pipeline_id, planner_id=self.planner_id, allowed_planning_time=self.allowed_planning_time, max_velocity_scaling_factor=self.max_velocity_scaling_factor,