Added more granular control for cartesian planning

ada_feeding/ada_feeding/behaviors/move_to.py

@@ -105,6 +105,20 @@ def __init__(
         self.move_to_blackboard.register_key(
             key="max_acceleration_scaling_factor", access=py_trees.common.Access.READ
         )
+        # Add the ability to set the cartesian jump threshold
+        self.move_to_blackboard.register_key(
+            key="cartesian_jump_threshold", access=py_trees.common.Access.READ
+        )
+        # Add the ability to set the cartesian max step
+        self.move_to_blackboard.register_key(
+            key="cartesian_max_step", access=py_trees.common.Access.READ
+        )
+        # Add the ability to set a cartesian fraction threshold (e.g., only
+        # accept plans that completed at least this fraction of the path)
+        self.move_to_blackboard.register_key(
+            key="cartesian_fraction_threshold", 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
@@ -184,6 +198,26 @@ def initialise(self) -> None:
             self.move_to_blackboard, "allowed_planning_time", 0.5
         )
 
+        # Set the cartesian jump threshold
+        self.moveit2.cartesian_jump_threshold = get_from_blackboard_with_default(
+            self.move_to_blackboard, "cartesian_jump_threshold", 0.0
+        )
+
+        # Get whether we should use the cartesian interpolator
+        self.cartesian = get_from_blackboard_with_default(
+            self.move_to_blackboard, "cartesian", False
+        )
+
+        # Get the cartesian max step
+        self.cartesian_max_step = get_from_blackboard_with_default(
+            self.move_to_blackboard, "cartesian_max_step", 0.0025
+        )
+
+        # Get the cartesian fraction threshold
+        self.cartesian_fraction_threshold = get_from_blackboard_with_default(
+            self.move_to_blackboard, "cartesian_fraction_threshold", 0.0
+        )
+
         # If the plan is cartesian, it should always avoid collisions
         self.moveit2.cartesian_avoid_collisions = True
 
@@ -201,14 +235,8 @@ def initialise(self) -> None:
         self.tree_blackboard.motion_initial_distance = 0.0
         self.tree_blackboard.motion_curr_distance = 0.0
 
-        # Get all parameters for motion, resorting to default values if unset.
-        self.joint_names = kinova.joint_names()
-        self.cartesian = get_from_blackboard_with_default(
-            self.move_to_blackboard, "cartesian", False
-        )
-
         # Set the joint names
-        self.distance_to_goal.set_joint_names(self.joint_names)
+        self.distance_to_goal.set_joint_names(kinova.joint_names())
 
     # pylint: disable=too-many-branches, too-many-return-statements
     # This is the heart of the MoveTo behavior, so the number of branches
@@ -237,7 +265,9 @@ def update(self) -> py_trees.common.Status:
                     )
                     return py_trees.common.Status.FAILURE
                 # Initiate an asynchronous planning call
-                self.planning_future = self.moveit2.plan_async(cartesian=self.cartesian)
+                self.planning_future = self.moveit2.plan_async(
+                    cartesian=self.cartesian, max_step=self.cartesian_max_step
+                )
                 return py_trees.common.Status.RUNNING
 
             # Check if planning is complete
@@ -248,7 +278,9 @@ def update(self) -> py_trees.common.Status:
 
                 # Get the trajectory
                 traj = self.moveit2.get_trajectory(
-                    self.planning_future, cartesian=self.cartesian
+                    self.planning_future,
+                    cartesian=self.cartesian,
+                    cartesian_fraction_threshold=self.cartesian_fraction_threshold,
                 )
                 self.logger.info(f"Trajectory: {traj} | type {type(traj)}")
                 if traj is None:

ada_feeding/ada_feeding/trees/move_from_mouth_tree.py

