Merge pull request #7 from PickNikRobotics/xacro-update

Updating xacro and favourites
PickNikRobotics · Nov 15, 2024 · 0d35cb7 · 0d35cb7
2 parents 853c6cc + ee54d37
commit 0d35cb7
Show file tree

Hide file tree

Showing 14 changed files with 34 additions and 91 deletions.
diff --git a/src/lab_sim/description/picknik_ur.xacro b/src/lab_sim/description/picknik_ur.xacro
@@ -7,6 +7,9 @@
   <material name="plastic">
     <color rgba="0.1 0.1 0.1 1" />
   </material>
+  <material name="offwhite">
+    <color rgba="0.01 0.01 0.01 1" />
+  </material>
   <link name="world" />
   <joint name="tool_changer_joint" type="fixed">
     <!-- The parent link must be read from the robot model it is attached to. -->
@@ -1174,57 +1177,15 @@
   <!-- Table -->
   <link name="table">
     <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.025" />
-      <geometry>
-        <mesh
-          filename="package://picknik_accessories/descriptions/furniture/generic_table/wood_block.dae"
-          scale="0.125 0.125 0.00625"
-        />
-      </geometry>
-    </visual>
-    <visual>
-      <origin rpy="0 0 0" xyz="0.4 0.4 -0.2" />
-      <geometry>
-        <box size="0.04 0.04 0.45" />
-      </geometry>
-      <material name="grey">
-        <color rgba="0.1 0.1 0.1 1.0" />
-      </material>
-    </visual>
-    <visual>
-      <origin rpy="0 0 0" xyz="0.4 -0.4 -0.2" />
-      <geometry>
-        <box size="0.04 0.04 0.45" />
-      </geometry>
-      <material name="grey">
-        <color rgba="0.1 0.1 0.1 1.0" />
-      </material>
-    </visual>
-    <visual>
-      <origin rpy="0 0 0" xyz="-0.4 -0.4 -0.2" />
+      <origin rpy="0 0 0" xyz="0 0 -0.45" />
       <geometry>
-        <box size="0.04 0.04 0.45" />
+        <box size="3.5 1 0.9" />
       </geometry>
-      <material name="grey">
-        <color rgba="0.1 0.1 0.1 1.0" />
-      </material>
-    </visual>
-    <visual>
-      <origin rpy="0 0 0" xyz="-0.4 0.4 -0.2" />
-      <geometry>
-        <box size="0.04 0.04 0.45" />
-      </geometry>
-      <material name="grey">
-        <color rgba="0.1 0.1 0.1 1.0" />
-      </material>
     </visual>
     <collision>
-      <origin rpy="0 0 0" xyz="0 0 0" />
+      <origin rpy="0 0 0" xyz="0 0 -0.45" />
       <geometry>
-        <mesh
-          filename="package://picknik_accessories/descriptions/furniture/generic_table/wood_block.dae"
-          scale="0.125 0.125 0.00625"
-        />
+        <box size="3.5 1 0.9" />
       </geometry>
     </collision>
   </link>

diff --git a/src/lab_sim/objectives/apriltag_pick_object.xml b/src/lab_sim/objectives/apriltag_pick_object.xml
@@ -3,7 +3,7 @@
   <BehaviorTree
     ID="Pick April Tag Labeled Object"
     _description="Picks up an object that has an AprilTag marker."
-    _favorite="false"
+    _favorite="true"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <SubTree ID="Look at table" _collapsed="true" />

diff --git a/src/lab_sim/objectives/constrained_pick_place.xml b/src/lab_sim/objectives/constrained_pick_place.xml
@@ -51,8 +51,10 @@
               constraints="{constraints}"
             />
           </Decorator>
-          <!--Pick-->
-          <SubTree ID="Close Gripper" />
+          <!--We force success as the gripper closes, since we are commanding a position it will never reach (fingers fully closed)-->
+          <Decorator ID="ForceSuccess">
+            <SubTree ID="Close Gripper" />
+          </Decorator>
           <!--Move to place (drop) location-->
           <Decorator ID="RetryUntilSuccessful" num_attempts="3">
             <SubTree
@@ -110,7 +112,9 @@
               waypoint_name="Place Cube"
             />
           </Decorator>
-          <SubTree ID="Close Gripper" />
+          <Decorator ID="ForceSuccess">
+            <SubTree ID="Close Gripper" />
+          </Decorator>
           <Decorator ID="RetryUntilSuccessful" num_attempts="3">
             <SubTree
               ID="Move to Waypoint"

