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

Commit

Permalink
update use_all_planners -> planner_interface
Browse files Browse the repository at this point in the history
  • Loading branch information
marioprats committed Jun 27, 2024
1 parent f3b2b42 commit a65a63c
Show file tree
Hide file tree
Showing 34 changed files with 138 additions and 106 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -5,28 +5,28 @@
<Control ID="Sequence">
<!-- Clear snapshot, move to center pose, and open gripper -->
<Action ID="ClearSnapshot" />
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
<!-- Keep executing the pick and place sequence until failure -->
<Decorator ID="KeepRunningUntilFailure">
<Control ID="Sequence">
<!-- Pick object from "Grab", put it down at "Place", and go back to center pose -->
<Control ID="Sequence">
<SubTree ID="Move to Waypoint" waypoint_name="Pick Block" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Pick Block" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Place Block" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<SubTree ID="Move to Waypoint" waypoint_name="Place Block" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
</Control>
<!-- Pick object from "Place", put it down at "Grab", and go back to center pose -->
<Control ID="Sequence">
<SubTree ID="Move to Waypoint" waypoint_name="Place Block" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Place Block" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Pick Block" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<SubTree ID="Move to Waypoint" waypoint_name="Pick Block" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
</Control>
</Control>
</Decorator>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,8 @@
<BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure">
<Decorator ID="KeepRunningUntilFailure">
<Control ID="Sequence">
<SubTree ID="Move to Waypoint" waypoint_name="Right Corner" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Place" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Right Corner" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<SubTree ID="Move to Waypoint" waypoint_name="Place" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
</Control>
</Decorator>
</BehaviorTree>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
<Action ID="RetrieveJointStateParameter" timeout_sec="-1" joint_state="{target_joint_state}"/>
<Action ID="InitializeMTCTask" task_id="move_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}" use_all_planners="false" />
<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}" planner_interface="moveit_default" />
<Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
Expand Down
2 changes: 1 addition & 1 deletion src/picknik_ur_base_config/objectives/move_to_pose.xml
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
<Action ID="RetrievePoseParameter" timeout_sec="-1" pose="{target_pose}"/>
<Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="{controller_names}" task="{move_to_pose_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
<Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" use_all_planners="false"/>
<Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" planner_interface="moveit_default"/>
<Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
<SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
Expand Down
25 changes: 14 additions & 11 deletions src/picknik_ur_base_config/objectives/move_to_waypoint.xml
Original file line number Diff line number Diff line change
@@ -1,13 +1,16 @@
<?xml version="1.0"?>
<?xml version='1.0' encoding='UTF-8'?>
<root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
<BehaviorTree ID="Move to Waypoint" _description="This Objective is used when moving to one of the saved waypoints in your site configuration">
<Control ID="Sequence" name="root">
<Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}" joint_group_name="{joint_group_name}"/>
<Action ID="InitializeMTCTask" task_id="move_to_named_pose" controller_names="{controller_names}" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="{joint_group_name}" use_all_planners="{use_all_planners}" constraints="{constraints}" task="{move_to_waypoint_task}"/>
<Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
</Control>
</BehaviorTree>
<BehaviorTree ID="Move to Waypoint" _description="This Objective is used when moving to one of the saved waypoints in your site configuration">
<Control ID="Sequence" name="root">
<Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}" joint_group_name="{joint_group_name}"/>
<Action ID="InitializeMTCTask" task_id="move_to_named_pose" controller_names="{controller_names}" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="{joint_group_name}" planner_interface="{planner_interface}" constraints="{constraints}" task="{move_to_waypoint_task}"/>
<Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Move to Waypoint"/>
</TreeNodesModel>
</root>
Original file line number Diff line number Diff line change
Expand Up @@ -5,28 +5,28 @@
<Control ID="Sequence">
<!-- Clear snapshot, move to center pose, and open gripper -->
<Action ID="ClearSnapshot" />
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
<!-- Keep executing the pick and place sequence until failure -->
<Decorator ID="KeepRunningUntilFailure">
<Control ID="Sequence">
<!-- Pick object from "Pick", put it down at "Place", and go back to center pose -->
<Control ID="Sequence">
<SubTree ID="Move to Waypoint" waypoint_name="Pick" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Pick" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Place" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<SubTree ID="Move to Waypoint" waypoint_name="Place" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
</Control>
<!-- Pick object from "Place", put it down at "Pick", and go back to center pose -->
<Control ID="Sequence">
<SubTree ID="Move to Waypoint" waypoint_name="Place" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Place" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Pick" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<SubTree ID="Move to Waypoint" waypoint_name="Pick" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
</Control>
</Control>
</Decorator>
Expand Down
15 changes: 15 additions & 0 deletions src/picknik_ur_gazebo_config/objectives/mtc_approach_grasp.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
<?xml version='1.0' encoding='UTF-8'?>
<root BTCPP_format="4" main_tree_to_execute="MTC Cartesian Move To JointState">
<!--//////////-->
<BehaviorTree ID="MTC Cartesian Move To JointState" _description="" _favorite="true" _hardcoded="false">
<Control ID="Sequence" name="TopLevelSequence">
<Action ID="InitializeMTCTask" task_id="test"/>
<Action ID="SetupMTCCurrentState"/>
<Action ID="CreateJointState" joint_names="elbow_joint" positions="1.10" velocities="0.0" efforts="0.0"/>
<Action ID="SetupMTCCartesianMoveToJointState" joint_state="{joint_state_msg}"/>
<Action ID="PlanMTCTask"/>
<Action ID="WaitForUserTrajectoryApproval"/>
<Action ID="ExecuteMTCTask"/>
</Control>
</BehaviorTree>
</root>
14 changes: 14 additions & 0 deletions src/picknik_ur_gazebo_config/objectives/mtc_move_along_frame.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
<?xml version='1.0' encoding='UTF-8'?>
<root BTCPP_format="4" main_tree_to_execute="MTC Move Along Frame">
<!--//////////-->
<BehaviorTree ID="MTC Move Along Frame" _description="" _favorite="true" _hardcoded="false">
<Control ID="Sequence" name="TopLevelSequence">
<Action ID="InitializeMTCTask" task_id="test"/>
<Action ID="SetupMTCCurrentState"/>
<Action ID="SetupMTCMoveAlongFrameAxis" axis_y="0.00000" axis_frame="world" max_distance="0.500000"/>
<Action ID="PlanMTCTask"/>
<Action ID="WaitForUserTrajectoryApproval"/>
<Action ID="ExecuteMTCTask"/>
</Control>
</BehaviorTree>
</root>
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
<!-- ////////// -->
<BehaviorTree ID="Object Segmentation 2D" _description="Detect objects using a 2D image segmentation model." _favorite="false" _hardcoded="false">
<Control ID="Sequence" name="TopLevelSequence">
<SubTree ID="Move to Waypoint" waypoint_name="Extended Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Extended Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
<Action ID="GetMasks2DAction" image="{image}" action_name="/get_masks_2d_maskrcnn" valid_classes="" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="-1" masks2d="{masks2d}"/>
</Control>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
<Control ID="Sequence" name="TopLevelSequence">
<Action ID="ClearSnapshot"/>
<Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
<SubTree ID="Move to Waypoint" waypoint_name="Extended Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<SubTree ID="Move to Waypoint" waypoint_name="Extended Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default"/>
<Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
<Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
<Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
Expand Down
Loading

0 comments on commit a65a63c

Please sign in to comment.