diff --git a/ManiSkill b/ManiSkill
new file mode 160000
index 00000000..954edc59
--- /dev/null
+++ b/ManiSkill
@@ -0,0 +1 @@
+Subproject commit 954edc594047b04674acbff54e84a66ab887818a
diff --git a/README.md b/README.md
index 6fd0d549..11bb21a3 100644
--- a/README.md
+++ b/README.md
@@ -1,138 +1,58 @@
 # Robot Control Stack
-RCS is a unified and multilayered robot control interface over a MuJoCo simulation and real world robot currently implemented for the FR3/Panda, xArm7, UR5e and SO101.
+
+**Robot Control Stack (RCS)** is a unified and multilayered robot control interface over a MuJoCo simulation and real world robot currently implemented for the FR3/Panda, xArm7, UR5e and SO101.
 
 ## Installation
+
 We build and test RCS on the latest Debian and on the latest Ubuntu LTS.
 
-1. Install the system dependencies:
-```shell
-sudo apt install $(cat debian_deps.txt)
-```
-2. Create, activate and configure a [Python virtual environment](https://docs.python.org/3/library/venv.html):
-```shell
-python3 -m venv .venv
-source .venv/bin/activate
-``` 
-Then, install the package:
-```shell
-pip install -r requirements_dev.txt
-pip config --site set global.no-build-isolation false
-```
-3. Build and install RCS:
-```shell
-pip install -ve .
-```
+1.  **System Dependencies**:
+    ```shell
+    sudo apt install $(cat debian_deps.txt)
+    ```
 
-For a docker deployment see the [docker](docker) folder.
-
-## Usage
-The python package is called `rcs`.
-
-### Direct Robot Control
-Simple direct robot control:
-```python
-import rcs
-from rcs import sim
-from rcs._core.sim import CameraType
-from rcs.camera.sim import SimCameraConfig, SimCameraSet
-from time import sleep
-simulation = sim.Sim(rcs.scenes["fr3_empty_world"].mjb)
-urdf_path = rcs.scenes["fr3_empty_world"].urdf
-ik = rcs.common.RL(str(urdf_path))
-cfg = sim.SimRobotConfig()
-cfg.add_id("0")
-cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
-robot = rcs.sim.SimRobot(simulation, ik, cfg)
-
-gripper_cfg_sim = sim.SimGripperConfig()
-gripper_cfg_sim.add_id("0")
-gripper = sim.SimGripper(simulation, gripper_cfg_sim)
-
-camera_set = SimCameraSet(simulation, {})
-simulation.open_gui()
-# wait for gui
-sleep(5)
-# step the robot 10 cm in x direction
-robot.set_cartesian_position(
-    robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0.1, 0, 0]))
-)
-# close gripper
-gripper.grasp()
-simulation.step_until_convergence()
-input("press enter to close")
-```
-### Gym Env Interface
-```python
-from rcs.envs.creators import SimEnvCreator
-from rcs.envs.utils import (
-    default_mujoco_cameraset_cfg,
-    default_sim_gripper_cfg,
-    default_sim_robot_cfg,
-)
-from rcs.envs.base import ControlMode, RelativeTo
-env_rel = SimEnvCreator()(
-    control_mode=ControlMode.JOINTS,
-    robot_cfg=default_sim_robot_cfg(),
-    gripper_cfg=default_sim_gripper_cfg(),
-    cameras=default_mujoco_cameraset_cfg(),
-    max_relative_movement=np.deg2rad(5),
-    relative_to=RelativeTo.LAST_STEP,
-)
-env_rel.get_wrapper_attr("sim").open_gui()
-
-for _ in range(100):
-    obs, info = env_rel.reset()
-    for _ in range(10):
-        # sample random relative action and execute it
-        act = env_rel.action_space.sample()
-        print(act)
-        obs, reward, terminated, truncated, info = env_rel.step(act)
-        print(obs)
-```
+2.  **Python Environment**:
+    ```shell
+    python3 -m venv .venv
+    source .venv/bin/activate
+    pip install -r requirements_dev.txt
+    ```
 
+3.  **Install RCS**:
+    ```shell
+    pip install -ve .
+    ```
 
-### Examples
-Checkout the python examples in the [examples](examples) folder. For example
-[fr3_direct_control.py](examples/fr3/fr3_direct_control.py) shows direct robot control with RCS's python bindings.
-And [fr3_env_joint_control.py](examples/fr3/fr3_env_joint_control.py) and [fr3_env_cartesian_control.py](examples/fr3/fr3_env_cartesian_control.py) demonstrates RCS's high level [gymnasium](https://gymnasium.farama.org/) interface both for joint- and end effector space control
-Checkout the other sub folders for other robot-specific examples.
-Most of these examples work both in the MuJoCo simulation as well as on hardware.
+## Hardware Extensions
 
+RCS supports various hardware extensions (e.g., FR3, xArm7, RealSense). These are located in the `extensions` directory.
+
+To install an extension:
 
-### Hardware Extensions
-To enable hardware usage in RCS, install the needed hardware extensions via pip. RCS itself comes with a couple of supported extensions e.g. control of the FR3 via the [`rcs_fr3`](extensions/rcs_fr3) extension. All native supported extension are located in [extensions](extensions).
-To install extensions:
 ```shell
 pip install -ve extensions/rcs_fr3
 ```
-For more details real the readme file of the respective extension.
 
-After the required hardware extensions are installed the examples also above work on real hardware:
-Switch to hardware by setting the following flag:
-```python
-# ROBOT_INSTANCE = RobotPlatform.SIMULATION
-ROBOT_INSTANCE = RobotPlatform.HARDWARE
-```
+For a full list of extensions and detailed documentation, visit [robot-control-stack.org/extensions](https://robot-control-stack.org/extensions).
+
+## Documentation
 
-#### Command Line Interface
-Some modules include command line interfaces, e.g. rcs_fr3 defines useful commands to handle the FR3 robot without the need to use the Desk Website.
-You can see the available subcommands as follows:
-```shell
-python -m rcs_fr3 --help
-python -m rcs_realsense --help
-```
 
-## Developer Documentation
-See [robot-control-stack.org](https://robot-control-stack.org) for the development documentation.
+For full documentation, including installation, usage, and API reference, please visit:
 
+**[robot-control-stack.org](https://robot-control-stack.org)**
 
 ## Citation
+
 If you find RCS useful for your academic work, please consider citing it:
-```
+
+```bibtex
 @misc{juelg2025robotcontrolstack,
   title={{Robot Control Stack}: {A} Lean Ecosystem for Robot Learning at Scale}, 
   author={Tobias J{\"u}lg and Pierre Krack and Seongjin Bien and Yannik Blei and Khaled Gamal and Ken Nakahara and Johannes Hechtl and Roberto Calandra and Wolfram Burgard and Florian Walter},
   year={2025},
   howpublished = {\url{https://arxiv.org/abs/2509.14932}}
 }
-```
\ No newline at end of file
+```
+
+For more scientific info, visit the [paper website](https://robotcontrolstack.github.io/).
\ No newline at end of file
diff --git a/docs/README.md b/docs/README.md
new file mode 100644
index 00000000..f1b07042
--- /dev/null
+++ b/docs/README.md
@@ -0,0 +1,34 @@
+# Documentation
+
+This directory contains the source code for the Robot Control Stack (RCS) documentation.
+
+## Building Locally
+
+To build the documentation locally, follow these steps:
+
+1.  **Install Dependencies**:
+    Ensure you have the documentation dependencies installed.
+
+    ```shell
+    pip install -r requirements.txt
+    ```
+
+2.  **Build**:
+    Run `sphinx-build` to generate the HTML documentation.
+
+    ```shell
+    sphinx-build -b html . _build/html
+    ```
+
+3.  **View**:
+    Open `_build/html/index.html` in your web browser.
+
+## Live Reloading
+
+For a better development experience, you can use `sphinx-autobuild` to automatically rebuild the documentation when you make changes.
+
+```shell
+sphinx-autobuild . _build/html
+```
+
+This will start a local server (usually at http://127.0.0.1:8000) and refresh the page whenever you save a file.
diff --git a/docs/_static/.keep b/docs/_static/.keep
deleted file mode 100644
index e69de29b..00000000
diff --git a/docs/images/favicon.ico b/docs/_static/favicon.ico
similarity index 100%
rename from docs/images/favicon.ico
rename to docs/_static/favicon.ico
diff --git a/docs/images/rcs_architecture_small.svg b/docs/_static/rcs_architecture_small.svg
similarity index 100%
rename from docs/images/rcs_architecture_small.svg
rename to docs/_static/rcs_architecture_small.svg
diff --git a/docs/images/rcs_logo.png b/docs/_static/rcs_logo.png
similarity index 100%
rename from docs/images/rcs_logo.png
rename to docs/_static/rcs_logo.png
diff --git a/docs/images/rcs_logo_multiline.svg b/docs/_static/rcs_logo_multiline.svg
similarity index 100%
rename from docs/images/rcs_logo_multiline.svg
rename to docs/_static/rcs_logo_multiline.svg
diff --git a/docs/_static/version_switcher.json b/docs/_static/version_switcher.json
new file mode 100644
index 00000000..4fb75bc0
--- /dev/null
+++ b/docs/_static/version_switcher.json
@@ -0,0 +1,7 @@
+[
+    {
+        "name": "latest",
+        "version": "latest",
+        "url": "https://robot-control-stack.org/"
+    }
+]
\ No newline at end of file
diff --git a/docs/api/index.md b/docs/api/index.md
new file mode 100644
index 00000000..dd927c46
--- /dev/null
+++ b/docs/api/index.md
@@ -0,0 +1,10 @@
+# API
+
+```{toctree}
+:maxdepth: 2
+
+../getting_started/index
+../user_guide/architecture
+../user_guide/gym_interface
+../user_guide/low_level_api
+```
diff --git a/docs/changelog.md b/docs/changelog.md
deleted file mode 100644
index 825c32f0..00000000
--- a/docs/changelog.md
+++ /dev/null
@@ -1 +0,0 @@
-# Changelog
diff --git a/docs/conf.py b/docs/conf.py
index d4395e70..2049f264 100644
--- a/docs/conf.py
+++ b/docs/conf.py
@@ -1,48 +1,73 @@
 import os
 import sys
-sys.path.insert(0, os.path.abspath('..'))
 
-project = 'Robot Control Stack'
-author = 'Tobias Jülg'
-release = '0.4'
+# inject path to rcs package to enable autodoc/autoapi to find packages
+sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), "../python")))
+
+project = "Robot Control Stack"
+copyright = "2025, RCS Contributors"
+author = "Tobias Jülg"
+release = "0.5.2"
+version = "0.5.2"
 
 extensions = [
-    'myst_parser',
-    'sphinx.ext.autodoc',
-    'sphinx.ext.napoleon',
-    'sphinx.ext.viewcode',
+    "sphinx.ext.autodoc",
+    "sphinx.ext.autosummary",
+    "sphinx.ext.mathjax",
+    "sphinx.ext.viewcode",
+    "sphinx.ext.napoleon",
+    "sphinx.ext.intersphinx",
+    "sphinx_copybutton",
+    "myst_parser",
+    "sphinx_design",
 ]
 
-templates_path = ['_templates']
-exclude_patterns = []
+# https://myst-parser.readthedocs.io/en/latest/syntax/optional.html
+myst_enable_extensions = ["colon_fence", "dollarmath"]
+myst_heading_anchors = 4
 
-html_theme = 'pydata_sphinx_theme'
-html_static_path = ['_static']
-html_logo = "images/rcs_logo_multiline.svg"
-html_favicon = "images/favicon.ico"
+exclude_patterns = ["README.md"]
+
+templates_path = ["_templates"]
+
+html_theme = "pydata_sphinx_theme"
+html_logo = "_static/rcs_logo_multiline.svg"
+html_favicon = "_static/favicon.ico"
 
 html_theme_options = {
-    "github_url": "https://github.com/RobotControlStack/robot-control-stack",
     "use_edit_page_button": True,
-    "show_prev_next": False,
-    "navbar_start": ["navbar-logo"],  # ensures the logo is shown
+    "icon_links": [
+        {
+            "name": "GitHub",
+            "url": "https://github.com/RobotControlStack/robot-control-stack",
+            "icon": "fa-brands fa-github",
+        },
+    ],
     "logo": {
-        "image_light": "images/rcs_logo_multiline.svg",  # your PNG
-        "image_dark": "images/rcs_logo_multiline.svg",   # can be same or a dark-mode version
+        "image_dark": "_static/rcs_logo_multiline.svg",
+    },
+    "navbar_center": ["version-switcher", "navbar-nav"],
+    "show_version_warning_banner": False,
+    "switcher": {
+        "json_url": "/_static/version_switcher.json",
+        "version_match": "latest",
     },
-
 }
 
 html_context = {
+    "display_github": True,
     "github_user": "RobotControlStack",
     "github_repo": "robot-control-stack",
-    "github_version": "main",  # branch name
-    "doc_path": "docs",        # relative path in the repo where your docs live
+    "github_version": "main",
+    "conf_py_path": "/docs/",
 }
 
+html_static_path = ['_static']
+
+# autodoc configs
+autosummary_generate = True
+autodoc_typehints = "description"
+autodoc_member_order = "groupwise"
 
-myst_enable_extensions = [
-    "colon_fence",
-    "deflist",
-    "linkify",
-]
+# Intersphinx mapping
+intersphinx_mapping = {'gymnasium': ('https://gymnasium.farama.org/', None)}
diff --git a/docs/contributing.md b/docs/contributing.md
deleted file mode 100644
index 854139a3..00000000
--- a/docs/contributing.md
+++ /dev/null
@@ -1 +0,0 @@
-# Contributing
diff --git a/docs/contributing/index.md b/docs/contributing/index.md
new file mode 100644
index 00000000..5c493844
--- /dev/null
+++ b/docs/contributing/index.md
@@ -0,0 +1,102 @@
+# Contributing
+
+We welcome contributions to the Robot Control Stack!
+
+## Development Setup
+
+1.  Clone the repository.
+2.  Install dependencies (see [Getting Started](../getting_started/index.md)).
+3.  Install pre-commit hooks:
+    ```shell
+    pre-commit install
+    ```
+
+## Development Tools
+
+We provide a `Makefile` with several useful commands for development.
+
+### Python
+
+- **Formatting**:
+  ```shell
+  make pyformat
+  ```
+  Uses `isort` and `black` to format code.
+
+- **Linting**:
+  ```shell
+  make pylint
+  ```
+  Runs `ruff` and `mypy`.
+
+- **Testing**:
+  ```shell
+  make pytest
+  ```
+  Runs the test suite.
+
+- **Type Stubs**:
+  ```shell
+  make stubgen
+  ```
+  Generates Python type stubs for the C++ bindings.
+
+### C++
+
+- **Formatting**:
+  ```shell
+  make cppformat
+  ```
+  Uses `clang-format` to format C++ code.
+
+- **Linting**:
+  ```shell
+  make cpplint
+  ```
+  Runs `clang-tidy`.
+
+### General
+
+- **Commit**:
+  ```shell
+  make commit
+  ```
+  Uses `commitizen` to help you create conventional commits.
+
+## Code Style
+
+
+- **Python**: We use `ruff` for linting and formatting.
+- **C++**: We use `clang-format` and `clang-tidy`.
+
+## Commit Messages
+
+We follow the **Conventional Commits** specification. This allows us to automatically generate changelogs and determine semantic versioning.
+
+**Format**: `<type>[optional scope]: <description>`
+
+**Types**:
+- `feat`: A new feature
+- `fix`: A bug fix
+- `docs`: Documentation only changes
+- `style`: Changes that do not affect the meaning of the code (white-space, formatting, etc)
+- `refactor`: A code change that neither fixes a bug nor adds a feature
+- `perf`: A code change that improves performance
+- `test`: Adding missing tests or correcting existing tests
+- `build`: Changes that affect the build system or external dependencies
+- `ci`: Changes to our CI configuration files and scripts
+- `chore`: Other changes that don't modify src or test files
+
+**Examples**:
+- `feat(fr3): add support for new gripper`
+- `fix(sim): fix collision detection bug`
+- `docs: update installation instructions`
+
+See [conventionalcommits.org](https://www.conventionalcommits.org/) for more details.
+
+## Pull Requests
+
+1.  Create a new branch for your feature or fix.
+2.  Make your changes and commit them using the conventional commit format.
+3.  Push your branch and open a Pull Request.
+4.  Ensure all CI checks pass.
diff --git a/docs/development/cpp_extension.md b/docs/development/cpp_extension.md
new file mode 100644
index 00000000..42deac43
--- /dev/null
+++ b/docs/development/cpp_extension.md
@@ -0,0 +1,58 @@
+# Creating a C++ Extension
+
+For performance-critical hardware drivers or integration with C++ libraries, you can create a C++ extension. This involves writing C++ code and exposing it to Python using `pybind11`.
+
+## Structure
+
+A C++ extension typically looks like this:
+
+```text
+rcs_mycppext/
+├── CMakeLists.txt
+├── pyproject.toml
+├── src/
+│   ├── MyDevice.cpp
+│   ├── MyDevice.h
+│   └── bindings.cpp
+└── ...
+```
+
+## Steps
+
+1.  **CMake Configuration**: Use `CMakeLists.txt` to configure your build. You'll need to link against `rcs` (if it exposes C++ headers) and `pybind11`.
+
+2.  **Implement C++ Class**: Write your device driver in C++.
+
+    ```cpp
+    #include <rcs/Robot.h>
+
+    class MyRobot : public rcs::Robot {
+    public:
+        void setJointPosition(const Eigen::VectorXd& q) override {
+            // ... implementation ...
+        }
+        // ... other methods ...
+    };
+    ```
+
+3.  **Create Bindings**: Use `pybind11` to expose your class to Python.
+
+    ```cpp
+    #include <pybind11/pybind11.h>
+    #include "MyDevice.h"
+
+    namespace py = pybind11;
+
+    PYBIND11_MODULE(rcs_mycppext, m) {
+        py::class_<MyRobot, rcs::Robot>(m, "MyRobot")
+            .def(py::init<>())
+            .def("set_joint_position", &MyRobot::setJointPosition);
+    }
+    ```
+
+4.  **Build System**: Use `scikit-build` or similar tools in `pyproject.toml` to compile the C++ extension during installation.
+
+## Examples
+
+- **rcs_fr3**: Implements the driver for the Franka Research 3 robot in C++ using `libfranka`.
+- **rcs_robotics_library**: Wraps the Robotics Library (RL) for kinematics and path planning.
diff --git a/docs/development/index.md b/docs/development/index.md
new file mode 100644
index 00000000..42cc9a5f
--- /dev/null
+++ b/docs/development/index.md
@@ -0,0 +1,9 @@
+# Extending RCS
+
+```{toctree}
+:maxdepth: 2
+
+python_extension
+cpp_extension
+../contributing/index
+```
diff --git a/docs/development/python_extension.md b/docs/development/python_extension.md
new file mode 100644
index 00000000..13b5b859
--- /dev/null
+++ b/docs/development/python_extension.md
@@ -0,0 +1,56 @@
+# Creating a Python Extension
+
+Creating a Python-based extension is the easiest way to add new functionality to RCS, especially for hardware that already has a Python API.
+
+## Structure
+
+A typical Python extension has the following structure:
+
+```text
+rcs_myext/
+├── pyproject.toml
+├── README.md
+└── src/
+    └── rcs_myext/
+        ├── __init__.py
+        └── my_device.py
+```
+
+## Steps
+
+1.  **Create `pyproject.toml`**: Define your package metadata and dependencies.
+
+    ```toml
+    [build-system]
+    requires = ["setuptools>=61.0"]
+    build-backend = "setuptools.build_meta"
+
+    [project]
+    name = "rcs_myext"
+    version = "0.1.0"
+    dependencies = [
+        "rcs",
+        # Add other dependencies here
+    ]
+    ```
+
+2.  **Implement the Interface**: Create your device class in `src/rcs_myext/my_device.py`. You should inherit from the appropriate RCS base class (e.g., `Camera`, `Gripper`) if applicable, or implement the required methods.
+
+3.  **Register the Extension**: If your extension needs to be discoverable by RCS (e.g., for CLI tools or automatic loading), ensure it's installed in the same environment.
+
+## Example: USB Camera
+
+The `rcs_usb_cam` extension is a good example of a pure Python extension. It wraps `cv2.VideoCapture` to provide a camera interface compatible with RCS.
+
+```python
+from rcs.camera import Camera
+
+class WebCam(Camera):
+    def __init__(self, device_id=0):
+        # ... initialization ...
+        pass
+
+    def get_image(self):
+        # ... capture frame ...
+        return image
+```
diff --git a/docs/documentation.md b/docs/documentation.md
deleted file mode 100644
index 4eb41a7f..00000000
--- a/docs/documentation.md
+++ /dev/null
@@ -1,9 +0,0 @@
-# Documentation
-
-```{toctree}
-:maxdepth: 1
-documentation/getting_started.md
-documentation/usage.md
-documentation/hardware_extentions.md
-documentation/development.md
-```
\ No newline at end of file
diff --git a/docs/documentation/development.md b/docs/documentation/development.md
deleted file mode 100644
index 84468879..00000000
--- a/docs/documentation/development.md
+++ /dev/null
@@ -1,24 +0,0 @@
-# Development
-```shell
-# check for c++ formatting errors
-make cppcheckformat
-# fix them
-make cppformat
-# Linting with clang tidy
-make cpplint
-# check for python formatting errors
-make pycheckformat
-# fix them
-make pyformat
-# Linting with ruff and mypy
-make pylint
-# Testing
-make pytest
-```
-
-## Stub Files for Python Bindings
-We use autogenerated python stub files (`.pyi`) in the [`_core`](python/rcs/_core/) folder to show our linters the expected types of the C++ Python bindings.
-If the python bindings in the C++ code have changed you might need to regenerate them by using:
-```shell
-make stubgen
-```
\ No newline at end of file
diff --git a/docs/documentation/getting_started.md b/docs/documentation/getting_started.md
deleted file mode 100644
index ec0d5049..00000000
--- a/docs/documentation/getting_started.md
+++ /dev/null
@@ -1,10 +0,0 @@
-# Getting Started
-
-```{toctree}
-:maxdepth: 1
-
-getting_started/installation.md
-getting_started/docker_installation.md
-getting_started/concepts.md
-
-```
\ No newline at end of file
diff --git a/docs/documentation/getting_started/build_system.md b/docs/documentation/getting_started/build_system.md
deleted file mode 100644
index 58415107..00000000
--- a/docs/documentation/getting_started/build_system.md
+++ /dev/null
@@ -1,158 +0,0 @@
-# Build System
-
-## 1. You Run
-
-    pip install -e .
-
-This tells `pip` to:
-
-- Perform an **editable install** of the current project.
-- Use the `pyproject.toml` as the **single source of truth** for building and packaging.
-
----
-
-## 2. `pip` Reads `pyproject.toml`
-
-It sees:
-
-    [build-system]
-    build-backend = "scikit_build_core.build"
-
-So `pip` uses **`scikit-build-core`** as the **build backend** (instead of legacy `setuptools`).
-
----
-
-## 3. `scikit-build-core` Invokes CMake
-
-It starts a CMake build, just like if you had run:
-
-    cmake -S . -B build
-    cmake --build build
-
-It uses the `CMakeLists.txt` at the project root, which:
-
-- Declares the project:
-  
-      project(rcs LANGUAGES C CXX VERSION 0.4.0)
-  
-- Sets modern C++20 and compiler policies.
-- Locates external dependencies:
-  - `Eigen3`, `Python3`, `MuJoCo`, `pinocchio`
-- Downloads with `FetchContent`: `rl`, `pybind11`
-- Includes:
-  
-      add_subdirectory(src)
-
----
-
-## 4. CMake Enters `src/CMakeLists.txt`
-
-    add_subdirectory(rcs)
-    target_include_directories(rcs INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
-    add_subdirectory(sim)
-    add_subdirectory(pybind)
-
-This loads and builds 3 subcomponents.
-
----
-
-## 5. Subcomponent Builds
-
-### `src/rcs/CMakeLists.txt`
-
-    add_library(rcs SHARED)
-    target_sources(rcs PRIVATE Pose.cpp Robot.cpp IK.cpp utils.cpp)
-    target_link_libraries(rcs PUBLIC Eigen3::Eigen mdl pinocchio::all)
-
-- Builds a **shared C++ library**: `rcs`
-- Contains your **robot control logic**
-- Exposes `include/` headers
-- Linked against external libraries
-
-### `src/sim/CMakeLists.txt`
-
-    add_library(sim)
-    target_sources(sim PRIVATE sim.cpp SimRobot.cpp ...)
-    target_link_libraries(sim PUBLIC rcs MuJoCo::MuJoCo)
-
-- Builds a **C++ simulation library**
-- Depends on:
-  - Your own `rcs` library
-  - MuJoCo physics engine
-
-### `src/pybind/CMakeLists.txt`
-
-    pybind11_add_module(_core MODULE rcs.cpp)
-    target_link_libraries(_core PRIVATE sim rcs)
-
-- Compiles a Python extension module: `_core.so`
-- Uses `pybind11` to bind C++ classes/functions
-- Links to both `sim` and `rcs` native libraries
-- Adds install instructions:
-  
-      install(TARGETS _core rcs DESTINATION rcs COMPONENT python_package)
-
----
-
-## 6. Packaging into Python
-
-From `pyproject.toml`:
-
-    [tool.scikit-build]
-    build.targets = ["_core", "scenes", "rcs"]
-    wheel.packages = ["python/rcs"]
-    install.components = ["python_package"]
-
-- `scikit-build-core` installs `_core.so` into:
-  
-      python/rcs/_core.so
-  
-- That directory becomes a valid **Python package**
-
----
-
-## 7. Result: Python Import Works
-
-After install:
-
-    from rcs import _core
-
-- You now access the C++ functionality exposed in `rcs.cpp` through Python.
-- `_core` contains Python-wrapped C++ objects/functions via `pybind11`.
-
----
-
-## Summary Diagram
-
-    pip install -e .
-            │
-            ▼
-    Reads pyproject.toml (scikit_build_core used)
-            │
-            ▼
-    CMakeLists.txt (root) → add_subdirectory(src)
-            │
-            ▼
-    src/rcs → builds C++ library `rcs`
-    src/sim → builds C++ library `sim`
-    src/pybind → builds Python extension `_core`
-            │
-            ▼
-    All .so libraries installed in `python/rcs/`
-    `_core` accessible from Python as import
-
----
-
-## Recap of Build Layers
-
-| Layer | Role | Tool |
-| --- | --- | --- |
-| `pyproject.toml` | Project metadata, build backend | `pip`, `scikit-build-core` |
-| `CMakeLists.txt` (root) | Configure the full C++ build | `CMake` |
-| `src/rcs` | Core robot logic in C++ | `C++`, `Eigen`, `pinocchio` |
-| `src/sim` | Simulation logic, MuJoCo | `C++`, `MuJoCo` |
-| `src/pybind` | Bindings to Python | `pybind11` |
-| `python/rcs/` | Python package with compiled `.so` | `scikit-build`, `pip` |
-
-
-
diff --git a/docs/documentation/getting_started/concepts.md b/docs/documentation/getting_started/concepts.md
deleted file mode 100644
index 39053f95..00000000
--- a/docs/documentation/getting_started/concepts.md
+++ /dev/null
@@ -1,7 +0,0 @@
-# Concepts
-```{toctree}
-:maxdepth: 1
-:caption: Contents:
-docs_action_observation_flow/action_obersvation_flow.md
-build_system.md
-```
diff --git a/docs/documentation/getting_started/docker_installation.md b/docs/documentation/getting_started/docker_installation.md
deleted file mode 100644
index 687a074f..00000000
--- a/docs/documentation/getting_started/docker_installation.md
+++ /dev/null
@@ -1,122 +0,0 @@
-# Docker
-This setup lets you **build once** and then add capabilities as overrides — GUI, GPU, and Hardware (HW) — onto a single runtime service **without touching your main repo files**.
-
----
-
-## Prerequisites
-
-- **Docker** + **docker-compose** (either v1 or v2 plugin)
-- **NVIDIA drivers** on host (`nvidia-smi` works on host)
-- **NVIDIA Container Toolkit** installed on host (legacy runtime: `nvidia`)
-- **X11** on host (Linux)  
----
-
-## Layout
-
-    docker/
-      Dockerfile             # Your build definition
-      compose/
-        base.yml   # dev (build + source mount) and run (no mount)
-        gui.yml    # adds DISPLAY, XAUTHORITY, X11 socket
-        gpu.yml    # NVIDIA runtime + envs, mounts nvidia-smi
-        hw.yml     # /dev, caps, ulimits (heavy hardware access)
-
----
-
-## What Each File Does
-
-- **base.yml**  
-  - `dev`: builds the image from `docker/Dockerfile` and mounts your source  
-  - `run`: clean runtime service (no source mount)
-- **gui.yml** — Adds X11 env/socket so GUI apps can display on host
-- **gpu.yml** — Enables GPU via legacy runtime: `nvidia` (for older compose) and ensures `nvidia-smi` is available in the container
-- **hw.yml** — Grants broad hardware access
-
----
-
-## How to Use
-
-From the repo root:
-
-### 1) Build the image
-Uses `docker/Dockerfile`. Only `dev` mounts your source.
-
-    docker-compose -f docker/compose/base.yml build dev
-
-### 2) (For GUI) Allow root to use your X server
-
-    xhost +local:docker
-
-### 3) Run with different capability combinations
-
-**GUI + GPU**
-
-    docker-compose \
-      -f docker/compose/base.yml \
-      -f docker/compose/gui.yml \
-      -f docker/compose/gpu.yml \
-      run --rm run bash
-
-**GUI + GPU + HW**
-
-    docker-compose \
-      -f docker/compose/base.yml \
-      -f docker/compose/gui.yml \
-      -f docker/compose/gpu.yml \
-      -f docker/compose/hw.yml \
-      run --rm run bash
-
----
-
-## Quick Checks Inside the Container
-
-Verify GPU:
-
-    nvidia-smi
-
-Test GUI apps:
-
-    apt-get update && apt-get install -y x11-apps && xclock &
-
----
-
-## Sanity Check Before Running
-
-See the fully merged service to confirm all env/volumes are present:
-
-    docker-compose \
-      -f docker/compose/base.yml \
-      -f docker/compose/gui.yml \
-      -f docker/compose/gpu.yml \
-      -f docker/compose/hw.yml \
-      config | sed -n '/run:/,/^[^ ]/p'
-
----
-
-## Example: FR3 Environment in Docker
-
-1) Build the image:
-
-       docker-compose -f docker/compose/base.yml build dev
-
-2) Run with full capabilities:
-
-       docker-compose \
-         -f docker/compose/base.yml \
-         -f docker/compose/gui.yml \
-         -f docker/compose/gpu.yml \
-         -f docker/compose/hw.yml \
-         run --rm run bash
-
-3) Install FR3 extension:
-
-       pip install -ve extensions/rcs_fr3
-
-4) Run example:
-
-       cd examples
-       python fr3_env_cartesian_control.py
-
-
-
-
diff --git a/docs/documentation/getting_started/docs_action_observation_flow/action_obersvation_flow.md b/docs/documentation/getting_started/docs_action_observation_flow/action_obersvation_flow.md
deleted file mode 100644
index 613dbbb1..00000000
--- a/docs/documentation/getting_started/docs_action_observation_flow/action_obersvation_flow.md
+++ /dev/null
@@ -1,135 +0,0 @@
-# Action-Observation Flow 
-
-## wrapper Hierarchy 
-
-
-Vertical Order:
-
-- Top: Highest-level wrapper (first applied)
-
-- Bottom: Base environment
-
-Inheritance Notation:
-
-- Use (ParentClass) next to wrapper names
-
-Wrapping Notation
-
-- Show ← wraps arrows between layers
-Here is a class hierarchy diagram:
-
-```{image} images/class_hierarchy.png
-:width: 350px
-:align: center
-```
-## Flow
-1. **`RelativeActionSpace(ActionWrapper).step(act)`**
-   
-   -   2- Calls step function of ActionWrapper (→2)
-
-2. **`ActionWrapper.step(act)`**
-   - 3. Calls `RelativeActionSpace.action(act)`
-   - 4. Calls `GripperWrapper.step(act)`
-
-3. **`RelativeActionSpace.action(act)`**
-   - Processes action: `{"tquart": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0}`
-   - Operations:
-     - Clips action within min/max limits
-     - Makes action relative (current_pose + action)
-     - Updates `action["tquart"]`
-
-4. **`GripperWrapper(ActObsInfoWrapper).step(act)`**
-   - →5. Calls `GripperWrapper.action(act)`
-   - →6. Calls `CameraSetWrapper.step(act)`
-   - →15. Calls `GripperWrapper.observation(obs)`
-
-5. **`GripperWrapper.action(act)`**
-   - Uses `act["gripper"]` to open/close gripper
-   - Only executes if state change needed
-   - Deletes `"gripper"` key from action dict
-
-6. **`CameraSetWrapper(ActObsInfoWrapper).step(act)`**
-   - →7. Calls `CameraSetWrapper.action(act)`
-   - →8. Calls `CameraSetWrapper(ActObsInfoWrapper).step(act)`
-   - →14. Calls `CameraSetWrapper.observation(obs)`
-
-7. **`CameraSetWrapper.action(act)`**
-   - (Pass-through) Returns original action
-
-8. **`CameraSetWrapper(ActObsInfoWrapper).step(act)`**
-   - →9. Calls `CameraSetWrapper.action(act)`
-   - →10. Calls `FR3Sim.step(act)`
-
-9. **`CameraSetWrapper.action(act)`**
-   - (Pass-through) Returns original action
-
-10. **`FR3Sim.step(act)`**
-    - →11. Calls `FR3Env.step(act)`
-    - 13. Executes:
-      ```python
-      self.sim.step_until_convergence()
-      state = self.sim_robot.get_state()
-      ```
-    - Returns observation
-
-11. **`FR3Env.step(act)`**
-    - Sets new pose:
-      ```python
-      self.robot.set_cartesian_position(
-          common.Pose(
-              translation=action_dict[self.tquart_key][:3],
-              quaternion=action_dict[self.tquart_key][3:]
-          )
-      )
-      ```
-    - →12. Calls `FR3Env.get_obs()`
-
-12. **`FR3Env.get_obs()`**
-    - Returns:
-      ```python
-      (
-          tquart=np.concatenate([
-              self.robot.get_cartesian_position().translation(),
-              self.robot.get_cartesian_position().rotation_q()
-          ]),
-          joints=self.robot.get_joint_position(),
-          xyzrpy=self.robot.get_cartesian_position().xyzrpy()
-      )
-      ```
-    - Shapes:
-      - `joints`: (7,)
-      - `tquart`: (7,) [x,y,z, qx,qy,qz,qw]
-      - `xyzrpy`: (6,) [x,y,z, roll,pitch,yaw]
-
-14. **`CameraSetWrapper.observation(obs)`**
-    - Adds camera data:
-      ```python
-      {
-          ...original_obs...,
-          "frames": {
-              "wrist": {
-                  "rgb": (256,256,3),
-                  "depth": (256,256,3)
-              },
-              "default_free": {
-                  "wrist": {
-                      "rgb": (256,256,3),
-                      "depth": (256,256,3)
-                  }
-              }
-          }
-      }
-      ```
-
-15. **`GripperWrapper.observation(obs)`**
-    - Adds gripper state:
-      ```python
-      {
-          ...previous_data...,
-          "gripper": float
-      }
-      ```
-## Sequence Diagram
-![image](images/sequence_diagram.png)
-
-
diff --git a/docs/documentation/getting_started/docs_action_observation_flow/images/class_hierarchy.png b/docs/documentation/getting_started/docs_action_observation_flow/images/class_hierarchy.png
deleted file mode 100644
index eb7be6d1..00000000
Binary files a/docs/documentation/getting_started/docs_action_observation_flow/images/class_hierarchy.png and /dev/null differ
diff --git a/docs/documentation/getting_started/docs_action_observation_flow/images/sequence_diagram.png b/docs/documentation/getting_started/docs_action_observation_flow/images/sequence_diagram.png
deleted file mode 100644
index 0ea1df9e..00000000
Binary files a/docs/documentation/getting_started/docs_action_observation_flow/images/sequence_diagram.png and /dev/null differ
diff --git a/docs/documentation/getting_started/installation.md b/docs/documentation/getting_started/installation.md
deleted file mode 100644
index 8557a54a..00000000
--- a/docs/documentation/getting_started/installation.md
+++ /dev/null
@@ -1,40 +0,0 @@
-# Installation Guide
-
-## Requirements
-RCS is developed and tested on the latest Debian and Ubuntu LTS versions.
-
-### Step 1: Install System Dependencies
-First, update your package list and install dependencies listed in `debian_deps.txt`:
-
-- `sudo apt update`
-
-- `sudo apt install -y $(cat debian_deps.txt)`
-
-
-### Step 2: Set Up Python Virtual Environment
-Create and activate a virtual environment to isolate Python dependencies:
-
-- `python3 -m venv .venv`
-- `source .venv/bin/activate`
-
-Upgrade packaging tools and install Python dependencies:
-
-- `pip install --upgrade pip setuptools wheel`
-- `pip install -r requirements_dev.txt`
-
-Configure pip to allow build isolation:
-
-- `pip config --site set global.no-build-isolation false`
-
-### Step 3: Build and Install RCS
-Install the package in editable mode for active development:
-
-- `pip install -ve .`
-
----
-
-
-```{toctree}
-:maxdepth: 1
-
-```
\ No newline at end of file
diff --git a/docs/documentation/hardware_extentions.md b/docs/documentation/hardware_extentions.md
deleted file mode 100644
index d7fe32bc..00000000
--- a/docs/documentation/hardware_extentions.md
+++ /dev/null
@@ -1,121 +0,0 @@
-# Hardware Extensions
-
-RCS supports integration with various hardware platforms via dedicated **hardware extensions**.  
-These allow you to run the same RCS APIs on **real robots and sensors** without modifying your main codebase.
-
-For example, the **Franka Emika Research 3 (FR3)** robot is supported via the [`rcs_fr3`](extensions/rcs_fr3) extension.
-
-All natively supported extensions are located in the [`extensions`](extensions) directory.
-
----
-
-## 1. Installing Hardware Extensions
-
-To enable hardware usage in RCS, install the desired hardware extension via `pip`:
-
-    pip install -ve extensions/<extension_name>
-
-For example, to install the FR3 extension:
-
-    pip install -ve extensions/rcs_fr3
-
->  **Tip:** Each extension may have its own setup requirements. See the specific extension's section below for additional configuration steps.
-
----
-
-## 2. Switching Between Simulation and Hardware
-
-After installing the required extension, you can switch your RCS instance from simulation to hardware by setting:
-
-    from rcs.common import RobotPlatform
-
-    ROBOT_INSTANCE = RobotPlatform.SIMULATION   # Default: Simulation
-    # ROBOT_INSTANCE = RobotPlatform.HARDWARE   # Uncomment to use real hardware
-
----
-
-## 3. Command-Line Interfaces (CLI)
-
-Some extensions provide CLI commands to interact with hardware without writing Python code.
-
-For example:
-
-    python -m rcs_fr3 --help
-    python -m rcs_realsense --help
-
-These commands allow you to perform basic operations directly from your terminal.
-
----
-
-## 4. FR3 Hardware Extension
-
-The **RCS FR3** extension enables control of the **Franka Emika Research 3** robot via RCS.
-
-### 4.1 Additional Configuration
-
-1. Create a `.env` file in your working directory with your FR3 Desk credentials:
-
-       DESK_USERNAME=your_username
-       DESK_PASSWORD=your_password
-
-2. Set your FR3’s IP address in your script:
-
-       ROBOT_IP = "192.168.0.1"  # Replace with your robot's IP
-
----
-
-### 4.2 Usage Example
-
-    import numpy as np
-    import rcs
-    import rcs_fr3
-    from rcs_fr3._core import hw
-    from rcs_fr3.desk import FCI, Desk, load_creds_fr3_desk
-    from rcs_fr3.config import FR3Config, IKSolver
-    from rcs.common import Pose, FrankaHandTCPOffset, RobotPlatform
-
-    ROBOT_IP = "192.168.0.1"
-    ROBOT_INSTANCE = RobotPlatform.HARDWARE
-
-    # Load credentials
-    user, pw = load_creds_fr3_desk()
-
-    # Connect to the robot
-    with FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False):
-        urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
-        ik = rcs.common.RL(str(urdf_path))
-
-        # Initialize robot
-        robot = hw.FR3(ROBOT_IP, ik)
-        robot_cfg = FR3Config()
-        robot_cfg.tcp_offset = Pose(FrankaHandTCPOffset())
-        robot_cfg.ik_solver = IKSolver.rcs_ik
-        robot.set_parameters(robot_cfg)
-
-        # Configure gripper
-        gripper_cfg = hw.FHConfig()
-        gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1
-        gripper_cfg.speed = 0.1
-        gripper_cfg.force = 30
-        gripper = hw.FrankaHand(ROBOT_IP, gripper_cfg)
-
-        # Move and grasp
-        robot.set_cartesian_position(
-            robot.get_cartesian_position() * Pose(translation=np.array([0.05, 0, 0]))
-        )
-        gripper.grasp()
-
----
-
-### 4.3 CLI for FR3
-
-The FR3 extension also defines useful CLI commands for controlling the robot without the Desk website:
-
-    python -m rcs_fr3 --help
-
----
-
-## 5. Additional Resources
-
-- **Examples:** See the [`examples`](../../examples/) folder for more usage samples.
--  **Franka Desk Documentation:** Refer to the FR3 manufacturer’s manual for setup and safety.
diff --git a/docs/documentation/usage.md b/docs/documentation/usage.md
deleted file mode 100644
index 301e9f22..00000000
--- a/docs/documentation/usage.md
+++ /dev/null
@@ -1,83 +0,0 @@
-# Library Usage / API
-The python package is called `rcs`.
-
-## Direct Robot Control
-Simple direct robot control:
-```python
-import rcs
-from rcs import sim
-from rcs._core.sim import CameraType
-from rcs.camera.sim import SimCameraConfig, SimCameraSet
-simulation = sim.Sim(rcs.scenes["fr3_empty_world"]["mjb"])
-urdf_path = rcs.scenes["fr3_empty_world"]["urdf"]
-ik = rcs.common.RL(str(urdf_path))
-cfg = sim.SimRobotConfig()
-cfg.add_id("0")
-cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
-robot = rcs.sim.SimRobot(simulation, ik, cfg)
-
-gripper_cfg_sim = sim.SimGripperConfig()
-gripper_cfg_sim.add_id("0")
-gripper = sim.SimGripper(simulation, gripper_cfg_sim)
-
-# add camera to have a rendering gui
-cameras = {
-    "wrist": SimCameraConfig(
-        identifier="wrist_0",
-        type=CameraType.fixed,
-        resolution_width=640,
-        resolution_height=480,
-        frame_rate=30,
-    ),
-}
-camera_set = SimCameraSet(simulation, cameras)
-simulation.open_gui()
-robot.set_cartesian_position(
-    robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0.05, 0, 0]))
-)
-gripper.grasp()
-simulation.step_until_convergence()
-```
-## Gym Env Interface
-```python
-from rcs.envs.creators import SimEnvCreator
-from rcs.envs.utils import (
-    default_mujoco_cameraset_cfg,
-    default_sim_gripper_cfg,
-    default_sim_robot_cfg,
-)
-from rcs.envs.base import ControlMode, RelativeTo
-env_rel = SimEnvCreator()(
-    control_mode=ControlMode.JOINTS,
-    collision_guard=False,
-    robot_cfg=default_sim_robot_cfg(),
-    gripper_cfg=default_sim_gripper_cfg(),
-    cameras=default_mujoco_cameraset_cfg(),
-    max_relative_movement=np.deg2rad(5),
-    relative_to=RelativeTo.LAST_STEP,
-)
-env_rel.get_wrapper_attr("sim").open_gui()
-
-for _ in range(10):
-    obs, info = env_rel.reset()
-    for _ in range(10):
-        # sample random relative action and execute it
-        act = env_rel.action_space.sample()
-        print(act)
-        obs, reward, terminated, truncated, info = env_rel.step(act)
-        print(obs)
-        if truncated or terminated:
-            logger.info("Truncated or terminated!")
-            return
-```
-## Examples
-Checkout the python examples in the [examples](examples) folder:
-- [fr3_direct_control.py](examples/fr3.py) shows direct robot control with RCS's python bindings
-- [fr3_env_joint_control.py](examples/env_joint_control.py) and [fr3_env_cartesian_control.py](examples/env_cartesian_control.py) demonstrates RCS's high level [gymnasium](https://gymnasium.farama.org/) interface both for joint- and end effector space control
-All of these examples work both in the MuJoCo simulation as well as on your hardware FR3.
-
-
-```{toctree}
-:maxdepth: 1
-
-```
\ No newline at end of file
diff --git a/docs/extensions/index.md b/docs/extensions/index.md
new file mode 100644
index 00000000..f72ce7e7
--- /dev/null
+++ b/docs/extensions/index.md
@@ -0,0 +1,15 @@
+# Extensions
+
+```{toctree}
+:maxdepth: 2
+
+overview
+rcs_fr3
+rcs_panda
+rcs_xarm7
+rcs_so101
+rcs_realsense
+rcs_usb_cam
+rcs_tacto
+rcs_robotics_library
+```
diff --git a/docs/extensions/overview.md b/docs/extensions/overview.md
new file mode 100644
index 00000000..efe3138f
--- /dev/null
+++ b/docs/extensions/overview.md
@@ -0,0 +1,38 @@
+# Extensions Overview
+
+RCS is designed to be modular. Core functionality is kept minimal, while specific hardware support and additional features are provided through **extensions**.
+
+## What is an Extension?
+
+An extension is a separate Python package that integrates with RCS. Extensions can provide:
+- **Hardware Support**: Drivers for specific robots (e.g., FR3, xArm7) or sensors (e.g., RealSense).
+- **Simulation Assets**: MJCF/URDF files for new robots.
+- **Additional Functionality**: Integrations with other libraries (e.g., Robotics Library).
+
+## Installing Extensions
+
+Extensions are typically installed via `pip`.
+
+```shell
+pip install -ve extensions/rcs_fr3
+```
+
+## Available Extensions
+
+RCS comes with several supported extensions:
+
+- **rcs_fr3**: Support for the Franka Research 3 robot.
+- **rcs_panda**: Support for the Franka Emika Panda robot.
+- **rcs_xarm7**: Support for the xArm7 robot.
+- **rcs_ur5e**: Support for the UR5e robot.
+- **rcs_so101**: Support for the SO101 robot.
+- **rcs_realsense**: Support for Intel RealSense cameras.
+- **rcs_usb_cam**: Support for generic USB webcams.
+- **rcs_tacto**: Integration with the Tacto tactile sensor simulator.
+- **rcs_robotics_library**: Integration with the Robotics Library (RL).
+
+## Creating Extensions
+
+You can create your own extensions to add support for new hardware or features.
+- [Creating a Python Extension](../development/python_extension.md)
+- [Creating a C++ Extension](../development/cpp_extension.md)
diff --git a/docs/extensions/rcs_fr3.md b/docs/extensions/rcs_fr3.md
new file mode 100644
index 00000000..ee501e39
--- /dev/null
+++ b/docs/extensions/rcs_fr3.md
@@ -0,0 +1,64 @@
+# RCS FR3 Extension
+
+This extension provides support for the Franka Research 3 (FR3) robot in RCS.
+
+## Installation
+
+```shell
+pip install -ve extensions/rcs_fr3
+```
+
+### Configuration
+
+Add your FR3 credentials to a `.env` file:
+
+```bash
+DESK_USERNAME=...
+DESK_PASSWORD=...
+```
+
+## Usage
+
+```python
+import rcs_fr3
+from rcs_fr3._core import hw
+from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_franka_desk
+import rcs
+import numpy as np
+
+ROBOT_IP = "172.16.0.2" # Replace with your robot IP
+
+user, pw = load_creds_franka_desk()
+with FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False):
+    urdf_path = rcs.scenes["fr3_empty_world"].urdf
+    ik = rcs.common.RL(str(urdf_path))
+    
+    # Configure Robot
+    robot = hw.Franka(ROBOT_IP, ik)
+    robot_cfg = hw.FR3Config()
+    robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
+    robot.set_config(robot_cfg)
+
+    # Configure Gripper
+    gripper_cfg_hw = hw.FHConfig()
+    gripper_cfg_hw.epsilon_inner = gripper_cfg_hw.epsilon_outer = 0.1
+    gripper_cfg_hw.speed = 0.1
+    gripper_cfg_hw.force = 30
+    gripper = hw.FrankaHand(ROBOT_IP, gripper_cfg_hw)
+    
+    # Move Robot
+    robot.set_cartesian_position(
+        robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0.05, 0, 0]))
+    )
+    
+    # Grasp
+    gripper.grasp()
+```
+
+## CLI
+
+The extension defines useful commands to handle the FR3 robot without the need to use the Desk Website.
+
+```shell
+python -m rcs_fr3 --help
+```
diff --git a/docs/extensions/rcs_panda.md b/docs/extensions/rcs_panda.md
new file mode 100644
index 00000000..10846fe9
--- /dev/null
+++ b/docs/extensions/rcs_panda.md
@@ -0,0 +1,13 @@
+# RCS Panda Extension
+
+This extension provides support for the Franka Emika Panda robot in RCS.
+
+## Installation
+
+```shell
+pip install -ve extensions/rcs_panda
+```
+
+## Usage
+
+Please refer to the `rcs_fr3` documentation for similar usage patterns, or check the examples in the repository.
diff --git a/docs/extensions/rcs_realsense.md b/docs/extensions/rcs_realsense.md
new file mode 100644
index 00000000..578b3373
--- /dev/null
+++ b/docs/extensions/rcs_realsense.md
@@ -0,0 +1,15 @@
+# RCS RealSense Extension
+
+This extension provides support for Intel RealSense cameras in RCS.
+
+## Installation
+
+```shell
+pip install -ve extensions/rcs_realsense
+```
+
+## CLI
+
+```shell
+python -m rcs_realsense --help
+```
diff --git a/docs/extensions/rcs_robotics_library.md b/docs/extensions/rcs_robotics_library.md
new file mode 100644
index 00000000..d7cf7a87
--- /dev/null
+++ b/docs/extensions/rcs_robotics_library.md
@@ -0,0 +1,9 @@
+# RCS Robotics Library Extension
+
+This extension provides integration with the [Robotics Library (RL)](https://www.roboticslibrary.org/) for kinematics and path planning.
+
+## Installation
+
+```shell
+pip install -ve extensions/rcs_robotics_library
+```
diff --git a/docs/extensions/rcs_so101.md b/docs/extensions/rcs_so101.md
new file mode 100644
index 00000000..48576d05
--- /dev/null
+++ b/docs/extensions/rcs_so101.md
@@ -0,0 +1,9 @@
+# RCS SO101 Extension
+
+This extension provides support for the SO101 robot in RCS.
+
+## Installation
+
+```shell
+pip install -ve extensions/rcs_so101
+```
diff --git a/docs/extensions/rcs_tacto.md b/docs/extensions/rcs_tacto.md
new file mode 100644
index 00000000..a1b94112
--- /dev/null
+++ b/docs/extensions/rcs_tacto.md
@@ -0,0 +1,9 @@
+# RCS Tacto Extension
+
+This extension provides integration with the [Tacto](https://github.com/facebookresearch/tacto) tactile sensor simulator.
+
+## Installation
+
+```shell
+pip install -ve extensions/rcs_tacto
+```
diff --git a/docs/extensions/rcs_usb_cam.md b/docs/extensions/rcs_usb_cam.md
new file mode 100644
index 00000000..aaa27574
--- /dev/null
+++ b/docs/extensions/rcs_usb_cam.md
@@ -0,0 +1,9 @@
+# RCS USB Cam Extension
+
+This extension provides support for generic USB webcams in RCS.
+
+## Installation
+
+```shell
+pip install -ve extensions/rcs_usb_cam
+```
diff --git a/docs/extensions/rcs_xarm7.md b/docs/extensions/rcs_xarm7.md
new file mode 100644
index 00000000..43449254
--- /dev/null
+++ b/docs/extensions/rcs_xarm7.md
@@ -0,0 +1,13 @@
+# RCS xArm7 Extension
+
+This extension provides support for the xArm7 robot in RCS.
+
+## Installation
+
+```shell
+pip install -ve extensions/rcs_xarm7
+```
+
+## Usage
+
+Please refer to the examples in the repository for usage details.
diff --git a/docs/getting_started/index.md b/docs/getting_started/index.md
new file mode 100644
index 00000000..8ac559a0
--- /dev/null
+++ b/docs/getting_started/index.md
@@ -0,0 +1,136 @@
+# Getting Started
+
+## Installation
+
+We build and test RCS on the latest Debian and on the latest Ubuntu LTS.
+
+### Prerequisites
+
+1.  Install the system dependencies:
+
+    ```shell
+    sudo apt install $(cat debian_deps.txt)
+    ```
+
+2.  Create, activate and configure a [Python virtual environment](https://docs.python.org/3/library/venv.html):
+
+    ```shell
+    python3 -m venv .venv
+    source .venv/bin/activate
+    ```
+
+3.  Install the package dependencies:
+
+    ```shell
+    pip install -r requirements_dev.txt
+    pip config --site set global.no-build-isolation false
+    ```
+
+### Building RCS
+
+Build and install RCS in editable mode:
+
+```shell
+pip install -ve .
+```
+
+For a docker deployment, see the `docker` folder in the repository.
+
+## Basic Usage
+
+The python package is called `rcs`.
+
+### Direct Robot Control
+
+Here is a simple example of direct robot control using the low-level API:
+
+```python
+import rcs
+from rcs import sim
+from rcs._core.sim import CameraType
+from rcs.camera.sim import SimCameraConfig, SimCameraSet
+from time import sleep
+import numpy as np
+
+# Load simulation scene
+simulation = sim.Sim(rcs.scenes["fr3_empty_world"].mjb)
+urdf_path = rcs.scenes["fr3_empty_world"].urdf
+ik = rcs.common.RL(str(urdf_path))
+
+# Configure robot
+cfg = sim.SimRobotConfig()
+cfg.add_id("0")
+cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
+robot = rcs.sim.SimRobot(simulation, ik, cfg)
+
+# Configure gripper
+gripper_cfg_sim = sim.SimGripperConfig()
+gripper_cfg_sim.add_id("0")
+gripper = sim.SimGripper(simulation, gripper_cfg_sim)
+
+# Configure cameras
+camera_set = SimCameraSet(simulation, {})
+
+# Open GUI
+simulation.open_gui()
+sleep(5)
+
+# Step the robot 10 cm in x direction
+robot.set_cartesian_position(
+    robot.get_cartesian_position() * rcs.common.Pose(translation=np.array([0.1, 0, 0]))
+)
+
+# Close gripper
+gripper.grasp()
+
+# Step simulation
+simulation.step_until_convergence()
+input("press enter to close")
+```
+
+### Gymnasium Interface
+
+RCS provides a high-level [Gymnasium](https://gymnasium.farama.org/) interface for Reinforcement Learning and general control.
+
+```python
+from rcs.envs.creators import SimEnvCreator
+from rcs.envs.utils import (
+    default_mujoco_cameraset_cfg,
+    default_sim_gripper_cfg,
+    default_sim_robot_cfg,
+)
+from rcs.envs.base import ControlMode, RelativeTo
+import numpy as np
+
+# Create environment
+env_rel = SimEnvCreator()(
+    control_mode=ControlMode.JOINTS,
+    robot_cfg=default_sim_robot_cfg(),
+    gripper_cfg=default_sim_gripper_cfg(),
+    cameras=default_mujoco_cameraset_cfg(),
+    max_relative_movement=np.deg2rad(5),
+    relative_to=RelativeTo.LAST_STEP,
+)
+
+# Open GUI
+env_rel.get_wrapper_attr("sim").open_gui()
+
+# Run loop
+for _ in range(100):
+    obs, info = env_rel.reset()
+    for _ in range(10):
+        # Sample random relative action and execute it
+        act = env_rel.action_space.sample()
+        print(act)
+        obs, reward, terminated, truncated, info = env_rel.step(act)
+        print(obs)
+```
+
+## Examples
+
+Check out the python examples in the `examples` folder of the repository.
+- `fr3_direct_control.py`: Direct robot control with RCS's python bindings.
+- `fr3_env_joint_control.py`: Gymnasium interface with joint control.
+- `fr3_env_cartesian_control.py`: Gymnasium interface with Cartesian control.
+
+Most examples work both in the MuJoCo simulation as well as on hardware (with appropriate extensions installed).
diff --git a/docs/index.md b/docs/index.md
index ab71c596..f426022e 100644
--- a/docs/index.md
+++ b/docs/index.md
@@ -1,38 +1,62 @@
-# Robotic Control Stack (RCS)
+# Robot Control Stack
 
+**Robot Control Stack (RCS)** is a unified and multilayered robot control interface over a MuJoCo simulation and real-world robots. It is designed to be a lean ecosystem for robot learning at scale.
 
-![image](images/rcs_architecture_small.svg)
+```{image} _static/rcs_architecture_small.svg
+:alt: RCS Architecture
+:align: center
+```
 
-**RCS**  is a modular toolset designed to streamline the process of setting up and running robotics experiments and simulations. Rather than being a rigid framework, RCS acts as a minimalist, scalable architecture that brings together existing tools into an integrated system with simple, unified interfaces.
+## Features
 
-## Key Characteristics
+- **Unified Interface**: Seamlessly switch between simulation (MuJoCo) and real hardware.
+- **Layered Architecture**: 
+    - **High-Level**: Gymnasium-based Python API for RL and general control.
+    - **Low-Level**: C++ core with Python bindings for performance-critical tasks.
+- **Extensible**: Easy to add new robots and sensors via C++ or Python extensions.
+- **Lean**: Minimal dependencies and overhead.
 
-- **Unified Simulation and Hardware Interface**  
-  Seamless transitions between simulation and physical robot execution, reducing potential obstacles in development and testing cycles.
+## Documentation
 
-- **Minimal Dependencies**  
-  Lightweight and easy to set up, ensuring high portability across environments.
-
-- **Robot-Agnostic Design**  
-  Initially developed for the Franka Research 3 robot, but equipped with abstraction layers to easily support other robot types.
+```{toctree}
+:maxdepth: 2
+:caption: API
 
-- **Gymnasium-Style API**  
-  Offers a familiar and standardized interface for reinforcement learning, promoting ease of integration.
+api/index
+```
 
----
+```{toctree}
+:maxdepth: 2
+:caption: Extensions
 
-## Use Cases of RCS
+extensions/index
+```
 
-- **Teleoperation for Data Collection**  
-  RCS supports teleoperation to manually control robots and collect expert task demonstrations, which are essential for training robotics foundation models (RFMs).
+```{toctree}
+:maxdepth: 2
+:caption: Extending RCS
 
-- **Policy-Based Control**  
-  RCS allows robots to be controlled using trained imitation and reinforcement learning policies by providing a modular interface for easy policy switching and remote inference execution.
+development/index
+```
 
 ```{toctree}
-:maxdepth: 1
-documentation.md
-contributing.md
-changelog.md
-roadmap.md
-```
\ No newline at end of file
+:maxdepth: 2
+:caption: Project Info
+
+meta/index
+```
+
+## Citation
+
+If you find RCS useful for your academic work, please consider citing it:
+
+```bibtex
+@misc{juelg2025robotcontrolstack,
+  title={{Robot Control Stack}: {A} Lean Ecosystem for Robot Learning at Scale}, 
+  author={Tobias J{\"u}lg and Pierre Krack and Seongjin Bien and Yannik Blei and Khaled Gamal and Ken Nakahara and Johannes Hechtl and Roberto Calandra and Wolfram Burgard and Florian Walter},
+  year={2025},
+  howpublished = {\url{https://arxiv.org/abs/2509.14932}}
+}
+```
+
+For more scientific info, visit the [paper website](https://robotcontrolstack.github.io/).
diff --git a/docs/meta/changelog.md b/docs/meta/changelog.md
new file mode 100644
index 00000000..61b65eec
--- /dev/null
+++ b/docs/meta/changelog.md
@@ -0,0 +1,88 @@
+# Changelog
+
+## v0.5.2 (2025-10-09)
+
+### Features
+- Added OMPL example and cleaned up OMPL code.
+
+## v0.5.1 (2025-09-29)
+
+### Fixes
+- Fixed RCS versioning in extensions.
+
+## v0.5.0 (2025-09-26)
+
+### Features
+- **Extensions**: Refactored Robotics Library IK into its own extension.
+- **Simulation**: Added support for async and realtime mode in SimConfig.
+- **Environment**: Added new environment creators for diffpol and agent evaluation.
+- **Hardware**: Added digital twin support for xArm and initial support for SO101.
+- **Docker**: Added full Docker support with GPU acceleration.
+- **Kinematics**: Added Pinocchio IK support with MJCF.
+- **Calibration**: Added full calibration support and cache for RealSense.
+
+### Fixes
+- Fixed async joint control mode.
+- Fixed random object orientation setting.
+- Resolved various simulation issues (joint/actuator confusion, robot IDs).
+
+## v0.4.0 (2025-05-12)
+
+### Features
+- **Recording**: Added HDF5 recorder wrapper with gzip compression.
+- **Teleoperation**: Added async support for teleoperation and webcam live viewer.
+- **Environment**: Added collision guard and random cube placement wrapper.
+- **Camera**: Added video recording support and rate limiter.
+
+### Fixes
+- Fixed FR3 desk errors.
+- Improved environment type assertions and tests.
+- Fixed simulation GUI rendering limits.
+
+## v0.3.1 (2024-10-02)
+
+### Fixes
+- Fixed optional IK bug and FR3 example.
+
+## v0.3.0 (2024-10-02)
+
+### Features
+- **Simulation**: Added interactive sim viewer in a separate process.
+- **GUI**: Refactored GUI with base class and added MuJoCo UI library.
+
+### Fixes
+- Fixed missing depth data in camera environment.
+
+## v0.2.2 (2024-10-01)
+
+### Features
+- Added depth data to `CameraSetWrapper`.
+
+### Fixes
+- Fixed RGB+Depth mode in camera environment.
+
+## v0.2.1 (2024-09-13)
+
+### Fixes
+- Fixed imports and max movement in examples.
+
+## v0.2.0 (2024-09-13)
+
+### Features
+- **Teleoperation**: Added keyboard-based robot teleoperation.
+- **Camera**: Added ring buffer for hardware cameras.
+- **Environment**: Added collision guard environment and parameterizable max movement.
+- **Tools**: Added live plotter for robot poses.
+
+### Fixes
+- Fixed desk path issues.
+- Fixed camera thread checks.
+- Resolved various linting and type hinting issues.
+
+## v0.1.0 (2024-06-28)
+
+### Features
+- Initial release of RCS.
+- **Environment**: Added CameraSet Gym Env.
+- **Hardware**: Added support for RealSense cameras and basic robot/gripper interfaces.
+- **CI**: Added CI pipeline with linting and testing.
diff --git a/docs/meta/index.md b/docs/meta/index.md
new file mode 100644
index 00000000..0e9cccb7
--- /dev/null
+++ b/docs/meta/index.md
@@ -0,0 +1,8 @@
+# Project Info
+
+```{toctree}
+:maxdepth: 2
+
+changelog
+roadmap
+```
diff --git a/docs/meta/roadmap.md b/docs/meta/roadmap.md
new file mode 100644
index 00000000..a69cc089
--- /dev/null
+++ b/docs/meta/roadmap.md
@@ -0,0 +1,4 @@
+# Roadmap
+
+- [ ] Add support for more robots.
+- [ ] Improve simulation fidelity.
diff --git a/docs/requirements.txt b/docs/requirements.txt
index 236ac6bb..9de44838 100644
--- a/docs/requirements.txt
+++ b/docs/requirements.txt
@@ -2,4 +2,6 @@ sphinx
 pydata-sphinx-theme
 myst-parser
 linkify-it-py
-sphinx-autobuild
\ No newline at end of file
+sphinx-autobuild
+sphinx-design
+sphinx-copybutton
\ No newline at end of file
diff --git a/docs/roadmap.md b/docs/roadmap.md
deleted file mode 100644
index 7feee685..00000000
--- a/docs/roadmap.md
+++ /dev/null
@@ -1,2 +0,0 @@
-# Roadmap
-TODO
diff --git a/docs/user_guide/architecture.md b/docs/user_guide/architecture.md
new file mode 100644
index 00000000..bf5cdd2d
--- /dev/null
+++ b/docs/user_guide/architecture.md
@@ -0,0 +1,47 @@
+# Architecture
+
+RCS is designed from the ground up to support research in robot learning with large-scale generalist policies. It features a modular and easily extensible layered architecture with a unified interface for simulated and physical robots.
+
+```{image} ../_static/rcs_architecture_small.svg
+:alt: RCS Architecture
+:align: center
+```
+
+## Core Design Principles
+
+1.  **Unified Interface**: RCS provides a unified interface for both simulated (MuJoCo) and physical robots. This facilitates seamless sim-to-real transfer and enables using the simulation as a digital twin.
+2.  **Layered Architecture**:
+    *   **High-Level**: Applications access robots, sensors, and actuators through a [Gymnasium](https://gymnasium.farama.org/)-based Python API.
+    *   **Low-Level**: The lower layers expose a C++ API for performance-critical features.
+3.  **Environment Wrappers**: RCS is designed around the concept of environment wrappers. Each scene is a sequence of wrappers that can mutate the action and/or observation space.
+
+## Environment Wrappers
+
+An environment wrapper is a tuple $W = \langle f: S \to S', g: A' \to A, P', R' \rangle$, where $f$ and $g$ are mappings that transform the state and actions of a Markov Decision Process (MDP).
+
+Each scene is a sequence of $n$ wrappers. At each time step, an agent issues an action $A_t$ to the wrapped MDP that is propagated through the action mutation function chain of the wrappers. The action is then passed to the base MDP (the robot interface), which produces an observation state. The observation mutation function chain updates the state and returns it to the agent.
+
+Wrappers allow for modular additions of functionality, such as:
+- **Gripper Wrapper**: Adds gripper dimensions to action/observation spaces.
+- **Camera Wrapper**: Adds camera frames to the observation space.
+- **Recorder Wrapper**: Records data from the scene.
+
+## Hardware Abstraction
+
+RCS defines interfaces and off-the-shelf wrappers for common sensors and actuators.
+- **Cameras**: Wrapper implementing polling for a set of cameras.
+- **End Effectors**: Wrapper for grippers or robot hands.
+
+Adding new hardware typically involves writing a new wrapper or implementing the C++ interface for the device.
+
+## Simulation
+
+RCS leverages the [MuJoCo](https://mujoco.org/) physics simulation. It extends MuJoCo's API with customized functions for robotics use cases while leaving core data structures exposed.
+- **Synchronous Operation**: RCS implements a callback mechanism to enable synchronous operation and interrupts (e.g., stopping on collision).
+- **Digital Twin**: RCS supports running a digital twin by running both the physical robot and the MuJoCo-based simulation in parallel.
+
+## Robotics Tool Kit
+
+RCS integrates established tools:
+- **Pinocchio**: For kinematics (IK/FK), using MuJoCo MJCF descriptions.
+- **OMPL**: For motion planning.
diff --git a/docs/user_guide/gym_interface.md b/docs/user_guide/gym_interface.md
new file mode 100644
index 00000000..257a213f
--- /dev/null
+++ b/docs/user_guide/gym_interface.md
@@ -0,0 +1,54 @@
+# Gymnasium Interface
+
+The high-level interface of RCS is based on [Gymnasium](https://gymnasium.farama.org/). This allows for easy integration with Reinforcement Learning libraries and standard control pipelines.
+
+## Environment Creation
+
+To facilitate environment creation, RCS ships with environment factory classes that create an envrionment already wrapped with the most common wrappers.
+Simulated environments are created using the `SimEnvCreator`.
+
+```python
+from rcs.envs.creators import SimEnvCreator
+from rcs.envs.base import ControlMode
+
+env = SimEnvCreator()(
+    control_mode=ControlMode.JOINTS,
+    # ... configuration objects ...
+)
+```
+
+Hardware environments are created using the robot-specific environment creator functions, located in the hardware extensions, usually named `<RobotName>EnvCreator`.
+```python
+from rcs_fr3.creators import RCSFR3EnvCreator
+from rcs.envs.base import ControlMode
+
+env = RCSFR3EnvCreator()(
+    ip="192.168.100.1",
+    control_mode=ControlMode.JOINTS,
+    # ... configuration objects ...
+)
+```
+
+
+
+## Control Modes
+
+RCS supports various control modes:
+- **Joint Control**: Control the robot's joint positions or velocities.
+- **Cartesian Control**: Control the end-effector pose.
+
+## Synchronous vs Asynchronous
+
+By default, Gymnasium environments in RCS are **synchronous**. The `step()` function returns only once the action has been fully executed and the environment has reached the target state.
+
+It is possible to configure RCS to execute actions **asynchronously**, where `step()` returns instantly. This is useful for teleoperation or high-frequency control loops where the agent doesn't wait for the robot to settle.
+
+## Wrappers
+
+RCS uses standard Gymnasium wrappers to extend functionality.
+- **Observation Wrappers**: Modify the observation space (e.g., stacking frames, processing images).
+- **Action Wrappers**: Modify the action space (e.g., normalizing actions).
+
+## Reset Stack
+
+RCS implements a flexible reset mechanism. When `reset()` is called, the environment can be randomized or set to a specific state based on the configuration.
diff --git a/docs/user_guide/low_level_api.md b/docs/user_guide/low_level_api.md
new file mode 100644
index 00000000..79fea2e0
--- /dev/null
+++ b/docs/user_guide/low_level_api.md
@@ -0,0 +1,35 @@
+# Low-Level API
+
+At its core, RCS provides a C++ interface that defines all functions needed to control a robot in an abstract manner. This interface has Python bindings, allowing for direct control without the overhead of the Gymnasium interface.
+
+## C++ Interface
+
+The C++ layer handles:
+- **Real-time Control**: Communication with robot hardware drivers.
+- **Simulation Stepping**: Interfacing with MuJoCo.
+- **Kinematics**: Fast IK/FK calculations using Pinocchio.
+
+## Python Bindings
+
+The Python bindings expose the C++ functionality to Python. This allows you to:
+- Create `SimRobot` or `HardwareRobot` instances.
+- Send joint or Cartesian commands directly.
+- Read robot state (positions, velocities, torques).
+- Interface with sensors (cameras, grippers).
+
+### Example: Direct Control
+
+```python
+import rcs.sim as sim
+# ... setup ...
+robot.set_cartesian_position(target_pose)
+simulation.step_until_convergence()
+```
+
+## Adding New Robots
+
+Support for new robots can be implemented in both C++ and Python.
+- **C++**: Implement the `Robot` interface for high-performance drivers.
+- **Python**: Implement the python-side interface for easier prototyping or python-only drivers.
+
+The base environment is implementation-agnostic and works with any robot that adheres to the interface.
diff --git a/extensions/README.md b/extensions/README.md
index 33a88b48..fa27f7e2 100644
--- a/extensions/README.md
+++ b/extensions/README.md
@@ -1,8 +1,7 @@
 # Hardware Extensions
 
-To install hardware exentsion use
-```shell
-pip install -ve <extension> # e.g. rcs_fr3
-```
-Some extensions need further dependencies or further setups. Please to check out their readme pages for details.
-When installing the SO101 extension, make sure to check its `README_IMPORTANT.md` for dealing with dependency conflict.
\ No newline at end of file
+RCS supports various hardware extensions for robots and sensors.
+
+For detailed documentation on available extensions and how to create your own, please visit:
+
+**[robot-control-stack.org/extensions](https://robot-control-stack.org/extensions)**
\ No newline at end of file