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

Commit

Permalink
Fix conflicting behavior IDs in ur_site_config (#389)
Browse files Browse the repository at this point in the history
  • Loading branch information
@dyackzan
dyackzan authored Oct 17, 2024
1 parent 80204ac commit bd2bd59
Show file tree
Hide file tree
Showing 4 changed files with 70 additions and 178 deletions.
61 changes: 2 additions & 59 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 @@ -6,7 +6,7 @@
>
<Control ID="Sequence">
<SubTree ID="Re-Zero Force-Torque Sensors" />
<SubTree ID="CloseGripper" />
<SubTree ID="Close Gripper" />
<Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}" />
<Control ID="Sequence">
<Action
Expand Down Expand Up @@ -116,67 +116,10 @@
</Control>
</Control>
<SubTree
ID="RetreatToInitialPose"
ID="Retreat To Initial Pose"
pre_approach_robot_state="{pre_approach_robot_state}"
initial_robot_state="{initial_robot_state}"
/>
</Control>
</BehaviorTree>
<BehaviorTree ID="CloseGripper">
<Control ID="Sequence" name="close_gripper_main">
<Action
ID="MoveGripperAction"
gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
position="0.7929"
/>
</Control>
</BehaviorTree>
<BehaviorTree ID="RetreatToInitialPose">
<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"
group_name="gripper"
object_name="&lt;octomap&gt;"
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"
group_name="gripper"
object_name="&lt;octomap&gt;"
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}"
/>
</Control>
</BehaviorTree>
</root>
61 changes: 2 additions & 59 deletions src/picknik_ur_site_config/objectives/push_button.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
>
<Control ID="Sequence">
<SubTree ID="Re-Zero Force-Torque Sensors" />
<SubTree ID="CloseGripper" />
<SubTree ID="Close Gripper" />
<Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}" />
<Control ID="Sequence">
<Action
Expand Down Expand Up @@ -116,67 +116,10 @@
</Control>
</Control>
<SubTree
ID="RetreatToInitialPose"
ID="Retreat To Initial Pose"
pre_approach_robot_state="{pre_approach_robot_state}"
initial_robot_state="{initial_robot_state}"
/>
</Control>
</BehaviorTree>
<BehaviorTree ID="CloseGripper">
<Control ID="Sequence" name="close_gripper_main">
<Action
ID="MoveGripperAction"
gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
position="0.7929"
/>
</Control>
</BehaviorTree>
<BehaviorTree ID="RetreatToInitialPose">
<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"
group_name="gripper"
object_name="&lt;octomap&gt;"
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"
group_name="gripper"
object_name="&lt;octomap&gt;"
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}"
/>
</Control>
</BehaviorTree>
</root>
62 changes: 2 additions & 60 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 @@ -94,7 +94,7 @@
graspable_object="{button}"
pose="{button_pose}"
/>
<SubTree ID="CloseGripper" _collapsed="true" />
<SubTree ID="Close Gripper" _collapsed="true" />
<Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}" />
<Control ID="Sequence">
<Action
Expand Down Expand Up @@ -169,69 +169,11 @@
</Control>
</Control>
<SubTree
ID="RetreatToInitialPose"
ID="Retreat To Initial Pose"
pre_approach_robot_state="{pre_approach_robot_state}"
initial_robot_state="{initial_robot_state}"
_collapsed="true"
/>
</Control>
</BehaviorTree>
<BehaviorTree ID="CloseGripper">
<Control ID="Sequence" name="close_gripper_main">
<Action
ID="MoveGripperAction"
gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
position="0.7929"
/>
</Control>
</BehaviorTree>
<BehaviorTree ID="RetreatToInitialPose">
<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"
group_name="gripper"
object_name="&lt;octomap&gt;"
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"
group_name="gripper"
object_name="&lt;octomap&gt;"
allow_collision="false"
task="{retreat_task}"
/>
<Action
ID="SetupMTCMoveToJointState"
joint_state="{initial_robot_state}"
planning_group_name="manipulator"
task="{retreat_task}"
planner_interface="moveit_default"
/>
<Action
ID="PlanMTCTask"
solution="{return_to_initial_waypoint_solution}"
task="{retreat_task}"
/>
<Action
ID="ExecuteMTCTask"
solution="{return_to_initial_waypoint_solution}"
/>
</Control>
</BehaviorTree>
</root>
64 changes: 64 additions & 0 deletions src/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Retreat To Initial Pose">
<BehaviorTree
ID="Retreat To Initial Pose"
_description=""
_subtreeOnly="true"
>
<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"
group_name="gripper"
object_name="&lt;octomap&gt;"
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"
group_name="gripper"
object_name="&lt;octomap&gt;"
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}"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Retreat To Initial Pose">
<input_port
name="pre_approach_robot_state"
default="{pre_approach_robot_state}"
/>
<input_port name="initial_robot_state" default="{initial_robot_state}" />
</SubTree>
</TreeNodesModel>
</root>

0 comments on commit bd2bd59

Please sign in to comment.