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

Commit

Permalink
Use updated YAML-free version of SetupMTCUpdateGroupCollisionRule (#256)
Browse files Browse the repository at this point in the history
  • Loading branch information
@schornakj
schornakj authored Apr 22, 2024
1 parent 964ee94 commit 5021b3a
Show file tree
Hide file tree
Showing 4 changed files with 15 additions and 27 deletions.
11 changes: 5 additions & 6 deletions src/picknik_ur_site_config/objectives/close_cabinet_door.xml
View file
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

This file was deleted.

11 changes: 5 additions & 6 deletions src/picknik_ur_site_config/objectives/push_button.xml
View file
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
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

0 comments on commit 5021b3a

Please sign in to comment.