Merge pull request #430 from PickNikRobotics/14875-move-flasks-fix

14875 Moving waypoint in Move Flasks to Burners Objective to be more robust and not collide with table

Author: MikeWrock

src/lab_sim/objectives/constrained_pick_place.xml

@@ -20,6 +20,7 @@
         controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
         keep_orientation_tolerance="0.05"
         keep_orientation_link_names="grasp_link"
+        seed="0"
       />
       <SubTree ID="Open Gripper" />
       <SubTree
@@ -30,14 +31,6 @@
         place="Flask 1d - Place"
         name="Flask 1"
       />
-      <SubTree
-        ID="Constrained Pick and Place Subtree"
-        pre_pick="Flask 2a - Pre Pick"
-        pick="Flask 2b - Pick"
-        pre_place="Flask 2c - Pre Place"
-        place="Flask 2d - Place"
-        name="Flask 2"
-      />
       <SubTree
         ID="Constrained Pick and Place Subtree"
         pre_pick="Flask 3a - Pre Pick"
@@ -46,6 +39,15 @@
         place="Flask 3d - Place"
         name="Flask 3"
       />
+      <SubTree
+        ID="Constrained Pick and Place Subtree"
+        pre_pick="Flask 2a - Pre Pick"
+        pick="Flask 2b - Pick"
+        pre_place="Flask 2c - Pre Place"
+        place="Flask 2d - Place"
+        name="Flask 2"
+        _collapsed="true"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>

src/lab_sim/objectives/load_and_execute_joint_trajectory.xml

@@ -1,10 +1,10 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Load and Execute Joint Trajectory">
   <!--//////////-->
   <BehaviorTree
     ID="Load and Execute Joint Trajectory"
     _description="Load a trajectory that was saved as a YAML file"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <!--Make sure to move the robot to the same starting position as the saved trajectory starts, otherwise the joint_trajectory_controller will use joint interpolation to align the robot with the path-->

src/lab_sim/objectives/ml_auto_grasp_object_from_clicked_point.xml

@@ -119,6 +119,7 @@
             marker_lifetime="0.000000"
             mesh_path="package://lab_sim/objectives/gripper_token.dae"
             mesh_pose="{grasp_pose}"
+            mesh_name="mesh"
           />
           <SubTree
             ID="Pick from Pose With Approval"

src/lab_sim/objectives/show_grasp_link.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="show grasp_link">
+  <!--//////////-->
+  <BehaviorTree ID="show grasp_link" _description="" _favorite="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <SubTree
+        ID="Get Transform Frame Pose"
+        _collapsed="true"
+        requested_pose="{grasp_pose}"
+        requested_frame="grasp_link"
+      />
+      <Action
+        ID="VisualizePose"
+        marker_lifetime="0.000000"
+        marker_name="pose"
+        marker_size="0.100000"
+        pose="{grasp_pose}"
+      />
+      <Action
+        ID="VisualizeMesh"
+        marker_lifetime="0.000000"
+        mesh_name="mesh"
+        mesh_path="package://lab_sim/objectives/gripper_token.dae"
+        mesh_pose="{grasp_pose}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="show grasp_link">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>

src/lab_sim/waypoints/ur_waypoints.yaml

@@ -107,14 +107,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.012216800212355133
-    - 0.04970573711721682
-    - 0.8984514823861834
-    - 0.1063125865651509
-    - -0.41508272007941566
-    - -2.751881697859383
-    - -2.4680827148226405
-    - 0.06614325541815676
+    - 0.7639671853765404
+    - 0.6551230244120644
+    - -2.597276923961381
+    - -1.7207992223871735
+    - -1.8054851963579275
+    - -2.9105970459798978
+    - -1.012936176528235
+    - 3.2513966966550787
     velocity: []
   multi_dof_joint_state:
     header:
@@ -150,14 +150,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.052558135449284636
-    - 0.1015560185661144
-    - 0.8742197383524934
-    - 0.596069253342475
-    - -0.5146934305221308
-    - -3.1414852154720943
-    - -2.4442469657377055
-    - 0.06294310366684684
+    - 0.7639662602883518
+    - 0.6416717485181368
+    - -2.4872028523038185
+    - -2.2797066973266604
+    - -2.135135409489318
+    - -2.0350474070457296
+    - -0.9043501601840921
+    - 3.2728332839692875
     velocity: []
   multi_dof_joint_state:
     header:
@@ -193,14 +193,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.552721299565486
-    - -0.8026817877894441
-    - 0.7832655701857458
-    - 0.2347983960726958
-    - -0.47185366935901807
-    - -2.944152059324353
-    - -2.3543647658190983
-    - -0.0324954245702678
+    - 0.7639676351734388
+    - -0.14695746092055925
+    - -2.8650633002008017
+    - -1.8611906531500544
+    - -1.7478027758503683
+    - -2.810467735524929
+    - -1.2774332801079342
+    - 3.2091308552581377
     velocity: []
   multi_dof_joint_state:
     header:
