diff --git a/ada_feeding/ada_feeding/behaviors/move_to.py b/ada_feeding/ada_feeding/behaviors/move_to.py index d4eb3fbf..d9dbfe3a 100644 --- a/ada_feeding/ada_feeding/behaviors/move_to.py +++ b/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: diff --git a/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py index 3a6c1f45..122ed382 100644 --- a/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py +++ b/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=( diff --git a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py index b7095f29..b2070dbb 100644 --- a/ada_feeding/ada_feeding/trees/move_to_mouth_tree.py +++ b/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 @@ -383,6 +392,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, 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 98cd3134..de3b791c 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,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, 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 4b59b32a..d1be88b6 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,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, diff --git a/ada_feeding/config/ada_feeding_action_servers.yaml b/ada_feeding/config/ada_feeding_action_servers.yaml index 87f5e759..6ed3064b 100644 --- a/ada_feeding/config/ada_feeding_action_servers.yaml +++ b/ada_feeding/config/ada_feeding_action_servers.yaml @@ -80,6 +80,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 @@ -112,9 +114,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. @@ -131,6 +135,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 @@ -159,6 +165,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. @@ -178,6 +186,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 @@ -215,7 +225,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