Fixes for the Mock Sim and Gazebo Simulated Configurations (#66)

* Fix Inspect Surface in Gazebo config * Also remove the inspect surface objective * Favorite joint diagnostics, unfavorite waypoint cycles * Cleanup default start position * Fix looping pick and place and Gazebo Look at Pick and Place waypoint --------- Co-authored-by: Erik Holum <erik.holum@picknik.ai>
PickNikRobotics · Jul 14, 2023 · 8bc3511 · 8bc3511
1 parent dc93acf
commit 8bc3511
Show file tree

Hide file tree

Showing 8 changed files with 15 additions and 50 deletions.
diff --git a/src/picknik_ur_base_config/config/initial_positions.yaml b/src/picknik_ur_base_config/config/initial_positions.yaml
@@ -1,6 +1,6 @@
 shoulder_pan_joint: 0.0
-shoulder_lift_joint: -0.6
-elbow_joint: -1.4
-wrist_1_joint: -1.2
-wrist_2_joint: 1.7
+shoulder_lift_joint: -1.41
+elbow_joint: -0.7
+wrist_1_joint: -2.12
+wrist_2_joint: 1.67
 wrist_3_joint: 0.0
diff --git a/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml b/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
   <root BTCPP_format="4" main_tree_to_execute="Cycle Between Waypoints">
-    <BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure" _favorite="true">
+    <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"/>

diff --git a/src/picknik_ur_base_config/objectives/joint_diagnostic.xml b/src/picknik_ur_base_config/objectives/joint_diagnostic.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Joint Diagnostic">
   <!-- ////////// -->
-  <BehaviorTree ID="Joint Diagnostic" _description="Move to a known pose and cycle through the min and max limits of a joint repeatedly">
+  <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"/>
       <Decorator ID="KeepRunningUntilFailure">

diff --git a/src/picknik_ur_base_config/objectives/looping_pick_and_place_object.xml b/src/picknik_ur_base_config/objectives/looping_pick_and_place_object.xml
@@ -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 Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
 
             <!-- This loops forever because the behavior tree under this decorator never returns FAILURE. -->
             <Decorator ID="KeepRunningUntilFailure">
@@ -69,22 +69,22 @@
                                     </Control>
                                 </Decorator>
                                 <!-- Move home. -->
-                                <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 Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
 
                                 <!-- 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"/>
                                 <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 Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                                <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                             </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 Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <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="WaitForDuration" delay_duration="5"/>
                         <Action ID="AlwaysSuccess"/>
                     </Control>

diff --git a/src/picknik_ur_gazebo_config/config/initial_positions.yaml b/src/picknik_ur_gazebo_config/config/initial_positions.yaml
diff --git a/src/picknik_ur_gazebo_config/objectives/inspect_surface.xml b/src/picknik_ur_gazebo_config/objectives/inspect_surface.xml
diff --git a/src/picknik_ur_gazebo_config/objectives/take_snapshot.xml b/src/picknik_ur_gazebo_config/objectives/take_snapshot.xml
diff --git a/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml b/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
@@ -165,11 +165,11 @@
       - wrist_2_joint
       - wrist_3_joint
     position:
-      - 3.03
-      - -0.78
-      - -0.51
-      - -2.39
-      - 1.65
+      - 0
+      - -1.41
+      - -0.7
+      - -2.12
+      - 1.67
       - 0
     velocity: []
   name: Look at Pick and Place Zone