diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
new file mode 100644
index 0000000..e658f0a
--- /dev/null
+++ b/.devcontainer/devcontainer.json
@@ -0,0 +1,68 @@
+{
+  "name": "Whole Body MPC Dev Container",
+  "initializeCommand": "xhost +local:",
+  "build": {
+    "dockerfile": "${localWorkspaceFolder}/docker/Dockerfile",
+    "context": "..",
+    "target": "base",
+    "args": {
+      "WB_HUMANOID_MPC_DIR": "/wb_humanoid_mpc_ws",
+      "PYTHON_VERSION": "3.12",
+      "USER_ID": "${localEnv:USER_ID}",
+      "GROUP_ID": "${localEnv:GROUP_ID}",
+      "GIT_USER_NAME": "${command:git config --global user.name}",
+      "GIT_USER_EMAIL": "${command:git config --global user.email}"
+    }
+  },
+  "runArgs": [
+    "--rm",
+    "-it",
+    "--net=host",
+    "--privileged",
+    "-u", "${localEnv:USER_ID}:${localEnv:GROUP_ID}",
+    "-e", "DISPLAY",
+    "-e", "QT_X11_NO_MITSHM=1",
+    "-e", "XDG_RUNTIME_DIR=${localEnv:XDG_RUNTIME_DIR}",
+    "-e",
+    "GIT_USER_NAME=$(git config --global user.name)",
+    "-e",
+    "GIT_USER_EMAIL=$(git config --global user.email)",
+    "-v", "/tmp/.X11-unix:/tmp/.X11-unix",
+    "-v", "${localEnv:XDG_RUNTIME_DIR}:${localEnv:XDG_RUNTIME_DIR}"
+  ],
+  "overrideCommand": true,
+  "workspaceFolder": "/wb_humanoid_mpc_ws/src/wb_humanoid_mpc",
+  "customizations": {
+    "vscode": {
+      "settings": {
+        "terminal.integrated.defaultProfile.linux": "bash",
+        "editor.formatOnSave": true,
+        "files.trimTrailingWhitespace": true
+      },
+      "extensions": [
+        "ms-vscode.cpptools",
+        "ms-vscode.cmake-tools",
+        "ms-vscode-remote.remote-containers",
+        "ms-azuretools.vscode-docker",
+        "ms-python.python",
+        "ms-python.vscode-pylance",
+        "eamodio.gitlens",
+        "GitHub.copilot",
+        "GitHub.copilot-chat",
+        "twxs.cmake",
+        "cheshirekow.cmake-format",
+        "xaver.clang-format",
+        "ms-toolsai.jupyter-keymap",
+        "ms-vscode-remote.remote-ssh",
+        "ms-vscode-remote.remote-ssh-edit",
+        "ms-vscode.remote-explorer",
+        "sankethdev.vscode-proto"
+      ]
+    }
+  },
+  "mounts": [
+    "source=${localWorkspaceFolder}/../..,target=/wb_humanoid_mpc_ws,type=bind,consistency=cached",
+    "source=${localEnv:HOME}/.gitconfig,target=/home/vscode/.gitconfig,type=bind"
+  ],
+  "postCreateCommand": "git config --global user.name \"$GIT_USER_NAME\" && git config --global user.email \"$GIT_USER_EMAIL\" && git config --global --add safe.directory /wb_humanoid_mpc_ws/src/wb_humanoid_mpc && curl -o .git-completion.bash https://raw.githubusercontent.com/git/git/master/contrib/completion/git-completion.bash && echo 'source /opt/ros/jazzy/setup.bash' >> ~/.bashrc && echo 'source ./.git-completion.bash' >> ~/.bashrc"
+}
diff --git a/.gitignore b/.gitignore
index e0e9715..1c0fa61 100644
--- a/.gitignore
+++ b/.gitignore
@@ -51,7 +51,7 @@ install/
 
 **/__pycache__/**
 
-# MuJoCo log 
+# MuJoCo log
 */MUJOCO_LOG.TXT
 *mjData.bin
 
