Squashed 'src/fanuc_sim/' changes from 0b1e054c..6bf7e238

6bf7e238 Merge pull request #1 from PickNikRobotics/subtree-sync 6c95bd1c Sync 6.5 to main (#37) 964e43fb Sync V6.5 to main (#29) baf81139 patching fanuc (#22) 6db5e61f Removes visualization pane changes in teleoperation ca45e9aa version bump c19efc19 formatting git-subtree-dir: src/fanuc_sim git-subtree-split: 6bf7e238341a69ffc88faca491423f88de4a31d9
PickNikRobotics · Dec 6, 2024 · d111c2c · d111c2c
1 parent 52f447d
commit d111c2c
Show file tree

Hide file tree

Showing 10 changed files with 99 additions and 51 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -2,7 +2,18 @@ cmake_minimum_required(VERSION 3.22)
 project(fanuc_sim)
 
 find_package(ament_cmake REQUIRED)
+find_package(picknik_accessories REQUIRED)
 
+# Install all XML files in directory
+set(PICKNIK_ACCESSORIES_SHARE_DIR
+"${CMAKE_INSTALL_PREFIX}/../picknik_accessories/share/picknik_accessories/mujoco_assets/"
+)
+# Destination directory
+set(DEST_DIR "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/description/")
+
+install(DIRECTORY "${PICKNIK_ACCESSORIES_SHARE_DIR}"
+        DESTINATION "${DEST_DIR}"
+        FILES_MATCHING PATTERN "*")
 install(
   DIRECTORY
     config

diff --git a/description/scene.xml b/description/scene.xml
@@ -6,12 +6,12 @@
 
   <asset>
     <!-- Define textures and materials -->
-    <texture name="wood" type="cube" file="meshes/wood.png" />
+    <texture name="wood" type="cube" file="assets/wood.png" />
     <material name="wood" texture="wood" texrepeat="1 1" />
 
     <!-- Define AprilTag textures -->
-    <texture name="apriltag1" type="2d" file="meshes/tag36_11_00000.png" />
-    <texture name="apriltag2" type="2d" file="meshes/tag36_11_00001.png" />
+    <texture name="apriltag1" type="2d" file="assets/tag36_11_00000.png" />
+    <texture name="apriltag2" type="2d" file="assets/tag36_11_00001.png" />
 
     <!-- Define block materials with AprilTags -->
     <material
@@ -63,7 +63,7 @@
     />
   </asset>
   <asset>
-    <mesh name="cube" file="meshes/Cube.stl" />
+    <mesh name="cube" file="assets/Cube.stl" />
   </asset>
 
   <worldbody>
@@ -117,6 +117,8 @@
       resolution="640 480"
       euler="-0.9 0 3.1415"
     />
-    <include file="lrmate200id/lrmate200id.xml" />
+    <body name="arm_mount">
+      <include file="lrmate200id/lrmate200id.xml" />
+    </body>
   </worldbody>
 </mujoco>
diff --git a/objectives/interpolate_to_joint_state.xml b/objectives/interpolate_to_joint_state.xml
@@ -34,6 +34,9 @@
   <TreeNodesModel>
     <SubTree ID="Interpolate to Joint State">
       <input_port name="target_joint_state" default="{target_joint_state}" />
+      <MetadataFields>
+        <Metadata subcategory="Motion - Execute" />
+      </MetadataFields>
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/objectives/move_to_joint_state.xml b/objectives/move_to_joint_state.xml
@@ -32,6 +32,9 @@
   <TreeNodesModel>
     <SubTree ID="Move to Joint State">
       <input_port name="target_joint_state" default="{target_joint_state}" />
+      <MetadataFields>
+        <Metadata subcategory="Motion - Execute" />
+      </MetadataFields>
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/objectives/move_to_pose.xml b/objectives/move_to_pose.xml
@@ -36,6 +36,9 @@
   <TreeNodesModel>
     <SubTree ID="Move to Pose">
       <input_port name="target_pose" default="{target_pose}" />
+      <MetadataFields>
+        <Metadata subcategory="Motion - Execute" />
+      </MetadataFields>
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/objectives/move_to_waypoint.xml b/objectives/move_to_waypoint.xml
@@ -1,20 +1,55 @@
 <root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
-    <BehaviorTree ID="Move to Waypoint" _description="This Objective is used when moving to one of the saved waypoints in your site configuration" _subtreeOnly="true">
-        <Control ID="Sequence" name="root">
-            <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}" joint_group_name="{joint_group_name}"/>
-            <Action ID="InitializeMTCTask" task_id="move_to_named_pose" controller_names="{controller_names}" task="{move_to_waypoint_task}"/>
-            <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
-            <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="{joint_group_name}" planner_interface="{planner_interface}" constraints="{constraints}" task="{move_to_waypoint_task}"/>
-            <Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
-            <Action ID="BlockingExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
-        </Control>
-    </BehaviorTree>
-    <TreeNodesModel>
-        <SubTree ID="Move to Waypoint">
-            <input_port name="waypoint_name" default="Point A"/>
-            <input_port name="joint_group_name" default="manipulator"/>
-            <input_port name="planner_interface" default="moveit_default"/>
-            <input_port name="controller_names" default="/joint_trajectory_controller"/>
-        </SubTree>
-    </TreeNodesModel>
-</root>
+  <BehaviorTree
+    ID="Move to Waypoint"
+    _description="This Objective is used when moving to one of the saved waypoints in your site configuration"
+    _subtreeOnly="true"
+  >
+    <Control ID="Sequence" name="root">
+      <Action
+        ID="RetrieveWaypoint"
+        waypoint_joint_state="{target_joint_state}"
+        waypoint_name="{waypoint_name}"
+        joint_group_name="{joint_group_name}"
+      />
+      <Action
+        ID="InitializeMTCTask"
+        task_id="move_to_named_pose"
+        controller_names="{controller_names}"
+        task="{move_to_waypoint_task}"
+      />
+      <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
+      <Action
+        ID="SetupMTCMoveToJointState"
+        joint_state="{target_joint_state}"
+        name="SetupMTCMoveToJointState"
+        planning_group_name="{joint_group_name}"
+        planner_interface="{planner_interface}"
+        constraints="{constraints}"
+        task="{move_to_waypoint_task}"
+      />
+      <Action
+        ID="PlanMTCTask"
+        solution="{move_to_waypoint_solution}"
+        task="{move_to_waypoint_task}"
+      />
+      <Action
+        ID="BlockingExecuteMTCTask"
+        solution="{move_to_waypoint_solution}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Move to Waypoint">
+      <input_port name="waypoint_name" default="Point A" />
+      <input_port name="joint_group_name" default="manipulator" />
+      <input_port name="planner_interface" default="moveit_default" />
+      <input_port
+        name="controller_names"
+        default="/joint_trajectory_controller"
+      />
+      <MetadataFields>
+        <Metadata subcategory="Motion - Execute" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/objectives/request_teleoperation.xml b/objectives/request_teleoperation.xml
@@ -29,10 +29,6 @@
             <!--Joint sliders interpolate to joint state-->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 5">
               <Control ID="Sequence">