@@ -57,6 +57,8 @@ def __init__(
         allowed_planning_time_to_end_configuration: float = 0.5,
         max_velocity_scaling_factor_to_staging_configuration: float = 0.1,
         max_velocity_scaling_factor_to_end_configuration: float = 0.1,
+        cartesian_jump_threshold_to_staging_configuration: float = 0.0,
+        cartesian_max_step_to_staging_configuration: float = 0.0025,
         wheelchair_collision_object_id: str = "wheelchair_collision",
         force_threshold: float = 4.0,
         torque_threshold: float = 4.0,
@@ -95,6 +97,12 @@ def __init__(
         max_velocity_scaling_factor_to_end_configuration: The maximum
             velocity scaling factor for the MoveIt2 motion planner to move to
             the end config.
+        cartesian_jump_threshold_to_staging_configuration: The cartesian
+            jump threshold for the MoveIt2 motion planner to move to the
+            staging config.
+        cartesian_max_step_to_staging_configuration: The cartesian
+            max step for the MoveIt2 motion planner to move to the
+            staging config.
         wheelchair_collision_object_id: The ID of the wheelchair collision object
             in the MoveIt2 planning scene.
         force_threshold: The force threshold (N) for the ForceGateController.
@@ -139,6 +147,12 @@ def __init__(
         self.max_velocity_scaling_factor_to_end_configuration = (
             max_velocity_scaling_factor_to_end_configuration
         )
+        self.cartesian_jump_threshold_to_staging_configuration = (
+            cartesian_jump_threshold_to_staging_configuration
+        )
+        self.cartesian_max_step_to_staging_configuration = (
+            cartesian_max_step_to_staging_configuration
+        )
         self.wheelchair_collision_object_id = wheelchair_collision_object_id
         self.force_threshold = force_threshold
         self.torque_threshold = torque_threshold
@@ -213,6 +227,9 @@ def create_move_to_tree(
                 tolerance_orientation_goal=(0.6, 0.5, 0.5),
                 parameterization_orientation_goal=1,  # Rotation vector
                 cartesian=True,
+                cartesian_jump_threshold=self.cartesian_jump_threshold_to_staging_configuration,
+                cartesian_max_step=self.cartesian_max_step_to_staging_configuration,
+                cartesian_fraction_threshold=0.85,
                 planner_id=self.planner_id,
                 allowed_planning_time=self.allowed_planning_time_to_staging_configuration,
                 max_velocity_scaling_factor=(

ada_feeding/ada_feeding/trees/move_to_mouth_tree.py

@@ -65,6 +65,8 @@ def __init__(
         allowed_planning_time_to_mouth: float = 0.5,
         max_velocity_scaling_factor_to_staging_configuration: float = 0.1,
         max_velocity_scaling_factor_to_mouth: float = 0.1,
+        cartesian_jump_threshold_to_mouth: float = 0.0,
+        cartesian_max_step_to_mouth: float = 0.0025,
         head_object_id: str = "head",
         wheelchair_collision_object_id: str = "wheelchair_collision",
         force_threshold: float = 4.0,
@@ -95,6 +97,11 @@ def __init__(
             the staging config.
         max_velocity_scaling_factor_to_mouth: The maximum velocity scaling
             factor for the MoveIt2 motion planner to move to the user's mouth.
+        cartesian_jump_threshold_to_mouth: The maximum allowed jump in the
+            cartesian space for the MoveIt2 motion planner to move to the user's
+            mouth.
+        cartesian_max_step_to_mouth: The maximum allowed step in the cartesian
+            space for the MoveIt2 motion planner to move to the user's mouth.
         head_object_id: The ID of the head collision object in the MoveIt2
             planning scene.
         wheelchair_collision_object_id: The ID of the wheelchair collision object
@@ -131,6 +138,8 @@ def __init__(
             max_velocity_scaling_factor_to_staging_configuration
         )
         self.max_velocity_scaling_factor_to_mouth = max_velocity_scaling_factor_to_mouth
+        self.cartesian_jump_threshold_to_mouth = cartesian_jump_threshold_to_mouth
+        self.cartesian_max_step_to_mouth = cartesian_max_step_to_mouth
         self.head_object_id = head_object_id
         self.wheelchair_collision_object_id = wheelchair_collision_object_id
         self.force_threshold = force_threshold
@@ -384,6 +393,9 @@ def create_move_to_tree(
                 tolerance_orientation_goal=(0.6, 0.5, 0.5),
                 parameterization_orientation_goal=1,  # Rotation vector
                 cartesian=True,
+                cartesian_jump_threshold=self.cartesian_jump_threshold_to_mouth,
+                cartesian_max_step=self.cartesian_max_step_to_mouth,
+                cartesian_fraction_threshold=0.85,
                 planner_id=self.planner_id,
                 allowed_planning_time=self.allowed_planning_time_to_mouth,
                 max_velocity_scaling_factor=self.max_velocity_scaling_factor_to_mouth,

ada_feeding/ada_feeding/trees/move_to_pose_tree.py

@@ -56,6 +56,9 @@ def __init__(
         weight_position: float = 1.0,
         weight_orientation: float = 1.0,
         cartesian: bool = False,
+        cartesian_jump_threshold: float = 0.0,
+        cartesian_max_step: float = 0.0025,
+        cartesian_fraction_threshold: float = 0.0,
         pipeline_id: str = "ompl",
         planner_id: str = "RRTstarkConfigDefault",
         allowed_planning_time: float = 0.5,
@@ -80,6 +83,10 @@ 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.
+        cartesian_jump_threshold: the jump threshold for cartesian path planning.
+        cartesian_max_step: the maximum step for cartesian path planning.
+        cartesian_fraction_threshold: Reject cartesian plans that don't reach
+            at least this fraction of the path to the goal.
         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.
@@ -106,6 +113,9 @@ def __init__(
         self.weight_position = weight_position
         self.weight_orientation = weight_orientation
         self.cartesian = cartesian
+        self.cartesian_jump_threshold = cartesian_jump_threshold
+        self.cartesian_max_step = cartesian_max_step
+        self.cartesian_fraction_threshold = cartesian_fraction_threshold
         self.pipeline_id = pipeline_id
         self.planner_id = planner_id
         self.allowed_planning_time = allowed_planning_time
@@ -231,6 +241,25 @@ def create_move_to_tree(
         self.blackboard.register_key(
             key=cartesian_key, access=py_trees.common.Access.WRITE
         )
+        cartesian_jump_threshold_key = Blackboard.separator.join(
+            [move_to_namespace_prefix, "cartesian_jump_threshold"]
+        )
+        self.blackboard.register_key(
+            key=cartesian_jump_threshold_key, access=py_trees.common.Access.WRITE
+        )
+        cartesian_max_step_key = Blackboard.separator.join(
+            [move_to_namespace_prefix, "cartesian_max_step"]
+        )
+        self.blackboard.register_key(
+            key=cartesian_max_step_key, access=py_trees.common.Access.WRITE
+        )
+        cartesian_fraction_threshold_key = Blackboard.separator.join(
+            [move_to_namespace_prefix, "cartesian_fraction_threshold"]
+        )
+        self.blackboard.register_key(
+            key=cartesian_fraction_threshold_key,
+            access=py_trees.common.Access.WRITE,
+        )
         pipeline_id_key = Blackboard.separator.join(
             [move_to_namespace_prefix, "pipeline_id"]
         )
@@ -338,6 +367,24 @@ def create_move_to_tree(
             self.cartesian,
             self.keys_to_not_write_to_blackboard,
         )
+        set_to_blackboard(
+            self.blackboard,
+            cartesian_jump_threshold_key,
+            self.cartesian_jump_threshold,
+            self.keys_to_not_write_to_blackboard,
+        )
+        set_to_blackboard(
+            self.blackboard,
+            cartesian_max_step_key,
+            self.cartesian_max_step,
+            self.keys_to_not_write_to_blackboard,
+        )
+        set_to_blackboard(
+            self.blackboard,
+            cartesian_fraction_threshold_key,
+            self.cartesian_fraction_threshold,
+            self.keys_to_not_write_to_blackboard,
+        )
         set_to_blackboard(
             self.blackboard,
             pipeline_id_key,

ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py

@@ -48,6 +48,9 @@ def __init__(
         weight_position_goal: float = 1.0,
         weight_orientation_goal: float = 1.0,
         cartesian: bool = False,
+        cartesian_jump_threshold: float = 0.0,
+        cartesian_max_step: float = 0.0025,
+        cartesian_fraction_threshold: float = 0.0,
         pipeline_id: str = "ompl",
         planner_id: str = "RRTstarkConfigDefault",
         allowed_planning_time: float = 0.5,
@@ -82,6 +85,10 @@ 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.
+        cartesian_jump_threshold: the jump threshold for cartesian path planning.
+        cartesian_max_step: the maximum step for cartesian path planning.
+        cartesian_fraction_threshold: if a cartesian plan does not reach at least
+            this fraction of the way to the goal, the plan is rejected.
         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
@@ -121,6 +128,9 @@ def __init__(
         self.weight_position_goal = weight_position_goal
         self.weight_orientation_goal = weight_orientation_goal
         self.cartesian = cartesian
+        self.cartesian_jump_threshold = cartesian_jump_threshold
+        self.cartesian_max_step = cartesian_max_step
+        self.cartesian_fraction_threshold = cartesian_fraction_threshold
         self.pipeline_id = pipeline_id
         self.planner_id = planner_id
         self.allowed_planning_time = allowed_planning_time
@@ -178,6 +188,9 @@ def create_move_to_tree(
                 weight_position=self.weight_position_goal,
                 weight_orientation=self.weight_orientation_goal,
                 cartesian=self.cartesian,
+                cartesian_jump_threshold=self.cartesian_jump_threshold,
+                cartesian_max_step=self.cartesian_max_step,
+                cartesian_fraction_threshold=self.cartesian_fraction_threshold,
                 pipeline_id=self.pipeline_id,
                 planner_id=self.planner_id,
                 allowed_planning_time=self.allowed_planning_time,

ada_feeding/config/ada_feeding_action_servers.yaml

@@ -84,6 +84,8 @@ ada_feeding_action_servers:
         - allowed_planning_time_to_staging_configuration
         - max_velocity_scaling_factor_to_staging_configuration
         - max_velocity_scaling_factor_to_mouth
+        - cartesian_jump_threshold_to_mouth
+        - cartesian_max_step_to_mouth
       tree_kwargs: # optional
         staging_configuration:
           # # Side-staging configuration
@@ -116,9 +118,11 @@ ada_feeding_action_servers:
           - 0.6 # x, rad
           - 3.14159 # y, rad
           - 0.5 # z, rad
-        allowed_planning_time_to_staging_configuration: 1.5 # secs
+        allowed_planning_time_to_staging_configuration: 2.0 # secs
         max_velocity_scaling_factor_to_staging_configuration: 0.65 # optional in (0.0, 1.0], default: 0.1
         max_velocity_scaling_factor_to_mouth: 0.4 # optional in (0.0, 1.0], default: 0.1
+        cartesian_jump_threshold_to_mouth: 4.0 # If a jump for a joint across consecutive IK solutions is more than this factor greater than the average, it is rejected
+        cartesian_max_step_to_mouth: 0.01 # m
       tick_rate: 10 # Hz, optional, default: 30
 
     # Parameters for the MoveFromMouthToAbovePlate action.
@@ -135,6 +139,8 @@ ada_feeding_action_servers:
         - orientation_constraint_to_staging_configuration_tolerances
         - max_velocity_scaling_factor_to_staging_configuration
         - max_velocity_scaling_factor_to_end_configuration
+        - cartesian_jump_threshold_to_staging_configuration
+        - cartesian_max_step_to_staging_configuration
       tree_kwargs: # optional
         staging_configuration_position:
           -  0.27971 # x
@@ -163,6 +169,8 @@ ada_feeding_action_servers:
           - 0.5 # z, rad
         max_velocity_scaling_factor_to_staging_configuration: 0.4 # optional in (0.0, 1.0], default: 0.1
         max_velocity_scaling_factor_to_end_configuration: 0.65 # optional in (0.0, 1.0], default: 0.1
+        cartesian_jump_threshold_to_staging_configuration: 4.0 # If a jump for a joint across consecutive IK solutions is more than this factor greater than the average, it is rejected
+        cartesian_max_step_to_staging_configuration: 0.01 # m
       tick_rate: 10 # Hz, optional, default: 30
 
     # Parameters for the MoveFromMouthToRestingPosition action.
@@ -182,6 +190,8 @@ ada_feeding_action_servers:
         - allowed_planning_time_to_end_configuration
         - max_velocity_scaling_factor_to_staging_configuration
         - max_velocity_scaling_factor_to_end_configuration
+        - cartesian_jump_threshold_to_staging_configuration
+        - cartesian_max_step_to_staging_configuration
       tree_kwargs: # optional
         staging_configuration_position:
           -  0.27971 # x
@@ -219,7 +229,9 @@ ada_feeding_action_servers:
           - 0.5 # z, rad
         max_velocity_scaling_factor_to_staging_configuration: 0.4 # optional in (0.0, 1.0], default: 0.1
         max_velocity_scaling_factor_to_end_configuration: 0.65 # optional in (0.0, 1.0], default: 0.1
-        allowed_planning_time_to_end_configuration: 1.5 # secs
+        cartesian_jump_threshold_to_staging_configuration: 4.0 # If a jump for a joint across consecutive IK solutions is more than this factor greater than the average, it is rejected
+        cartesian_max_step_to_staging_configuration: 0.01 # m
+        allowed_planning_time_to_end_configuration: 2.5 # secs
       tick_rate: 10 # Hz, optional, default: 30
 
     # Parameters for the MoveToStowLocation action