@@ -62,3 +62,4 @@ install/
 
 **/.gdb_history
 
+.git-completion.bash
diff --git a/.gitmodules b/.gitmodules
index 6e990f1..4ca0249 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -7,9 +7,6 @@
 [submodule "lib/cmake_modules"]
 	path = lib/cmake_modules
 	url = https://github.com/ros/cmake_modules.git
-[submodule "lib/pinocchio"]
-	path = lib/pinocchio
-	url = https://github.com/manumerous/pinocchio.git
 [submodule "lib/mujoco"]
 	path = lib/mujoco
 	url = https://github.com/google-deepmind/mujoco.git
diff --git a/dependencies.txt b/dependencies.txt
index 4f10d81..63a0eaf 100644
--- a/dependencies.txt
+++ b/dependencies.txt
@@ -1,7 +1,6 @@
 clang-format
 make
 build-essential
-locales
 libeigen3-dev
 libglpk-dev
 libboost-all-dev
@@ -18,3 +17,6 @@ ros-${ROS_DISTRO}-joint-state-publisher-gui
 ros-${ROS_DISTRO}-xacro
 ros-${ROS_DISTRO}-mcap-vendor
 ros-${ROS_DISTRO}-interactive-markers
+ros-${ROS_DISTRO}-pinocchio
+ros-${ROS_DISTRO}-rviz2
+ros-${ROS_DISTRO}-rosidl-typesupport-fastrtps-c
\ No newline at end of file
diff --git a/docker/Dockerfile b/docker/Dockerfile
new file mode 100644
index 0000000..f2c019f
--- /dev/null
+++ b/docker/Dockerfile
@@ -0,0 +1,45 @@
+ARG ROS_DISTRO=jazzy
+ARG BASE_IMAGE=ros:${ROS_DISTRO}-ros-base
+FROM ${BASE_IMAGE} AS base
+
+ARG USER_ID=1000
+ARG GROUP_ID=1000
+ARG WB_HUMANOID_MPC_DIR="/wb_humanoid_mpc_ws"
+ARG PYTHON_VERSION=3.12
+ENV DEBIAN_FRONTEND=noninteractive
+ENV ROS_DISTRO=${ROS_DISTRO}
+
+# Install system dependencies, ROS, Python, and development tools in a single step
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    curl gnupg2 lsb-release software-properties-common \
+    locales \
+    git \
+    x11-apps \
+    mesa-utils \
+    gettext-base \
+    && rm -rf /var/lib/apt/lists/*
+
+COPY dependencies.txt /tmp/dependencies.txt
+
+RUN apt-get update && \
+    envsubst < /tmp/dependencies.txt | xargs apt-get install -y --no-install-recommends && \
+    rm -rf /var/lib/apt/lists/*
+
+# Set default shell to bash
+SHELL ["/bin/bash", "-c"]
+
+# Set up environment variables
+ENV CMAKE_PREFIX_PATH="/opt/ros/${ROS_DISTRO}:${CMAKE_PREFIX_PATH}"
+ENV DISPLAY=${DISPLAY} QT_X11_NO_MITSHM=1
+
+# Set up user and workspace
+WORKDIR ${WB_HUMANOID_MPC_DIR}
+RUN mkdir -p ${WB_HUMANOID_MPC_DIR}/src/wb_humanoid_mpc
+
+# Ensure correct UID/GID
+RUN if ! getent group $GROUP_ID > /dev/null; then groupadd --gid $GROUP_ID devgroup; fi && \
+    if ! getent passwd $USER_ID > /dev/null; then useradd --uid $USER_ID --gid $GROUP_ID --create-home devuser; fi && \
+    chown -R $USER_ID:$GROUP_ID .
+
+# Copy the Makefile and other necessary files
+COPY . ${WB_HUMANOID_MPC_DIR}/src/wb_humanoid_mpc
diff --git a/lib/pinocchio b/lib/pinocchio
deleted file mode 160000
index 16d84f4..0000000
--- a/lib/pinocchio
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 16d84f46e24fbcf95ad69fa4a7b120da773765c9