Add Machine Tending Mock Hardware Configuration

src/moveit_studio_agent_examples/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>moveit_studio_agent_examples</name>
-  <version>2.4.0</version>
+  <version>2.5.0</version>
   <description>Package containing scripts for interacting with MoveIt Studio Agent</description>
 
   <maintainer email="[email protected]">Chance Cardona</maintainer>

src/picknik_ur_base_config/README.md

@@ -1,5 +1,5 @@
 # picknik_ur_base_config
 
-A MoveIt Studio base configuration for PickNik's UR5e simulation.
+A MoveIt Studio base configuration for PickNik's Universal Robots (UR) arms.
 
 For detailed documentation see: [MoveIt Studio Documentation](https://docs.picknik.ai/)

src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml

@@ -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"/>
+            <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="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"/>
+                        <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="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"/>
-                        <Action ID="MoveToJointState" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <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="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"/>
+                        <Action ID="MoveToJointState" 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"/>
+                        <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="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"/>
-                        <Action ID="MoveToJointState" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <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="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"/>
+                        <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
                     </Control>
                 </Control>
             </Decorator>

src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml

@@ -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"/>
-                <Action ID="MoveToJointState" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                <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"/>
             </Control>
         </Decorator>
     </BehaviorTree>

src/picknik_ur_base_config/objectives/looping_pick_and_place_object.xml

@@ -0,0 +1,95 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Looping Pick and Place Object" _favorite="true">
+    <!-- ////////// -->
+    <BehaviorTree ID="Looping Pick and Place Object" _description="Repeatedly attempt to detect and grasp a cuboid from a target bin, and drop it in the target area." _favorite="true">
+        <Control ID="Sequence">
+            <Action ID="LoadObjectiveParameters" config_file_name="looping_pick_and_place_object_config.yaml" parameters="{parameters}"/>
+
+            <!-- Remove any objects which might remain in the planning scene from previous pick attempts -->
+            <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="apply_planning_scene"/>
+
+            <!-- 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"/>
+
+            <!-- This loops forever because the behavior tree under this decorator never returns FAILURE. -->
+            <Decorator ID="KeepRunningUntilFailure">
+                <Control ID="Fallback" name="TryGraspOuter">
+                    <Decorator ID="KeepRunningUntilFailure">
+                        <Decorator ID="ForEachString" name="IterateThroughPlaceWaypoints" vector_in="Place Right;Place Left" out="{place_waypoint}">
+                            <Control ID="Sequence">
+                                <!-- This decorator contains the logic to detect an object and then execute motion to grasp it. If grasping is aborted because the object changed while the robot was moving towards it, we will retry up to 5 times before giving up. -->
+                                <Decorator ID="RetryUntilSuccessful" num_attempts="5">
+                                    <Control ID="Fallback" name="TryGraspInner">
+                                        <Control ID="Sequence">
+                                            <!-- Clear the collision octree. -->
+                                            <Action ID="ClearSnapshot"/>
+
+                                            <!-- Remove any objects which might remain in the planning scene from previous pick attempts -->
+                                            <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="apply_planning_scene"/>
+
+                                            <!-- Get the latest point cloud from the target topic-->
+                                            <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" />
+
+                                            <!-- Detect all cuboids in that point cloud-->
+                                            <Action ID="FindSingularCuboids" point_cloud="{point_cloud}" parameters="{parameters}" detected_shapes="{cuboids}"/>
+
+                                            <!-- Get the first detected cuboid. NOTE: this is a small hack to get ForEachCollisionObject to iterate just once, which returns the first object in the vector. -->
+                                            <Control ID="Sequence">
+                                                <Decorator ID="ForceSuccess">
+                                                    <Decorator ID="ForEachCollisionObject" vector_in="{cuboids}" out="{cuboid_object}">
+                                                        <Control ID="Sequence">
+                                                            <Action ID="AlwaysFailure"/>
+                                                        </Control>
+                                                    </Decorator>
+                                                </Decorator>
+                                            </Control>
+
+                                            <!-- Add this cuboid to the planning scene. -->
+                                            <Action ID="ModifyObjectInPlanningScene" object="{cuboid_object}" apply_planning_scene_service="/apply_planning_scene"/>
+
+                                            <!-- Use point cloud to update the collision octomap (happens after ModifyObjectInPlanningScene so the cuboid is excluded from the octomap)-->
+                                            <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
+
+                                            <!-- Set up an MTC task to pick the cuboid and then plan the task. -->
+                                            <Action ID="InitializeMTCTask" task_id="looping_pick_and_place_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{task}"/>
+                                            <Action ID="SetupMTCCurrentState" task="{task}"/>
+                                            <Action ID="SetupMTCPickCuboid" cuboid_object="{cuboid_object}" task="{task}" parameters="{parameters}"/>
+                                            <Action ID="PlanMTCTask" solution="{solution}" task="{task}"/>
+
+                                            <!-- Execute the planned motion. -->
+                                            <Action ID="ExecuteMTCTask" solution="{solution}"/>
+                                        </Control>
+
+                                        <!-- TryGraspInner fallback condition. Wait for a short time before triggering a retry. -->
+                                        <Control ID="Sequence">
+                                            <Action ID="WaitForDuration" delay_duration="1"/>
+                                            <Action ID="AlwaysFailure"/>
+                                        </Control>
+                                    </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"/>
+
+                                <!-- 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="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"/>
+                            </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" use_all_planners="true"/>
+                        <Action ID="WaitForDuration" delay_duration="5"/>
+                        <Action ID="AlwaysSuccess"/>
+                    </Control>
+                </Control>
+            </Decorator>
+        </Control>
+    </BehaviorTree>
+</root>

src/picknik_ur_base_config/objectives/move_to_joint_state.xml

@@ -3,7 +3,7 @@
     <BehaviorTree ID="Move to Joint State" _description="Single-node behavior to move to a specified joint state">
         <Control ID="Sequence">
             <Action ID="GetStringFromUser" parameter_name="move_to_joint_state.waypoint_name" parameter_value="{waypoint_name}" />
-            <Action ID="MoveToJointState" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="{waypoint_name}"/>
+            <Action ID="MoveToJointState" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="{waypoint_name}" use_all_planners="true"/>
         </Control>
     </BehaviorTree>
 </root>

src/picknik_ur_base_config/package.xml

@@ -3,7 +3,7 @@
   <name>picknik_ur_base_config</name>
   <version>2.5.0</version>
 
-  <description>Base config package for a UR5e robot</description>
+  <description>Base configuration package for Picknik's UR robot arms</description>
 
   <maintainer email="[email protected]">MoveIt Studio Maintainer</maintainer>
 

src/picknik_ur_gazebo_config/LICENSE

@@ -0,0 +1,25 @@
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+
+   * Neither the name of the copyright holder nor the names of its
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.