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

Fix conflicting behavior IDs in ur_site_config #389

Merged
merged 1 commit into from
Oct 17, 2024
Merged

Fix conflicting behavior IDs in ur_site_config #389

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
61 changes: 2 additions & 59 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 @@ -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
Open in desktop
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
Open in desktop
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
Open in desktop
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>
Loading