Skip to content

Commit

Permalink
remove RetryUntilSuccessful decorators around planning Behaviors
Browse files Browse the repository at this point in the history
  • Loading branch information
@marioprats
marioprats committed Jan 17, 2025
1 parent 13d0f1a commit 8713357
Show file tree
Hide file tree
Showing 4 changed files with 221 additions and 220 deletions.
163 changes: 90 additions & 73 deletions src/lab_sim/objectives/constrained_pick_and_place_subtree.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,91 +11,108 @@
>
<Control ID="Sequence">
<!--Move to pick location-->
<!--Note: Sampling based planners can be non-deterministic. The retry decorator improves the likelihood of success-->
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
<SubTree
ID="Move to Waypoint"
waypoint_name="{pre_pick}"
joint_group_name="manipulator"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
planner_interface="moveit_default"
keep_orientation="true"
keep_orientation_tolerance="0.2"
/>
</Decorator>
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
<SubTree
ID="Move to Waypoint"
waypoint_name="{pick}"
joint_group_name="manipulator"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
planner_interface="moveit_default"
keep_orientation="true"
keep_orientation_tolerance="0.2"
/>
</Decorator>
<SubTree
ID="Move to Waypoint"
waypoint_name="{pre_pick}"
joint_group_name="manipulator"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
planner_interface="moveit_default"
keep_orientation="true"
keep_orientation_tolerance="0.2"
acceleration_scale_factor="1.0"
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
link_padding="0.01"
velocity_scale_factor="1.0"
/>
<SubTree
ID="Move to Waypoint"
waypoint_name="{pick}"
joint_group_name="manipulator"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
planner_interface="moveit_default"
keep_orientation="true"
keep_orientation_tolerance="0.2"
acceleration_scale_factor="1.0"
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
link_padding="0.01"
velocity_scale_factor="1.0"
/>
<!--We force success as the gripper closes, since we are commanding a position it will never reach (fingers fully closed)-->
<Decorator ID="ForceSuccess">
<SubTree ID="Close Gripper" />
</Decorator>
<!--Move to place (drop) location-->
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
<SubTree
ID="Move to Waypoint"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
joint_group_name="manipulator"
planner_interface="moveit_default"
keep_orientation="true"
keep_orientation_tolerance="0.2"
_collapsed="true"
waypoint_name="{pre_pick}"
/>
</Decorator>
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
<SubTree
ID="Move to Waypoint"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
joint_group_name="manipulator"
planner_interface="moveit_default"
keep_orientation="true"
keep_orientation_tolerance="0.2"
_collapsed="true"
waypoint_name="{pre_place}"
/>
</Decorator>
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
<SubTree
ID="Move to Waypoint"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
joint_group_name="manipulator"
planner_interface="moveit_default"
keep_orientation="true"
keep_orientation_tolerance="0.2"
_collapsed="true"
waypoint_name="{place}"
/>
</Decorator>
<SubTree
ID="Move to Waypoint"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
joint_group_name="manipulator"
planner_interface="moveit_default"
keep_orientation="true"
keep_orientation_tolerance="0.2"
_collapsed="true"
waypoint_name="{pre_pick}"
acceleration_scale_factor="1.0"
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
link_padding="0.01"
velocity_scale_factor="1.0"
/>
<SubTree
ID="Move to Waypoint"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
joint_group_name="manipulator"
planner_interface="moveit_default"
keep_orientation="true"
keep_orientation_tolerance="0.2"
_collapsed="true"
waypoint_name="{pre_place}"
acceleration_scale_factor="1.0"
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
link_padding="0.01"
velocity_scale_factor="1.0"
/>
<SubTree
ID="Move to Waypoint"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
joint_group_name="manipulator"
planner_interface="moveit_default"
keep_orientation="true"
keep_orientation_tolerance="0.2"
_collapsed="true"
waypoint_name="{place}"
acceleration_scale_factor="1.0"
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
link_padding="0.01"
velocity_scale_factor="1.0"
/>
<SubTree ID="Open Gripper" />
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
<SubTree
ID="Move to Waypoint"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
joint_group_name="manipulator"
planner_interface="moveit_default"
keep_orientation="true"
keep_orientation_tolerance="0.2"
_collapsed="true"
waypoint_name="{pre_place}"
/>
</Decorator>
<SubTree
ID="Move to Waypoint"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
joint_group_name="manipulator"
planner_interface="moveit_default"
keep_orientation="true"
keep_orientation_tolerance="0.2"
_collapsed="true"
waypoint_name="{pre_place}"
acceleration_scale_factor="1.0"
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
link_padding="0.01"
velocity_scale_factor="1.0"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Constrained Pick and Place Subtree">
<inout_port name="pre_pick" default="" />
<inout_port name="pick" default="" />
<inout_port name="pre_place" default="" />
<inout_port name="place" default="" />
<inout_port name="pre_pick" default="" />
<inout_port name="pre_place" default="" />
<MetadataFields>
<Metadata runnable="false" />
</MetadataFields>
<MetadataFields>
<Metadata runnable="false" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
</root>
49 changes: 29 additions & 20 deletions ...moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/constrained_pick_place.xml
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8" ?>
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Constrained Pick and Place">
<!-- ////////// -->
<!--//////////-->
<BehaviorTree
ID="Constrained Pick and Place"
_description="Pick and place an object with constraints on the gripper's range of motion"
Expand All @@ -10,27 +10,36 @@
<Control ID="Sequence">
<Control ID="Sequence" name="TopLevelSequence">
<SubTree ID="Open Gripper" />
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
<SubTree
ID="Move to Waypoint"
waypoint_name="Grasp Right"
joint_group_name="manipulator"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
/>
</Decorator>
<SubTree
ID="Move to Waypoint"
waypoint_name="Pick"
joint_group_name="manipulator"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
acceleration_scale_factor="1.0"
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
keep_orientation="false"
keep_orientation_tolerance="0.05"
link_padding="0.01"
velocity_scale_factor="1.0"
/>
<SubTree ID="Close Gripper" />
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
<SubTree
ID="Move to Waypoint"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
joint_group_name="manipulator"
waypoint_name="Grasp Left"
keep_orientation="true"
keep_orientation_tolerance="0.2"
/>
</Decorator>
<SubTree
ID="Move to Waypoint"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
joint_group_name="manipulator"
waypoint_name="Place"
keep_orientation="true"
keep_orientation_tolerance="0.2"
acceleration_scale_factor="1.0"
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
link_padding="0.01"
velocity_scale_factor="1.0"
/>
</Control>
</Control>
</Decorator>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Constrained Pick and Place" />
</TreeNodesModel>
</root>
Loading

0 comments on commit 8713357

Please sign in to comment.