diff --git a/src/lab_sim/objectives/create_pose_vector.xml b/src/lab_sim/objectives/create_pose_vector.xml
@@ -1,7 +1,6 @@
-<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Create Pose Vector">
   <!--//////////-->
-  <BehaviorTree ID="Create Pose Vector">
+  <BehaviorTree ID="Create Pose Vector" _favorite="false">
     <Control ID="Sequence">
       <Action
         ID="CreateStampedPose"

diff --git a/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml b/src/lab_sim/objectives/load_and_execute_joint_trajectory.xml
@@ -17,7 +17,7 @@
       />
       <Action
         ID="LoadJointTrajectoryFromYaml"
-        file_path="/home/michael/user_ws/src/lab_sim/objectives/joint_trajectory.yaml"
+        file_path="joint_trajectory.yaml"
       />
       <Action ID="ExecuteFollowJointTrajectory" />
     </Control>

diff --git a/src/lab_sim/objectives/look_at_table.xml b/src/lab_sim/objectives/look_at_table.xml
@@ -1,6 +1,6 @@
 <root BTCPP_format="4" main_tree_to_execute="Look at table">
   <!--//////////-->
-  <BehaviorTree ID="Look at table" _description="" _favorite="false">
+  <BehaviorTree ID="Look at table" _description="" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <SubTree
         ID="Move to Waypoint"

diff --git a/src/lab_sim/objectives/move_with_velocity_and_force.xml b/src/lab_sim/objectives/move_with_velocity_and_force.xml
@@ -3,7 +3,7 @@
   <BehaviorTree
     ID="Move until Force Contact"
     _description="An example for how to use the VFC to move with a velocity / force reference until it collides"
-    _favorite="false"
+    _favorite="true"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <SubTree

diff --git a/src/lab_sim/objectives/place_object.xml b/src/lab_sim/objectives/place_object.xml
@@ -1,6 +1,11 @@
-<root BTCPP_format="4" main_tree_to_execute="Place object">
+<root BTCPP_format="4" main_tree_to_execute="Place Object">
   <!--//////////-->
-  <BehaviorTree ID="Place object" _description="" _favorite="false">
+  <BehaviorTree
+    ID="Place Object"
+    _description=""
+    _favorite="false"
+    _subtreeOnly="false"
+  >
     <Control ID="Sequence" name="root">
       <Control ID="Sequence">
         <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />

diff --git a/src/lab_sim/objectives/record_trajectory.xml b/src/lab_sim/objectives/record_trajectory.xml
diff --git a/src/lab_sim/objectives/register_cad_part.xml b/src/lab_sim/objectives/register_cad_part.xml
@@ -1,6 +1,6 @@
 <root BTCPP_format="4" main_tree_to_execute="Register CAD Part">
   <!--//////////-->
-  <BehaviorTree ID="Register CAD Part" _description="" _favorite="false">
+  <BehaviorTree ID="Register CAD Part" _description="" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <Action ID="ClearSnapshot" />
       <SubTree ID="Look at table" _collapsed="true" />

diff --git a/src/lab_sim/objectives/take_snap.xml b/src/lab_sim/objectives/take_snap.xml
@@ -1,6 +1,6 @@
 <root BTCPP_format="4" main_tree_to_execute="Scene camera snapshot">
   <!--//////////-->
-  <BehaviorTree ID="Scene camera snapshot" _description="" _favorite="false">
+  <BehaviorTree ID="Scene camera snapshot" _description="" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Clear out old snapshot data -->
       <Action ID="ClearSnapshot" />

diff --git a/src/lab_sim/objectives/wrist_snap.xml b/src/lab_sim/objectives/wrist_snap.xml
@@ -1,6 +1,6 @@
 <root BTCPP_format="4" main_tree_to_execute="Wrist camera snapshot">
   <!--//////////-->
-  <BehaviorTree ID="Wrist camera snapshot" _description="" _favorite="false">
+  <BehaviorTree ID="Wrist camera snapshot" _description="" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Do not clear snapshots first, so we can take multiple snapshots from different angles -->
       <Action ID="GetPointCloud" topic_name="/wrist_camera/points" />

diff --git a/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml b/src/moveit_pro_ur_configs/picknik_ur_base_config/objectives/close_gripper.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="UTF-8" ?>
+<?xml version="1.0" encoding="utf-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Close Gripper">
   <BehaviorTree ID="Close Gripper" _description="Close the gripper">
     <Control ID="Sequence" name="root">
@@ -10,4 +10,7 @@
       />
     </Control>
   </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Close Gripper" />
+  </TreeNodesModel>
 </root>
diff --git a/src/picknik_accessories b/src/picknik_accessories