Integration of Articutool System and Dockerized Workflow (Jose's Master's Thesis) (#219)

* Add 8-DOF baseline

* Update analysis script to allow for multiple baselines

* Use only Articutool joints during leveling for 8-DOF baseline

* Update LevelTool stage for 6-DOF baseline to simultaneously re-orient tool tip to level, and vertically offset from the plate

* Significantly tighten orientation tolerance for AbovePlate configuration

* Update LevelTool stage for 8-DOF baseline to simultaneously re-orient tool tip to level, and vertically offset from the plate

* Remove multiple attempts for AbovePlate motion

* Small fix

* Loosen roll constraint for path orientation constraint

* Loose position constraints on all motions, tighten S2 path orientation constraint, and increase granularity of Jaco EE pitch candidates

* Increase minimum height for food frame, and increase presentation offset distance

* Tighten inner and outer radius for resting pose

* Validate the start state to check for collisions before the resting stage

* Add minimum horizontal radius parameter for spherical sampling

* Push valid resting samples outwards radially

* Update min horizontal radius for resting to match inner radius

* Add ground plane

* Loosen position constraint for AbovePlate stage

* fix(benchmark): Resolve transport failures with dynamic path constraints

The 'Resting' stage was the primary bottleneck in the benchmark, with a
high rate of immediate planning failures. Diagnostics revealed this was
caused by using a static orientation path constraint with a fixed yaw.

If the robot's starting orientation had a different yaw, the error
calculation (when converted to Euler angles) would produce large,
non-intuitive pitch and roll errors, causing the start state to
violate the constraint and the planner to fail.

This commit resolves the issue by implementing a dynamic, yaw-aware path
constraint for the transport stages:

- Before planning, the robot's current end-effector orientation and yaw
  are calculated using Forward Kinematics.
- A new target orientation for the path constraint is constructed on the
  fly. This new target is perfectly level (zero pitch/roll) but has the
  *exact same yaw* as the robot's starting pose.
- This dynamic constraint ensures the start state is always valid from
  the planner's perspective, eliminating the immediate failures.

Additionally, this commit adds a `compute_moveit_orientation_error`
helper function to provide detailed logging and analysis of constraint
satisfaction, which was instrumental in diagnosing this issue.

* Major refactor to abstract away functionality from the monolithic benchmark script

* Add dynamic path constraint to staging

* Remove pre-posture analysis

* Update analysis script to give failure mode breakdown by stage

* Enable logging directly to file

* Enable visualizing an entire trial in sequence

* Enable recording and image capture

* Resolve ambiguity in analytical IK for Articutool which otherwise would choose a solution where the tool tip was pointing up or down, instead of just up

* Generate a natural-looking mouth pose with a tangential orientation to the robot base

* Restrict key landmark generation to semantically meaningful regions

* Deterministically force mouth frame to be oriented towards food frame

* Analyze end pose for Staging relative to dynamically computed path orientation constraint

* Tighten path orientation constraint tolerance

* feat(benchmark): Use SLERP for Staging path constraint

The `Resting -> Staging` plan frequently failed because the orientation
path constraint was biased towards either the start or goal pose. This
created a reference frame mismatch that made one of the poses appear to
violate the constraint's pitch/roll tolerances.

This commit resolves the issue by using Spherical Linear Interpolation
(SLERP) to compute a midpoint orientation between the start and goal
quaternions. This average orientation is then used as the reference
for the path constraint, ensuring both the start and end of the
trajectory are valid with respect to the constraint.

* Loosen theta range for resting and mouth poses, keeping the food pose theta range tight for semantic relation to the home configuration

* Simplify Presentation stage

* Fix elbow-up configuration check

* Add log to verify time parameterization

* Add analytical Jacobian method

* Add constant for Articutool max velocity

* Implement dynamic feasibility check

* Add plot for induced Articutool joint velocities relative to threshold for time parameterized trajectories

* Small fix

* Add mode to experiment directory name

* Dynamically compute orientation for LevelTool Cartesian motion for 8-DOF baseline

* Constrain IK solutions to elbow-up configurations for 8-DOF baseline

* Use Articutool's solver to find ideal Jaco EE pose for 8-DOF coupled system, then use IK and compose with Articutool configuration to plan full 8-DOF joint space motion to AboveFood configuration

* Increase extraction height for 8-DOF baseline

* Use SLERP method for dynamic path constraints for 8-DOF baseline during Resting motion

* Update Staging stage for 8-DOF baseline to use dynamic orientation constraint with SLERP

* Update LevelTool stage for 6-DOF baseline

* Update Resting stage for 6-DOF baseline with dynamic orientation constraints via SLERP

* Update Staging stage for 6-DOF baseline to use dynamic orientation constraints via SLERP

* Remove time parameterization log message

* Update baseline path orientation constraint constant

* Characterize food pose for analysis

* Add analysis of 6-DOF baseline reachability

* Try to improve success rate for LevelTool for 6-DOF baseline, this one really doesn't want to work often

* Significantly tighten pitch and roll tolerances for tool leveling for 6-DOF and 8-DOF coupled baselines. This tolerance is probably good enough for granular foods and noodles, but not for liquids.

* Retry scene generation if key poses are unreachable

* Add recovery state for 6-DOF baseline if LevelTool fails which finds valid IK solution in level configuration so that downstream tasks can still be tested

* Increase max planning time to 60s

* Move benchmark code

* Integrate benchmark strategy for AboveFood and InFood motions into BT implementation

* Add logs for tilt angles

* Fix sign issue when generating candidate tilt angles

* Adjust planning parameters

* WIP Skewer + Twirl action for noodles

* Update to adhere to convention where 0 deg corresponds to the Jaco EE horizontal and looking at food, and 90 deg corresponds to the Jaco EE looking down at food

* Update conditional rotation of food frame to face towards robot base instead of away

* Add behavior to publish pose to TF tree

* Publish AboveFood and InFood poses to TF tree

* fix(acquisition): Align skewer pose derivation with benchmark logic

Reworks the `CalculateSkewerPoseForTilt` behavior to correctly
implement the geometric logic from the benchmark's
`find_optimal_skewer_config`.

This fixes the persistent bug where the skewer angle was incorrectly
calculated as 90 degrees by ensuring the angle is measured relative
to the food frame and that the Jaco EE poses are derived
geometrically from the desired tool tip pose.

* Implement PreMoveInto primitive, and use it for the Skewer + Twirl action

* Loose position constraints and adjust Cartesian parameters for InFood motion

* Add scooping action with 45 deg polar tilt

* Add action for 70 deg polar skewer angle scoop

* Disable collision objects (body, table, head) by default, and conditionally disable face update stuff

* Temporarily disable in front of face wall for all motions

* Add experimental configurations for staging

* Remove artifacts

* Update and integrate dynamic feasiblity check (that also does kinematic check)

* Add SLERP-based dynamic orientation constraint behavior, and integrate into motion to Resting configuration

* Add dynamic path constraints to Staging motion

* Increase allowed planning time and increase velocity of Articutool joint trajectories

* Add SLERP midpoint computing behavior

* Increase twirl speed

* Fix timing issues

* Increase planning time for Staging in config file

* Actually increase Staging planning time, I think I updated the Resting planning time before instead

* Conditionally home Articutool after AbovePlate motion

* Add conditional simplified Presentation motion for experiments, defaults to original face detection-based approach

* Add post-presentation Articutool primitive to experimental scenes to dump contents of tool

* Modify MoveFromMouth tree conditionally for simplified Cartesian motion that is the opposite of the MoveToMouth motion

* Add plotting for end-to-end success and stage survivability

* Add pre-verification to check for complete scene reachability before starting trials, re-generating the scene if it is found to be unreachable

* Remove old scene reachability check

* Refactor benchmark to remove PreAcquisition stage

* Add plots for transport success rate and dynamic feasibility

* Adjust Articutool actions

* Lower pre_offset for Articutool actions

* Tell Cartesian motions in the benchmark to avoid collisions

* Consolidate planning times for Articutool system that uses orientation path constraints for key leveling transport motions. Notably, setting the planning time too high for RRT* is a bad idea since it's an optimization based planner that will keep trying to iterate on the solution with the allowed planning time instead of returning as soon as a solution is found.

* Add flag to disable table detection

* Add benchmark type that only tests transport motions

* Add plots for joint travel and planning time

* Add 95% CIs to plots

* Improve color scheme and organization of plots

* Enhance stage-by-stage results with finer granularity in statistics

* Increase extract torque for 45 deg scooping action

* Set default namespace to demo

* Increase Staging planning time to 4 seconds

* Add elbow-up config check

* Adjust Home configuration for slightly further table placement

* Tune 45 deg scooping motion

* Tune planning times, parameters for MoveInto motion, and temporarily disable recovery tree

* Update gitignore

* Removing old junk

* Remove Articutool benchmark project from within ada_feeding, I'll find a new place for it eventually

* Remove dependencies on Articutool benchmark code

* Remove old logic to conditionally rotate the food frame, which has been abstracted to a separate behavior

* Restore original ComputeFoodFrame

* Revert to old logging in MoveIt2Constraints behavior

* Remove old benchmark analysis script

* Increase Staging allowed planning time

* Re-enable face, wheelchair, and body collision objects, as well as face detection updates

* Restore wheelchair wall spawning

* Adjust Staging configuration to account for length of Articutool

* Significantly increase on-preempt timeout to account for Articutool overhead

* Improve move to mouth logic to no longer use tool_tip frame, instead opting to servo towards a goal pose for the Jaco EE offset from the mouth frame to achieve the desired tool tip pose. Also, adjust logic to compute mouth frame so that its xy-plane is always level with the world xy-plane, and the x-axis looks towards the Jaco EE frame

* User simple retreat for MoveFromMouth tree

* Restrict valid skewer tilt angles

* Tune 45 deg scoop

* Implement retract twist to re-center tool tip over bowl post-acquisition

* Also update 60 deg scoop with same tuned parameters as 45 deg scoop

* Update group name to jaco_arm

* Reduce retraction distance

* Add move_above_dist_m to acquisition schema

* Add scooping action for liquids

* Remove old Articutool actions

* Add lock_joints parameter

* Add lock_joints parameter to key behavior trees that will use modified logic for Articutool system v.s. 6-DOF baseline with locked Articutool joints

* Add behavior to compute a level pose from an input pose (gravity-level)

* Modify ComputeSlerpMidpointOrientation behavior to take path constraint tolerance as input

* Conditionall define joint names for jaco_arm_with_articutool group depending on whether lock_joints is set

* Modify Resting configuration to result in a level tool tip regardless of whether the Articutool joints are locked or not

* Major refactor to enable two modes: 1. the original Articutool sequences for feeding, and 2. a 6-DOF baseline that locks the Articutool joints

* Change planning group for 6-DOF baseline

* Conditionally modify allowed planning time for Resting and Staging motions based on whether the joints are locked

* Tune Articutool actions

* Move Post-MoveInto primitive to execute after grasp and extract twists (otherwise things like twirl grind into the plate). We should probably rename this also, maybe to Post-Extract primitive

* Tune Skewer+Twirl action

* Move PostMoveInto primitive to after grasp, but before extract twist

* Further tuning of Skewer+Twirl action

* Add target_link parameter to planning

* Add path orientation constraint for 6-DOF leveling motion

* Integrate all 5 experimental staging configurations

* Reduce Cartesian fraction threshold for 6-DOF baseline MoveInto motion and remove joint path length threshold

* Tune skewer+twirl action linear retraction

* Add 6-DOF baseline scoop and level action. This action executes a twist that roughly gets the tool to a level orientation, but it's followed up by a small joint space motion to achieve true level.

* Add jaco_ee_tilt_angle_min and max to AcquisitionSchema so that each action can customize this as needed. For example, the Skewer+Twirl action restricts the angle range more than the default

* Missed this file in last commit

* Update MoveToResting goal configuration

* Increase default planning time for MoveToStagingConfiguration to 6 sec

* Update planning groups for 6-DOF baseline. Also increase allowed planning time for Resting and Staging to 12 seconds

* Limit Jaco EE tilt angle for liquid scoop

* Propagated lock_joints argument to Articutool stack, and fix bug where comma was preventing lock_joints parameter to propagate to the ADA MoveIt2 stack

* Adjust 6-DOF baseline scoop extract twist

* Set Resting and Staging planning time to max of Articutool planning time from simulation benchmark

* Restore in front of face wall

* Increase 6-DOF baseline pitch/roll tolerance to 20 deg

* Revert 6-DOF baseline leveling motion to Cartesian using Jaco EE frame

* Add tilted scoop action for 6-DOF baseline

* Tune planning times for seamless end-to-end feeding

* Tune Articutool liquid scooping action

* Remove conditional change in staging planning time

* Adjust 6-DOF baseline post-leveling motion to use joint-space motion with refined constraints to successfully plan consistently without spilling

* Update 6-DOF baseline granular scoop action

* Disable leveling motion for 6-DOF baseline, we instead rely on the Cartesian extraction motion

* Tune 6-DOF baseline granular scoop

* Tune 6-DOF liquid scoop

* Update cartesian fraction threshold for MoveInto for 6-DOF baseline

* Tune 6-DOF baseline liquid scoop

* Add 6-DOF baseline noodle scooping action

* Add NOODLE_SHED primitive to Articutool noodle action

* Increase retraction distance for Articutool noodle action

* Set Resting planning time to 6 seconds

* Replace NOODLE_SHED post-acquisition primitive with simple CCW Twirl for Articutool noodle action

* Tune noodle+twirl action

* Replace joint space plan to home Articutool with HOMING primitive

* Set allowed planning time for Articutool leveling to 0.1 sec

* Tune noodle skewer + twirl

* Tune noodle skewer+twirl

* Tune Articutool granule scoop

* Remove comment

* Use jaco_arm move group for collisions

* Cleanup comments

* Expands Python requirements

* Let pip handle versions for basic scientific libraries

* Specify correction versions for Python dependencies

* Add Dockerfile to containerize the ADA + Articutool stack

* Significantly reduce the allowed planning time for motions to Resting and Staging

* Update README for added details on running system via Docker

* Adjust Resting and Staging planning times (maybe for the last time??)

* Fix bug where AboveFood pose orientation was not achieved, also adjust acquisition library

* Remove debug logs

* Restore comment

* Restore whitespace

* Comment cleanup

* Remove MODE_FULL_ORIENTATION from CallSetOrientationControl behavior

* Remove all traces of the lock_joints parameter

* Remove unused behaviors

* Remove 6-DOF baseline acquisition strategies

* Remove comments

* Update comment

* Update comment

* Remove comment

* Remove face detection flag

* Major refactor to consolidate analytic kinematic model

* Use abstracted kinematic model instead of hard-coding it

* Pre-load Pinocchio model instead of instantiating it during runtime

* Reduce number of waypoints checked during dynamic feasibility check

* Add license

* Remove LoadPinocchioModel behavior

* Adjust retry loop to prioritize higher planning success rate

* Use median-out strategy for iterating through candidate tilt angles

* Reduce width of in_front_of_face_wall for seated planning scene

* Tune acquisition strategies

* Update README with more thorough instructions for Docker development environment

* Pre-trust SSH hosts in Dockerfile

* Add SSH hosts in Docker run

* Fix typo in README

* Update requirements

* Pre-bake public keys for nano and babbage

* Update requirements

* Revert EfficientSAM source to specific commit

* Downgrade scikit-image

* Cast faces to uint32 to satisfy ROS 2 shape_msgs requirements

* Lock ROS 2 Humble base image version

* refactor(trees): Modularize AcquireFoodTree using acquisition idioms

Extracted monolithic internal sequences from `AcquireFoodTree` into reusable idioms in `ada_feeding/idioms/acquisition.py`. This refactor aligns the code structure with the "High-Level Task Orchestration" architecture.

Key Changes:
- **Plan-Then-Verify Abstraction**: Moved the robust transport planning logic into `get_robust_move_above_sequence`, encapsulating the IK and dynamic feasibility checks.
- **Articutool Primitives**: Abstracted localized primitive execution into `get_articutool_move_into_sequence` and `get_post_retract_primitive_sequence`.
- **Active Leveling**: Moved the post-acquisition leveling logic (FK -> IK -> Plan) into `get_post_acquisition_leveling_sequence`.
- **Tree Orchestration**: Cleaned up `acquire_food_tree.py` to function as a high-level orchestrator, improving readability and maintainability.

* Add missing acquisition idioms

* Revert formatting in servo_until

* Revert formatting changes

* Don't skip ada_hardware during build in Docker image

* Set Docker image ROS_DOMAIN_ID to 42

* Tune liquid scoop

* Update start.py to work with new Articutool Docker image, and implement an optional time sync property

* Restrict joint 1 max path length

* Tweak action

* Restore start.py commands for Lovelace configuration

* Run pre-commit

Author: jjaime2

.dockerignore

@@ -0,0 +1,30 @@
+# Git directory
+.git/
+
+# Local Python virtual environments
+.venv/
+.ada_venv/
+
+# Colcon build/install artifacts
+# These will be generated INSIDE the container,
+# so we never want to copy the local ones.
+build/
+install/
+log/
+
+# Compiled Python files and caches
+**/*.pyc
+**/__pycache__/
+
+# Local IDE and editor config
+.vscode/
+.idea/
+
+# OS-specific metadata
+.DS_Store
+Thumbs.db
+
+# Project-specific data and artifacts to ignore
+data/
+screenshots/
+*.orig

.gitignore

@@ -66,3 +66,39 @@ trees/debug.xml
 ada_feeding_perception/test/food_img/
 ada_feeding_perception/test/output/
 ada_feeding_action_select/data
+
+# Debug/scratch tree
+trees/debug.xml
+
+# ===============================================================
+# Python Artifacts (Consolidated and Added)
+# ===============================================================
+__pycache__/
+*.pyc
+.ada_venv/
+venv/
+.venv/
+env/
+
+# ===============================================================
+# Project-Specific Data, Plots, and Benchmark Outputs (Added)
+# ===============================================================
+# Ignore generated data, plots, and saved trajectories
+ada_feeding/data/
+
+# Ignore all benchmark and analysis output directories
+ada_feeding/scripts/benchmark_analysis_plots/
+ada_feeding/scripts/e2e_benchmark_output/
+ada_feeding/scripts/holistic_benchmark_output/
+ada_feeding/scripts/manifold_data/
+ada_feeding/scripts/visualization_output/
+ada_feeding/ada_feeding/articutool_benchmark/*.html
+ada_feeding/ada_feeding/articutool_benchmark/*.pdf
+ada_feeding/ada_feeding/articutool_benchmark/results/
+ada_feeding/ada_feeding/articutool_benchmark/visualization_output/
+
+# Ignore specific generated files in the scripts directory
+ada_feeding/scripts/camera_poses.json
+
+# Ignore screenshots directory at the root
+screenshots/

Dockerfile

@@ -0,0 +1,177 @@
+# 1. Start from the official ROS 2 Humble base image
+FROM ros:humble-ros-base@sha256:dfc94c85d8e01f230951b2a85ca5576e08473081c3da5fbbe8f3ab844703b9ba
+
+# Add a default rosinstall file argument
+ARG ROSINSTALL_FILE=articutool.https.rosinstall
+
+# Set non-interactive mode for apt to prevent it from hanging
+ENV DEBIAN_FRONTEND=noninteractive
+
+# Install core tools
+RUN apt-get update && apt-get install -y \
+    git \
+    python3-wstool \
+    python3-rosdep \
+    unzip \
+    curl \
+    lsb-release \
+    gnupg2 \
+    python3 \
+    python3-pip \
+    python3-venv \
+    net-tools \
+    iputils-ping \
+    vim \
+    git \
+    build-essential \
+    cmake \
+    python3-dev \
+    libboost-all-dev \
+    libeigen3-dev \
+    portaudio19-dev \
+    tzdata \
+    usbutils \
+    sudo \
+    screen \
+    python3-colcon-common-extensions \
+    python3-colcon-mixin \
+    python3-rosdep \
+    python3-vcstool \
+    ros-humble-ros2-control \
+    ros-humble-ros2-controllers \
+    ros-humble-controller-manager \
+    ros-humble-joint-state-publisher \
+    ros-humble-joint-trajectory-controller \
+    ros-humble-diff-drive-controller \
+    ros-humble-xacro \
+    ros-humble-joint-state-publisher-gui \
+    ros-humble-dynamixel-sdk \
+    ros-humble-dynamixel-workbench-toolbox \
+    ros-humble-pinocchio \
+    ros-humble-tf-transformations \
+    ros-humble-imu-tools \
+    ros-humble-rmw-cyclonedds-cpp
+
+RUN useradd -m -s /bin/bash ros && \
+    echo "ros:ros" | chpasswd && \
+    adduser ros sudo
+
+# Give the new 'ros' user passwordless sudo
+RUN echo "ros ALL=(ALL) NOPASSWD: ALL" >> /etc/sudoers
+
+# Install ssh client tools
+USER root
+RUN apt-get update && apt-get install -y ssh-client
+USER ros
+
+# --- Pre-trust SSH hosts ---
+# We "pre-bake" the known public keys for the lab machines to avoid
+# a network dependency during the build.
+RUN mkdir -p $HOME/.ssh && \
+    touch $HOME/.ssh/known_hosts && \
+    chmod 700 $HOME/.ssh && \
+    \
+    # Keys for babbage,192.168.4.50
+    echo "|1|49Ea8J/IKuDiiPlq0KhUdb3xYlg=|wK+sSomPzi+Had5r5MxHXoCODdw= ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABgQDwLNAnsiJA6IaQQ2owzltW1DbVKlCbXn48UupSPx05qyaNJtc/zczW5Zsxp8JJhRZdzlnOkjDwinoRXRHLkbnmilyLjkS3GjMZIIonSRiTJEF1Fhb291JO83o0JQTPajh7OZBKQvTO/1/oLEl1KcggDueBPMyjKnG6LWDcLKqQ1diTKCKGoQpEmG01A/rWVXI+5guyMGYyXGj+pkY/C+bYBWVVQ1RdLjkQxbe8z2BQ1H3AWA2i8Eqd/lGfYQ+1YoApqh5zxJ0hadxK/zlyZb72SwzU/yTqTMdt87A9nbqU6lo0oja6KvNZLvBqYf3Cb/Eja+7mKa3jUtxXIleN/HeVW/fj+PcXBs66+lrhO58DVV4YwIf1/T30cT0SYFNQq+XrMf9ETWUYFpAM7MfPE/3Hvh1UyMUL73QfgQ8DuGQIusqxciSGqd+RAK5KX06+bsHKVYQ4+X/m7etSgFOZ68gDBOFX9NDj5SwUc4xCVqkg9O+U5RXLnsM6hPA983nWofc=" >> $HOME/.ssh/known_hosts && \
+    echo "|1|YLqSxQZzlSg4MzjgVxDiSKn6kdU=|pT73D0e781iwFr2QvbDUWs8qGo8= ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABgQDwLNAnsiJA6IaQQ2owzltW1DbVKlCbXn48UupSPx05qyaNJtc/zczW5Zsxp8JJhRZdzlnOkjDwinoRXRHLkbnmilyLjkS3GjMZIIonSRiTJEF1Fhb291JO83o0JQTPajh7OZBKQvTO/1/oLEl1KcggDueBPMyjKnG6LWDcLKqQ1diTKCKGoQpEmG01A/rWVXI+5guyMGYyXGj+pkY/C+bYBWVVQ1RdLjkQxbe8z2BQ1H3AWA2i8Eqd/lGfYQ+1YoApqh5zxJ0hadxK/zlyZb72SwzU/yTqTMdt87A9nbqU6lo0oja6KvNZLvBqYf3Cb/Eja+7mKa3jUtxXIleN/HeVW/fj+PcXBs66+lrhO58DVV4YwIf1/T30cT0SYFNQq+XrMf9ETWUYFpAM7MfPE/3Hvh1UyMUL73QfgQ8DuGQIusqxciSGqd+RAK5KX06+bsHKVYQ4+X/m7etSgFOZ68gDBOFX9NDj5SwUc4xCVqkg9O+U5RXLnsM6hPA983nWofc=" >> $HOME/.ssh/known_hosts && \
+    echo "|1|zEtEQrjMQT3B6kPHgsb/6EZjHZY=|eCiGTvNte3FLFv0wDbxO8V+qCo4= ecdsa-sha2-nistp256 AAAAE2VjZHNhLXNoYTItbmlzdHAyNTYAAAAIbmlzdHAyNTYAAABBBFiunkK0t3pBUYfUun1pNmcdss7ZNWP4WLIKpL3CQv2LdylcM6srKeVkiXS7GMvnpgacmXmuNDJhgltj1VjlpRs=" >> $HOME/.ssh/known_hosts && \
+    echo "|1|gon+l4h21AEk1xTMb/r51fUERjY=|4LeKTthihgoFxV7zxy19Cuwfi2I= ecdsa-sha2-nistp256 AAAAE2VjZHNhLXNoYTItbmlzdHAyNTYAAAAIbmlzdHAyNTYAAABBBFiunkK0t3pBUYfUun1pNmcdss7ZNWP4WLIKpL3CQv2LdylcM6srKeVkiXS7GMvnpgacmXmuNDJhgltj1VjlpRs=" >> $HOME/.ssh/known_hosts && \
+    echo "|1|LM0LKILGN+VlrcQzEXM9ogYX3tg=|N8qmNqTaZf6f4zbEMH/VwPY9sFQ= ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAIF5kaTGWJITm9CM75x5++mNQyBM1Hxiabs8ucTanMBKa" >> $HOME/.ssh/known_hosts && \
+    echo "|1|PEAm29WqwueLOvWxnt/CCk1HyJU=|Hec3gUZ7WVqiJxobmdcPaMeihrU= ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAIF5kaTGWJITm9CM75x5++mNQyBM1Hxiabs8ucTanMBKa" >> $HOME/.ssh/known_hosts && \
+    \
+    # Keys for nano,192.168.4.4
+    echo "|1|2Y0YXbnrNBg5E2fAPvSA7CPbdBM=|XqLcG4rQNf22WSiKhAR0siC3Zd0= ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAIMs4PtLKyG0UQY2GygkmQtTZGjqB4uvmZIRO0v9Ah34g" >> $HOME/.ssh/known_hosts && \
+    echo "|1|bMR2xxJknX2KHFckxwyLc2IQdQE=|wdKgKePqGQ3BZ8sdsq0xdyZNFNg= ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAIMs4PtLKyG0UQY2GygkmQtTZGjqB4uvmZIRO0v9Ah34g" >> $HOME/.ssh/known_hosts && \
+    echo "|1|q21hHbzafG7fraNHiEsb+uPyFyc=|nv3h7kDtfjzbK/rkpLIAo72vqHo= ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQDeN41SxRLDs6QnbyiSYTBIiCr6D9ihcfXdHeA7sMchCMiCUnTM6yVj2xMdIdVIk9oh+PNh5HN/Z4S98rJHxgRIADcjPQ4M19UK2O6fMVdn8g1GvdrMTsMAo+SAmY8fMjj9XWNR+JR1mru9+Sf1EgJ/yzzj2Z+C+8brNJFYky89JblqDnYzeD/XVvzc9E78b8Qnro55+0A/0ms0QAulbACwlBBUNE/o+G7+caV024Q4gx7VjQALwrc1qxWl1/Ekh7c64yHfO7caC3cpHuvICn5COBKUc0TIEL2wmXiNEtP2quKFr/1ZreIWHjLDCRBPsNZRwTli9N+GeWP+ZP9iab9n" >> $HOME/.ssh/known_hosts && \
+    echo "|1|C+TkorkKWD5Mk5+ZHAcpE6XFafI=|KxsqH0jhdRmXsPphRKEF9DlwI6o= ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQDeN41SxRLDs6QnbyiSYTBIiCr6D9ihcfXdHeA7sMchCMiCUnTM6yVj2xMdIdVIk9oh+PNh5HN/Z4S98rJHxgRIADcjPQ4M19UK2O6fMVdn8g1GvdrMTsMAo+SAmY8fMjj9XWNR+JR1mru9+Sf1EgJ/yzzj2Z+C+8brNJFYky89JblqDnYzeD/XVvzc9E78b8Qnro55+0A/0ms0QAulbACwlBBUNE/o+G7+caV024Q4gx7VjQALwrc1qxWl1/Ekh7c64yHfO7caC3cpHuvICn5COBKUc0TIEL2wmXiNEtP2quKFr/1ZreIWHjLDCRBPsNZRwTli9N+GeWP+ZP9iab9n" >> $HOME/.ssh/known_hosts && \
+    echo "|1|hzTc2C/DNzv5lhwhPvOkqJ8D/k8=|C2EmuGI97pgtXat1AmzxBM1WB+8= ecdsa-sha2-nistp256 AAAAE2VjZHNhLXNoYTItbmlzdHAyNTYAAAAIbmlzdHAyNTYAAABBBHG0KGLLjvhCsPWAowtNapo+H49ORNvksA5/T4jPEvkWB77SPHnJAcTlGm0p3gi2nnFsRSpnpRDja9+dM3WYg+Y=" >> $HOME/.ssh/known_hosts && \
+    echo "|1|Hf4F9psUIaNVLIdK9x6c3BRi9zM=|h9ekiLF6+E9Ygo7Z6Wz/jbNHDBY= ecdsa-sha2-nistp256 AAAAE2VjZHNhLXNoYTItbmlzdHAyNTYAAAAIbmlzdHAyNTYAAABBBHG0KGLLjvhCsPWAowtNapo+H49ORNvksA5/T4jPEvkWB77SPHnJAcTlGm0p3gi2nnFsRSpnpRDja9+dM3WYg+Y=" >> $HOME/.ssh/known_hosts && \
+    \
+    # Set final permissions
+    chmod 600 $HOME/.ssh/known_hosts
+
+# Switch back to the ros user for the rest of the build
+USER ros
+ENV HOME=/home/ros
+WORKDIR $HOME
+
+# 4. Configure pr-rosinstalls
+RUN git clone --branch jjaime2/articutool-ros-install https://github.com/personalrobotics/pr-rosinstalls.git $HOME/pr-rosinstalls
+RUN mkdir -p $HOME/colcon_ws/src
+
+# 5. WORKDIR
+WORKDIR $HOME/colcon_ws/src
+
+# 6. wstool init
+RUN wstool init
+
+# 7. wstool merge
+RUN wstool merge $HOME/pr-rosinstalls/$ROSINSTALL_FILE
+
+# 8. wstool up
+RUN wstool up
+
+# 9. WORKDIR
+WORKDIR $HOME/colcon_ws
+
+# 10. rosdep update
+RUN rosdep update
+
+# 11. rosdep install
+RUN rosdep install --from-paths src -y --ignore-src --as-root=pip:false
+
+# 12. COPY Kinova SDK
+COPY kinova_sdk.zip /tmp/
+
+# 13. Install Kinova SDK
+RUN cd /tmp && \
+    unzip kinova_sdk.zip && \
+    sudo dpkg -i "Ubuntu/16_04/64 bits/KinovaAPI-6.1.0-amd64.deb" && \
+    sudo rm -rf /tmp/*
+
+# 14. Setup NVM
+ENV NVM_DIR=$HOME/.nvm
+ENV NODE_VERSION=21
+RUN curl -o- https://raw.githubusercontent.com/nvm-sh/nvm/v0.39.7/install.sh | bash
+ENV PATH="$NVM_DIR/versions/node/v$NODE_VERSION/bin:$PATH"
+
+# 15. Install global npm packages
+RUN . "$NVM_DIR/nvm.sh" && npm install -g serve pm2@latest
+
+# 16. Install pip requirements
+# This COPY command makes this step cache-aware.
+# If requirements.txt changes, this step and all following steps will re-run.
+COPY requirements.txt /tmp/requirements.txt
+RUN pip install -r /tmp/requirements.txt
+RUN pip install transforms3d -U
+RUN pip uninstall -y matplotlib
+
+# 17. Install web app dependencies
+WORKDIR $HOME/colcon_ws/src/feeding_web_interface/feedingwebapp
+RUN . "$NVM_DIR/nvm.sh" && npm install --legacy-peer-deps
+RUN . "$NVM_DIR/nvm.sh" && npx playwright install
+
+# 18. Build workspace
+WORKDIR $HOME/colcon_ws
+RUN . /opt/ros/humble/setup.sh && \
+    . "$NVM_DIR/nvm.sh" && \
+    colcon build --symlink-install
+
+# Force 'screen' to always use bash for interactive shells
+RUN echo "shell /bin/bash" > /home/ros/.screenrc
+
+RUN mkdir -p $HOME/colcon_ws/install/ada_feeding_action_select/share/ada_feeding_action_select/data/checkpoint/adapter
+# 19. Configure Environment
+ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+ENV CYCLONEDDS_URI=$HOME/colcon_ws/src/ada_feeding/cyclonedds.xml
+ENV ROS_DOMAIN_ID=42
+
+# 20. Final Configuration
+# We copy the entrypoint script and set permissions
+USER root
+COPY docker-entrypoint.sh /
+RUN chmod +x /docker-entrypoint.sh
+USER ros
+ENTRYPOINT ["/docker-entrypoint.sh"]
+
+CMD ["bash"]