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

Rename MoveToJointState to MoveToWaypoint in configs #95

Merged
merged 1 commit into from
Aug 31, 2023
Merged
Changes from all commits
Commits
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
Rename MoveToJointState to MoveToWaypoint in configs
marioprats committed Aug 29, 2023
commit 8b2a9863e20c3d486cbfd4c65a124c7e83ab5621
Original file line number Diff line number Diff line change
@@ -5,28 +5,28 @@
<Control ID="Sequence">
<!-- Clear snapshot, move to center pose, and open gripper -->
<Action ID="ClearSnapshot" />
<Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<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">
<Action ID="MoveToJointState" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
<Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToJointState" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
<Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
</Control>
<!-- Pick object from "Place", put it down at "Grab", and go back to center pose -->
<Control ID="Sequence">
<Action ID="MoveToJointState" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
<Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToJointState" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
<Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
</Control>
</Control>
</Decorator>
Original file line number Diff line number Diff line change
@@ -3,8 +3,8 @@
<BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure">
<Decorator ID="KeepRunningUntilFailure">
<Control ID="Sequence">
<Action ID="MoveToJointState" waypoint_name="Right Corner" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToJointState" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Right Corner" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
</Control>
</Decorator>
</BehaviorTree>
9 changes: 0 additions & 9 deletions src/picknik_ur_base_config/objectives/move_to_joint_state.xml

This file was deleted.

9 changes: 9 additions & 0 deletions src/picknik_ur_base_config/objectives/move_to_waypoint.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
<?xml version="1.0"?>
<root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
<BehaviorTree ID="Move to Waypoint" _description="Single-node behavior to move to a specified waypoint">
<Control ID="Sequence">
<Action ID="GetStringFromUser" parameter_name="move_to_waypoint.waypoint_name" parameter_value="{waypoint_name}" />
<Action ID="MoveToWaypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="{waypoint_name}" use_all_planners="true"/>
</Control>
</BehaviorTree>
</root>
Original file line number Diff line number Diff line change
@@ -5,28 +5,28 @@
<Control ID="Sequence">
<!-- Clear snapshot, move to center pose, and open gripper -->
<Action ID="ClearSnapshot" />
<Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<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">
<Action ID="MoveToJointState" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
<Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToJointState" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
<Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
</Control>
<!-- Pick object from "Place", put it down at "Pick", and go back to center pose -->
<Control ID="Sequence">
<Action ID="MoveToJointState" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
<Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToJointState" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
<Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
</Control>
</Control>
</Decorator>
Original file line number Diff line number Diff line change
@@ -6,7 +6,7 @@
<Control ID="Sequence" name="Setup">
<Action ID="LoadObjectiveParameters" config_file_name="apriltag_detection_config.yaml" parameters="{parameters}"/>
<SubTree ID="Open Gripper"/>
<Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
</Control>
<Control ID="Sequence" name="GetDetection">
<Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
@@ -19,7 +19,7 @@
<Action ID="TransformPose" input_pose="{detection_pose_world}" translation_xyz="-0.1;0.0;0.05" quaternion_xyzw="-0.707;0.707;0.0;0.0" output_pose="{grasp_pose}"/>
<Action ID="MoveToPose" target_pose="{grasp_pose}" ik_frame="manual_grasp_link" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<SubTree ID="Close Gripper"/>
<Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
</Control>
</Control>
</BehaviorTree>
Original file line number Diff line number Diff line change
@@ -5,28 +5,28 @@
<Control ID="Sequence">
<!-- Clear snapshot, move to center pose, and open gripper -->
<Action ID="ClearSnapshot" />
<Action ID="MoveToJointState" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<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 left, put it down at the CNC machine, and go back to center pose -->
<Control ID="Sequence">
<Action ID="MoveToJointState" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
<Action ID="MoveToJointState" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToJointState" waypoint_name="Grasp Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Grasp Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
<Action ID="MoveToJointState" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
</Control>
<!-- Pick object from the CNC machine, put it down on the right, and go back to center pose -->
<Control ID="Sequence">
<Action ID="MoveToJointState" waypoint_name="Grasp Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Grasp Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
<Action ID="MoveToJointState" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToJointState" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
<Action ID="MoveToJointState" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
</Control>
</Control>
</Decorator>
Original file line number Diff line number Diff line change
@@ -3,12 +3,12 @@
<!-- ////////// -->
<BehaviorTree ID="Constrained Pick and Place" _description="Pick and place an object with constraints on the gripper's range of motion" _favorite="true">
<Control ID="Sequence" name="TopLevelSequence">
<Action ID="MoveToJointState" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
<Action ID="MoveToWaypoint" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
<Action ID="InitializeMotionConstraints" constraints_name="gripper pointing down" constraints="{constraints}"/>
<Action ID="AppendOrientationConstraint" config_file_name="my_constraints.yaml" constraints="{constraints}"/>
<SubTree ID="Open Gripper"/>
<Action ID="MoveToJointState" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Gripper Down" use_all_planners="true" constraints="{constraints}"/>
<Action ID="MoveToJointState" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Grasp Right" use_all_planners="true" constraints="{constraints}"/>
<Action ID="MoveToWaypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Gripper Down" use_all_planners="true" constraints="{constraints}"/>
<Action ID="MoveToWaypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Grasp Right" use_all_planners="true" constraints="{constraints}"/>
<SubTree ID="Close Gripper"/>
</Control>
</BehaviorTree>
Original file line number Diff line number Diff line change
@@ -3,8 +3,8 @@
<BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure" _favorite="true">
<Decorator ID="KeepRunningUntilFailure">
<Control ID="Sequence">
<Action ID="MoveToJointState" waypoint_name="Look at Left Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToJointState" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Left Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
</Control>
</Decorator>
</BehaviorTree>
6 changes: 3 additions & 3 deletions src/picknik_ur_mock_hw_config/objectives/joint_diagnostic.xml
Original file line number Diff line number Diff line change
@@ -3,11 +3,11 @@
<!-- ////////// -->
<BehaviorTree ID="Joint Diagnostic" _description="Move to a known pose and cycle through the min and max limits of a joint repeatedly" _favorite="true">
<Control ID="Sequence" name="TopLevelSequence">
<Action ID="MoveToJointState" waypoint_name="Home" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Home" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Decorator ID="KeepRunningUntilFailure">
<Control ID="Sequence">
<Action ID="MoveToJointState" waypoint_name="Wrist 2 Max" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToJointState" waypoint_name="Wrist 2 Min" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Wrist 2 Max" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Wrist 2 Min" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
</Control>
</Decorator>
</Control>
Original file line number Diff line number Diff line change
@@ -10,7 +10,7 @@