-                <Action
-                  ID="SwitchUIPrimaryView"
-                  primary_view_name="Visualization"
-                />
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
                     <Action
@@ -67,10 +63,6 @@
             <!--Interactive markers move to pose-->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 4">
               <Control ID="Sequence">
-                <Action
-                  ID="SwitchUIPrimaryView"
-                  primary_view_name="Visualization"
-                />
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
                     <Action
@@ -105,10 +97,6 @@
             <!--Waypoint buttons move to joint state-->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 3">
               <Control ID="Sequence">
-                <Action
-                  ID="SwitchUIPrimaryView"
-                  primary_view_name="Visualization"
-                />
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
                     <Action
@@ -142,20 +130,12 @@
             </Decorator>
             <!--Cartesian and joint jogging-->
             <Control ID="Sequence" _while="teleop_mode == 2">
-              <Action
-                ID="SwitchUIPrimaryView"
-                primary_view_name="Visualization"
-              />
               <Action
                 ID="TeleoperateTwist"
                 controller_name="servo_controller"
               />
             </Control>
             <Control ID="Sequence" _while="teleop_mode == 1">
-              <Action
-                ID="SwitchUIPrimaryView"
-                primary_view_name="Visualization"
-              />
               <Action
                 ID="TeleoperateJointJog"
                 controller_name="servo_controller"

diff --git a/objectives/teleoperate.xml b/objectives/teleoperate.xml
@@ -1,6 +1,16 @@
 <root BTCPP_format="4" main_tree_to_execute="Teleoperate">
-    <!--//////////-->
-    <BehaviorTree ID="Teleoperate" _description="Handles the different variations of teleoperation from the web UI. Can be used standalone." _favorite="false" _hardcoded="false">
-        <SubTree ID="Request Teleoperation" enable_user_interaction="false" user_interaction_prompt="" initial_teleop_mode="3"/>
-    </BehaviorTree>
-</root>
+  <!--//////////-->
+  <BehaviorTree
+    ID="Teleoperate"
+    _description="Handles the different variations of teleoperation from the web UI. Can be used standalone."
+    _favorite="false"
+    _hardcoded="false"
+  >
+    <SubTree
+      ID="Request Teleoperation"
+      enable_user_interaction="false"
+      user_interaction_prompt=""
+      initial_teleop_mode="3"
+    />
+  </BehaviorTree>
+</root>
diff --git a/package.xml b/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0" ?>
 <package format="3">
   <name>fanuc_sim</name>
-  <version>6.0.0</version>
+  <version>6.5.0</version>
 
   <description>Base configuration package for Picknik's Fanuc arms</description>
 
@@ -11,6 +11,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>picknik_accessories</depend>
   <exec_depend>admittance_controller</exec_depend>
   <exec_depend>kinematics_interface_kdl</exec_depend>
   <exec_depend>moveit_planners_stomp</exec_depend>

diff --git a/waypoints/waypoints.yaml b/waypoints/waypoints.yaml
@@ -1 +1 @@
-[]
+[]