diff --git a/src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml
index c26ad822..83ea1374 100644
--- a/src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml
+++ b/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" use_all_planners="true"/>
+            <Action ID="MoveToWaypoint" 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" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" 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" 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="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" 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" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" 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" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" 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" 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="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" 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" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
                     </Control>
                 </Control>
             </Decorator>
diff --git a/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml b/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml
index 9db0de3a..7dbaf01c 100644
--- a/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml
+++ b/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" 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"/>
+                <Action ID="MoveToWaypoint" waypoint_name="Right Corner" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
             </Control>
         </Decorator>
     </BehaviorTree>
diff --git a/src/picknik_ur_base_config/objectives/move_to_joint_state.xml b/src/picknik_ur_base_config/objectives/move_to_joint_state.xml
deleted file mode 100644
index 18bb21fb..00000000
--- a/src/picknik_ur_base_config/objectives/move_to_joint_state.xml
+++ /dev/null
@@ -1,9 +0,0 @@
-<?xml version="1.0"?>
-<root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
-    <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}" use_all_planners="true"/>
-        </Control>
-    </BehaviorTree>
-</root>
diff --git a/src/picknik_ur_base_config/objectives/move_to_waypoint.xml b/src/picknik_ur_base_config/objectives/move_to_waypoint.xml
new file mode 100644
index 00000000..22188816
--- /dev/null
+++ b/src/picknik_ur_base_config/objectives/move_to_waypoint.xml
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
+    <BehaviorTree ID="Move to Waypoint" _description="Single-node behavior to move to a specified waypoint">
+        <Control ID="Sequence">
+            <Action ID="GetStringFromUser" parameter_name="move_to_waypoint.waypoint_name" parameter_value="{waypoint_name}" />
+            <Action ID="MoveToWaypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="{waypoint_name}" use_all_planners="true"/>
+        </Control>
+    </BehaviorTree>
+</root>
diff --git a/src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml
index 97fa3199..d1a2b3c9 100644
--- a/src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml
+++ b/src/picknik_ur_gazebo_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" use_all_planners="true"/>
+            <Action ID="MoveToWaypoint" 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 "Pick", put it down at "Place", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToJointState" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Pick" 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" 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"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Place" 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" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" 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 "Pick", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToJointState" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Place" 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" use_all_planners="true"/>
-                        <Action ID="MoveToJointState" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Pick" 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" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
                     </Control>
                 </Control>
             </Decorator>
diff --git a/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml b/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
index baece896..81b2d912 100644
--- a/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
+++ b/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
@@ -6,7 +6,7 @@
       <Control ID="Sequence" name="Setup">
         <Action ID="LoadObjectiveParameters" config_file_name="apriltag_detection_config.yaml" parameters="{parameters}"/>
         <SubTree ID="Open Gripper"/>
-        <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="false"/>
+        <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
       </Control>
       <Control ID="Sequence" name="GetDetection">
         <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
@@ -19,7 +19,7 @@
         <Action ID="TransformPose" input_pose="{detection_pose_world}" translation_xyz="-0.1;0.0;0.05" quaternion_xyzw="-0.707;0.707;0.0;0.0" output_pose="{grasp_pose}"/>
         <Action ID="MoveToPose" target_pose="{grasp_pose}" ik_frame="manual_grasp_link" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
         <SubTree ID="Close Gripper"/>
-        <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="false"/>
+        <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
       </Control>
     </Control>
   </BehaviorTree>
diff --git a/src/picknik_ur_mock_hw_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_ur_mock_hw_config/objectives/3_waypoint_pick_and_place.xml
index da6374a5..ffabac9d 100644
--- a/src/picknik_ur_mock_hw_config/objectives/3_waypoint_pick_and_place.xml
+++ b/src/picknik_ur_mock_hw_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 Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+            <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" 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 left, put it down at the CNC machine, and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToJointState" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Grasp Left" 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 Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                        <Action ID="MoveToJointState" waypoint_name="Grasp Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Grasp Machine" 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 Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
                     </Control>
                     <!-- Pick object from the CNC machine, put it down on the right, and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToJointState" waypoint_name="Grasp Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Grasp Machine" 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 Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                        <Action ID="MoveToJointState" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Grasp Left" 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 Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
                     </Control>
                 </Control>
             </Decorator>
diff --git a/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml b/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml
index c877ab4a..7c6442f5 100644
--- a/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml
+++ b/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml
@@ -3,12 +3,12 @@
   <!-- ////////// -->
   <BehaviorTree ID="Constrained Pick and Place" _description="Pick and place an object with constraints on the gripper's range of motion" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="MoveToJointState" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
+      <Action ID="MoveToWaypoint" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
       <Action ID="InitializeMotionConstraints" constraints_name="gripper pointing down" constraints="{constraints}"/>
       <Action ID="AppendOrientationConstraint" config_file_name="my_constraints.yaml" constraints="{constraints}"/>
       <SubTree ID="Open Gripper"/>
-      <Action ID="MoveToJointState" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Gripper Down" use_all_planners="true" constraints="{constraints}"/>
-      <Action ID="MoveToJointState" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Grasp Right" use_all_planners="true" constraints="{constraints}"/>
+      <Action ID="MoveToWaypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Gripper Down" use_all_planners="true" constraints="{constraints}"/>
+      <Action ID="MoveToWaypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Grasp Right" use_all_planners="true" constraints="{constraints}"/>
       <SubTree ID="Close Gripper"/>
     </Control>
   </BehaviorTree>