<!-- Open the gripper and move to a consistent state. -->
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
<Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>

<!-- This loops forever because the behavior tree under this decorator never returns FAILURE. -->
<Decorator ID="KeepRunningUntilFailure">
@@ -71,22 +71,22 @@
</Control>
</Decorator>
<!-- Move home. -->
<Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>

<!-- Move to the current place waypoint set by IterateThroughPlaceWaypoints, then open the gripper. -->
<Action ID="MoveToJointState" waypoint_name="{place_waypoint}" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="{place_waypoint}" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>

<!-- Finally, return home. -->
<Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
</Control>
</Decorator>
</Decorator>

<!-- TryGraspOuter fallback condition. Reset scene objects, move home, and then wait a short while before triggering a retry. -->
<Control ID="Sequence">
<Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
<Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
<Action ID="WaitForDuration" delay_duration="5"/>
<Action ID="AlwaysSuccess"/>
</Control>
Original file line number Diff line number Diff line change
@@ -24,7 +24,7 @@
<Action ID="SendPointCloudToUI" point_cloud="{point_cloud}"
sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures"
point_cloud_uuid="" />
<Action ID="MoveToJointState"
<Action ID="MoveToWaypoint"
waypoint_name="Home"
planning_group_name="manipulator"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
Original file line number Diff line number Diff line change
@@ -22,7 +22,7 @@
<Action ID="SendPointCloudToUI" point_cloud="{point_cloud}"
sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures"
point_cloud_uuid="" />
<Action ID="MoveToJointState"
<Action ID="MoveToWaypoint"
waypoint_name="Door"
planning_group_name="manipulator"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
6 changes: 3 additions & 3 deletions src/picknik_ur_site_config/objectives/pick_place_object.xml
Original file line number Diff line number Diff line change
@@ -99,15 +99,15 @@

<!-- The "Then" condition of the TryGraspOuter IfThenElse control. After successful grasp, move to dropoff pose and release object, then return to the Look at Pick and Place Zone pose. -->
<Control ID="Sequence">
<Action ID="MoveToJointState" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
<Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
</Control>

<!-- The "Else" condition of the TryGraspOuter IfThenElse control. Move to the Look at Pick and Place Zone pose if all retry attempts failed. -->
<Control ID="Sequence">
<Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
<Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
<Action ID="AlwaysFailure"/>
</Control>
</Control>
2 changes: 1 addition & 1 deletion src/picknik_ur_site_config/objectives/push_button_ml.xml
Original file line number Diff line number Diff line change
@@ -15,7 +15,7 @@
<Action ID="SendPointCloudToUI" point_cloud="{point_cloud}"
sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures"
point_cloud_uuid="" />
<Action ID="MoveToJointState"
<Action ID="MoveToWaypoint"
waypoint_name="Home"
planning_group_name="manipulator"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"