diff --git a/ada_feeding/ada_feeding/behaviors/move_to.py b/ada_feeding/ada_feeding/behaviors/move_to.py index 93e1b972..bf2289bf 100644 --- a/ada_feeding/ada_feeding/behaviors/move_to.py +++ b/ada_feeding/ada_feeding/behaviors/move_to.py @@ -101,6 +101,10 @@ def __init__( self.move_to_blackboard.register_key( key="max_velocity_scaling_factor", access=py_trees.common.Access.READ ) + # Add the ability to set acceleration scaling + self.move_to_blackboard.register_key( + key="max_acceleration_scaling_factor", access=py_trees.common.Access.READ + ) # Initialize the blackboard to read from the parent behavior tree self.tree_blackboard = self.attach_blackboard_client( name=name + " MoveTo", namespace=tree_name @@ -170,6 +174,11 @@ def initialise(self) -> None: self.move_to_blackboard, "max_velocity_scaling_factor", 0.1 ) + # Set the max acceleration + self.moveit2.max_acceleration = get_from_blackboard_with_default( + self.move_to_blackboard, "max_acceleration_scaling_factor", 0.1 + ) + # Set the allowed planning time self.moveit2.allowed_planning_time = get_from_blackboard_with_default( self.move_to_blackboard, "allowed_planning_time", 0.5 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 2fc20381..0a534236 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py @@ -45,6 +45,7 @@ def __init__( planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, + max_acceleration_scaling_factor: float = 0.1, keys_to_not_write_to_blackboard: Set[str] = set(), ): """ @@ -61,6 +62,8 @@ def __init__( planner. max_velocity_scaling_factor: The maximum velocity scaling factor for the MoveIt2 motion planner. + max_acceleration_scaling_factor: The maximum acceleration scaling factor + for the MoveIt2 motion planner. keys_to_not_write_to_blackboard: A set of keys that should not be written Note that the keys need to be exact e.g., "move_to.cartesian," "position_goal_constraint.tolerance," "orientation_goal_constraint.tolerance," @@ -78,6 +81,7 @@ def __init__( self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor + self.max_acceleration_scaling_factor = max_acceleration_scaling_factor self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard # pylint: disable=too-many-locals @@ -156,6 +160,12 @@ def create_move_to_tree( self.blackboard.register_key( key=max_velocity_scaling_factor_key, access=py_trees.common.Access.WRITE ) + max_acceleration_scaling_factor_key = Blackboard.separator.join( + [move_to_namespace_prefix, "max_acceleration_scaling_factor"] + ) + self.blackboard.register_key( + key=max_acceleration_scaling_factor_key, access=py_trees.common.Access.WRITE, + ) # Write the inputs to MoveToConfiguration to blackboard set_to_blackboard( @@ -200,6 +210,12 @@ def create_move_to_tree( self.max_velocity_scaling_factor, self.keys_to_not_write_to_blackboard, ) + set_to_blackboard( + self.blackboard, + max_acceleration_scaling_factor_key, + self.max_acceleration_scaling_factor, + self.keys_to_not_write_to_blackboard, + ) # Create the MoveTo behavior move_to_name = Blackboard.separator.join([name, move_to_namespace_prefix]) 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 75eb70a1..9de01ad7 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 @@ -41,6 +41,7 @@ def __init__( planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, + max_acceleration_scaling_factor: float = 0.1, # Optional parameters for the FT thresholds re_tare: bool = True, toggle_watchdog_listener: bool = True, @@ -68,6 +69,8 @@ def __init__( planner. max_velocity_scaling_factor: The maximum velocity scaling factor for the MoveIt2 motion planner. + max_acceleration_scaling_factor: The maximum acceleration scaling factor for the + MoveIt2 motion planner. re_tare: Whether to re-tare the force-torque sensor. toggle_watchdog_listener: Whether to toggle the watchdog listener on and off. In practice, if the watchdog listener is on, you should toggle it. @@ -100,6 +103,7 @@ def __init__( self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor + self.max_acceleration_scaling_factor = max_acceleration_scaling_factor # Store the parameters for the FT threshold self.re_tare = re_tare @@ -149,6 +153,7 @@ def create_move_to_tree( planner_id=self.planner_id, allowed_planning_time=self.allowed_planning_time, max_velocity_scaling_factor=self.max_velocity_scaling_factor, + max_acceleration_scaling_factor=self.max_acceleration_scaling_factor, keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard, ) .create_tree(name, self.action_type, tree_root_name, logger, node) 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 fa324b20..bfefe613 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 @@ -45,6 +45,7 @@ def __init__( planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, + max_acceleration_scaling_factor: float = 0.1, # Optional parameters for the pose path constraint position_path: Tuple[float, float, float] = None, quat_xyzw_path: Tuple[float, float, float, float] = None, @@ -71,6 +72,8 @@ def __init__( planner. max_velocity_scaling_factor: The maximum velocity scaling factor for the MoveIt2 motion planner. + max_acceleration_scaling_factor: The maximum acceleration scaling factor + for the MoveIt2 motion planner. position_path: the position for the path constraint. quat_xyzw_path: the orientation for the path constraint. frame_id: the frame id of the target pose, for the pose path constraint. @@ -103,6 +106,7 @@ def __init__( self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor + self.max_acceleration_scaling_factor = max_acceleration_scaling_factor # Store the parameters for the pose path constraint self.position_path = position_path @@ -152,6 +156,7 @@ def create_move_to_tree( planner_id=self.planner_id, allowed_planning_time=self.allowed_planning_time, max_velocity_scaling_factor=self.max_velocity_scaling_factor, + max_acceleration_scaling_factor=self.max_acceleration_scaling_factor, keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard, ) .create_tree(name, self.action_type, tree_root_name, logger, node) 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 1fbfb28f..d52986ed 100644 --- a/ada_feeding/ada_feeding/trees/move_to_pose_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_pose_tree.py @@ -60,6 +60,7 @@ def __init__( planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, + max_acceleration_scaling_factor: float = 0.1, keys_to_not_write_to_blackboard: Set[str] = set(), ): """ @@ -84,6 +85,8 @@ def __init__( allowed_planning_time: the allowed planning time for path planning. max_velocity_scaling_factor: the maximum velocity scaling factor for path planning. + max_acceleration_scaling_factor: the maximum acceleration scaling factor for + path planning. keys_to_not_write_to_blackboard: the keys to not write to the blackboard. Note that the keys need to be exact e.g., "move_to.cartesian," "position_goal_constraint.tolerance," "orientation_goal_constraint.tolerance," @@ -107,6 +110,7 @@ def __init__( self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor + self.max_acceleration_scaling_factor = max_acceleration_scaling_factor self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard # pylint: disable=too-many-locals, too-many-statements @@ -251,6 +255,12 @@ def create_move_to_tree( self.blackboard.register_key( key=max_velocity_scaling_factor_key, access=py_trees.common.Access.WRITE ) + max_acceleration_scaling_factor_key = Blackboard.separator.join( + [move_to_namespace_prefix, "max_acceleration_scaling_factor"] + ) + self.blackboard.register_key( + key=max_acceleration_scaling_factor_key, access=py_trees.common.Access.WRITE, + ) # Write the inputs to MoveToPose to blackboard if self.position is not None: @@ -351,6 +361,12 @@ def create_move_to_tree( self.max_velocity_scaling_factor, self.keys_to_not_write_to_blackboard, ) + set_to_blackboard( + self.blackboard, + max_acceleration_scaling_factor_key, + self.max_acceleration_scaling_factor, + self.keys_to_not_write_to_blackboard, + ) # Create the MoveTo behavior move_to_name = Blackboard.separator.join([name, move_to_namespace_prefix]) 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 a533cf76..4b59b32a 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 @@ -52,6 +52,7 @@ def __init__( planner_id: str = "RRTstarkConfigDefault", allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, + max_acceleration_scaling_factor: float = 0.1, # Optional parameters for the pose path constraint position_path: Tuple[float, float, float] = None, quat_xyzw_path: Tuple[float, float, float, float] = None, @@ -87,6 +88,8 @@ def __init__( planner. max_velocity_scaling_factor: the maximum velocity scaling factor for MoveIt2 motion planning. + max_acceleration_scaling_factor: the maximum acceleration scaling factor + for MoveIt2 motion planning. position_path: the target position relative to frame_id for path constraints. quat_xyzw_path: the target orientation relative to frame_id for path constraints. frame_id_path: the frame id of the target pose for path constraints. If None, @@ -122,6 +125,7 @@ def __init__( self.planner_id = planner_id self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor + self.max_acceleration_scaling_factor = max_acceleration_scaling_factor # Store the parameters for the pose path constraint self.position_path = position_path @@ -178,6 +182,7 @@ def create_move_to_tree( planner_id=self.planner_id, allowed_planning_time=self.allowed_planning_time, max_velocity_scaling_factor=self.max_velocity_scaling_factor, + max_acceleration_scaling_factor=self.max_acceleration_scaling_factor, keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard, ) .create_tree(name, self.action_type, tree_root_name, logger, node)