Skip to content
This repository has been archived by the owner on Dec 13, 2024. It is now read-only.

Use updated YAML-free version of SetupMTCUpdateGroupCollisionRule #256

Merged
merged 2 commits into from
Apr 22, 2024
Merged

Use updated YAML-free version of SetupMTCUpdateGroupCollisionRule #256

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
03b42d0
use updated YAML-free version of SetupMTCUpdateGroupCollisionRule
schornakj Apr 15, 2024
ee033d0
Merge branch 'main' into pr-parameterless-update-group-collision-rule
schornakj Apr 22, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 5 additions & 6 deletions src/picknik_ur_site_config/objectives/close_cabinet_door.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
<BehaviorTree ID="Close Cabinet Door" _description="Close a cabinet door by pushing along an axis with the end effector">
<Control ID="Sequence">
<SubTree ID="Re-Zero Force-Torque Sensors" />
<Action ID="LoadObjectiveParameters" config_file_name="push_along_axis_config.yaml" parameters="{parameters}" />
<SubTree ID="CloseGripper" />
<Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}" />
<Control ID="Sequence">
Expand All @@ -18,9 +17,9 @@
<Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
</Control>
</Decorator>
<Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" group_name="gripper" object_name="<octomap>" allow_collision="true" task="{close_cabinet_door_task}" />
<Action ID="SetupMTCMoveAlongFrameAxis" task="{close_cabinet_door_task}" hand_frame="grasp_link" axis_frame="grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" group_name="gripper" object_name="<octomap>" allow_collision="false" task="{close_cabinet_door_task}" />
<Action ID="PlanMTCTask" solution="{full_close_cabinet_door_solution}" task="{close_cabinet_door_task}"/>
<Action ID="SplitMTCSolution" solution_in="{full_close_cabinet_door_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
<Action ID="WaitForUserTrajectoryApproval" solution="{full_close_cabinet_door_solution}"/>
Expand All @@ -31,7 +30,7 @@
<Action ID="ExecuteMTCTask" solution="{push_solution}"/>
</Control>
</Control>
<SubTree ID="RetreatToInitialPose" pre_approach_robot_state="{pre_approach_robot_state}" initial_robot_state="{initial_robot_state}" parameters="{parameters}" />
<SubTree ID="RetreatToInitialPose" pre_approach_robot_state="{pre_approach_robot_state}" initial_robot_state="{initial_robot_state}" />
</Control>
</BehaviorTree>
<BehaviorTree ID="CloseGripper">
Expand All @@ -43,9 +42,9 @@
<Control ID="Sequence">
<Action ID="InitializeMTCTask" task_id="close_cabinet_door_retreat" controller_names="/joint_trajectory_controller" task="{retreat_task}" />
<Action ID="SetupMTCCurrentState" task="{retreat_task}" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{retreat_task}" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" group_name="gripper" object_name="<octomap>" allow_collision="true" task="{retreat_task}" />
<Action ID="SetupMTCCartesianMoveToJointState" joint_state="{pre_approach_robot_state}" planning_group_name="manipulator" task="{retreat_task}" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{retreat_task}" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" group_name="gripper" object_name="<octomap>" allow_collision="false" task="{retreat_task}" />
<Action ID="SetupMTCMoveToJointState" joint_state="{initial_robot_state}" planning_group_name="manipulator" task="{retreat_task}" />
<Action ID="PlanMTCTask" solution="{return_to_initial_waypoint_solution}" task="{retreat_task}" />
<Action ID="ExecuteMTCTask" solution="{return_to_initial_waypoint_solution}" />
Expand Down
9 changes: 0 additions & 9 deletions src/picknik_ur_site_config/objectives/push_along_axis_config.yaml
View file
Open in desktop

This file was deleted.

