Added ability to set acceleration scaling

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

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])

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)

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)

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])

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)