@@ -236,14 +236,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.5527237261550169
-    - -0.8095633402493997
-    - 0.8041978448752184
-    - 0.3469595920821291
-    - -0.4759133036391534
-    - -3.0449635096222445
-    - -2.3752901974217586
-    - -0.03355586908682823
+    - 0.7639636129146917
+    - -0.14375285311965005
+    - -2.8600433649921495
+    - -1.9461602101793198
+    - -1.8756681071759462
+    - -2.597844115240611
+    - -1.272458370221303
+    - 3.209846268837379
     velocity: []
   multi_dof_joint_state:
     header:
@@ -279,14 +279,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7888251346894957
-    - 0.28410448263455473
-    - 0.7522809111415887
-    - 0.14258472176098788
-    - -0.6567553362551677
-    - -2.6535637271089603
-    - -2.3237504598801015
-    - -0.010010484715838389
+    - 0.7639644752524631
+    - 0.8130633024853088
+    - -2.726859433670606
+    - -1.8891510728754208
+    - -1.424665285030513
+    - -3.112945310941763
+    - -1.1412179091798282
+    - 3.2296261212002166
     velocity: []
   multi_dof_joint_state:
     header:
@@ -322,14 +322,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.017721560439610148
-    - 0.2691509193711033
-    - 0.7727615348146952
-    - 0.5552397114893021
-    - -0.5137325863770072
-    - -3.0003468873078876
-    - -2.335831248791311
-    - 0.12529289653514364
+    - 0.7639671534487822
+    - 0.7786119481205361
+    - -2.5914980762265736
+    - -2.386004216551112
+    - -1.882582057672306
+    - -2.1695739396137577
+    - -1.0073664867894194
+    - 3.25251851297434
     velocity: []
   multi_dof_joint_state:
     header:
@@ -365,14 +365,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.5527279024765758
-    - -0.9244326009773784
-    - 0.8128532763795221
-    - 0.19375908538144387
-    - -0.4813430484021708
-    - -2.882600666994581
-    - -2.3839438626762592
-    - -0.033962625761634384
+    - 0.7639660563824677
+    - -0.2402305144870719
+    - -2.8832984482674955
+    - -1.8018009466305653
+    - -1.685247569441112
+    - -2.956647634694288
+    - -1.2898622616661428
+    - 3.2014481210116723
     velocity: []
   multi_dof_joint_state:
     header:
@@ -408,14 +408,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.5527294577060091
-    - -0.9178831799453755
-    - 0.8032232530956076
-    - 0.36305116959502753
-    - -0.4822710241456316
-    - -3.0466827988573497
-    - -2.3743200968681815
-    - -0.03356569648918818
+    - 0.7639675199131791
+    - -0.24086053204199623
+    - -2.876977231591623
+    - -1.939806049514917
+    - -1.8984021894234757
+    - -2.6063762242952886
+    - -1.2836686932398813
+    - 3.202498456119905
     velocity: []
   multi_dof_joint_state:
     header:
@@ -451,14 +451,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.017721719550362367
-    - 0.10614201950655074
-    - 0.6667921047157305
-    - -0.020173732725037447
-    - -0.48719718634786974
-    - -2.46144527046315
-    - -2.2314189585794835
-    - 0.10080448035214111
+    - 0.7639685849147584
+    - 0.6446458760506458
+    - -2.7780078487258892
+    - -1.9155374976991195
+    - -1.3864706183602202
+    - -3.1216050652579663
+    - -1.191550429146523
+    - 3.221797617570794
     velocity: []
   multi_dof_joint_state:
     header:
@@ -494,14 +494,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.01772162964006715
-    - 0.07592482714796696
-    - 0.7452091128812429
-    - 0.5731708863903833
-    - -0.5272977039075434
-    - -3.0065755351034587
-    - -2.308742635428776
-    - 0.11856214137460191
+    - 0.7639682422101212
+    - 0.6144316834131173
+    - -2.6658657366983283
+    - -2.367501290161342
+    - -1.887225784038829
+    - -2.176385304935698
+    - -1.0805693269230385
+    - 3.239557435454622
     velocity: []
   multi_dof_joint_state:
     header:
@@ -537,14 +537,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639667711919623
-    - -0.8205919083342896
-    - 0.6106411457210819
-    - 0.09629022558040691
-    - -0.4356003871725801
-    - -2.8163100209411844
-    - -2.181786596029688
-    - -0.027837851359850115
+    - 0.7639614914449789
+    - -0.24670890913790466
+    - -2.9097897371763177
+    - -2.057135798403447
+    - -1.2794632064231153
+    - -3.1064892677695757
+    - -1.3160558724133762
+    - 3.197049363506155
     velocity: []
   multi_dof_joint_state:
     header:
@@ -580,14 +580,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639656639313784
-    - -0.8204325716971375
-    - 0.6051040370509869
-    - 0.33021173155820605
-    - -0.441945944944294
-    - -3.047095890623255
-    - -2.176250244653335
-    - -0.02772141144783419
+    - 0.7639590920670264
+    - -0.25674010578815215
+    - -2.904024374297564
+    - -2.1809624103341
+    - -1.4905916971704432
+    - -2.771777026484768
+    - -1.3103568767884126
+    - 3.198001370172111
     velocity: []
   multi_dof_joint_state:
     header: