diff --git a/ros2_debian/Dockerfile.debian12 b/ros2_debian/Dockerfile.debian12
index 1093da1..48e5d3d 100644
--- a/ros2_debian/Dockerfile.debian12
+++ b/ros2_debian/Dockerfile.debian12
@@ -24,6 +24,7 @@ RUN apt-get update -y -qq && \
   automake \
   build-essential \
   cmake \
+  clang \
   curl \
   dnsutils \
   git \
@@ -31,6 +32,7 @@ RUN apt-get update -y -qq && \
   intltool \
   libacl1-dev \
   libasio-dev \
+  libassimp-dev \
   libblosc1 \
   libbondcpp-dev \
   libcap-dev \
@@ -38,6 +40,7 @@ RUN apt-get update -y -qq && \
   libeigen3-dev \
   libfmt-dev \
   liblttng-ust-dev lttng-tools python3-lttng \
+  liboctomap-dev \
   libssl-dev \
   libtinyxml-dev \
   libtinyxml2-dev \
@@ -154,15 +157,23 @@ RUN vcs import --input https://raw.githubusercontent.com/ros2/ros2/$ROS_DISTRO/r
 RUN \
   : "build ROS core from source" && \
   . "$HOME/.cargo/env" && \
-  colcon  build \
+  colcon build \
   --mixin release build-testing-off \
   --cmake-args --no-warn-unused-cli -DCMAKE_CXX_FLAGS="-Wno-maybe-uninitialized"  \
   --packages-up-to robot_state_publisher tf2_ros tf2_eigen tf2_kdl tf2_eigen_kdl yaml_cpp_vendor filters \
   ros2param ros2interface ros2topic ros2action ros2lifecycle ros2launch ros2run ros_testing \
   xacro diagnostic_updater generate_parameter_library angles example_interfaces \
-  backward_ros pal_statistics topic_tools \
+  backward_ros pal_statistics topic_tools ros_environment \
   ackermann_msgs trajectory_msgs tf2_msgs tf2_geometry_msgs sensor_msgs geometry_msgs nav_msgs \
   sdformat_urdf && \
+  # build pinocchio from source with different cmake args
+  . install/setup.sh && \
+  colcon build \
+    --mixin release build-testing-off \
+    --cmake-args --no-warn-unused-cli -DBUILD_EXAMPLES=OFF -DBUILD_PYTHON_INTERFACE=OFF \
+      -DCOAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL=TRUE \
+    --packages-select coal pinocchio && \
+  # cleanup
   rm -rf log src build
 
 # add default.yaml to the image
diff --git a/ros2_debian/ros-controls.jazzy.repos b/ros2_debian/ros-controls.jazzy.repos
index d1c7513..78a18ad 100644
--- a/ros2_debian/ros-controls.jazzy.repos
+++ b/ros2_debian/ros-controls.jazzy.repos
@@ -67,6 +67,14 @@ repositories:
     type: git
     url: https://github.com/ros2-gbp/pal_statistics-release.git
     version: debian/jazzy/bookworm/pal_statistics_msgs
+  pinocchio:
+    type: git
+    url: https://github.com/christophfroehlich/pinocchio.git
+    version: patch-1
+  coal:
+    type: git
+    url: https://github.com/ros2-gbp/coal-release.git
+    version: debian/jazzy/bookworm/coal
   topic_tools:
     type: git
     url: https://github.com/ros2-gbp/topic_tools-release.git
diff --git a/ros2_debian/ros-controls.kilted.repos b/ros2_debian/ros-controls.kilted.repos
index 8927e2a..d62b5e1 100644
--- a/ros2_debian/ros-controls.kilted.repos
+++ b/ros2_debian/ros-controls.kilted.repos
@@ -67,6 +67,14 @@ repositories:
     type: git
     url: https://github.com/ros2-gbp/pal_statistics-release.git
     version: debian/kilted/bookworm/pal_statistics_msgs
+  pinocchio:
+    type: git
+    url: https://github.com/christophfroehlich/pinocchio.git
+    version: patch-1
+  coal:
+    type: git
+    url: https://github.com/ros2-gbp/coal-release.git
+    version: debian/kilted/bookworm/coal
   topic_tools:
     type: git
     url: https://github.com/ros2-gbp/topic_tools-release.git
diff --git a/ros2_debian/ros-controls.rolling.repos b/ros2_debian/ros-controls.rolling.repos
index cc9a9cb..d20b5b9 100644
--- a/ros2_debian/ros-controls.rolling.repos
+++ b/ros2_debian/ros-controls.rolling.repos
@@ -67,6 +67,14 @@ repositories:
     type: git
     url: https://github.com/ros2-gbp/pal_statistics-release.git
     version: debian/rolling/bookworm/pal_statistics_msgs
+  pinocchio:
+    type: git
+    url: https://github.com/christophfroehlich/pinocchio.git
+    version: patch-1
+  coal:
+    type: git
+    url: https://github.com/ros2-gbp/coal-release.git
+    version: debian/rolling/bookworm/coal
   topic_tools:
     type: git
     url: https://github.com/ros2-gbp/topic_tools-release.git
diff --git a/ros2_rhel/Dockerfile.rhel9 b/ros2_rhel/Dockerfile.rhel9
index b2f44a6..aced391 100644
--- a/ros2_rhel/Dockerfile.rhel9
+++ b/ros2_rhel/Dockerfile.rhel9
@@ -83,7 +83,13 @@ RUN vcs import src < ros-controls.$ROS_DISTRO.repos && \
   rosdep install -iyr --from-path src && \
   colcon build \
   --mixin release build-testing-off \
-  --cmake-args --no-warn-unused-cli && \
+  --cmake-args --no-warn-unused-cli --packages-skip pinocchio && \
+  # build pinocchio from source
+  colcon build \
+    --mixin release build-testing-off \
+    --cmake-args --no-warn-unused-cli -DBUILD_EXAMPLES=OFF -DBUILD_PYTHON_INTERFACE=OFF \
+    --packages-select pinocchio && \
+  # cleanup
   rm -rf log src build
 
 # add default.yaml to the image
diff --git a/ros2_rhel/ros-controls.kilted.repos b/ros2_rhel/ros-controls.kilted.repos
index 56f46b6..01390fc 100644
--- a/ros2_rhel/ros-controls.kilted.repos
+++ b/ros2_rhel/ros-controls.kilted.repos
@@ -1 +1,5 @@
 repositories:
+  pinocchio:
+    type: git
+    url: https://github.com/ros2-gbp/pinocchio-release.git
+    version: rpm/kilted/9/pinocchio
diff --git a/ros2_rhel/ros-controls.rolling.repos b/ros2_rhel/ros-controls.rolling.repos
index 56f46b6..1b566ab 100644
--- a/ros2_rhel/ros-controls.rolling.repos
+++ b/ros2_rhel/ros-controls.rolling.repos
@@ -1 +1,5 @@
 repositories:
+  pinocchio:
+    type: git
+    url: https://github.com/ros2-gbp/pinocchio-release.git
+    version: rpm/rolling/9/pinocchio