11 changes: 5 additions & 6 deletions src/picknik_ur_site_config/objectives/push_button.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
<BehaviorTree ID="Push Button" _description="Push a button with the end effector">
<Control ID="Sequence">
<SubTree ID="Re-Zero Force-Torque Sensors" />
<Action ID="LoadObjectiveParameters" config_file_name="push_along_axis_config.yaml" parameters="{parameters}" />
<SubTree ID="CloseGripper" />
<Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}" />
<Control ID="Sequence">
Expand All @@ -18,9 +17,9 @@
<Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{push_button_task}" use_all_planners="false"/>
</Control>
</Decorator>
<Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" group_name="gripper" object_name="<octomap>" allow_collision="true" task="{push_button_task}" />
<Action ID="SetupMTCMoveAlongFrameAxis" task="{push_button_task}" hand_frame="grasp_link" axis_frame="grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{push_button_task}" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" group_name="gripper" object_name="<octomap>" allow_collision="false" task="{push_button_task}" />
<Action ID="PlanMTCTask" solution="{full_push_button_solution}" task="{push_button_task}"/>
<Action ID="SplitMTCSolution" solution_in="{full_push_button_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
<Action ID="WaitForUserTrajectoryApproval" solution="{full_push_button_solution}"/>
Expand All @@ -31,7 +30,7 @@
<Action ID="ExecuteMTCTask" solution="{push_solution}"/>
</Control>
</Control>
<SubTree ID="RetreatToInitialPose" pre_approach_robot_state="{pre_approach_robot_state}" initial_robot_state="{initial_robot_state}" parameters="{parameters}" />
<SubTree ID="RetreatToInitialPose" pre_approach_robot_state="{pre_approach_robot_state}" initial_robot_state="{initial_robot_state}" />
</Control>
</BehaviorTree>
<BehaviorTree ID="CloseGripper">
Expand All @@ -43,9 +42,9 @@
<Control ID="Sequence">
<Action ID="InitializeMTCTask" task_id="push_button_retreat" controller_names="/joint_trajectory_controller" task="{retreat_task}" />
<Action ID="SetupMTCCurrentState" task="{retreat_task}" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{retreat_task}" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" group_name="gripper" object_name="<octomap>" allow_collision="true" task="{retreat_task}" />
<Action ID="SetupMTCCartesianMoveToJointState" joint_state="{pre_approach_robot_state}" planning_group_name="manipulator" task="{retreat_task}" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{retreat_task}" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" group_name="gripper" object_name="<octomap>" allow_collision="false" task="{retreat_task}" />
<Action ID="SetupMTCMoveToJointState" joint_state="{initial_robot_state}" planning_group_name="manipulator" task="{retreat_task}" />
<Action ID="PlanMTCTask" solution="{return_to_initial_waypoint_solution}" task="{retreat_task}" />
<Action ID="ExecuteMTCTask" solution="{return_to_initial_waypoint_solution}" />
Expand Down
11 changes: 5 additions & 6 deletions src/picknik_ur_site_config/objectives/push_button_ml.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@
<root BTCPP_format="4" main_tree_to_execute="Push Button ML">
<BehaviorTree ID="Push Button ML" _description="Move to a waypoint, and then push a button detected by a image segmentation network with the end effector" _favorite="false">
<Control ID="Sequence">
<Action ID="LoadObjectiveParameters" config_file_name="push_along_axis_config.yaml" parameters="{parameters}"/>
<SubTree ID="Re-Zero Force-Torque Sensors" _collapsed="true"/>
<SubTree ID="Move to Waypoint" waypoint_name="Hinge View" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Clear Snapshot" _collapsed="true"/>
Expand Down Expand Up @@ -32,9 +31,9 @@
<Action ID="InitializeMTCTask" task_id="push_button_ml" controller_names="/joint_trajectory_controller" task="{push_along_axis_task}"/>
<Action ID="SetupMTCCurrentState" task="{push_along_axis_task}"/>
<Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{button_pose}" task="{push_along_axis_task}" use_all_planners="false"/>
<Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{push_along_axis_task}"/>
<Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" group_name="gripper" object_name="<octomap>" allow_collision="true" task="{push_along_axis_task}"/>
<Action ID="SetupMTCMoveAlongFrameAxis" task="{push_along_axis_task}" hand_frame="grasp_link" axis_frame="grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="0.2" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05"/>
<Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{push_along_axis_task}"/>
<Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" group_name="gripper" object_name="<octomap>" allow_collision="false" task="{push_along_axis_task}"/>
<Action ID="PlanMTCTask" solution="{full_push_along_axis_solution}" task="{push_along_axis_task}"/>
<Action ID="SplitMTCSolution" solution_in="{full_push_along_axis_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}"/>
<Action ID="WaitForUserTrajectoryApproval" solution="{full_push_along_axis_solution}"/>
Expand All @@ -45,7 +44,7 @@
<Action ID="ExecuteMTCTask" solution="{push_solution}"/>
</Control>
</Control>
<SubTree ID="RetreatToInitialPose" pre_approach_robot_state="{pre_approach_robot_state}" initial_robot_state="{initial_robot_state}" parameters="{parameters}" _collapsed="true"/>
<SubTree ID="RetreatToInitialPose" pre_approach_robot_state="{pre_approach_robot_state}" initial_robot_state="{initial_robot_state}" _collapsed="true"/>
</Control>
</BehaviorTree>
<BehaviorTree ID="CloseGripper">
Expand All @@ -57,9 +56,9 @@
<Control ID="Sequence">
<Action ID="InitializeMTCTask" task_id="push_button_ml_retreat" controller_names="/joint_trajectory_controller" task="{retreat_task}"/>
<Action ID="SetupMTCCurrentState" task="{retreat_task}"/>
<Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{retreat_task}"/>
<Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" group_name="gripper" object_name="<octomap>" allow_collision="true" task="{retreat_task}"/>
<Action ID="SetupMTCCartesianMoveToJointState" joint_state="{pre_approach_robot_state}" planning_group_name="manipulator" task="{retreat_task}"/>
<Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{retreat_task}"/>
<Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" group_name="gripper" object_name="<octomap>" allow_collision="false" task="{retreat_task}"/>
<Action ID="SetupMTCMoveToJointState" joint_state="{initial_robot_state}" planning_group_name="manipulator" task="{retreat_task}" use_all_planners="false" />
<Action ID="PlanMTCTask" solution="{return_to_initial_waypoint_solution}" task="{retreat_task}"/>
<Action ID="ExecuteMTCTask" solution="{return_to_initial_waypoint_solution}"/>
Expand Down
Loading