From b99e72bd107154f0c93b0a8043bbc8ff15f55d85 Mon Sep 17 00:00:00 2001
From: Richard Pratt <43280770+rlpratt12@users.noreply.github.com>
Date: Wed, 25 Sep 2024 13:14:43 -0600
Subject: [PATCH 01/14] Added default inputs to 'Request Teleoperation'
 behavior to match required inputs (#374)

* Added default inputs to Request Teleoperation behavior to match required inputs.

* Make precommit happy.
---
 .../objectives/request_teleoperation.xml                   | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/picknik_ur_base_config/objectives/request_teleoperation.xml b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
index 99480365..c02a3506 100644
--- a/src/picknik_ur_base_config/objectives/request_teleoperation.xml
+++ b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
@@ -167,4 +167,11 @@
       </Control>
     </Control>
   </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Request Teleoperation">
+      <input_port name="enable_user_interaction" default="false" />
+      <input_port name="user_interaction_prompt" default="" />
+      <input_port name="initial_teleop_mode" default="3" />
+    </SubTree>
+  </TreeNodesModel>
 </root>

From 204888bc28cf0a0a8d36e6df3ba1b5a57859c014 Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Fri, 27 Sep 2024 11:41:43 -0600
Subject: [PATCH 02/14] Fix picknik_registration gcov linking issue (#376)

---
 src/external_dependencies/picknik_registration | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/external_dependencies/picknik_registration b/src/external_dependencies/picknik_registration
index 7c654b0a..0111bcc7 160000
--- a/src/external_dependencies/picknik_registration
+++ b/src/external_dependencies/picknik_registration
@@ -1 +1 @@
-Subproject commit 7c654b0aa4c69ae39c4b3fa1e83d7a08aa8cad60
+Subproject commit 0111bcc7d1583bd307b65de0f81fa362166f1ac0

From 5b75ad9ee9973dd15f9038af56d6e2fd22a3697c Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Tue, 1 Oct 2024 09:25:06 -0600
Subject: [PATCH 03/14] Update UR hardware configs (#377)

* Add required parameter in realsense launch file

* Remove deprecated controllers and add base objectives so teleop works on hw config
---
 .../launch/pro_rs_launch.py                   |  11 +-
 src/picknik_ur_site_config/config/config.yaml |   2 -
 .../control/picknik_ur.ros2_control.yaml      | 136 ------------------
 3 files changed, 10 insertions(+), 139 deletions(-)

diff --git a/src/picknik_ur_base_config/launch/pro_rs_launch.py b/src/picknik_ur_base_config/launch/pro_rs_launch.py
index 685f5bde..7aaec2b2 100644
--- a/src/picknik_ur_base_config/launch/pro_rs_launch.py
+++ b/src/picknik_ur_base_config/launch/pro_rs_launch.py
@@ -56,5 +56,14 @@ def generate_launch_description():
     )
     return LaunchDescription(
         rs_launch.declare_configurable_parameters(rs_launch.configurable_parameters)
-        + [OpaqueFunction(function=rs_launch.launch_setup)]
+        + [
+            OpaqueFunction(
+                function=rs_launch.launch_setup,
+                kwargs={
+                    "params": rs_launch.set_configurable_parameters(
+                        rs_launch.configurable_parameters
+                    )
+                },
+            )
+        ]
     )
diff --git a/src/picknik_ur_site_config/config/config.yaml b/src/picknik_ur_site_config/config/config.yaml
index 575b8836..b722faeb 100644
--- a/src/picknik_ur_site_config/config/config.yaml
+++ b/src/picknik_ur_site_config/config/config.yaml
@@ -32,8 +32,6 @@ ros2_control:
   # [Optional, default=[]]
   controllers_inactive_at_startup:
     - "joint_trajectory_controller"
-    - "admittance_controller_open_door"
-    - "joint_trajectory_controller_chained_open_door"
     - "joint_trajectory_admittance_controller"
     - "velocity_force_controller"
   # Any controllers here will not be spawned by MoveIt Pro.
diff --git a/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml b/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml
index 138f849a..17851768 100644
--- a/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml
+++ b/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml
@@ -15,10 +15,6 @@ controller_manager:
       type: robotiq_controllers/RobotiqActivationController
     servo_controller:
       type: joint_trajectory_controller/JointTrajectoryController
-    admittance_controller_open_door:
-      type: admittance_controller/AdmittanceController
-    joint_trajectory_controller_chained_open_door:
-      type: joint_trajectory_controller/JointTrajectoryController
     joint_trajectory_admittance_controller:
       type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController
     velocity_force_controller:
@@ -81,47 +77,6 @@ joint_trajectory_controller:
       wrist_3_joint:
         goal: 0.05
 
-joint_trajectory_controller_chained_open_door:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-    command_interfaces:
-      - position
-      - velocity
-    state_interfaces:
-      - position
-      - velocity
-    command_joints:
-      - admittance_controller_open_door/shoulder_pan_joint
-      - admittance_controller_open_door/shoulder_lift_joint
-      - admittance_controller_open_door/elbow_joint
-      - admittance_controller_open_door/wrist_1_joint
-      - admittance_controller_open_door/wrist_2_joint
-      - admittance_controller_open_door/wrist_3_joint
-    state_publish_rate: 100.0
-    action_monitor_rate: 20.0
-    allow_partial_joints_goal: false
-    constraints:
-      stopped_velocity_tolerance: 0.0
-      goal_time: 0.0
-      shoulder_pan_joint:
-        goal: 0.25
-      shoulder_lift_joint:
-        goal: 0.25
-      elbow_joint:
-        goal: 0.25
-      wrist_1_joint:
-        goal: 0.25
-      wrist_2_joint:
-        goal: 0.25
-      wrist_3_joint:
-        goal: 0.25
-
 servo_controller:
   ros__parameters:
     joints:
@@ -172,97 +127,6 @@ robotiq_activation_controller:
   ros__parameters:
     default: true
 
-admittance_controller_open_door:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-    command_interfaces:
-      - position
-
-    state_interfaces:
-      - position
-      - velocity
-
-    chainable_command_interfaces:
-      - position
-      - velocity
-
-    kinematics:
-      plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL
-      plugin_package: kinematics_interface
-      base: base_link  # Assumed to be stationary
-      tip: grasp_link  # The end effector frame
-      alpha: 0.05
-
-    ft_sensor:
-      name: tcp_fts_sensor
-      frame:
-        id: wrist_3_link  # Wrench measurements are in this frame
-      filter_coefficient: 0.1
-
-    control:
-      frame:
-        id: grasp_link  # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
-
-    fixed_world_frame:
-      frame:            # Gravity points down (neg. Z) in this frame (Usually: world or base_link)
-        id: base_link   # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
-
-    gravity_compensation:
-      frame:
-        id: wrist_3_link
-
-      CoG:  # specifies the center of gravity of the end effector
-        pos:
-          - 0.1  # x
-          - 0.0  # y
-          - 0.0  # z
-        force: 0.0  # mass * 9.81
-
-    admittance:
-      selected_axes:
-        - true  # x
-        - true  # y
-        - true  # z
-        - true  # rx
-        - true  # ry
-        - true  # rz
-
-      # Having ".0" at the end is MUST, otherwise there is a loading error
-      # F = M*a + D*v + S*(x - x_d)
-      mass:
-        - 10.0  # x
-        - 10.0  # y
-        - 10.0  # z
-        - 5.0  # rx
-        - 5.0  # ry
-        - 5.0  # rz
-
-      damping_ratio:  # damping can be used instead: zeta = D / (2 * sqrt( M * S ))
-        - 5.0  # x
-        - 5.0  # y
-        - 5.0  # z
-        - 5.0  # rx
-        - 5.0  # ry
-        - 5.0  # rz
-
-      stiffness:
-        - 500.0  # x
-        - 500.0  # y
-        - 500.0  # z
-        - 100.0  # rx
-        - 100.0  # ry
-        - 100.0  # rz
-
-    # general settings
-    enable_parameter_update_without_reactivation: true
-
 joint_trajectory_admittance_controller:
   ros__parameters:
     joints:

From e8400ee1f1b68fea46aa8ea675cb9710ac8a965c Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Tue, 8 Oct 2024 16:41:27 -0600
Subject: [PATCH 04/14] Reduce build time and chance of OOM by removing
 unnecessary sources dependencies (#380)

Co-authored-by: Nathan Brooks <nathanbrooks@picknik.ai>
---
 .gitmodules                               | 8 --------
 src/external_dependencies/LMS1xx          | 1 -
 src/external_dependencies/gz_ros2_control | 1 -
 3 files changed, 10 deletions(-)
 delete mode 160000 src/external_dependencies/LMS1xx
 delete mode 160000 src/external_dependencies/gz_ros2_control

diff --git a/.gitmodules b/.gitmodules
index 3a0ee973..73b88210 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -10,14 +10,6 @@
 	path = src/external_dependencies/serial
 	url = https://github.com/tylerjw/serial.git
 	branch = ros2
-[submodule "src/external_dependencies/gz_ros2_control"]
-	path = src/external_dependencies/gz_ros2_control
-	url = https://github.com/PickNikRobotics/gz_ros2_control.git
-	branch = mimic-joint-feature-humble
-[submodule "src/external_dependencies/LMS1xx"]
-	path = src/external_dependencies/LMS1xx
-	url = https://github.com/clearpathrobotics/LMS1xx.git
-	branch = humble-devel
 [submodule "src/external_dependencies/ridgeback"]
 	path = src/external_dependencies/ridgeback
 	url = https://github.com/sjahr/ridgeback.git
diff --git a/src/external_dependencies/LMS1xx b/src/external_dependencies/LMS1xx
deleted file mode 160000
index 90001ac2..00000000
--- a/src/external_dependencies/LMS1xx
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 90001ac2849c9f6ab9ca9bbfeb816f9b66f966de
diff --git a/src/external_dependencies/gz_ros2_control b/src/external_dependencies/gz_ros2_control
deleted file mode 160000
index 1cffeb4f..00000000
--- a/src/external_dependencies/gz_ros2_control
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 1cffeb4f986ac6f45dddb3560e187813b775aead

From bd6bb7aefaaa52bf63171dd0a4c0304e766f4b0a Mon Sep 17 00:00:00 2001
From: parallels <bhoff1980@gmail.com>
Date: Tue, 8 Oct 2024 21:38:58 -0600
Subject: [PATCH 05/14] removed switch primary view on teleop joint jog

---
 .../objectives/request_teleoperation.xml                      | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/picknik_ur_base_config/objectives/request_teleoperation.xml b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
index 99480365..047af241 100644
--- a/src/picknik_ur_base_config/objectives/request_teleoperation.xml
+++ b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
@@ -30,10 +30,10 @@
             <!--Joint sliders interpolate to joint state-->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 5">
               <Control ID="Sequence">
-                <Action
+                <!-- <Action
                   ID="SwitchUIPrimaryView"
                   primary_view_name="Visualization"
-                />
+                /> -->
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
                     <Action

From 5e88e49a27d9cc3d391eb442f1d6fa2aa2accefe Mon Sep 17 00:00:00 2001
From: parallels <bhoff1980@gmail.com>
Date: Tue, 8 Oct 2024 22:08:20 -0600
Subject: [PATCH 06/14] update teleop 1 to not alter primary view

---
 .../objectives/request_teleoperation.xml                      | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/picknik_ur_base_config/objectives/request_teleoperation.xml b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
index 047af241..3b86e5ce 100644
--- a/src/picknik_ur_base_config/objectives/request_teleoperation.xml
+++ b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
@@ -153,10 +153,10 @@
               />
             </Control>
             <Control ID="Sequence" _while="teleop_mode == 1">
-              <Action
+              <!-- <Action
                 ID="SwitchUIPrimaryView"
                 primary_view_name="Visualization"
-              />
+              /> -->
               <Action
                 ID="TeleoperateJointJog"
                 controller_name="servo_controller"

From 58a5fd20f82a877d3848caecb2236f4bd92b07c7 Mon Sep 17 00:00:00 2001
From: parallels <bhoff1980@gmail.com>
Date: Tue, 8 Oct 2024 22:37:16 -0600
Subject: [PATCH 07/14] removed switch primary view from joint slider controls
 to allow multi pane fluidity

---
 .../objectives/request_teleoperation.xml                  | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/src/picknik_ur_base_config/objectives/request_teleoperation.xml b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
index 3b86e5ce..b34b6226 100644
--- a/src/picknik_ur_base_config/objectives/request_teleoperation.xml
+++ b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
@@ -30,10 +30,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
@@ -153,10 +149,6 @@
               />
             </Control>
             <Control ID="Sequence" _while="teleop_mode == 1">
-              <!-- <Action
-                ID="SwitchUIPrimaryView"
-                primary_view_name="Visualization"
-              /> -->
               <Action
                 ID="TeleoperateJointJog"
                 controller_name="servo_controller"

From 6f8afdaafb6752fe51dd1f3959689ecd8e5589bd Mon Sep 17 00:00:00 2001
From: Paul Gesel <paul.gesel@picknik.ai>
Date: Fri, 11 Oct 2024 17:01:25 -0600
Subject: [PATCH 08/14] Update  dynamic parameters (#384)

Signed-off-by: Paul Gesel <paul.gesel@picknik.ai>
---
 .../config/moveit/hard_joint_limits.yaml               | 10 +++++-----
 src/arm_on_rail_sim/description/ur5e.xml               | 10 +++++++---
 2 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/src/arm_on_rail_sim/config/moveit/hard_joint_limits.yaml b/src/arm_on_rail_sim/config/moveit/hard_joint_limits.yaml
index 60ddbf2c..53b218df 100644
--- a/src/arm_on_rail_sim/config/moveit/hard_joint_limits.yaml
+++ b/src/arm_on_rail_sim/config/moveit/hard_joint_limits.yaml
@@ -15,7 +15,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_acceleration: 0.1
+    max_acceleration: 10.0
     max_effort: 150.0
     max_position: 2.9
     max_velocity: 0.175
@@ -25,7 +25,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_acceleration: !degrees 90.0
+    max_acceleration: !degrees 180.0
     max_effort: 150.0
     max_position: !degrees  180.0
     max_velocity: !degrees  30.0
@@ -35,7 +35,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_acceleration: !degrees 90.0
+    max_acceleration: !degrees 180.0
     max_effort: 150.0
     max_position: !degrees  90.0
     max_velocity: !degrees  30.0
@@ -45,7 +45,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_acceleration: !degrees 90.0
+    max_acceleration: !degrees 180.0
     max_effort: 150.0
     # we artificially limit this joint to half its actual joint position limit
     # to avoid (MoveIt/OMPL) planning problems, as due to the physical
@@ -65,7 +65,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_acceleration: !degrees 90.0
+    max_acceleration: !degrees 180.0
     max_effort: 28.0
     max_position: !degrees  180.0
     max_velocity: !degrees  60.0
diff --git a/src/arm_on_rail_sim/description/ur5e.xml b/src/arm_on_rail_sim/description/ur5e.xml
index 4e6f28c3..5970e239 100644
--- a/src/arm_on_rail_sim/description/ur5e.xml
+++ b/src/arm_on_rail_sim/description/ur5e.xml
@@ -11,8 +11,8 @@
         gaintype="fixed"
         biastype="affine"
         ctrlrange="-6.2831 6.2831"
-        gainprm="2000"
-        biasprm="0 -2000 -400"
+        gainprm="20000"
+        biasprm="0 -20000 -1000"
         forcerange="-150 150"
       />
       <default class="size3">
@@ -22,7 +22,11 @@
         </default>
       </default>
       <default class="size1">
-        <general gainprm="500" biasprm="0 -500 -100" forcerange="-28 28" />
+        <general
+          gainprm="20000"
+          biasprm="0 -20000 -1000"
+          forcerange="-150 150"
+        />
       </default>
       <default class="visual">
         <geom type="mesh" contype="0" conaffinity="0" group="2" />

From 00920dd0d3cdb326d19b638108b561a830b22f41 Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Wed, 16 Oct 2024 11:37:03 -0600
Subject: [PATCH 09/14] Merge v6.2 into main (#387)

---
 .../launch/pro_rs_launch.py                   |  11 +-
 .../objectives/request_teleoperation.xml      |   7 +
 src/picknik_ur_site_config/config/config.yaml |   2 -
 .../control/picknik_ur.ros2_control.yaml      | 136 ------------------
 4 files changed, 17 insertions(+), 139 deletions(-)

diff --git a/src/picknik_ur_base_config/launch/pro_rs_launch.py b/src/picknik_ur_base_config/launch/pro_rs_launch.py
index 685f5bde..7aaec2b2 100644
--- a/src/picknik_ur_base_config/launch/pro_rs_launch.py
+++ b/src/picknik_ur_base_config/launch/pro_rs_launch.py
@@ -56,5 +56,14 @@ def generate_launch_description():
     )
     return LaunchDescription(
         rs_launch.declare_configurable_parameters(rs_launch.configurable_parameters)
-        + [OpaqueFunction(function=rs_launch.launch_setup)]
+        + [
+            OpaqueFunction(
+                function=rs_launch.launch_setup,
+                kwargs={
+                    "params": rs_launch.set_configurable_parameters(
+                        rs_launch.configurable_parameters
+                    )
+                },
+            )
+        ]
     )
diff --git a/src/picknik_ur_base_config/objectives/request_teleoperation.xml b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
index 99480365..c02a3506 100644
--- a/src/picknik_ur_base_config/objectives/request_teleoperation.xml
+++ b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
@@ -167,4 +167,11 @@
       </Control>
     </Control>
   </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Request Teleoperation">
+      <input_port name="enable_user_interaction" default="false" />
+      <input_port name="user_interaction_prompt" default="" />
+      <input_port name="initial_teleop_mode" default="3" />
+    </SubTree>
+  </TreeNodesModel>
 </root>
diff --git a/src/picknik_ur_site_config/config/config.yaml b/src/picknik_ur_site_config/config/config.yaml
index 575b8836..b722faeb 100644
--- a/src/picknik_ur_site_config/config/config.yaml
+++ b/src/picknik_ur_site_config/config/config.yaml
@@ -32,8 +32,6 @@ ros2_control:
   # [Optional, default=[]]
   controllers_inactive_at_startup:
     - "joint_trajectory_controller"
-    - "admittance_controller_open_door"
-    - "joint_trajectory_controller_chained_open_door"
     - "joint_trajectory_admittance_controller"
     - "velocity_force_controller"
   # Any controllers here will not be spawned by MoveIt Pro.
diff --git a/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml b/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml
index 138f849a..17851768 100644
--- a/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml
+++ b/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml
@@ -15,10 +15,6 @@ controller_manager:
       type: robotiq_controllers/RobotiqActivationController
     servo_controller:
       type: joint_trajectory_controller/JointTrajectoryController
-    admittance_controller_open_door:
-      type: admittance_controller/AdmittanceController
-    joint_trajectory_controller_chained_open_door:
-      type: joint_trajectory_controller/JointTrajectoryController
     joint_trajectory_admittance_controller:
       type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController
     velocity_force_controller:
@@ -81,47 +77,6 @@ joint_trajectory_controller:
       wrist_3_joint:
         goal: 0.05
 
-joint_trajectory_controller_chained_open_door:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-    command_interfaces:
-      - position
-      - velocity
-    state_interfaces:
-      - position
-      - velocity
-    command_joints:
-      - admittance_controller_open_door/shoulder_pan_joint
-      - admittance_controller_open_door/shoulder_lift_joint
-      - admittance_controller_open_door/elbow_joint
-      - admittance_controller_open_door/wrist_1_joint
-      - admittance_controller_open_door/wrist_2_joint
-      - admittance_controller_open_door/wrist_3_joint
-    state_publish_rate: 100.0
-    action_monitor_rate: 20.0
-    allow_partial_joints_goal: false
-    constraints:
-      stopped_velocity_tolerance: 0.0
-      goal_time: 0.0
-      shoulder_pan_joint:
-        goal: 0.25
-      shoulder_lift_joint:
-        goal: 0.25
-      elbow_joint:
-        goal: 0.25
-      wrist_1_joint:
-        goal: 0.25
-      wrist_2_joint:
-        goal: 0.25
-      wrist_3_joint:
-        goal: 0.25
-
 servo_controller:
   ros__parameters:
     joints:
@@ -172,97 +127,6 @@ robotiq_activation_controller:
   ros__parameters:
     default: true
 
-admittance_controller_open_door:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-    command_interfaces:
-      - position
-
-    state_interfaces:
-      - position
-      - velocity
-
-    chainable_command_interfaces:
-      - position
-      - velocity
-
-    kinematics:
-      plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL
-      plugin_package: kinematics_interface
-      base: base_link  # Assumed to be stationary
-      tip: grasp_link  # The end effector frame
-      alpha: 0.05
-
-    ft_sensor:
-      name: tcp_fts_sensor
-      frame:
-        id: wrist_3_link  # Wrench measurements are in this frame
-      filter_coefficient: 0.1
-
-    control:
-      frame:
-        id: grasp_link  # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
-
-    fixed_world_frame:
-      frame:            # Gravity points down (neg. Z) in this frame (Usually: world or base_link)
-        id: base_link   # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
-
-    gravity_compensation:
-      frame:
-        id: wrist_3_link
-
-      CoG:  # specifies the center of gravity of the end effector
-        pos:
-          - 0.1  # x
-          - 0.0  # y
-          - 0.0  # z
-        force: 0.0  # mass * 9.81
-
-    admittance:
-      selected_axes:
-        - true  # x
-        - true  # y
-        - true  # z
-        - true  # rx
-        - true  # ry
-        - true  # rz
-
-      # Having ".0" at the end is MUST, otherwise there is a loading error
-      # F = M*a + D*v + S*(x - x_d)
-      mass:
-        - 10.0  # x
-        - 10.0  # y
-        - 10.0  # z
-        - 5.0  # rx
-        - 5.0  # ry
-        - 5.0  # rz
-
-      damping_ratio:  # damping can be used instead: zeta = D / (2 * sqrt( M * S ))
-        - 5.0  # x
-        - 5.0  # y
-        - 5.0  # z
-        - 5.0  # rx
-        - 5.0  # ry
-        - 5.0  # rz
-
-      stiffness:
-        - 500.0  # x
-        - 500.0  # y
-        - 500.0  # z
-        - 100.0  # rx
-        - 100.0  # ry
-        - 100.0  # rz
-
-    # general settings
-    enable_parameter_update_without_reactivation: true
-
 joint_trajectory_admittance_controller:
   ros__parameters:
     joints:

From 6ce1935177b1cb91d259f215da681b2d3a5added Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Wed, 16 Oct 2024 11:44:18 -0600
Subject: [PATCH 10/14] Revert "Merge v6.2 into main (#387)"

This reverts commit 00920dd0d3cdb326d19b638108b561a830b22f41.
---
 .../launch/pro_rs_launch.py                   |  11 +-
 .../objectives/request_teleoperation.xml      |   7 -
 src/picknik_ur_site_config/config/config.yaml |   2 +
 .../control/picknik_ur.ros2_control.yaml      | 136 ++++++++++++++++++
 4 files changed, 139 insertions(+), 17 deletions(-)

diff --git a/src/picknik_ur_base_config/launch/pro_rs_launch.py b/src/picknik_ur_base_config/launch/pro_rs_launch.py
index 7aaec2b2..685f5bde 100644
--- a/src/picknik_ur_base_config/launch/pro_rs_launch.py
+++ b/src/picknik_ur_base_config/launch/pro_rs_launch.py
@@ -56,14 +56,5 @@ def generate_launch_description():
     )
     return LaunchDescription(
         rs_launch.declare_configurable_parameters(rs_launch.configurable_parameters)
-        + [
-            OpaqueFunction(
-                function=rs_launch.launch_setup,
-                kwargs={
-                    "params": rs_launch.set_configurable_parameters(
-                        rs_launch.configurable_parameters
-                    )
-                },
-            )
-        ]
+        + [OpaqueFunction(function=rs_launch.launch_setup)]
     )
diff --git a/src/picknik_ur_base_config/objectives/request_teleoperation.xml b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
index c02a3506..99480365 100644
--- a/src/picknik_ur_base_config/objectives/request_teleoperation.xml
+++ b/src/picknik_ur_base_config/objectives/request_teleoperation.xml
@@ -167,11 +167,4 @@
       </Control>
     </Control>
   </BehaviorTree>
-  <TreeNodesModel>
-    <SubTree ID="Request Teleoperation">
-      <input_port name="enable_user_interaction" default="false" />
-      <input_port name="user_interaction_prompt" default="" />
-      <input_port name="initial_teleop_mode" default="3" />
-    </SubTree>
-  </TreeNodesModel>
 </root>
diff --git a/src/picknik_ur_site_config/config/config.yaml b/src/picknik_ur_site_config/config/config.yaml
index b722faeb..575b8836 100644
--- a/src/picknik_ur_site_config/config/config.yaml
+++ b/src/picknik_ur_site_config/config/config.yaml
@@ -32,6 +32,8 @@ ros2_control:
   # [Optional, default=[]]
   controllers_inactive_at_startup:
     - "joint_trajectory_controller"
+    - "admittance_controller_open_door"
+    - "joint_trajectory_controller_chained_open_door"
     - "joint_trajectory_admittance_controller"
     - "velocity_force_controller"
   # Any controllers here will not be spawned by MoveIt Pro.
diff --git a/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml b/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml
index 17851768..138f849a 100644
--- a/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml
+++ b/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml
@@ -15,6 +15,10 @@ controller_manager:
       type: robotiq_controllers/RobotiqActivationController
     servo_controller:
       type: joint_trajectory_controller/JointTrajectoryController
+    admittance_controller_open_door:
+      type: admittance_controller/AdmittanceController
+    joint_trajectory_controller_chained_open_door:
+      type: joint_trajectory_controller/JointTrajectoryController
     joint_trajectory_admittance_controller:
       type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController
     velocity_force_controller:
@@ -77,6 +81,47 @@ joint_trajectory_controller:
       wrist_3_joint:
         goal: 0.05
 
+joint_trajectory_controller_chained_open_door:
+  ros__parameters:
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    command_interfaces:
+      - position
+      - velocity
+    state_interfaces:
+      - position
+      - velocity
+    command_joints:
+      - admittance_controller_open_door/shoulder_pan_joint
+      - admittance_controller_open_door/shoulder_lift_joint
+      - admittance_controller_open_door/elbow_joint
+      - admittance_controller_open_door/wrist_1_joint
+      - admittance_controller_open_door/wrist_2_joint
+      - admittance_controller_open_door/wrist_3_joint
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.0
+      goal_time: 0.0
+      shoulder_pan_joint:
+        goal: 0.25
+      shoulder_lift_joint:
+        goal: 0.25
+      elbow_joint:
+        goal: 0.25
+      wrist_1_joint:
+        goal: 0.25
+      wrist_2_joint:
+        goal: 0.25
+      wrist_3_joint:
+        goal: 0.25
+
 servo_controller:
   ros__parameters:
     joints:
@@ -127,6 +172,97 @@ robotiq_activation_controller:
   ros__parameters:
     default: true
 
+admittance_controller_open_door:
+  ros__parameters:
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+      - velocity
+
+    chainable_command_interfaces:
+      - position
+      - velocity
+
+    kinematics:
+      plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL
+      plugin_package: kinematics_interface
+      base: base_link  # Assumed to be stationary
+      tip: grasp_link  # The end effector frame
+      alpha: 0.05
+
+    ft_sensor:
+      name: tcp_fts_sensor
+      frame:
+        id: wrist_3_link  # Wrench measurements are in this frame
+      filter_coefficient: 0.1
+
+    control:
+      frame:
+        id: grasp_link  # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
+
+    fixed_world_frame:
+      frame:            # Gravity points down (neg. Z) in this frame (Usually: world or base_link)
+        id: base_link   # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
+
+    gravity_compensation:
+      frame:
+        id: wrist_3_link
+
+      CoG:  # specifies the center of gravity of the end effector
+        pos:
+          - 0.1  # x
+          - 0.0  # y
+          - 0.0  # z
+        force: 0.0  # mass * 9.81
+
+    admittance:
+      selected_axes:
+        - true  # x
+        - true  # y
+        - true  # z
+        - true  # rx
+        - true  # ry
+        - true  # rz
+
+      # Having ".0" at the end is MUST, otherwise there is a loading error
+      # F = M*a + D*v + S*(x - x_d)
+      mass:
+        - 10.0  # x
+        - 10.0  # y
+        - 10.0  # z
+        - 5.0  # rx
+        - 5.0  # ry
+        - 5.0  # rz
+
+      damping_ratio:  # damping can be used instead: zeta = D / (2 * sqrt( M * S ))
+        - 5.0  # x
+        - 5.0  # y
+        - 5.0  # z
+        - 5.0  # rx
+        - 5.0  # ry
+        - 5.0  # rz
+
+      stiffness:
+        - 500.0  # x
+        - 500.0  # y
+        - 500.0  # z
+        - 100.0  # rx
+        - 100.0  # ry
+        - 100.0  # rz
+
+    # general settings
+    enable_parameter_update_without_reactivation: true
+
 joint_trajectory_admittance_controller:
   ros__parameters:
     joints:

From bd2bd59f440d0def9928650a8cf69751f5a22ecc Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Thu, 17 Oct 2024 11:32:10 -0600
Subject: [PATCH 11/14] Fix conflicting behavior IDs in ur_site_config (#389)

---
 .../objectives/close_cabinet_door.xml         | 61 +-----------------
 .../objectives/push_button.xml                | 61 +-----------------
 .../objectives/push_button_ml.xml             | 62 +-----------------
 .../objectives/retreat_to_initial_pose.xml    | 64 +++++++++++++++++++
 4 files changed, 70 insertions(+), 178 deletions(-)
 create mode 100644 src/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml

diff --git a/src/picknik_ur_site_config/objectives/close_cabinet_door.xml b/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
index 0c52d689..fcca6052 100644
--- a/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
+++ b/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
@@ -6,7 +6,7 @@
   >
     <Control ID="Sequence">
       <SubTree ID="Re-Zero Force-Torque Sensors" />
-      <SubTree ID="CloseGripper" />
+      <SubTree ID="Close Gripper" />
       <Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}" />
       <Control ID="Sequence">
         <Action
@@ -116,67 +116,10 @@
         </Control>
       </Control>
       <SubTree
-        ID="RetreatToInitialPose"
+        ID="Retreat To Initial Pose"
         pre_approach_robot_state="{pre_approach_robot_state}"
         initial_robot_state="{initial_robot_state}"
       />
     </Control>
   </BehaviorTree>
-  <BehaviorTree ID="CloseGripper">
-    <Control ID="Sequence" name="close_gripper_main">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.7929"
-      />
-    </Control>
-  </BehaviorTree>
-  <BehaviorTree ID="RetreatToInitialPose">
-    <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="close_cabinet_door_retreat"
-        controller_names="/joint_trajectory_controller"
-        task="{retreat_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{retreat_task}" />
-      <Action
-        ID="SetupMTCUpdateGroupCollisionRule"
-        name="AllowGripperCollisionWithOctomap"
-        group_name="gripper"
-        object_name="&lt;octomap&gt;"
-        allow_collision="true"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCCartesianMoveToJointState"
-        joint_state="{pre_approach_robot_state}"
-        planning_group_name="manipulator"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCUpdateGroupCollisionRule"
-        name="ForbidGripperCollisionWithOctomap"
-        group_name="gripper"
-        object_name="&lt;octomap&gt;"
-        allow_collision="false"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{initial_robot_state}"
-        planning_group_name="manipulator"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{return_to_initial_waypoint_solution}"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{return_to_initial_waypoint_solution}"
-      />
-    </Control>
-  </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_site_config/objectives/push_button.xml b/src/picknik_ur_site_config/objectives/push_button.xml
index 21de49d5..2c6e75e9 100644
--- a/src/picknik_ur_site_config/objectives/push_button.xml
+++ b/src/picknik_ur_site_config/objectives/push_button.xml
@@ -6,7 +6,7 @@
   >
     <Control ID="Sequence">
       <SubTree ID="Re-Zero Force-Torque Sensors" />
-      <SubTree ID="CloseGripper" />
+      <SubTree ID="Close Gripper" />
       <Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}" />
       <Control ID="Sequence">
         <Action
@@ -116,67 +116,10 @@
         </Control>
       </Control>
       <SubTree
-        ID="RetreatToInitialPose"
+        ID="Retreat To Initial Pose"
         pre_approach_robot_state="{pre_approach_robot_state}"
         initial_robot_state="{initial_robot_state}"
       />
     </Control>
   </BehaviorTree>
-  <BehaviorTree ID="CloseGripper">
-    <Control ID="Sequence" name="close_gripper_main">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.7929"
-      />
-    </Control>
-  </BehaviorTree>
-  <BehaviorTree ID="RetreatToInitialPose">
-    <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="push_button_retreat"
-        controller_names="/joint_trajectory_controller"
-        task="{retreat_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{retreat_task}" />
-      <Action
-        ID="SetupMTCUpdateGroupCollisionRule"
-        name="AllowGripperCollisionWithOctomap"
-        group_name="gripper"
-        object_name="&lt;octomap&gt;"
-        allow_collision="true"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCCartesianMoveToJointState"
-        joint_state="{pre_approach_robot_state}"
-        planning_group_name="manipulator"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCUpdateGroupCollisionRule"
-        name="ForbidGripperCollisionWithOctomap"
-        group_name="gripper"
-        object_name="&lt;octomap&gt;"
-        allow_collision="false"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{initial_robot_state}"
-        planning_group_name="manipulator"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{return_to_initial_waypoint_solution}"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{return_to_initial_waypoint_solution}"
-      />
-    </Control>
-  </BehaviorTree>
 </root>
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 c99a0349..53359d83 100644
--- a/src/picknik_ur_site_config/objectives/push_button_ml.xml
+++ b/src/picknik_ur_site_config/objectives/push_button_ml.xml
@@ -94,7 +94,7 @@
         graspable_object="{button}"
         pose="{button_pose}"
       />
-      <SubTree ID="CloseGripper" _collapsed="true" />
+      <SubTree ID="Close Gripper" _collapsed="true" />
       <Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}" />
       <Control ID="Sequence">
         <Action
@@ -169,69 +169,11 @@
         </Control>
       </Control>
       <SubTree
-        ID="RetreatToInitialPose"
+        ID="Retreat To Initial Pose"
         pre_approach_robot_state="{pre_approach_robot_state}"
         initial_robot_state="{initial_robot_state}"
         _collapsed="true"
       />
     </Control>
   </BehaviorTree>
-  <BehaviorTree ID="CloseGripper">
-    <Control ID="Sequence" name="close_gripper_main">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.7929"
-      />
-    </Control>
-  </BehaviorTree>
-  <BehaviorTree ID="RetreatToInitialPose">
-    <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="push_button_ml_retreat"
-        controller_names="/joint_trajectory_controller"
-        task="{retreat_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{retreat_task}" />
-      <Action
-        ID="SetupMTCUpdateGroupCollisionRule"
-        name="AllowGripperCollisionWithOctomap"
-        group_name="gripper"
-        object_name="&lt;octomap&gt;"
-        allow_collision="true"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCCartesianMoveToJointState"
-        joint_state="{pre_approach_robot_state}"
-        planning_group_name="manipulator"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCUpdateGroupCollisionRule"
-        name="ForbidGripperCollisionWithOctomap"
-        group_name="gripper"
-        object_name="&lt;octomap&gt;"
-        allow_collision="false"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{initial_robot_state}"
-        planning_group_name="manipulator"
-        task="{retreat_task}"
-        planner_interface="moveit_default"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{return_to_initial_waypoint_solution}"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{return_to_initial_waypoint_solution}"
-      />
-    </Control>
-  </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml b/src/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml
new file mode 100644
index 00000000..ee4c82a9
--- /dev/null
+++ b/src/picknik_ur_site_config/objectives/retreat_to_initial_pose.xml
@@ -0,0 +1,64 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Retreat To Initial Pose">
+  <BehaviorTree
+    ID="Retreat To Initial Pose"
+    _description=""
+    _subtreeOnly="true"
+  >
+    <Control ID="Sequence">
+      <Action
+        ID="InitializeMTCTask"
+        task_id="close_cabinet_door_retreat"
+        controller_names="/joint_trajectory_controller"
+        task="{retreat_task}"
+      />
+      <Action ID="SetupMTCCurrentState" task="{retreat_task}" />
+      <Action
+        ID="SetupMTCUpdateGroupCollisionRule"
+        name="AllowGripperCollisionWithOctomap"
+        group_name="gripper"
+        object_name="&lt;octomap&gt;"
+        allow_collision="true"
+        task="{retreat_task}"
+      />
+      <Action
+        ID="SetupMTCCartesianMoveToJointState"
+        joint_state="{pre_approach_robot_state}"
+        planning_group_name="manipulator"
+        task="{retreat_task}"
+      />
+      <Action
+        ID="SetupMTCUpdateGroupCollisionRule"
+        name="ForbidGripperCollisionWithOctomap"
+        group_name="gripper"
+        object_name="&lt;octomap&gt;"
+        allow_collision="false"
+        task="{retreat_task}"
+      />
+      <Action
+        ID="SetupMTCMoveToJointState"
+        joint_state="{initial_robot_state}"
+        planning_group_name="manipulator"
+        task="{retreat_task}"
+      />
+      <Action
+        ID="PlanMTCTask"
+        solution="{return_to_initial_waypoint_solution}"
+        task="{retreat_task}"
+      />
+      <Action
+        ID="ExecuteMTCTask"
+        solution="{return_to_initial_waypoint_solution}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Retreat To Initial Pose">
+      <input_port
+        name="pre_approach_robot_state"
+        default="{pre_approach_robot_state}"
+      />
+      <input_port name="initial_robot_state" default="{initial_robot_state}" />
+    </SubTree>
+  </TreeNodesModel>
+</root>

From 859061b7a6aae06025cdd675272f68b9483a0d52 Mon Sep 17 00:00:00 2001
From: Michael Wrock <mike.wrock@gmail.com>
Date: Thu, 17 Oct 2024 14:01:48 -0700
Subject: [PATCH 12/14] Update Dockerfile (#386)

Co-authored-by: Chance Cardona <41308677+chancecardona@users.noreply.github.com>
---
 Dockerfile | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index 73d55590..8006e4e0 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -119,10 +119,6 @@ ENV USER_WS=${USER_WS}
 
 # Compile the workspace
 WORKDIR $USER_WS
-# hadolint ignore=SC1091
-RUN --mount=type=cache,target=/home/${USERNAME}/.ccache \
-    . /opt/overlay_ws/install/setup.sh && \
-    colcon build
 
 # Set up the user's .bashrc file and shell.
 CMD ["/usr/bin/bash"]

From 6facf496f900a8f1427f287752986cd7efc5ab7d Mon Sep 17 00:00:00 2001
From: Chance Cardona <41308677+chancecardona@users.noreply.github.com>
Date: Fri, 18 Oct 2024 19:13:08 -0600
Subject: [PATCH 13/14] Update Submodules (#390)

* Update picknik_accessories, picknik_registration, ros2_robotiq_gripper

* point robotiq to fixed humble
---
 src/external_dependencies/picknik_accessories  | 2 +-
 src/external_dependencies/picknik_registration | 2 +-
 src/external_dependencies/ros2_robotiq_gripper | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/external_dependencies/picknik_accessories b/src/external_dependencies/picknik_accessories
index fcdb1ff8..c8f11ba1 160000
--- a/src/external_dependencies/picknik_accessories
+++ b/src/external_dependencies/picknik_accessories
@@ -1 +1 @@
-Subproject commit fcdb1ff8b729c93dbd5f15a09c33098b32caeac5
+Subproject commit c8f11ba132ff65ea41c1fe1f2623e0ed56fa16dd
diff --git a/src/external_dependencies/picknik_registration b/src/external_dependencies/picknik_registration
index 5ce93428..220e5c96 160000
--- a/src/external_dependencies/picknik_registration
+++ b/src/external_dependencies/picknik_registration
@@ -1 +1 @@
-Subproject commit 5ce934289fddcdf84a13fc6b569d10c50a7f5da1
+Subproject commit 220e5c96aac04bf4879b4106f16238fee2a46825
diff --git a/src/external_dependencies/ros2_robotiq_gripper b/src/external_dependencies/ros2_robotiq_gripper
index 93ecde7f..2ff85455 160000
--- a/src/external_dependencies/ros2_robotiq_gripper
+++ b/src/external_dependencies/ros2_robotiq_gripper
@@ -1 +1 @@
-Subproject commit 93ecde7f66499d56e620d775eb984345889532aa
+Subproject commit 2ff85455d4b9f973c4b0bab1ce95fb09367f0d26

From f4be1bf40a52c083fd39671c444fd3e6dea347db Mon Sep 17 00:00:00 2001
From: Chance Cardona <41308677+chancecardona@users.noreply.github.com>
Date: Fri, 25 Oct 2024 17:25:11 -0600
Subject: [PATCH 14/14] Clean some ARGS in Dockerfile (#391)

* Clean dockerfile args
---
 Dockerfile | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index 8006e4e0..5e25c508 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -6,10 +6,7 @@
 #
 
 # Specify the MoveIt Pro release to build on top of.
-ARG MOVEIT_STUDIO_BASE_IMAGE
-ARG USERNAME=studio-user
-ARG USER_UID=1000
-ARG USER_GID=1000
+ARG MOVEIT_STUDIO_BASE_IMAGE=main
 
 ##################################################
 # Starting from the specified MoveIt Pro release #
@@ -22,7 +19,6 @@ FROM ${MOVEIT_STUDIO_BASE_IMAGE} AS base
 ARG USERNAME
 ARG USER_UID
 ARG USER_GID
-
 # Copy source code from the workspace's ROS 2 packages to a workspace inside the container
 ARG USER_WS=/home/${USERNAME}/user_ws
 ENV USER_WS=${USER_WS}