diff --git a/src/picknik_ur_mock_hw_config/objectives/cycle_between_waypoints.xml b/src/picknik_ur_mock_hw_config/objectives/cycle_between_waypoints.xml
index 5ac61e82..9eb88bfa 100644
--- a/src/picknik_ur_mock_hw_config/objectives/cycle_between_waypoints.xml
+++ b/src/picknik_ur_mock_hw_config/objectives/cycle_between_waypoints.xml
@@ -3,8 +3,8 @@
     <BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure" _favorite="true">
         <Decorator ID="KeepRunningUntilFailure">
             <Control ID="Sequence">
-                <Action ID="MoveToJointState" waypoint_name="Look at Left Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                <Action ID="MoveToJointState" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                <Action ID="MoveToWaypoint" waypoint_name="Look at Left Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                <Action ID="MoveToWaypoint" waypoint_name="Look at Machine" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
             </Control>
         </Decorator>
     </BehaviorTree>
diff --git a/src/picknik_ur_mock_hw_config/objectives/joint_diagnostic.xml b/src/picknik_ur_mock_hw_config/objectives/joint_diagnostic.xml
index 90025b45..de502164 100644
--- a/src/picknik_ur_mock_hw_config/objectives/joint_diagnostic.xml
+++ b/src/picknik_ur_mock_hw_config/objectives/joint_diagnostic.xml
@@ -3,11 +3,11 @@
   <!-- ////////// -->
   <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" use_all_planners="true"/>
+      <Action ID="MoveToWaypoint" waypoint_name="Home" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
       <Decorator ID="KeepRunningUntilFailure">
         <Control ID="Sequence">
-          <Action ID="MoveToJointState" waypoint_name="Wrist 2 Max" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-          <Action ID="MoveToJointState" waypoint_name="Wrist 2 Min" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+          <Action ID="MoveToWaypoint" waypoint_name="Wrist 2 Max" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+          <Action ID="MoveToWaypoint" waypoint_name="Wrist 2 Min" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
         </Control>
       </Decorator>
     </Control>
diff --git a/src/picknik_ur_site_config/objectives/looping_pick_and_place_object.xml b/src/picknik_ur_site_config/objectives/looping_pick_and_place_object.xml
index 54ff708f..e47dba1d 100644
--- a/src/picknik_ur_site_config/objectives/looping_pick_and_place_object.xml
+++ b/src/picknik_ur_site_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 Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+            <Action ID="MoveToWaypoint" 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">
@@ -71,14 +71,14 @@
                                     </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"/>
+                                <Action ID="MoveToWaypoint" 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="MoveToWaypoint" 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"/>
+                                <Action ID="MoveToWaypoint" 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>
@@ -86,7 +86,7 @@
                     <!-- 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"/>
+                        <Action ID="MoveToWaypoint" 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_site_config/objectives/open_cabinet_door_ml.xml b/src/picknik_ur_site_config/objectives/open_cabinet_door_ml.xml
index bb6c7d49..1145eef8 100644
--- a/src/picknik_ur_site_config/objectives/open_cabinet_door_ml.xml
+++ b/src/picknik_ur_site_config/objectives/open_cabinet_door_ml.xml
@@ -24,7 +24,7 @@
             <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}"
                 sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures"
                 point_cloud_uuid="" />
-            <Action ID="MoveToJointState"
+            <Action ID="MoveToWaypoint"
                 waypoint_name="Home"
                 planning_group_name="manipulator"
                 controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
diff --git a/src/picknik_ur_site_config/objectives/open_lever_handle_door_ml.xml b/src/picknik_ur_site_config/objectives/open_lever_handle_door_ml.xml
index f1d685f1..6815d4cd 100644
--- a/src/picknik_ur_site_config/objectives/open_lever_handle_door_ml.xml
+++ b/src/picknik_ur_site_config/objectives/open_lever_handle_door_ml.xml
@@ -22,7 +22,7 @@
             <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}"
                 sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures"
                 point_cloud_uuid="" />
-            <Action ID="MoveToJointState"
+            <Action ID="MoveToWaypoint"
                 waypoint_name="Door"
                 planning_group_name="manipulator"
                 controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
diff --git a/src/picknik_ur_site_config/objectives/pick_place_object.xml b/src/picknik_ur_site_config/objectives/pick_place_object.xml
index 96b790a0..54c748ac 100644
--- a/src/picknik_ur_site_config/objectives/pick_place_object.xml
+++ b/src/picknik_ur_site_config/objectives/pick_place_object.xml
@@ -99,15 +99,15 @@
 
                 <!-- The "Then" condition of the TryGraspOuter IfThenElse control. After successful grasp, move to dropoff pose and release object, then return to the Look at Pick and Place Zone pose. -->
                 <Control ID="Sequence">
-                    <Action ID="MoveToJointState" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                    <Action ID="MoveToWaypoint" waypoint_name="Place" 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"/>
-                    <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="MoveToWaypoint" 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>
 
                 <!-- The "Else" condition of the TryGraspOuter IfThenElse control. Move to the Look at Pick and Place Zone pose if all retry attempts failed. -->
                 <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="MoveToWaypoint" 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="AlwaysFailure"/>
                 </Control>
             </Control>
diff --git a/src/picknik_ur_site_config/objectives/push_button_ml.xml b/src/picknik_ur_site_config/objectives/push_button_ml.xml
index f95c3def..6205f61e 100644
--- a/src/picknik_ur_site_config/objectives/push_button_ml.xml
+++ b/src/picknik_ur_site_config/objectives/push_button_ml.xml
@@ -15,7 +15,7 @@
             <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}"
                 sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures"
                 point_cloud_uuid="" />
-            <Action ID="MoveToJointState"
+            <Action ID="MoveToWaypoint"
                 waypoint_name="Home"
                 planning_group_name="manipulator"
                 controller_names="/joint_trajectory_controller /robotiq_gripper_controller"