diff --git a/CHANGELOG.md b/CHANGELOG.md
index 696412334..33dbc55aa 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,6 +1,7 @@
 # CHANGELOG
 
 Release Versions:
+- [6.0.0](#600)
 - [5.2.0](#520)
 - [5.1.0](#510)
 - [5.0.0](#500)
@@ -11,6 +12,65 @@ Release Versions:
 - [2.0.0](#200)
 - [1.0.0](#100)
 
+## 6.0.0
+
+Version 6.0.0 refactors the StateType enum in state representation to be more descriptive and widely useful as well as 
+the dynamical systems factory for easier creation of these classes. This major change breaks implementations using
+StateType enums explicitly (e.g. for parameters) and dynamical systems from the prior version.
+
+See the updated documentation for usage guidelines for the DynamicalSystemsFactory.
+
+This release also includes a few new features, fixes and improvements as well as updated documentation in 
+all parts of the control libraries.
+
+### Breaking changes
+
+This release contains the following breaking changes:
+- Refactor `StateType` enum and `ParameterType` enum (#277, #278, #280, #284)
+- Relocate the `DYNAMICAL_SYSTEM_TYPE` enum to the general `dynamical_systems` namespace (#288)
+
+**state_representation**
+
+To make the `StateType` enum more descriptive and useful, individual spatial state types have been added
+and the `ParameterType` moved to a separate enum. This change helps with the translation of parameters and
+encoded states in downstream projects by allowing for greater introspection of data types from this enum.
+
+Any implementations that use `StateType` from a prior version to create parameters or compare state types should be
+updated.
+
+**dynamical_systems**
+
+The previous `DYNAMICAL_SYSTEM` enum has been removed from the dynamical system factory class and put directly in the
+general `dynamical_systems` namespace and renamed to `DYNAMICAL_SYSTEM_TYPE` to shorten the creation and to have the
+same structure as the controllers factory. 
+
+Any implementations that use the previous enum have to be updated. See the documentation for more information.
+
+### Features
+
+**state_representation**
+- Add CartesianState attribute setters from std vector and coefficients (#291)
+
+**python**
+- Use pyquaternion in bindings (#283)
+
+**protocol**
+- Add encode/decode options for shared pointer of State (#287)
+- Add CMake config for clproto (#292)
+
+### Fixes and improvements
+
+**state_representation**
+- Mark Parameter getters as const (#289)
+- Throw exception upon parameter validation in controllers (#290)
+
+**python**
+- Fix if else conditions in setup.py to correctly install modules (#285)
+
+**general**
+- Update and extend demos with Python examples and migrate ROS demos away (#293)
+- Update documentation (#294)
+
 ## 5.2.0
 
 Version 5.2.0 contains a few fixes and a new feature for the Impedance controller.
diff --git a/README.md b/README.md
index 0e2dec760..68819083c 100644
--- a/README.md
+++ b/README.md
@@ -12,7 +12,7 @@
         </td>
     </tr>
     <tr>
-        <td width="25%"><a href="https://github.com/epfl-lasa/control-libraries/tree/develop">Development</a></td>
+        <td width="25%"><a href="https://github.com/epfl-lasa/control-libraries/tree/develop">Develop</a></td>
         <td width="75%"><img src="https://github.com/epfl-lasa/control-libraries/actions/workflows/build-test.yml/badge.svg?branch=develop"></td>
     </tr>
 </table>
@@ -21,18 +21,30 @@
 A set of libraries to facilitate the creation of full control loop algorithms,
 including trajectory planning, kinematics, dynamics and control.
 
-Documentation is available at <a href="https://epfl-lasa.github.io/control-libraries/versions/main/">epfl-lasa.github.io/control-libraries</a>
+Documentation is available at <a href="https://epfl-lasa.github.io/control-libraries/versions/">epfl-lasa.github.io/control-libraries</a>.
 
 ## Core libraries
 
-For the implementation, installation and documentation of the core libraries, see the
-<a href="https://github.com/epfl-lasa/control-libraries/tree/main/source">source</a> folder.
+For the implementation, installation and documentation of the core libraries, see the [source](./source) folder.
+
+## Protocol
+
+There is a module that defines the protocol for sending and receiving messages containing control libraries
+data across any network, based on the Google Protocol Buffer. For its implementation, installation and
+documentation, see the [protocol](./protocol) folder.
 
 ## Python bindings
 
-There exist Python bindings for core control library modules. See the <a href="https://github.com/epfl-lasa/control-libraries/tree/main/python">python</a>
-folder for installation instructions and currently supported libraries.
+There exist Python bindings for the control library modules and the protocol module. See the [python](./python)</a>
+folder for installation instructions.
 
 ## Demos
 
-For examples and demos in plain C++, ROS, and ROS2, refer to the <a href="https://github.com/epfl-lasa/control-libraries/tree/main/demos">demos</a> folder.
+For examples and demos in C++ and Python, refer to the [demos](./demos) folder.
+TODO link ros demos repo
+
+## External resources
+
+- C++ remote development in CLion [here](https://github.com/eeberhard/docker-clion-cpp-env)
+- ROS and ROS2 demos using control libraries [here](https://github.com/domire8/control-libraries-ros-demos)
+- ROS and ROS2 control libraries images [here](https://github.com/aica-technology/docker-images)
diff --git a/VERSION b/VERSION
index 91ff57278..f4965a313 100644
--- a/VERSION
+++ b/VERSION
@@ -1 +1 @@
-5.2.0
+6.0.0
\ No newline at end of file
diff --git a/demos/control_loop_examples/CMakeLists.txt b/demos/CMakeLists.txt
similarity index 79%
rename from demos/control_loop_examples/CMakeLists.txt
rename to demos/CMakeLists.txt
index e1110fd7c..19ee77b0d 100644
--- a/demos/control_loop_examples/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -26,13 +26,10 @@ set(DEMOS_SCRIPTS
 )
 
 set(FIXTURE_INSTALL_PATH /etc/fixtures/)
-install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/scripts/fixtures/ DESTINATION ${FIXTURE_INSTALL_PATH})
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/fixtures/ DESTINATION ${FIXTURE_INSTALL_PATH})
 
 foreach(SCRIPT ${DEMOS_SCRIPTS})
-  add_executable(${SCRIPT} scripts/${SCRIPT}.cpp)
+  add_executable(${SCRIPT} cpp_scripts/${SCRIPT}.cpp)
   target_link_libraries(${SCRIPT} ${control_libraries_LIBRARIES})
   target_compile_definitions(${SCRIPT} PRIVATE SCRIPT_FIXTURES="${FIXTURE_INSTALL_PATH}")
-  install(TARGETS ${SCRIPT}
-    RUNTIME DESTINATION bin
-  )
 endforeach()
\ No newline at end of file
diff --git a/demos/Dockerfile b/demos/Dockerfile
new file mode 100644
index 000000000..079a92d04
--- /dev/null
+++ b/demos/Dockerfile
@@ -0,0 +1,16 @@
+FROM ghcr.io/epfl-lasa/control-libraries/development-dependencies
+
+WORKDIR /tmp
+ARG CONTROL_LIBRARIES_BRANCH=develop
+RUN git clone -b ${CONTROL_LIBRARIES_BRANCH} --depth 1 https://github.com/epfl-lasa/control-libraries.git
+RUN cd control-libraries/source && ./install.sh --auto
+RUN cd control-libraries/protocol && ./install.sh --auto
+RUN pip3 install control-libraries/python
+
+RUN rm -rf /tmp/*
+
+USER developer
+WORKDIR ${HOME}/control_loop_examples
+COPY ./ ./
+
+RUN mkdir build && cd build && cmake .. && sudo make -j all && sudo make install
diff --git a/demos/README.md b/demos/README.md
new file mode 100644
index 000000000..202667304
--- /dev/null
+++ b/demos/README.md
@@ -0,0 +1,94 @@
+# `control_loop_examples` demonstration scripts
+
+## Table of contents:
+* [Running a demo script](#running-demonstration-scripts)
+* [Task Space control loop example](#task_space_control_loop)
+* [Robot kinematics control loop example](#robot_kinematics_control_loop)
+
+## Running demonstration scripts
+This package contains a set of demonstration scripts in both C++ and Python that showcase the functionalities introduced
+in the different libraries of `control-libraries`.
+The easiest way to run them is to use the `run-demo.sh` file.
+Without arguments, this script creates a container and opens in interactive mode, allowing you to browse the different
+demo scripts and run the one of your choice:
+
+```console
+./run-demo-script.sh
+# Run a python script
+developer@xxxxxxxxx:~/control_loop_examples$ python3 python_scripts/<script>.py
+# Run a cpp script from the build folder
+developer@xxxxxxxxx:~/control_loop_examples$ ./build/<script>
+```
+
+The available scripts are:
+
+### `task_space_control_loop`
+- **Showcased libraries:** `state_representation`, `dynamical_systems`
+
+This simple demonstration shows how to create a control loop with a `PointAttractor` dynamical system in task space
+(`CartesianState`). It moves a pose towards a random attractor in a `100Hz` control loop.
+The script outputs the current pose and distance to the attractor at each timestep, and the final pose on reaching it:
+
+```console
+...
+frame CartesianPose expressed in world frame
+position: (0.596701, 0.822976, -0.604581)
+orientation: (0.246491, -0.314866, -0.896822, 0.189239) <=> theta: 2.64348, axis: (-0.32489, -0.925375, 0.195264)
+distance to attractor: 0.001004
+-----------
+frame CartesianPose expressed in world frame
+position: (0.596702, 0.822979, -0.604584)
+orientation: (0.246488, -0.314866, -0.896823, 0.189239) <=> theta: 2.64348, axis: (-0.324891, -0.925375, 0.195264)
+distance to attractor: 0.000994
+-----------
+##### TARGET #####
+frame CartesianPose expressed in world frame
+position: (0.59688, 0.823295, -0.604897)
+orientation: (0.246242, -0.314924, -0.896867, 0.189256) <=> theta: 2.64399, axis: (-0.324929, -0.92536, 0.195269)
+##### CURRENT POSE #####
+frame CartesianPose expressed in world frame
+position: (0.596702, 0.822979, -0.604584)
+orientation: (0.246488, -0.314866, -0.896823, 0.189239) <=> theta: 2.64348, axis: (-0.324891, -0.925375, 0.195264)
+```
+
+### `robot_kinematics_control_loop`
+- **Showcased libraries:** `state_representation`, `dynamical_systems`, `robot_model`
+
+This demonstration reuses the previous dynamical system control loop but adds the robot component.
+The desired command, i.e. desired twist of the robot end-effector (eef) is sent to a dummy robot.
+At each timestep, the new joint state and eef state of the robot is computed using the robot kinematics from the robot
+model corresponding to the provided URDF.
+The robot is assumed to perfectly follow the computed desired state matching the eef desired twist.
+Similarly to the previous demonstration, the script outputs the current joint positions, eef pose and distance to the
+attractor at each timestep, and the final joint positions and pose on reaching it:
+
+```
+franka JointPositions
+names: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6, panda_joint7, ]
+positions: [-1.39453, 0.34338, 1.78372, -1.21918, -0.365912, 1.1687, -2.88819, ]
+panda_link8 CartesianPose expressed in panda_link0 frame
+position: (0.500692, 4.30377e-05, 0.750494)
+orientation: (-9.36521e-05, 8.70069e-05, 1, -0.000152405) <=> theta: 3.14141, axis: (-8.70069e-05, -1, 0.000152405)
+distance to attractor: 0.001249
+-----------
+franka JointPositions
+names: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6, panda_joint7, ]
+positions: [-1.39627, 0.352394, 1.79511, -1.2227, -0.374677, 1.16757, -2.88157, ]
+panda_link8 CartesianPose expressed in panda_link0 frame
+position: (0.500192, 1.07333e-05, 0.750183)
+orientation: (-1.9014e-05, 4.77102e-05, 1, -7.96025e-05) <=> theta: 3.14155, axis: (-4.77102e-05, -1, 7.96025e-05)
+distance to attractor: 0.000455
+-----------
+##### TARGET #####
+panda_link8 CartesianPose expressed in panda_link0 frame
+position: (0.5, 0, 0.75)
+orientation: (6.12323e-17, 0, 1, 0) <=> theta: 3.14159, axis: (0, 1, 0)
+##### CURRENT STATES #####
+franka JointPositions
+names: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6, panda_joint7, ]
+positions: [-1.39215, 0.362964, 1.80315, -1.22376, -0.385704, 1.16489, -2.87388, ]
+panda_link8 CartesianPose expressed in panda_link0 frame
+position: (0.500192, 1.07333e-05, 0.750183)
+orientation: (-1.9014e-05, 4.77102e-05, 1, -7.96025e-05) <=> theta: 3.14155, axis: (-4.77102e-05, -1, 7.96025e-05)
+
+```
\ No newline at end of file
diff --git a/demos/control_loop_examples/Dockerfile b/demos/control_loop_examples/Dockerfile
deleted file mode 100644
index d1b556100..000000000
--- a/demos/control_loop_examples/Dockerfile
+++ /dev/null
@@ -1,12 +0,0 @@
-FROM epfl-lasa/control-libraries/source:install
-
-WORKDIR /tmp/
-COPY . ./control_loop_examples
-
-WORKDIR /tmp/control_loop_examples/build
-RUN cmake .. && make -j all && make install
-
-WORKDIR /usr/local/bin
-RUN rm -rf /tmp/demos/
-
-USER developer
\ No newline at end of file
diff --git a/demos/control_loop_examples/README.md b/demos/control_loop_examples/README.md
deleted file mode 100644
index cef1e1a5e..000000000
--- a/demos/control_loop_examples/README.md
+++ /dev/null
@@ -1,107 +0,0 @@
-# `control_loop_examples` demonstration scripts
-
-## Table of contents:
-* [Running a demo script](#running-demonstration-scripts)
-* [Task Space control loop example](#task_space_control_loop)
-* [Robot kinematics control loop example](#robot_kinematics_control_loop)
-
-## Running demonstration scripts
-This package contains a set of demonstration scripts that showcase the functionalities introduced in the different
-libraries of `control-libraries`.
-The easiest way to run them is to use the `run-demo-script.sh` file.
-Without arguments, this script create a demo container and opens in interactive mode, allowing you to browse the different
-demo scripts and run the one of your choice:
-
-```console
-./run-demo-script.sh
-root@xxxxxxxx:/tmp/demos/build# ls
-CMakeCache.txt  CMakeFiles  Makefile  cmake_install.cmake  task_space_control_loop ...
-root@xxxxxxxx:/tmp/demos/build# ./task_space_control_loop
-```
-
-You can also directly specify the name of the demo script to execute in the arguments of the script. In this way,
-it runs the demo as command in the container and exit when the demo script finishes:
-
-```console
-./run-demo-script.sh task_space_control_loop
-```
-
-## `task_space_control_loop`
-* **Showcased libraries:** `state_representation`, `dynamical_systems`
-
-This simple demonstration shows how to create a control loop with a `Linear` dynamical system in task space (`CartesianState`).
-It moves a pose towards a random attractor in a `100Hz` control loop.
-The script outputs the current pose and distance to the attractor at each timestep, and the final pose on reaching it:
-
-```console
-...
-frame CartesianPose expressed in world frame
-position: (0.596701, 0.822976, -0.604581)
-orientation: (0.246491, -0.314866, -0.896822, 0.189239) <=> theta: 2.64348, axis: (-0.32489, -0.925375, 0.195264)
-distance to attractor: 0.001004
------------
-frame CartesianPose expressed in world frame
-position: (0.596702, 0.822979, -0.604584)
-orientation: (0.246488, -0.314866, -0.896823, 0.189239) <=> theta: 2.64348, axis: (-0.324891, -0.925375, 0.195264)
-distance to attractor: 0.000994
------------
-##### TARGET #####
-frame CartesianPose expressed in world frame
-position: (0.59688, 0.823295, -0.604897)
-orientation: (0.246242, -0.314924, -0.896867, 0.189256) <=> theta: 2.64399, axis: (-0.324929, -0.92536, 0.195269)
-##### CURRENT POSE #####
-frame CartesianPose expressed in world frame
-position: (0.596702, 0.822979, -0.604584)
-orientation: (0.246488, -0.314866, -0.896823, 0.189239) <=> theta: 2.64348, axis: (-0.324891, -0.925375, 0.195264)
-```
-
-## `robot_kinematics_control_loop`
-* **Showcased libraries:** `state_representation`, `dynamical_systems`, `robot_model`
-
-This demonstration reuses the previous linear dynamical system control loop but adds the robot component.
-The desired command, i.e. desired twist of the robot end-effector (eef) is sent to a dummy robot.
-At each timestep, the new joint state and eef state of the robot is computed using the robot kinematics from the robot
-model corresponding to the provided URDF.
-The robot is assumed to perfectly follow the computed desired state matching the eef desired twist.
-Similarly to the previous demonstration, the script outputs the current eef pose and distance to the attractor at each
-timestep, and the final pose on reaching it:
-
-```
-franka JointState
-names: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6, panda_joint7, ]
-positions: [-1.38116, 1.75208, 1.77134, -2.26031, 1.55573, 1.84016, -0.858975, ]
-velocities: [-0.00535077, -0.0246532, -0.00449986, 0.0149801, -0.0205184, 0.0108183, 0.00971553, ]
-accelerations: [-0.00259107, -0.0113026, -0.00313417, -0.00478477, -0.0268934, -0.019793, 0.00379833, ]
-torques: [-0.0131606, -0.0179626, -0.0166403, -0.0313393, 0.0205044, 0.021926, 0.0116933, ]
-panda_link8 CartesianState expressed in world frame
-position: (0.500567, 0.000338813, 0.500072)
-orientation: (1, -0.000121156, 5.94241e-05, -8.19635e-05) <=> theta: 0.000315772, axis: (-0.767363, 0.376373, -0.519131)
-linear velocity: (0, 0, 0)
-angular velocity: (0, 0, 0)
-linear acceleration: (0, 0, 0)
-angular acceleration: (0, 0, 0)
-force: (0, 0, 0)
-torque: (0, 0, 0)
-distance to attractor: 0.000980
------------
-##### TARGET #####
-panda_link8 CartesianPose expressed in world frame
-position: (0.5, 0, 0.5)
-orientation: (1, 0, 0, 0) <=> theta: 0, axis: (1, 0, 0)
-##### CURRENT STATES #####
-franka JointState
-names: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6, panda_joint7, ]
-positions: [-1.38265, 1.75477, 1.77195, -2.26216, 1.55451, 1.84208, -0.858673, ]
-velocities: [-0.00535077, -0.0246532, -0.00449986, 0.0149801, -0.0205184, 0.0108183, 0.00971553, ]
-accelerations: [-0.00259107, -0.0113026, -0.00313417, -0.00478477, -0.0268934, -0.019793, 0.00379833, ]
-torques: [-0.0131606, -0.0179626, -0.0166403, -0.0313393, 0.0205044, 0.021926, 0.0116933, ]
-panda_link8 CartesianState expressed in world frame
-position: (0.500567, 0.000338813, 0.500072)
-orientation: (1, -0.000121156, 5.94241e-05, -8.19635e-05) <=> theta: 0.000315772, axis: (-0.767363, 0.376373, -0.519131)
-linear velocity: (0, 0, 0)
-angular velocity: (0, 0, 0)
-linear acceleration: (0, 0, 0)
-angular acceleration: (0, 0, 0)
-force: (0, 0, 0)
-torque: (0, 0, 0)
-```
\ No newline at end of file
diff --git a/demos/control_loop_examples/scripts/robot_kinematics_control_loop.cpp b/demos/cpp_scripts/robot_kinematics_control_loop.cpp
similarity index 94%
rename from demos/control_loop_examples/scripts/robot_kinematics_control_loop.cpp
rename to demos/cpp_scripts/robot_kinematics_control_loop.cpp
index b68003d33..87fb7231d 100644
--- a/demos/control_loop_examples/scripts/robot_kinematics_control_loop.cpp
+++ b/demos/cpp_scripts/robot_kinematics_control_loop.cpp
@@ -1,16 +1,16 @@
 #include <chrono>
 #include <thread>
 #include <utility>
+#include "dynamical_systems/DynamicalSystemFactory.hpp"
+#include <robot_model/Model.hpp>
 #include <state_representation/space/cartesian/CartesianPose.hpp>
 #include <state_representation/space/cartesian/CartesianTwist.hpp>
 #include <state_representation/space/joint/JointPositions.hpp>
 #include <state_representation/space/joint/JointVelocities.hpp>
-#include "dynamical_systems/DynamicalSystemFactory.hpp"
-#include <robot_model/Model.hpp>
 
-using namespace state_representation;
 using namespace dynamical_systems;
 using namespace robot_model;
+using namespace state_representation;
 using namespace std::chrono_literals;
 
 namespace {
@@ -67,10 +67,7 @@ void control_loop(DummyRobotInterface& robot, const std::chrono::nanoseconds& dt
   target.set_position(.5, .0, .75);
   Eigen::Quaterniond orientation(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()));
   target.set_orientation(orientation);
-  std::shared_ptr<IDynamicalSystem<CartesianState>>
-      ds = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-      DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR
-  );
+  auto ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR);
   ds->set_parameter(make_shared_parameter("attractor", target));
 
   double distance;
diff --git a/demos/control_loop_examples/scripts/task_space_control_loop.cpp b/demos/cpp_scripts/task_space_control_loop.cpp
similarity index 86%
rename from demos/control_loop_examples/scripts/task_space_control_loop.cpp
rename to demos/cpp_scripts/task_space_control_loop.cpp
index 8297b211f..94be41159 100644
--- a/demos/control_loop_examples/scripts/task_space_control_loop.cpp
+++ b/demos/cpp_scripts/task_space_control_loop.cpp
@@ -1,11 +1,11 @@
 #include <chrono>
 #include <thread>
+#include <dynamical_systems/DynamicalSystemFactory.hpp>
 #include <state_representation/space/cartesian/CartesianPose.hpp>
 #include <state_representation/space/cartesian/CartesianTwist.hpp>
-#include "dynamical_systems/DynamicalSystemFactory.hpp"
 
-using namespace state_representation;
 using namespace dynamical_systems;
+using namespace state_representation;
 using namespace std::chrono_literals;
 
 CartesianPose control_loop_step(
@@ -25,10 +25,7 @@ CartesianPose control_loop_step(
 void control_loop(const std::chrono::nanoseconds& dt, double tolerance) {
   // set a desired target and a point attractor ds toward the target
   CartesianPose target = CartesianPose::Random("frame");
-  std::shared_ptr<IDynamicalSystem<CartesianState>>
-      ds = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-      DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR
-  );
+  auto ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR);
   ds->set_parameter(make_shared_parameter("attractor", target));
   // set a starting pose
   CartesianPose current_pose = CartesianPose::Random("frame");
diff --git a/demos/control_loop_examples/scripts/fixtures/panda_arm.urdf b/demos/fixtures/panda_arm.urdf
similarity index 100%
rename from demos/control_loop_examples/scripts/fixtures/panda_arm.urdf
rename to demos/fixtures/panda_arm.urdf
diff --git a/demos/python_scripts/robot_kinematics_control_loop.py b/demos/python_scripts/robot_kinematics_control_loop.py
new file mode 100644
index 000000000..387cc4ac9
--- /dev/null
+++ b/demos/python_scripts/robot_kinematics_control_loop.py
@@ -0,0 +1,75 @@
+import math
+import os
+import state_representation as sr
+import time
+from datetime import timedelta
+from dynamical_systems import create_cartesian_ds, DYNAMICAL_SYSTEM_TYPE
+from pyquaternion import Quaternion
+from robot_model import Model, QPInverseVelocityParameters
+
+
+class DummyRobotInterface:
+
+    def __init__(self, robot_name, urdf_path):
+        self._robot = Model(robot_name, urdf_path)
+        self.eef_pose = None
+        self.joint_positions = sr.JointPositions().Zero(robot_name, self._robot.get_joint_frames())
+        self.read_robot_state()
+
+    def read_robot_state(self):
+        # this is a dummy robot, we assume the joint state is executed
+        self.eef_pose = self._robot.forward_kinematics(self.joint_positions)
+
+    def send_control_command(self, desired_eef_twist, dt):
+        # create the inverse velocity parameters and set the period of the control loop
+        parameters = QPInverseVelocityParameters()
+        parameters.dt = dt
+        # apply the inverse velocity
+        desired_joint_velocities = self._robot.inverse_velocity(desired_eef_twist, self.joint_positions, parameters)
+        # integrate the new position
+        self.joint_positions = dt * desired_joint_velocities + self.joint_positions
+
+
+def control_loop_step(robot, ds, dt):
+    # read the robot state
+    robot.read_robot_state()
+    # print the state and eef pose
+    print(robot.joint_positions)
+    print(robot.eef_pose)
+    # get the twist evaluated at current pose
+    desired_twist = sr.CartesianTwist(ds.evaluate(robot.eef_pose))
+    # send the desired twist to the robot
+    robot.send_control_command(desired_twist, dt)
+
+
+def control_loop(robot, dt, tolerance):
+    target = sr.CartesianPose(robot.eef_pose.get_name(), robot.eef_pose.get_reference_frame())
+    target.set_position(.5, .0, .75)
+    target.set_orientation(Quaternion(axis=[.0, 1., .0], radians=math.pi))
+    ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR)
+    ds.set_parameter(sr.Parameter("attractor", target, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE))
+
+    # loop until target is reached
+    distance = sr.dist(robot.eef_pose, target, sr.CartesianStateVariable.POSE)
+    while distance > tolerance:
+        control_loop_step(robot, ds, dt)
+        distance = sr.dist(robot.eef_pose, target, sr.CartesianStateVariable.POSE)
+        print(f"Distance to attractor: {distance}")
+        print("-----------")
+        time.sleep(dt.total_seconds())
+
+    print("##### TARGET #####")
+    print(target)
+    print("##### CURRENT STATES #####")
+    print(robot.joint_positions)
+    print(robot.eef_pose)
+
+
+def main():
+    urdf_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), os.pardir, "fixtures", "panda_arm.urdf")
+    robot = DummyRobotInterface("franka", urdf_path)
+    control_loop(robot, timedelta(milliseconds=100), 1e-3)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/demos/python_scripts/task_space_control_loop.py b/demos/python_scripts/task_space_control_loop.py
new file mode 100644
index 000000000..2d4a6c2d0
--- /dev/null
+++ b/demos/python_scripts/task_space_control_loop.py
@@ -0,0 +1,44 @@
+import state_representation as sr
+import time
+from datetime import timedelta
+from dynamical_systems import create_cartesian_ds, DYNAMICAL_SYSTEM_TYPE
+
+
+def control_loop_step(pose, ds, dt):
+    # get thew twist evaluated at the current pose
+    desired_twist = sr.CartesianTwist(ds.evaluate(pose))
+    # integrate the twist and add it to the current pose
+    new_pose = dt * desired_twist + pose
+    # print the new pose
+    print(new_pose)
+    return new_pose
+
+
+def control_loop(dt, tolerance):
+    # set a desired target and a point attractor DS toward the target
+    target = sr.CartesianPose.Random("frame")
+    ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR)
+    ds.set_parameter(sr.Parameter("attractor", target, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE))
+    # set a starting pose
+    current_pose = sr.CartesianPose.Random("frame")
+    # loop until target is reached
+    distance = sr.dist(current_pose, target, sr.CartesianStateVariable.POSE)
+    while distance > tolerance:
+        current_pose = control_loop_step(current_pose, ds, dt)
+        distance = sr.dist(current_pose, target, sr.CartesianStateVariable.POSE)
+        print(f"Distance to attractor: {distance}")
+        print("-----------")
+        time.sleep(dt.total_seconds())
+
+    print("##### TARGET #####")
+    print(target)
+    print("##### CURRENT POSE #####")
+    print(current_pose)
+
+
+def main():
+    control_loop(timedelta(milliseconds=10), 1e-3)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/demos/ros2_examples/.dockerignore b/demos/ros2_examples/.dockerignore
deleted file mode 100644
index d7edb4002..000000000
--- a/demos/ros2_examples/.dockerignore
+++ /dev/null
@@ -1 +0,0 @@
-run-demo.sh
diff --git a/demos/ros2_examples/.gitignore b/demos/ros2_examples/.gitignore
deleted file mode 100644
index c3a384c41..000000000
--- a/demos/ros2_examples/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-rviz/
\ No newline at end of file
diff --git a/demos/ros2_examples/CMakeLists.txt b/demos/ros2_examples/CMakeLists.txt
deleted file mode 100644
index 9a32f1882..000000000
--- a/demos/ros2_examples/CMakeLists.txt
+++ /dev/null
@@ -1,46 +0,0 @@
-cmake_minimum_required(VERSION 3.9)
-project(ros2_examples)
-
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-    set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++17
-if(NOT CMAKE_CXX_STANDARD)
-    set(CMAKE_CXX_STANDARD 17)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-    add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-list(APPEND CMAKE_PREFIX_PATH /opt/openrobots)
-include_directories(/opt/openrobots/include)
-
-find_package(control_libraries 5.0.0 CONFIG REQUIRED)
-
-find_package(ament_cmake_auto REQUIRED)
-ament_auto_find_build_dependencies()
-
-
-set(DEMOS_SCRIPTS
-    task_space_control_loop
-)
-
-set(FIXTURE_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/fixtures/)
-install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/scripts/fixtures/ DESTINATION ${FIXTURE_INSTALL_PATH})
-
-foreach(SCRIPT ${DEMOS_SCRIPTS})
-    ament_auto_add_executable(${SCRIPT} scripts/${SCRIPT}.cpp)
-    target_link_libraries(${SCRIPT}
-        ${control_libraries_LIBRARIES}
-    )
-    target_compile_definitions(${SCRIPT} PRIVATE SCRIPT_FIXTURES="${FIXTURE_INSTALL_PATH}")
-endforeach()
-
-ament_auto_package()
-
-install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
-install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
-
diff --git a/demos/ros2_examples/Dockerfile b/demos/ros2_examples/Dockerfile
deleted file mode 100644
index 182362f0d..000000000
--- a/demos/ros2_examples/Dockerfile
+++ /dev/null
@@ -1,54 +0,0 @@
-FROM epfl-lasa/control-libraries/source:install as ros-dependencies
-
-# install ROS
-ENV ROS_DISTRO="foxy"
-RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key  -o /usr/share/keyrings/ros-archive-keyring.gpg
-RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | \
-    tee /etc/apt/sources.list.d/ros2.list > /dev/null
-
-RUN apt update && apt install -y ros-${ROS_DISTRO}-ros-base \
-    ros-${ROS_DISTRO}-rviz2 \
-    python3-colcon-common-extensions \
-    python3-argcomplete \
-    build-essential \
-    sudo \
-    && apt-get clean && rm -rf /var/lib/apt/lists/*
-
-USER developer
-
-# add and build a ros workspace
-ENV ROS_WORKSPACE ${HOME}/ros2_ws
-RUN mkdir -p ${ROS_WORKSPACE}/src
-WORKDIR ${ROS_WORKSPACE}
-RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; colcon build"
-
-# change .bashrc
-COPY config/update_bashrc /sbin/update_bashrc
-RUN sudo chmod +x /sbin/update_bashrc ; \
-    sudo chown ${USER} /sbin/update_bashrc sync ; \
-    /bin/bash -c /sbin/update_bashrc ; \
-    sudo rm /sbin/update_bashrc
-
-# change entrypoint to source ~/.bashrc and start in ~
-COPY config/entrypoint.sh /ros_entrypoint.sh
-RUN sudo chmod +x /ros_entrypoint.sh ; sudo chown ${USER} /ros_entrypoint.sh ;
-
-# env vars for the nvidia-container-runtime.
-ENV NVIDIA_VISIBLE_DEVICES all
-ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute
-
-
-FROM ros-dependencies as runtime-demonstrations
-
-# copy the whole folder in the ros_ws
-COPY . ${ROS_WORKSPACE}/src/ros2_examples
-
-# build and install the package
-WORKDIR ${ROS_WORKSPACE}
-RUN /bin/bash -c "source ${ROS_WORKSPACE}/install/setup.bash; colcon build --symlink-install"
-
-# Clean image
-RUN sudo apt-get clean && sudo rm -rf /var/lib/apt/lists/*
-
-ENTRYPOINT ["/ros_entrypoint.sh"]
-CMD ["bash"]
diff --git a/demos/ros2_examples/README.md b/demos/ros2_examples/README.md
deleted file mode 100644
index d7b9c570b..000000000
--- a/demos/ros2_examples/README.md
+++ /dev/null
@@ -1,33 +0,0 @@
-# `ros2_examples` demonstration scripts
-
-## Table of contents:
-* [Running a demo script](#running-demonstration-scripts)
-* [Task Space control loop example](#task_space_control_loop)
-
-## Running demonstration scripts
-This package contains the same control loop examples as in the [`control_loop_examples`](../control_loop_examples) demos
-folder, but adapted for usage in a ROS2 environment.
-The whole folder is a fully functional ROS2 package that can be directly copied in a ROS2 workspace.
-There is also a provided `Dockerfile` that encapsulates the whole in a containerized ROS2 environment.
-Running the `run-demo.sh` script opens an interactive container with the ROS2 environment already setup for running
-the demonstration scripts.
-
-As opposed to ROS, there is no `roscore` in ROS2.
-Running the scripts uses ROS2 commands, e.g. to run the `task_space_control_loop` script:
-
-```console
-ros2 run ros2_examples task_space_control_loop
-```
-
-You can also directly use the launch file that starts any requirement for the demo to run correctly:
-
-```console
-ros2 launch ros2_examples task_space_control_loop.launch.py
-```
-
-## `task_space_control_loop`
-* **Showcased libraries:** `state_representation`, `dynamical_systems`
-
-This simple demonstration shows how to create a control loop with a `Linear` dynamical system in task space (`CartesianState`).
-It moves a pose towards a random attractor in a `100Hz` control loop.
-At each control step, the scripts publishes the current and attractor poses using ROS2 `tf2` for a visualization in `rviz2`.
diff --git a/demos/ros2_examples/config/entrypoint.sh b/demos/ros2_examples/config/entrypoint.sh
deleted file mode 100644
index 36360bbcb..000000000
--- a/demos/ros2_examples/config/entrypoint.sh
+++ /dev/null
@@ -1,10 +0,0 @@
-#!/bin/bash
-set -e
-
-# setup environment
-source "${HOME}/.bashrc"
-
-# start in home directory 
-cd 
-
-exec bash -i -c "$@"
\ No newline at end of file
diff --git a/demos/ros2_examples/config/update_bashrc b/demos/ros2_examples/config/update_bashrc
deleted file mode 100644
index b633a05ad..000000000
--- a/demos/ros2_examples/config/update_bashrc
+++ /dev/null
@@ -1,7 +0,0 @@
-# Adding all the necessary ros sourcing
-echo "" >> ~/.bashrc
-echo "## ROS" >> ~/.bashrc
-echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc
-echo "source ${ROS_WORKSPACE}/install/setup.bash" >> ~/.bashrc
-echo "source /usr/share/colcon_cd/function/colcon_cd.sh" >> ~/.bashrc
-echo "export _colcon_cd_root=~/ros2_install" >> ~/.bashrc
\ No newline at end of file
diff --git a/demos/ros2_examples/launch/task_space_control_loop.launch.py b/demos/ros2_examples/launch/task_space_control_loop.launch.py
deleted file mode 100644
index ccb85f602..000000000
--- a/demos/ros2_examples/launch/task_space_control_loop.launch.py
+++ /dev/null
@@ -1,20 +0,0 @@
-from launch import LaunchDescription
-from ament_index_python.packages import get_package_share_directory
-from launch_ros.actions import Node
-import os.path
-
-def generate_launch_description():
-    return LaunchDescription([
-        Node(
-            package='ros2_examples',
-            namespace='',
-            executable='task_space_control_loop',
-            name='task_space_control_loop'
-        ),
-        Node(
-            package='rviz2',
-            namespace='',
-            executable='rviz2',
-            arguments=['-d' + os.path.join(get_package_share_directory('ros2_examples'), 'rviz', 'config_file.rviz')]
-        )
-    ])
\ No newline at end of file
diff --git a/demos/ros2_examples/package.xml b/demos/ros2_examples/package.xml
deleted file mode 100644
index 0a46442a7..000000000
--- a/demos/ros2_examples/package.xml
+++ /dev/null
@@ -1,20 +0,0 @@
-<package format="3">
-  <name>ros2_examples</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="baptiste.busch@epfl.ch">"Baptiste Busch"</maintainer>
-  <license>GPLv3</license>
-
-  <buildtool_depend>ament_cmake_auto</buildtool_depend>
-
-  <depend>rclcpp</depend>
-  <depend>tf2_ros</depend>
-  <depend>tf2_geometry_msgs</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
diff --git a/demos/ros2_examples/run-demo.sh b/demos/ros2_examples/run-demo.sh
deleted file mode 100755
index 9cfde2c49..000000000
--- a/demos/ros2_examples/run-demo.sh
+++ /dev/null
@@ -1,74 +0,0 @@
-#!/usr/bin/env bash
-
-# change to  false to use host network
-IS_ISOLATED=true
-
-MULTISTAGE_TARGET="runtime-demonstrations"
-IMAGE_NAME=epfl-lasa/control-libraries/ros2-examples
-IMAGE_TAG="${MULTISTAGE_TARGET}"
-GPUS=""
-
-NETWORK=host
-if [ "${IS_ISOLATED}" = true ]; then
-    docker network inspect isolated >/dev/null 2>&1 || docker network create --driver bridge isolated
-    NETWORK=isolated
-fi
-
-HELP_MESSAGE="Usage: run-demo.sh [-r] [-v]
-Options:
-  --gpus <gpu_options>     Add GPU access for applications that
-                           require hardware acceleration (e.g. Gazebo)
-                           For the list of gpu_options parameters see:
-      >>> https://docs.docker.com/config/containers/resource_constraints
-  -r, --rebuild            Rebuild the image using the docker
-                           --no-cache option.
-  -v, --verbose            Use the verbose option during the building
-                           process.
-  -h, --help               Show this help message.
-"
-
-BUILD_FLAGS=()
-while [[ $# -gt 0 ]]; do
-  opt="$1"
-  case $opt in
-    --gpus) GPUS=$2; shift 2;;
-    -r|--rebuild) BUILD_FLAGS+=(--no-cache); shift ;;
-    -v|--verbose) BUILD_FLAGS+=(--progress=plain); shift ;;
-    -h|--help) echo "${HELP_MESSAGE}" ; exit 0 ;;
-    *) echo 'Error in command line parsing' >&2
-       echo -e "\n${HELP_MESSAGE}"
-       exit 1
-  esac
-done
-
-docker pull ghcr.io/epfl-lasa/control-libraries/development-dependencies
-DOCKER_BUILDKIT=1 docker build --target install \
-  -t epfl-lasa/control-libraries/source:install \
-  --build-arg BUILD_TESTING=OFF \
-  "${BUILD_FLAGS[@]}" \
-  -f ../../source/Dockerfile.source ../../source || exit 1
-DOCKER_BUILDKIT=1 docker build "${BUILD_FLAGS[@]}" . --target "${MULTISTAGE_TARGET}" -t "${IMAGE_NAME}:${IMAGE_TAG}" || exit 1
-
-RUN_FLAGS=(-u developer)
-if [ -n "${GPUS}" ]; then
-  RUN_FLAGS+=(--gpus "${GPUS}")
-  RUN_FLAGS+=(--env DISPLAY="${DISPLAY}")
-  RUN_FLAGS+=(--env NVIDIA_VISIBLE_DEVICES="${NVIDIA_VISIBLE_DEVICES:-all}")
-  RUN_FLAGS+=(--env NVIDIA_DRIVER_CAPABILITIES="${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics")
-fi
-
-if [[ "${OSTYPE}" == "darwin"* ]]; then
-  RUN_FLAGS+=(-e DISPLAY=host.docker.internal:0)
-else
-  xhost +
-  RUN_FLAGS+=(-e DISPLAY="${DISPLAY}")
-  RUN_FLAGS+=(-e XAUTHORITY="${XAUTHORITY}")
-  RUN_FLAGS+=(-v /tmp/.X11-unix:/tmp/.X11-unix:rw)
-  RUN_FLAGS+=(--device=/dev/dri:/dev/dri)
-fi
-
-docker run -it --rm --net="${NETWORK}" \
-  "${RUN_FLAGS[@]}" \
-  --volume="$(pwd)/rviz:/home/developer/ros2_ws/src/ros2_examples/rviz/:rw" \
-  --name "${CONTAINER_NAME}" \
-  "${IMAGE_NAME}:${IMAGE_TAG}" /bin/bash
diff --git a/demos/ros2_examples/rviz/config_file.rviz b/demos/ros2_examples/rviz/config_file.rviz
deleted file mode 100644
index f36c62d89..000000000
--- a/demos/ros2_examples/rviz/config_file.rviz
+++ /dev/null
@@ -1,148 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-        - /TF1
-      Splitter Ratio: 0.5
-    Tree Height: 623
-  - Class: rviz_common/Selection
-    Name: Selection
-  - Class: rviz_common/Tool Properties
-    Expanded:
-      - /2D Goal Pose1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz_common/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz_default_plugins/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Class: rviz_default_plugins/TF
-      Enabled: true
-      Frame Timeout: 15
-      Frames:
-        All Enabled: true
-        attractor_frame:
-          Value: true
-        frame:
-          Value: true
-        world:
-          Value: true
-      Marker Scale: 1
-      Name: TF
-      Show Arrows: true
-      Show Axes: true
-      Show Names: true
-      Tree:
-        world:
-          attractor_frame:
-            {}
-          frame:
-            {}
-      Update Interval: 0
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: world
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz_default_plugins/Interact
-      Hide Inactive Objects: true
-    - Class: rviz_default_plugins/MoveCamera
-    - Class: rviz_default_plugins/Select
-    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz_default_plugins/Measure
-      Line color: 128; 128; 0
-    - Class: rviz_default_plugins/SetInitialPose
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /initialpose
-    - Class: rviz_default_plugins/SetGoal
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /goal_pose
-    - Class: rviz_default_plugins/PublishPoint
-      Single click: true
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /clicked_point
-  Transformation:
-    Current:
-      Class: rviz_default_plugins/TF
-  Value: true
-  Views:
-    Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 10
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.785398006439209
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 0.785398006439209
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 846
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002f8000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002f8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1200
-  X: 72
-  Y: 60
diff --git a/demos/ros2_examples/scripts/fixtures/panda_arm.urdf b/demos/ros2_examples/scripts/fixtures/panda_arm.urdf
deleted file mode 100644
index cc92817ca..000000000
--- a/demos/ros2_examples/scripts/fixtures/panda_arm.urdf
+++ /dev/null
@@ -1,134 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from franka_panda_description/robots/panda_arm.urdf.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="panda">
-  <link name="panda_link0">
-  </link>
-  <link name="panda_link1">
-    <inertial>
-      <origin rpy="0 0 0" xyz="3.875e-03 2.081e-03 -0.1750"/>
-      <mass value="4.970684"/>
-      <inertia ixx="7.0337e-01" ixy="-1.3900e-04" ixz="6.7720e-03" iyy="7.0661e-01" iyz="1.9169e-02" izz="9.1170e-03"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint1" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-    <origin rpy="0 0 0" xyz="0 0 0.333"/>
-    <parent link="panda_link0"/>
-    <child link="panda_link1"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
-    <dynamics damping="10.0" friction="5.0"/>
-  </joint>
-  <link name="panda_link2">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-3.141e-03 -2.872e-02 3.495e-03"/>
-      <mass value="0.646926"/>
-      <inertia ixx="7.9620e-03" ixy="-3.9250e-03" ixz="1.0254e-02" iyy="2.8110e-02" iyz="7.0400e-04" izz="2.5995e-02"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint2" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
-    <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
-    <parent link="panda_link1"/>
-    <child link="panda_link2"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
-    <dynamics damping="5.0" friction="2.0"/>
-  </joint>
-  <link name="panda_link3">
-    <inertial>
-      <origin rpy="0 0 0" xyz="2.7518e-02 3.9252e-02 -6.6502e-02"/>
-      <mass value="3.228604"/>
-      <inertia ixx="3.7242e-02" ixy="-4.7610e-03" ixz="-1.1396e-02" iyy="3.6155e-02" iyz="-1.2805e-02" izz="1.0830e-02"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint3" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-    <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
-    <parent link="panda_link2"/>
-    <child link="panda_link3"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
-    <dynamics damping="5.0" friction="2.0"/>
-  </joint>
-  <link name="panda_link4">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-5.317e-02 1.04419e-01 2.7454e-02"/>
-      <mass value="3.587895"/>
-      <inertia ixx="2.5853e-02" ixy="7.7960e-03" ixz="-1.3320e-03" iyy="1.9552e-02" iyz="8.6410e-03" izz="2.8323e-02"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint4" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
-    <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
-    <parent link="panda_link3"/>
-    <child link="panda_link4"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
-    <dynamics damping="1.0" friction="0.5"/>
-  </joint>
-  <link name="panda_link5">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-1.1953e-02 4.1065e-02 -3.8437e-02"/>
-      <mass value="1.225946"/>
-      <inertia ixx="3.5549e-02" ixy="-2.1170e-03" ixz="-4.0370e-03" iyy="2.9474e-02" iyz="2.2900e-04" izz="8.6270e-03"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint5" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-    <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
-    <parent link="panda_link4"/>
-    <child link="panda_link5"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
-    <dynamics damping="2.0" friction="1.0"/>
-  </joint>
-  <link name="panda_link6">
-    <inertial>
-      <origin rpy="0 0 0" xyz="6.0149e-02 -1.4117e-02 -1.0517e-02"/>
-      <mass value="1.666555"/>
-      <inertia ixx="1.9640e-03" ixy="1.0900e-04" ixz="-1.1580e-03" iyy="4.3540e-03" iyz="3.4100e-04" izz="5.4330e-03"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint6" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
-    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
-    <parent link="panda_link5"/>
-    <child link="panda_link6"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
-    <dynamics damping="1.0" friction="0.5"/>
-  </joint>
-  <link name="panda_link7">
-    <inertial>
-      <origin rpy="0 0 0" xyz="1.0517e-02 -4.252e-03 6.1597e-02"/>
-      <mass value="7.35522e-01"/>
-      <inertia ixx="1.2516e-02" ixy="-4.2800e-04" ixz="-1.1960e-03" iyy="1.0027e-02" iyz="-7.4100e-04" izz="4.8150e-03"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint7" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-    <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
-    <parent link="panda_link6"/>
-    <child link="panda_link7"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
-    <dynamics damping="1.0" friction="0.5"/>
-  </joint>
-  <link name="panda_link8">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0"/>
-      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint8" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.107"/>
-    <parent link="panda_link7"/>
-    <child link="panda_link8"/>
-    <axis xyz="0 0 0"/>
-  </joint>
-</robot>
diff --git a/demos/ros2_examples/scripts/task_space_control_loop.cpp b/demos/ros2_examples/scripts/task_space_control_loop.cpp
deleted file mode 100644
index ccf0bc325..000000000
--- a/demos/ros2_examples/scripts/task_space_control_loop.cpp
+++ /dev/null
@@ -1,105 +0,0 @@
-#include <chrono>
-#include <thread>
-#include <rclcpp/rclcpp.hpp>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2/LinearMath/Quaternion.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <state_representation/space/cartesian/CartesianPose.hpp>
-#include <state_representation/space/cartesian/CartesianTwist.hpp>
-#include "dynamical_systems/DynamicalSystemFactory.hpp"
-
-using namespace state_representation;
-using namespace dynamical_systems;
-using namespace std::chrono_literals;
-
-namespace {
-class TaskSpaceControlLoop : public rclcpp::Node {
-private:
-  std::chrono::nanoseconds dt_; ///< period for the control loop
-  double distance_tolerance_; ///< tolerance to the attractor to stop the control loop
-  std::thread run_thread_; ///< thread object to start the main loop, i.e. the run function, in parallel of the rest
-  CartesianPose current_pose_; ///< current pose
-  CartesianPose target_; ///< attractor pose
-  std::shared_ptr<IDynamicalSystem<CartesianState>> dynamical_system_; ///< dynamical system to the attractor
-
-  void send_transform(const CartesianPose& pose, const std::string& pose_name_prefix = "") {
-    auto node_handle = this->shared_from_this();
-    static tf2_ros::TransformBroadcaster br(node_handle);
-    geometry_msgs::msg::TransformStamped pose_tf;
-    pose_tf.header.stamp = node_handle->now();
-    pose_tf.header.frame_id = pose.get_reference_frame();
-    std::string prefix = (pose_name_prefix != "") ? pose_name_prefix + "_" : "";
-    pose_tf.child_frame_id = prefix + pose.get_name();
-    // populated the transform
-    tf2::Transform transform;
-    transform.setOrigin(tf2::Vector3(pose.get_position()(0), pose.get_position()(1), pose.get_position()(2)));
-    transform.setRotation(tf2::Quaternion(pose.get_orientation().x(),
-                                          pose.get_orientation().y(),
-                                          pose.get_orientation().z(),
-                                          pose.get_orientation().w()));
-    pose_tf.transform = tf2::toMsg(transform);
-    br.sendTransform(pose_tf);
-  }
-
-public:
-  explicit TaskSpaceControlLoop(const std::string& node_name,
-                                const std::chrono::nanoseconds& dt,
-                                double distance_tolerance) :
-      Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(false)),
-      dt_(dt),
-      distance_tolerance_(distance_tolerance) {
-    this->init();
-  }
-
-  void init() {
-    // set a desired target and a point attractor ds toward the target
-    this->target_ = CartesianPose::Random("frame");
-    this->dynamical_system_ = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-        DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR
-    );
-    this->dynamical_system_->set_parameter(make_shared_parameter("attractor", this->target_));
-    // set a starting pose
-    this->current_pose_ = CartesianPose::Random("frame");
-    // add the run thread
-    std::function<void(void)> run_fnc = std::bind(&TaskSpaceControlLoop::run, this);
-    this->run_thread_ = std::thread(run_fnc);
-  }
-
-  void step() {
-    // get the twist evaluated at current pose
-    CartesianTwist desired_twist = this->dynamical_system_->evaluate(this->current_pose_);
-    // integrate the twist and add it to the current pose
-    this->current_pose_ = this->dt_ * desired_twist + this->current_pose_;
-  }
-
-  void run() {
-    // loop until target is reached
-    double distance;
-    do {
-      auto start = std::chrono::steady_clock::now();
-      this->step();
-      distance = dist(this->current_pose_, this->target_, CartesianStateVariable::POSE);
-      this->send_transform(this->target_, "attractor");
-      this->send_transform(this->current_pose_);
-      auto end = std::chrono::steady_clock::now();
-      auto elapsed = end - start;
-      auto timeToWait = this->dt_ - elapsed;
-      if (timeToWait > std::chrono::nanoseconds::zero()) {
-        std::this_thread::sleep_for(timeToWait);
-      }
-      RCLCPP_INFO(this->get_logger(), "Distance to attractor: %f", distance);
-    } while (distance > this->distance_tolerance_ && rclcpp::ok());
-    rclcpp::shutdown();
-  }
-};
-}
-
-int main(int argc, char** argv) {
-  rclcpp::init(argc, argv);
-  auto dt = 10ms;
-  rclcpp::executors::SingleThreadedExecutor exe;
-  auto node = std::make_shared<TaskSpaceControlLoop>("task_space_control_loop", dt, 1e-3);
-  exe.add_node(node->get_node_base_interface());
-  exe.spin();
-  return 0;
-}
\ No newline at end of file
diff --git a/demos/ros_examples/.dockerignore b/demos/ros_examples/.dockerignore
deleted file mode 100644
index d7edb4002..000000000
--- a/demos/ros_examples/.dockerignore
+++ /dev/null
@@ -1 +0,0 @@
-run-demo.sh
diff --git a/demos/ros_examples/.gitignore b/demos/ros_examples/.gitignore
deleted file mode 100644
index c3a384c41..000000000
--- a/demos/ros_examples/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-rviz/
\ No newline at end of file
diff --git a/demos/ros_examples/CMakeLists.txt b/demos/ros_examples/CMakeLists.txt
deleted file mode 100644
index 854aed540..000000000
--- a/demos/ros_examples/CMakeLists.txt
+++ /dev/null
@@ -1,61 +0,0 @@
-cmake_minimum_required(VERSION 3.9)
-project(ros_examples)
-
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-    set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++17
-if(NOT CMAKE_CXX_STANDARD)
-    set(CMAKE_CXX_STANDARD 17)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-    add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-list(APPEND CMAKE_PREFIX_PATH /opt/openrobots)
-
-find_package(control_libraries 5.0.0 CONFIG REQUIRED)
-
-find_package(catkin REQUIRED COMPONENTS
-    roscpp
-    tf
-    std_msgs
-    sensor_msgs
-)
-
-find_package(Boost REQUIRED COMPONENTS system)
-add_compile_definitions(BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS)
-add_compile_definitions(BOOST_MPL_LIMIT_LIST_SIZE=30)
-
-catkin_package(
-    CATKIN_DEPENDS roscpp tf
-)
-
-include_directories(
-    ${catkin_INCLUDE_DIRS}
-    /opt/openrobots/include
-)
-
-set(DEMOS_SCRIPTS
-    task_space_control_loop
-    joint_space_velocity_control
-)
-set(FIXTURE_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/fixtures/)
-install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/scripts/fixtures/ DESTINATION ${FIXTURE_INSTALL_PATH})
-
-foreach(SCRIPT ${DEMOS_SCRIPTS})
-    add_executable(${SCRIPT} scripts/${SCRIPT}.cpp)
-    target_link_libraries(${SCRIPT}
-        ${control_libraries_LIBRARIES}
-        ${catkin_LIBRARIES}
-    )
-    target_compile_definitions(${SCRIPT} PRIVATE SCRIPT_FIXTURES="${FIXTURE_INSTALL_PATH}")
-    install(TARGETS ${SCRIPT}
-      RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-    )
-endforeach()
-
-install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
\ No newline at end of file
diff --git a/demos/ros_examples/Dockerfile b/demos/ros_examples/Dockerfile
deleted file mode 100644
index b32e80a73..000000000
--- a/demos/ros_examples/Dockerfile
+++ /dev/null
@@ -1,54 +0,0 @@
-FROM epfl-lasa/control-libraries/source:install as ros-dependencies
-
-# install ROS
-ENV ROS_DISTRO="noetic"
-RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' \
-    && apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
-RUN apt update && apt install -y ros-${ROS_DISTRO}-ros-base \
-    ros-${ROS_DISTRO}-tf2-tools \
-    ros-${ROS_DISTRO}-tf \
-    ros-${ROS_DISTRO}-rviz \
-    python3-rosdep \
-    python3-rosinstall \
-    python3-rosinstall-generator \
-    python3-wstool \
-    build-essential \
-    sudo \
-    && apt-get clean && rm -rf /var/lib/apt/lists/*
-
-USER developer
-
-# add and build a ros workspace
-ENV ROS_WORKSPACE ${HOME}/ros_ws
-RUN mkdir -p ${ROS_WORKSPACE}/src
-WORKDIR ${ROS_WORKSPACE}/src
-RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace"
-WORKDIR ${ROS_WORKSPACE}
-RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make"
-
-# change .bashrc
-COPY config/update_bashrc /sbin/update_bashrc
-RUN sudo chmod +x /sbin/update_bashrc ; \
-    sudo chown ${USER} /sbin/update_bashrc sync ; \
-    /bin/bash -c /sbin/update_bashrc ; \
-    sudo rm /sbin/update_bashrc
-
-# change entrypoint to source ~/.bashrc and start in ~
-COPY config/entrypoint.sh /ros_entrypoint.sh
-RUN sudo chmod +x /ros_entrypoint.sh ; sudo chown ${USER} /ros_entrypoint.sh ;
-
-
-FROM ros-dependencies as runtime-demonstrations
-
-# copy the whole folder in the ros_ws
-COPY . ${ROS_WORKSPACE}/src/ros_examples
-
-# build and install the package
-WORKDIR ${ROS_WORKSPACE}
-RUN /bin/bash -c "source ${ROS_WORKSPACE}/devel/setup.bash; catkin_make install"
-
-# Clean image
-RUN sudo apt-get clean && sudo rm -rf /var/lib/apt/lists/*
-
-ENTRYPOINT ["/ros_entrypoint.sh"]
-CMD ["bash"]
diff --git a/demos/ros_examples/README.md b/demos/ros_examples/README.md
deleted file mode 100644
index 8c82e393a..000000000
--- a/demos/ros_examples/README.md
+++ /dev/null
@@ -1,63 +0,0 @@
-# `ros_examples` demonstration scripts
-
-## Table of contents:
-* [Running a demo script](#running-demonstration-scripts)
-* [Task Space control loop example](#task_space_control_loop)
-
-## Running demonstration scripts
-This package contains the same control loop examples as in [`control_loop_examples`](../control_loop_examples) demos
-folder, but adapted for usage in a ROS environment.
-The whole folder is a fully functional ROS package that can be directly copied in a ROS workspace.
-There is also a provided `Dockerfile` that encapsulates the whole in a containerized ROS environment.
-Running the `run-demo.sh` script opens an interactive container with the ROS environment already setup for running
-the demonstration scripts.
-
-In order to run the demonstrations, a `roscore` needs to be running.
-One can be started from the interactive container.
-Running the scripts uses ROS commands, e.g. to run the `task_space_control_loop` script:
-
-```console
-rosrun ros_examples task_space_control_loop
-```
-
-You can also directly use the launch file that starts any requirement for the demo to run correctly:
-
-```console
-roslaunch ros_examples task_space_control_loop.launch
-```
-
-## `task_space_control_loop`
-* **Showcased libraries:** `state_representation`, `dynamical_systems`
-
-This simple demonstration shows how to create a control loop with a `Linear` dynamical system in task space (`CartesianState`).
-It moves a pose towards a random attractor in a `100Hz` control loop.
-At each control step, the scripts publishes the current and attractor poses using ROS `tf` for a visualization in `rviz`.
-
-## `joint_space_velocity_control`
-* **Showcased libraries:** `state_representation`, `dynamical_systems`, `robot_model`
-
-This demonstration shows how to create a control loop with a `Linear` dynamical system in task space (`CartesianState`) 
-and compute forward kinematics and inverse velocity with a robot `Model`. A ROS subscriber is set up to receive the joint 
-state from the `joint_states` topic.
-The robot moves a pose towards a random attractor in a `500Hz` control loop.
-At each control step, the scripts publishes the desired joint velocity to the `velocity_controller/command` topic.
-
-To run this script and visualize the robot, follow the steps below to launch a *PyBullet* simulation with a ROS interface:
-
-- in a first terminal, run:
-    ```bash
-    # in a directory of your choice (preferably not in the control-libraries directory)
-    git clone https://github.com/domire8/pybullet_ros.git  --depth 1 --branch control-libraries-demo
-    cd pybullet_ros/docker
-    bash build-run.sh # this will build a docker image and spin up a container
-    # inside the container
-    roslaunch pybullet_ros franka.launch
-    ```
-
-- in a second terminal, run:
-    ```bash
-    cd control-libraries/demos/ros_examples
-    bash run-demo.sh # this will build a docker image and spin up a container
-    # inside the container
-    roslaunch ros_examples joint_space_velocity_control.launch robot_name:=franka
-    ```
diff --git a/demos/ros_examples/config/entrypoint.sh b/demos/ros_examples/config/entrypoint.sh
deleted file mode 100644
index 36360bbcb..000000000
--- a/demos/ros_examples/config/entrypoint.sh
+++ /dev/null
@@ -1,10 +0,0 @@
-#!/bin/bash
-set -e
-
-# setup environment
-source "${HOME}/.bashrc"
-
-# start in home directory 
-cd 
-
-exec bash -i -c "$@"
\ No newline at end of file
diff --git a/demos/ros_examples/config/update_bashrc b/demos/ros_examples/config/update_bashrc
deleted file mode 100644
index d04481902..000000000
--- a/demos/ros_examples/config/update_bashrc
+++ /dev/null
@@ -1,6 +0,0 @@
-# Adding all the necessary ros sourcing
-echo "" >> ~/.bashrc
-echo "## ROS" >> ~/.bashrc
-echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
-echo "source ${ROS_WORKSPACE}/devel/setup.bash" >> ~/.bashrc
-echo "source ${ROS_WORKSPACE}/install/setup.bash" >> ~/.bashrc
\ No newline at end of file
diff --git a/demos/ros_examples/launch/joint_space_velocity_control.launch b/demos/ros_examples/launch/joint_space_velocity_control.launch
deleted file mode 100644
index 178e1fec4..000000000
--- a/demos/ros_examples/launch/joint_space_velocity_control.launch
+++ /dev/null
@@ -1,11 +0,0 @@
-<launch>
-    <arg name="robot_name" default=""/>
-
-    <group if="$(eval robot_name == '')">
-        <node type="joint_space_velocity_control" name="joint_space_velocity_control" pkg="ros_examples" output="screen"/>
-    </group>
-
-    <group unless="$(eval robot_name == '')">
-        <node ns="$(arg robot_name)" type="joint_space_velocity_control" name="joint_space_velocity_control" pkg="ros_examples" output="screen"/>
-    </group>
-</launch>
\ No newline at end of file
diff --git a/demos/ros_examples/launch/task_space_control_loop.launch b/demos/ros_examples/launch/task_space_control_loop.launch
deleted file mode 100644
index 0d30c6667..000000000
--- a/demos/ros_examples/launch/task_space_control_loop.launch
+++ /dev/null
@@ -1,4 +0,0 @@
-<launch>
-    <node type="rviz" name="rviz" pkg="rviz" args="-d $(find ros_examples)/rviz/config_file.rviz"/>
-    <node type="task_space_control_loop" name="task_space_control_loop" pkg="ros_examples"/>
-</launch>
\ No newline at end of file
diff --git a/demos/ros_examples/package.xml b/demos/ros_examples/package.xml
deleted file mode 100644
index 87a3e3308..000000000
--- a/demos/ros_examples/package.xml
+++ /dev/null
@@ -1,14 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>ros_examples</name>
-  <version>0.0.0</version>
-  <description>Control libraries demos included in a ROS package</description>
-  <maintainer email="baptiste.busch@epfl.ch">"Baptiste Busch"</maintainer>
-  <license>GPLv3</license>
-
-  <buildtool_depend>catkin</buildtool_depend>
-  <depend>roscpp</depend>
-  <depend>tf</depend>
-  <depend>std_msgs</depend>
-  <depend>sensor_msgs</depend>
-</package>
diff --git a/demos/ros_examples/run-demo.sh b/demos/ros_examples/run-demo.sh
deleted file mode 100755
index 09915fd47..000000000
--- a/demos/ros_examples/run-demo.sh
+++ /dev/null
@@ -1,65 +0,0 @@
-#!/usr/bin/env bash
-
-MULTISTAGE_TARGET="runtime-demonstrations"
-IMAGE_NAME=epfl-lasa/control-libraries/ros-examples
-IMAGE_TAG="${MULTISTAGE_TARGET}"
-GPUS=""
-
-HELP_MESSAGE="Usage: run-demo.sh [-r] [-v]
-Options:
-  --gpus <gpu_options>     Add GPU access for applications that
-                           require hardware acceleration (e.g. Gazebo)
-                           For the list of gpu_options parameters see:
-      >>> https://docs.docker.com/config/containers/resource_constraints
-  -r, --rebuild            Rebuild the image using the docker
-                           --no-cache option.
-  -v, --verbose            Use the verbose option during the building
-                           process.
-  -h, --help               Show this help message.
-"
-
-BUILD_FLAGS=()
-while [[ $# -gt 0 ]]; do
-  opt="$1"
-  case $opt in
-    --gpus) GPUS=$2; shift 2;;
-    -r|--rebuild) BUILD_FLAGS+=(--no-cache); shift ;;
-    -v|--verbose) BUILD_FLAGS+=(--progress=plain); shift ;;
-    -h|--help) echo "${HELP_MESSAGE}" ; exit 0 ;;
-    *) echo 'Error in command line parsing' >&2
-       echo -e "\n${HELP_MESSAGE}"
-       exit 1
-  esac
-done
-
-docker pull ghcr.io/epfl-lasa/control-libraries/development-dependencies
-DOCKER_BUILDKIT=1 docker build --target install \
-  -t epfl-lasa/control-libraries/source:install \
-  --build-arg BUILD_TESTING=OFF \
-  "${BUILD_FLAGS[@]}" \
-  -f ../../source/Dockerfile.source ../../source || exit 1
-DOCKER_BUILDKIT=1 docker build "${BUILD_FLAGS[@]}" . --target "${MULTISTAGE_TARGET}" -t "${IMAGE_NAME}:${IMAGE_TAG}" || exit 1
-
-RUN_FLAGS=(-u developer)
-if [ -n "${GPUS}" ]; then
-  RUN_FLAGS+=(--gpus "${GPUS}")
-  RUN_FLAGS+=(--env DISPLAY="${DISPLAY}")
-  RUN_FLAGS+=(--env NVIDIA_VISIBLE_DEVICES="${NVIDIA_VISIBLE_DEVICES:-all}")
-  RUN_FLAGS+=(--env NVIDIA_DRIVER_CAPABILITIES="${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics")
-fi
-
-if [[ "${OSTYPE}" == "darwin"* ]]; then
-  RUN_FLAGS+=(-e DISPLAY=host.docker.internal:0)
-else
-  xhost +
-  RUN_FLAGS+=(-e DISPLAY="${DISPLAY}")
-  RUN_FLAGS+=(-e XAUTHORITY="${XAUTHORITY}")
-  RUN_FLAGS+=(-v /tmp/.X11-unix:/tmp/.X11-unix:rw)
-  RUN_FLAGS+=(--device=/dev/dri:/dev/dri)
-fi
-
-docker run -it --rm --net=host \
-  "${RUN_FLAGS[@]}" \
-  --volume="$(pwd)/rviz:/home/developer/ros_ws/src/ros_examples/rviz/:rw" \
-  --name "${CONTAINER_NAME}" \
-  "${IMAGE_NAME}:${IMAGE_TAG}" /bin/bash
\ No newline at end of file
diff --git a/demos/ros_examples/rviz/config_file.rviz b/demos/ros_examples/rviz/config_file.rviz
deleted file mode 100644
index b6c3f8734..000000000
--- a/demos/ros_examples/rviz/config_file.rviz
+++ /dev/null
@@ -1,145 +0,0 @@
-Panels:
-  - Class: rviz/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-      Splitter Ratio: 0.5
-    Tree Height: 555
-  - Class: rviz/Selection
-    Name: Selection
-  - Class: rviz/Tool Properties
-    Expanded:
-      - /2D Pose Estimate1
-      - /2D Nav Goal1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz/Time
-    Experimental: false
-    Name: Time
-    SyncMode: 0
-    SyncSource: ""
-Preferences:
-  PromptSaveOnExit: true
-Toolbars:
-  toolButtonStyle: 2
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Class: rviz/TF
-      Enabled: true
-      Frame Timeout: 15
-      Frames:
-        All Enabled: true
-        attractorframe:
-          Value: true
-        frame:
-          Value: true
-        world:
-          Value: true
-      Marker Alpha: 1
-      Marker Scale: 1
-      Name: TF
-      Show Arrows: true
-      Show Axes: true
-      Show Names: true
-      Tree:
-        world:
-          attractorframe:
-            {}
-          frame:
-            {}
-      Update Interval: 0
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Default Light: true
-    Fixed Frame: world
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz/Interact
-      Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-    - Class: rviz/FocusCamera
-    - Class: rviz/Measure
-    - Class: rviz/SetInitialPose
-      Theta std deviation: 0.2617993950843811
-      Topic: /initialpose
-      X std deviation: 0.5
-      Y std deviation: 0.5
-    - Class: rviz/SetGoal
-      Topic: /move_base_simple/goal
-    - Class: rviz/PublishPoint
-      Single click: true
-      Topic: /clicked_point
-  Value: true
-  Views:
-    Current:
-      Class: rviz/Orbit
-      Distance: 10
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Field of View: 0.7853981852531433
-      Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.785398006439209
-      Target Frame: <Fixed Frame>
-      Yaw: 0.785398006439209
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 846
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1200
-  X: 72
-  Y: 27
diff --git a/demos/ros_examples/scripts/fixtures/panda_arm.urdf b/demos/ros_examples/scripts/fixtures/panda_arm.urdf
deleted file mode 100644
index cc92817ca..000000000
--- a/demos/ros_examples/scripts/fixtures/panda_arm.urdf
+++ /dev/null
@@ -1,134 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from franka_panda_description/robots/panda_arm.urdf.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="panda">
-  <link name="panda_link0">
-  </link>
-  <link name="panda_link1">
-    <inertial>
-      <origin rpy="0 0 0" xyz="3.875e-03 2.081e-03 -0.1750"/>
-      <mass value="4.970684"/>
-      <inertia ixx="7.0337e-01" ixy="-1.3900e-04" ixz="6.7720e-03" iyy="7.0661e-01" iyz="1.9169e-02" izz="9.1170e-03"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint1" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-    <origin rpy="0 0 0" xyz="0 0 0.333"/>
-    <parent link="panda_link0"/>
-    <child link="panda_link1"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
-    <dynamics damping="10.0" friction="5.0"/>
-  </joint>
-  <link name="panda_link2">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-3.141e-03 -2.872e-02 3.495e-03"/>
-      <mass value="0.646926"/>
-      <inertia ixx="7.9620e-03" ixy="-3.9250e-03" ixz="1.0254e-02" iyy="2.8110e-02" iyz="7.0400e-04" izz="2.5995e-02"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint2" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
-    <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
-    <parent link="panda_link1"/>
-    <child link="panda_link2"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
-    <dynamics damping="5.0" friction="2.0"/>
-  </joint>
-  <link name="panda_link3">
-    <inertial>
-      <origin rpy="0 0 0" xyz="2.7518e-02 3.9252e-02 -6.6502e-02"/>
-      <mass value="3.228604"/>
-      <inertia ixx="3.7242e-02" ixy="-4.7610e-03" ixz="-1.1396e-02" iyy="3.6155e-02" iyz="-1.2805e-02" izz="1.0830e-02"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint3" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-    <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
-    <parent link="panda_link2"/>
-    <child link="panda_link3"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
-    <dynamics damping="5.0" friction="2.0"/>
-  </joint>
-  <link name="panda_link4">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-5.317e-02 1.04419e-01 2.7454e-02"/>
-      <mass value="3.587895"/>
-      <inertia ixx="2.5853e-02" ixy="7.7960e-03" ixz="-1.3320e-03" iyy="1.9552e-02" iyz="8.6410e-03" izz="2.8323e-02"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint4" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
-    <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
-    <parent link="panda_link3"/>
-    <child link="panda_link4"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
-    <dynamics damping="1.0" friction="0.5"/>
-  </joint>
-  <link name="panda_link5">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-1.1953e-02 4.1065e-02 -3.8437e-02"/>
-      <mass value="1.225946"/>
-      <inertia ixx="3.5549e-02" ixy="-2.1170e-03" ixz="-4.0370e-03" iyy="2.9474e-02" iyz="2.2900e-04" izz="8.6270e-03"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint5" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-    <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
-    <parent link="panda_link4"/>
-    <child link="panda_link5"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
-    <dynamics damping="2.0" friction="1.0"/>
-  </joint>
-  <link name="panda_link6">
-    <inertial>
-      <origin rpy="0 0 0" xyz="6.0149e-02 -1.4117e-02 -1.0517e-02"/>
-      <mass value="1.666555"/>
-      <inertia ixx="1.9640e-03" ixy="1.0900e-04" ixz="-1.1580e-03" iyy="4.3540e-03" iyz="3.4100e-04" izz="5.4330e-03"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint6" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
-    <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
-    <parent link="panda_link5"/>
-    <child link="panda_link6"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
-    <dynamics damping="1.0" friction="0.5"/>
-  </joint>
-  <link name="panda_link7">
-    <inertial>
-      <origin rpy="0 0 0" xyz="1.0517e-02 -4.252e-03 6.1597e-02"/>
-      <mass value="7.35522e-01"/>
-      <inertia ixx="1.2516e-02" ixy="-4.2800e-04" ixz="-1.1960e-03" iyy="1.0027e-02" iyz="-7.4100e-04" izz="4.8150e-03"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint7" type="revolute">
-    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
-    <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
-    <parent link="panda_link6"/>
-    <child link="panda_link7"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
-    <dynamics damping="1.0" friction="0.5"/>
-  </joint>
-  <link name="panda_link8">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="0.0"/>
-      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-    </inertial>
-  </link>
-  <joint name="panda_joint8" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0.107"/>
-    <parent link="panda_link7"/>
-    <child link="panda_link8"/>
-    <axis xyz="0 0 0"/>
-  </joint>
-</robot>
diff --git a/demos/ros_examples/scripts/joint_space_velocity_control.cpp b/demos/ros_examples/scripts/joint_space_velocity_control.cpp
deleted file mode 100644
index 960edc99a..000000000
--- a/demos/ros_examples/scripts/joint_space_velocity_control.cpp
+++ /dev/null
@@ -1,117 +0,0 @@
-#include <chrono>
-#include <ros/ros.h>
-#include <std_msgs/Float64MultiArray.h>
-#include <sensor_msgs/JointState.h>
-#include <state_representation/space/cartesian/CartesianPose.hpp>
-#include <state_representation/space/cartesian/CartesianTwist.hpp>
-#include <state_representation/space/joint/JointVelocities.hpp>
-#include "dynamical_systems/DynamicalSystemFactory.hpp"
-#include <robot_model/Model.hpp>
-
-using namespace state_representation;
-using namespace dynamical_systems;
-using namespace robot_model;
-using namespace std::chrono_literals;
-
-namespace {
-class RobotInterface {
-private:
-  Model robot_;
-  ros::Subscriber subscriber_;
-  ros::Publisher publisher_;
-  JointState joint_state_;
-
-public:
-  bool state_received = false;
-
-  explicit RobotInterface(ros::NodeHandle* node_handle, const std::string& robot_name, const std::string& urdf_path) :
-      robot_(robot_name, urdf_path) {
-    subscriber_ = node_handle->subscribe("joint_states", 10, &RobotInterface::robot_state_callback, this);
-    publisher_ = node_handle->advertise<std_msgs::Float64MultiArray>("velocity_controller/command", 10, false);
-    joint_state_ = JointState(robot_name, robot_.get_joint_frames());
-  }
-
-private:
-  void robot_state_callback(const sensor_msgs::JointState::ConstPtr& msg) {
-    if (!state_received) {
-      state_received = true;
-    }
-    joint_state_.set_positions(msg->position);
-    joint_state_.set_velocities(msg->velocity);
-    joint_state_.set_torques(msg->effort);
-  }
-
-public:
-  void publish(const JointVelocities& command) {
-    std_msgs::Float64MultiArray msg;
-    msg.data = command.to_std_vector();
-    publisher_.publish(msg);
-  }
-
-  CartesianPose get_eef_pose() {
-    return robot_.forward_kinematics(joint_state_);
-  }
-
-  CartesianTwist get_eef_twist() {
-    return robot_.forward_velocity(joint_state_);
-  }
-
-  JointVelocities inverse_velocity(const CartesianTwist& twist) {
-    return robot_.inverse_velocity(twist, joint_state_);
-  }
-
-  const std::string& get_robot_name() {
-    return robot_.get_robot_name();
-  }
-
-  const std::vector<std::string> get_robot_frames() {
-    return robot_.get_frames();
-  }
-};
-}
-
-void control_loop(RobotInterface& robot, const int& freq) {
-  // set a desired target and a point attractor ds toward the target
-  CartesianPose target(robot.get_robot_frames().back(), robot.get_robot_frames().front());
-  target.set_position(.5, .0, .5);
-  target.set_orientation(Eigen::Quaterniond(0, 1, 0, 0));
-  std::vector<double> gains = {50.0, 50.0, 50.0, 10.0, 10.0, 10.0};
-
-  std::shared_ptr<IDynamicalSystem<CartesianState>>
-      ds = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-      DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR
-  );
-  ds->set_parameter(make_shared_parameter("attractor", target));
-  ds->set_parameter(make_shared_parameter("gain", gains));
-
-  ros::Rate rate(freq);
-  while (ros::ok()) {
-    if (robot.state_received) {
-      CartesianTwist twist = ds->evaluate(robot.get_eef_pose());
-      twist.clamp(0.25, 0.5);
-      JointVelocities command = robot.inverse_velocity(twist);
-      robot.publish(command);
-      rate.sleep();
-    }
-    ros::spinOnce();
-  }
-}
-
-int main(int argc, char** argv) {
-  ros::init(argc, argv, "joint_space_velocity_control_loop");
-  ros::NodeHandle node_handle;
-
-  std::string robot_description;
-  if (!node_handle.getParam("robot_description", robot_description)) {
-    ROS_ERROR("Could load parameter 'robot_description' from parameter server.");
-    return -1;
-  }
-
-  std::string robot_name = ros::this_node::getNamespace();
-  std::string urdf_path = std::string(SCRIPT_FIXTURES) + robot_name + ".urdf";
-  Model::create_urdf_from_string(robot_description, urdf_path);
-  RobotInterface robot(&node_handle, robot_name, urdf_path);
-
-  int freq = 500;
-  control_loop(robot, freq);
-}
\ No newline at end of file
diff --git a/demos/ros_examples/scripts/task_space_control_loop.cpp b/demos/ros_examples/scripts/task_space_control_loop.cpp
deleted file mode 100644
index d970a3555..000000000
--- a/demos/ros_examples/scripts/task_space_control_loop.cpp
+++ /dev/null
@@ -1,64 +0,0 @@
-#include <chrono>
-#include <thread>
-#include <ros/ros.h>
-#include <tf/transform_broadcaster.h>
-#include <state_representation/space/cartesian/CartesianPose.hpp>
-#include <state_representation/space/cartesian/CartesianTwist.hpp>
-#include "dynamical_systems/DynamicalSystemFactory.hpp"
-
-using namespace state_representation;
-using namespace dynamical_systems;
-using namespace std::chrono_literals;
-
-void send_transform(const CartesianPose& pose, const std::string& pose_name_prefix = "") {
-  static tf::TransformBroadcaster br;
-  tf::Transform transform;
-  transform.setOrigin(tf::Vector3(pose.get_position()(0), pose.get_position()(1), pose.get_position()(2)));
-  transform.setRotation(
-      tf::Quaternion(
-          pose.get_orientation().x(), pose.get_orientation().y(), pose.get_orientation().z(),
-          pose.get_orientation().w()));
-  std::string prefix = (pose_name_prefix != "") ? pose_name_prefix + "_" : "";
-  br.sendTransform(
-      tf::StampedTransform(
-          transform, ros::Time::now(), pose.get_reference_frame(), prefix + pose.get_name()));
-}
-
-CartesianPose control_loop_step(
-    const CartesianPose& pose, const std::shared_ptr<IDynamicalSystem<CartesianState>>& ds,
-    const std::chrono::nanoseconds& dt
-) {
-  // get the twist evaluated at current pose
-  CartesianTwist desired_twist = ds->evaluate(pose);
-  // integrate the twist and add it to the current pose
-  CartesianPose new_pose = dt * desired_twist + pose;
-  // return it
-  return new_pose;
-}
-
-void control_loop(const std::chrono::nanoseconds& dt, double tolerance) {
-  // set a desired target and a point attractor ds toward the target
-  CartesianPose target = CartesianPose::Random("frame");
-  std::shared_ptr<IDynamicalSystem<CartesianState>>
-      ds = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-      DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR
-  );
-  ds->set_parameter(make_shared_parameter("attractor", target));
-  // set a starting pose
-  CartesianPose current_pose = CartesianPose::Random("frame");
-  // loop until target is reached
-  double distance;
-  do {
-    current_pose = control_loop_step(current_pose, ds, dt);
-    distance = dist(current_pose, target, CartesianStateVariable::POSE);
-    send_transform(target, "attractor");
-    send_transform(current_pose);
-    std::this_thread::sleep_for(dt);
-  } while (distance > tolerance && ros::ok());
-}
-
-int main(int argc, char** argv) {
-  ros::init(argc, argv, "task_space_control_loop");
-  auto dt = 10ms;
-  control_loop(dt, 1e-3);
-}
\ No newline at end of file
diff --git a/demos/control_loop_examples/run-demo.sh b/demos/run-demo.sh
similarity index 58%
rename from demos/control_loop_examples/run-demo.sh
rename to demos/run-demo.sh
index b23ac496f..308338491 100755
--- a/demos/control_loop_examples/run-demo.sh
+++ b/demos/run-demo.sh
@@ -3,10 +3,12 @@
 IMAGE_NAME=epfl-lasa/control-libraries/control-loop-examples
 IMAGE_TAG=latest
 
-HELP_MESSAGE="Usage: run-demo.sh [-s <script>] [-r] [-v]
+BRANCH=$(git branch --show-current)
+
+HELP_MESSAGE="Usage: run-demo.sh [-b <branch>] [-r] [-v]
 Options:
-  -s, --script             If provided, the desired script that should be
-                           executed when starting the container.
+  -b, --branch <branch>    Specify the branch of control libraries
+                           that should be used to build the image.
   -r, --rebuild            Rebuild the image using the docker
                            --no-cache option.
   -v, --verbose            Use the verbose option during the building
@@ -15,11 +17,10 @@ Options:
 "
 
 BUILD_FLAGS=()
-TARGET_SCRIPT=""
 while [[ $# -gt 0 ]]; do
   opt="$1"
   case $opt in
-    -s|--script) TARGET_SCRIPT="$2"; shift;;
+    -b|--branch) BRANCH=$2; shift 2;;
     -r|--rebuild) BUILD_FLAGS+=(--no-cache); shift ;;
     -v|--verbose) BUILD_FLAGS+=(--progress=plain); shift ;;
     -h|--help) echo "${HELP_MESSAGE}" ; exit 0 ;;
@@ -29,16 +30,10 @@ while [[ $# -gt 0 ]]; do
   esac
 done
 
+echo "Using control libraries branch ${BRANCH}"
+BUILD_FLAGS+=(--build-arg CONTROL_LIBRARIES_BRANCH="${BRANCH}")
+
 docker pull ghcr.io/epfl-lasa/control-libraries/development-dependencies
-DOCKER_BUILDKIT=1 docker build --target install \
-  -t epfl-lasa/control-libraries/source:install \
-  --build-arg BUILD_TESTING=OFF \
-  "${BUILD_FLAGS[@]}" \
-  -f ../../source/Dockerfile.source ../../source || exit 1
 DOCKER_BUILDKIT=1 docker build "${BUILD_FLAGS[@]}" . -t "${IMAGE_NAME}:${IMAGE_TAG}" || exit 1
 
-if [ -z "${TARGET_SCRIPT}" ]; then
-  docker run -it --rm "${IMAGE_NAME}:${IMAGE_TAG}"
-else
-  docker run --rm "${IMAGE_NAME}:${IMAGE_TAG}" "./${TARGET_SCRIPT}"
-fi
+docker run -it --rm "${IMAGE_NAME}:${IMAGE_TAG}"
\ No newline at end of file
diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf
index 16e0b0ef3..45cef1e1c 100644
--- a/doxygen/doxygen.conf
+++ b/doxygen/doxygen.conf
@@ -38,7 +38,7 @@ PROJECT_NAME           = "Control Libraries"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER = 5.2.0
+PROJECT_NUMBER = 6.0.0
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a
diff --git a/protocol/Dockerfile.protocol b/protocol/Dockerfile.protocol
index a13d3336c..4b65830f0 100644
--- a/protocol/Dockerfile.protocol
+++ b/protocol/Dockerfile.protocol
@@ -16,8 +16,7 @@ FROM dependencies as build
 ARG BUILD_TESTING=ON
 
 WORKDIR /tmp/protocol/clproto_cpp/build
-RUN cmake -DBUILD_TESTING="${BUILD_TESTING}" .. \
-  && make -j all
+RUN cmake -DBUILD_TESTING="${BUILD_TESTING}" .. && make -j all
 
 
 FROM build as testing
diff --git a/protocol/clproto_cpp/CMakeLists.txt b/protocol/clproto_cpp/CMakeLists.txt
index 81b9e7445..877290960 100644
--- a/protocol/clproto_cpp/CMakeLists.txt
+++ b/protocol/clproto_cpp/CMakeLists.txt
@@ -1,29 +1,44 @@
-cmake_minimum_required(VERSION 3.9)
-project(clproto VERSION 5.2.0)
+cmake_minimum_required(VERSION 3.15)
 
-set(CMAKE_CXX_STANDARD 17)
+project(clproto VERSION 6.0.0)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+include(GNUInstallDirs)
+include(CMakePackageConfigHelpers)
 
 include(FindProtobuf)
-find_package(Protobuf REQUIRED)
-find_package(Eigen3 REQUIRED)
-find_package(control_libraries ${PROJECT_VERSION} REQUIRED state_representation)
+find_package(Protobuf 3.17 REQUIRED)
+find_package(control_libraries ${PROJECT_VERSION} REQUIRED COMPONENTS state_representation)
 
 if(BUILD_TESTING)
-    enable_testing()
-    find_package(GTest REQUIRED)
-    if (APPLE)
-        add_definitions(-DGTEST_USE_OWN_TR1_TUPLE)
-        add_definitions(-D__GLIBCXX__)
-    endif (APPLE)
+  enable_testing()
+  find_package(GTest REQUIRED)
+  if (APPLE)
+    add_definitions(-DGTEST_USE_OWN_TR1_TUPLE)
+    add_definitions(-D__GLIBCXX__)
+  endif (APPLE)
 else()
-    find_package(GTest QUIET)
+  find_package(GTest QUIET)
 endif()
 
-set(PROTOBUF_DIR ${PROJECT_SOURCE_DIR}/../protobuf)
+set(PROTOBUF_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../protobuf)
 set(PROTOBUF_BINDINGS_DIR ${PROTOBUF_DIR}/bindings/cpp)
 
 add_custom_target(generate_proto_bindings COMMAND make cpp_bindings
-    WORKING_DIRECTORY ${PROTOBUF_DIR}
+  WORKING_DIRECTORY ${PROTOBUF_DIR}
 )
 
 file(GLOB_RECURSE GENERATED_PROTO_BINDINGS "${PROTOBUF_BINDINGS_DIR}/*.pb.cc" "${PROTOBUF_BINDINGS_DIR}/*.pb.h")
@@ -33,29 +48,67 @@ target_include_directories(${PROJECT_NAME}_bindings PUBLIC ${PROTOBUF_BINDINGS_D
 set_property(TARGET ${PROJECT_NAME}_bindings PROPERTY POSITION_INDEPENDENT_CODE ON)
 
 add_library(${PROJECT_NAME} SHARED
-    ${PROJECT_SOURCE_DIR}/src/clproto.cpp
-    ${PROJECT_SOURCE_DIR}/src/decoders.cpp
-    ${PROJECT_SOURCE_DIR}/src/encoders.cpp
+  ${PROJECT_SOURCE_DIR}/src/clproto.cpp
+  ${PROJECT_SOURCE_DIR}/src/decoders.cpp
+  ${PROJECT_SOURCE_DIR}/src/encoders.cpp
+)
+target_include_directories(${PROJECT_NAME}
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
 )
-target_include_directories(${PROJECT_NAME} PUBLIC ${PROJECT_SOURCE_DIR}/include)
-set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER ${PROJECT_SOURCE_DIR}/include/clproto.h)
+set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/include/clproto.h)
 target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} state_representation PRIVATE ${PROJECT_NAME}_bindings)
 
+# install the target and create export-set
 install(TARGETS ${PROJECT_NAME}
-    PUBLIC_HEADER DESTINATION include
-    ARCHIVE DESTINATION lib
-    LIBRARY DESTINATION lib
+  EXPORT ${PROJECT_NAME}_targets
+  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+  RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+  INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+)
+
+install(TARGETS ${PROJECT_NAME}
+  PUBLIC_HEADER DESTINATION include
+  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+)
+
+# generate and install export file
+install(EXPORT ${PROJECT_NAME}_targets
+  FILE ${PROJECT_NAME}_targets.cmake
+  DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}
 )
 
 if (BUILD_TESTING)
-    file(GLOB_RECURSE TEST_SOURCES test/ test_*.cpp)
-    add_executable(test_${PROJECT_NAME} ${TEST_SOURCES})
-    target_link_libraries(test_${PROJECT_NAME}
-        protobuf
-        ${PROJECT_NAME}
-        state_representation
-        ${GTEST_LIBRARIES}
-        pthread
-    )
-    add_test(NAME test_${PROJECT_NAME} COMMAND test_${PROJECT_NAME})
+  file(GLOB_RECURSE TEST_SOURCES test/ test_*.cpp)
+  add_executable(test_${PROJECT_NAME} ${TEST_SOURCES})
+  target_link_libraries(test_${PROJECT_NAME}
+    protobuf
+    ${PROJECT_NAME}
+    ${GTEST_LIBRARIES}
+    pthread
+  )
+  add_test(NAME test_${PROJECT_NAME} COMMAND test_${PROJECT_NAME})
 endif ()
+
+# generate the version file for the config file
+write_basic_package_version_file(
+  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake"
+  VERSION "${PROJECT_VERSION}"
+  COMPATIBILITY SameMajorVersion
+)
+
+# create config file
+configure_package_config_file(${CMAKE_CURRENT_SOURCE_DIR}/Config.cmake.in
+  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake"
+  INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}
+)
+
+# install config files
+install(FILES
+  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake"
+  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake"
+  DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}
+)
diff --git a/protocol/clproto_cpp/Config.cmake.in b/protocol/clproto_cpp/Config.cmake.in
new file mode 100644
index 000000000..2f7c7d738
--- /dev/null
+++ b/protocol/clproto_cpp/Config.cmake.in
@@ -0,0 +1,5 @@
+@PACKAGE_INIT@
+
+if(NOT TARGET @PROJECT_NAME@)
+  include("${CMAKE_CURRENT_LIST_DIR}/clproto_targets.cmake")
+endif()
\ No newline at end of file
diff --git a/protocol/clproto_cpp/src/clproto.cpp b/protocol/clproto_cpp/src/clproto.cpp
index 37b19be7a..2f6ebcae1 100644
--- a/protocol/clproto_cpp/src/clproto.cpp
+++ b/protocol/clproto_cpp/src/clproto.cpp
@@ -7,6 +7,7 @@
 #include "clproto/decoders.h"
 
 #include <state_representation/State.hpp>
+#include <state_representation/parameters/Parameter.hpp>
 #include <state_representation/space/SpatialState.hpp>
 #include <state_representation/space/cartesian/CartesianState.hpp>
 #include <state_representation/space/cartesian/CartesianPose.hpp>
@@ -19,8 +20,6 @@
 #include <state_representation/space/joint/JointVelocities.hpp>
 #include <state_representation/space/joint/JointAccelerations.hpp>
 #include <state_representation/space/joint/JointTorques.hpp>
-#include <state_representation/geometry/Shape.hpp>
-#include <state_representation/geometry/Ellipsoid.hpp>
 
 #include "state_representation/state_message.pb.h"
 
@@ -1011,6 +1010,279 @@ bool decode(const std::string& msg, Parameter<Eigen::MatrixXd>& obj) {
   return decode_parameter(msg, obj);
 }
 
+/*-----------------------
+ * STD::SHARED_PTR<STATE>
+ * ---------------------- */
+template<> std::string encode<std::shared_ptr<State>>(const std::shared_ptr<State>& obj);
+template<> std::shared_ptr<State> decode(const std::string& msg);
+template<> bool decode(const std::string& msg, std::shared_ptr<State>& obj);
+template<> std::string encode<std::shared_ptr<State>>(const std::shared_ptr<State>& obj) {
+  std::string message;
+  switch (obj->get_type()) {
+    case StateType::STATE:
+      message = encode<State>(*obj);
+      break;
+    case StateType::SPATIAL_STATE:
+      message = encode<SpatialState>(*std::dynamic_pointer_cast<SpatialState>(obj));
+      break;
+    case StateType::CARTESIAN_STATE:
+      message = encode<CartesianState>(*std::dynamic_pointer_cast<CartesianState>(obj));
+      break;
+    case StateType::CARTESIAN_POSE:
+      message = encode<CartesianPose>(*std::dynamic_pointer_cast<CartesianPose>(obj));
+      break;
+    case StateType::CARTESIAN_TWIST:
+      message = encode<CartesianTwist>(*std::dynamic_pointer_cast<CartesianTwist>(obj));
+      break;
+    case StateType::CARTESIAN_ACCELERATION:
+      message = encode<CartesianAcceleration>(*std::dynamic_pointer_cast<CartesianAcceleration>(obj));
+      break;
+    case StateType::CARTESIAN_WRENCH:
+      message = encode<CartesianWrench>(*std::dynamic_pointer_cast<CartesianWrench>(obj));
+      break;
+    case StateType::JOINT_STATE:
+      message = encode<JointState>(*std::dynamic_pointer_cast<JointState>(obj));
+      break;
+    case StateType::JOINT_POSITIONS:
+      message = encode<JointPositions>(*std::dynamic_pointer_cast<JointPositions>(obj));
+      break;
+    case StateType::JOINT_VELOCITIES:
+      message = encode<JointVelocities>(*std::dynamic_pointer_cast<JointVelocities>(obj));
+      break;
+    case StateType::JOINT_ACCELERATIONS:
+      message = encode<JointAccelerations>(*std::dynamic_pointer_cast<JointAccelerations>(obj));
+      break;
+    case StateType::JOINT_TORQUES:
+      message = encode<JointTorques>(*std::dynamic_pointer_cast<JointTorques>(obj));
+      break;
+    case StateType::JACOBIAN:
+      message = encode<Jacobian>(*std::dynamic_pointer_cast<Jacobian>(obj));
+      break;
+    case StateType::PARAMETER: {
+      auto param_ptr = std::dynamic_pointer_cast<ParameterInterface>(obj);
+      switch (param_ptr->get_parameter_type()) {
+        case ParameterType::BOOL:
+          message = encode<Parameter<bool>>(*std::dynamic_pointer_cast<Parameter<bool>>(param_ptr));
+          break;
+        case ParameterType::BOOL_ARRAY:
+          message = encode<Parameter<std::vector<bool>>>(*std::dynamic_pointer_cast<Parameter<std::vector<bool>>>(param_ptr));
+          break;
+        case ParameterType::INT:
+          message = encode<Parameter<int>>(*std::dynamic_pointer_cast<Parameter<int>>(param_ptr));
+          break;
+        case ParameterType::INT_ARRAY:
+          message = encode<Parameter<std::vector<int>>>(*std::dynamic_pointer_cast<Parameter<std::vector<int>>>(param_ptr));
+          break;
+        case ParameterType::DOUBLE:
+          message = encode<Parameter<double>>(*std::dynamic_pointer_cast<Parameter<double>>(param_ptr));
+          break;
+        case ParameterType::DOUBLE_ARRAY:
+          message = encode<Parameter<std::vector<double>>>(*std::dynamic_pointer_cast<Parameter<std::vector<double>>>(param_ptr));
+          break;
+        case ParameterType::STRING:
+          message = encode<Parameter<std::string>>(*std::dynamic_pointer_cast<Parameter<std::string>>(param_ptr));
+          break;
+        case ParameterType::STRING_ARRAY:
+          message = encode<Parameter<std::vector<std::string>>>(*std::dynamic_pointer_cast<Parameter<std::vector<std::string>>>(param_ptr));
+          break;
+        case ParameterType::VECTOR:
+          message = encode<Parameter<Eigen::VectorXd>>(*std::dynamic_pointer_cast<Parameter<Eigen::VectorXd>>(param_ptr));
+          break;
+        case ParameterType::MATRIX:
+          message = encode<Parameter<Eigen::MatrixXd>>(*std::dynamic_pointer_cast<Parameter<Eigen::MatrixXd>>(param_ptr));
+          break;
+        default:
+          throw std::invalid_argument("The ParameterType contained by parameter " + param_ptr->get_name() + " is unsupported.");
+          break;
+      }
+      break;
+    }
+    default:
+      throw std::invalid_argument("The StateType contained by state " + obj->get_name() + " is unsupported.");
+      break;
+  }
+  return message;
+}
+template<> std::shared_ptr<State> decode(const std::string& msg) {
+  std::shared_ptr<State> obj;
+  switch (check_message_type(msg)) {
+    case MessageType::STATE_MESSAGE:
+      obj = make_shared_state(State());
+      break;
+    case MessageType::SPATIAL_STATE_MESSAGE:
+      obj = make_shared_state(SpatialState());
+      break;
+    case MessageType::CARTESIAN_STATE_MESSAGE:
+      obj = make_shared_state(CartesianState());
+      break;
+    case MessageType::CARTESIAN_POSE_MESSAGE:
+      obj = make_shared_state(CartesianPose());
+      break;
+    case MessageType::CARTESIAN_TWIST_MESSAGE:
+      obj = make_shared_state(CartesianTwist());
+      break;
+    case MessageType::CARTESIAN_ACCELERATION_MESSAGE:
+      obj = make_shared_state(CartesianAcceleration());
+      break;
+    case MessageType::CARTESIAN_WRENCH_MESSAGE:
+      obj = make_shared_state(CartesianWrench());
+      break;
+    case MessageType::JOINT_STATE_MESSAGE:
+      obj = make_shared_state(JointState());
+      break;
+    case MessageType::JOINT_POSITIONS_MESSAGE:
+      obj = make_shared_state(JointPositions());
+      break;
+    case MessageType::JOINT_VELOCITIES_MESSAGE:
+      obj = make_shared_state(JointVelocities());
+      break;
+    case MessageType::JOINT_ACCELERATIONS_MESSAGE:
+      obj = make_shared_state(JointAccelerations());
+      break;
+    case MessageType::JOINT_TORQUES_MESSAGE:
+      obj = make_shared_state(JointTorques());
+      break;
+    case MessageType::JACOBIAN_MESSAGE:
+      obj = make_shared_state(Jacobian());
+      break;
+    case MessageType::PARAMETER_MESSAGE: {
+      switch (check_parameter_message_type(msg)) {
+        case ParameterMessageType::BOOL:
+          obj = make_shared_state(Parameter<bool>(""));
+          break;
+        case ParameterMessageType::BOOL_ARRAY:
+          obj = make_shared_state(Parameter<std::vector<bool>>(""));
+          break;
+        case ParameterMessageType::INT:
+          obj = make_shared_state(Parameter<int>(""));
+          break;
+        case ParameterMessageType::INT_ARRAY:
+          obj = make_shared_state(Parameter<std::vector<int>>(""));
+          break;
+        case ParameterMessageType::DOUBLE:
+          obj = make_shared_state(Parameter<double>(""));
+          break;
+        case ParameterMessageType::DOUBLE_ARRAY:
+          obj = make_shared_state(Parameter<std::vector<double>>(""));
+          break;
+        case ParameterMessageType::STRING:
+          obj = make_shared_state(Parameter<std::string>(""));
+          break;
+        case ParameterMessageType::STRING_ARRAY:
+          obj = make_shared_state(Parameter<std::vector<std::string>>(""));
+          break;
+        case ParameterMessageType::VECTOR:
+          obj = make_shared_state(Parameter<Eigen::VectorXd>(""));
+          break;
+        case ParameterMessageType::MATRIX:
+          obj = make_shared_state(Parameter<Eigen::MatrixXd>(""));
+          break;
+        default:
+          throw std::invalid_argument("The ParameterMessageType contained by this message is unsupported.");
+          break;
+      }
+      break;
+    }
+    default:
+      throw std::invalid_argument("The MessageType contained by this message is unsupported.");
+      break;
+  }
+  if (!decode(msg, obj)) {
+    throw DecodingException("Could not decode the message into a std::shared_ptr<State>");
+  }
+  return obj;
+}
+template<> bool decode(const std::string& msg, std::shared_ptr<State>& obj) {
+  try {
+    switch (obj->get_type()) {
+      case StateType::STATE:
+        obj = make_shared_state(decode<State>(msg));
+        break;
+      case StateType::SPATIAL_STATE:
+        obj = make_shared_state(decode<SpatialState>(msg));
+        break;
+      case StateType::CARTESIAN_STATE:
+        obj = make_shared_state(decode<CartesianState>(msg));
+        break;
+      case StateType::CARTESIAN_POSE:
+        obj = make_shared_state(decode<CartesianPose>(msg));
+        break;
+      case StateType::CARTESIAN_TWIST:
+        obj = make_shared_state(decode<CartesianTwist>(msg));
+        break;
+      case StateType::CARTESIAN_ACCELERATION:
+        obj = make_shared_state(decode<CartesianAcceleration>(msg));
+        break;
+      case StateType::CARTESIAN_WRENCH:
+        obj = make_shared_state(decode<CartesianWrench>(msg));
+        break;
+      case StateType::JOINT_STATE:
+        obj = make_shared_state(decode<JointState>(msg));
+        break;
+      case StateType::JOINT_POSITIONS:
+        obj = make_shared_state(decode<JointPositions>(msg));
+        break;
+      case StateType::JOINT_VELOCITIES:
+        obj = make_shared_state(decode<JointVelocities>(msg));
+        break;
+      case StateType::JOINT_ACCELERATIONS:
+        obj = make_shared_state(decode<JointAccelerations>(msg));
+        break;
+      case StateType::JOINT_TORQUES:
+        obj = make_shared_state(decode<JointTorques>(msg));
+        break;
+      case StateType::JACOBIAN:
+        obj = make_shared_state(decode<Jacobian>(msg));
+        break;
+      case StateType::PARAMETER: {
+        auto param_ptr = std::dynamic_pointer_cast<ParameterInterface>(obj);
+        switch (param_ptr->get_parameter_type()) {
+          case ParameterType::BOOL:
+            obj = make_shared_state(decode<Parameter<bool>>(msg));
+            break;
+          case ParameterType::BOOL_ARRAY:
+            obj = make_shared_state(decode<Parameter<std::vector<bool>>>(msg));
+            break;
+          case ParameterType::INT:
+            obj = make_shared_state(decode<Parameter<int>>(msg));
+            break;
+          case ParameterType::INT_ARRAY:
+            obj = make_shared_state(decode<Parameter<std::vector<int>>>(msg));
+            break;
+          case ParameterType::DOUBLE:
+            obj = make_shared_state(decode<Parameter<double>>(msg));
+            break;
+          case ParameterType::DOUBLE_ARRAY:
+            obj = make_shared_state(decode<Parameter<std::vector<double>>>(msg));
+            break;
+          case ParameterType::STRING:
+            obj = make_shared_state(decode<Parameter<std::string>>(msg));
+            break;
+          case ParameterType::STRING_ARRAY:
+            obj = make_shared_state(decode<Parameter<std::vector<std::string>>>(msg));
+            break;
+          case ParameterType::VECTOR:
+            obj = make_shared_state(decode<Parameter<Eigen::VectorXd>>(msg));
+            break;
+          case ParameterType::MATRIX:
+            obj = make_shared_state(decode<Parameter<Eigen::MatrixXd>>(msg));
+            break;
+          default:
+            throw std::invalid_argument("The ParameterType contained by parameter " + param_ptr->get_name() + " is unsupported.");
+            break;
+        }
+        break;
+      }
+      default:
+        throw std::invalid_argument("The StateType contained by state " + obj->get_name() + " is unsupported.");
+        break;
+    }
+    return true;
+  } catch (...) {
+    return false;
+  }
+}
+
 // Generic template code for future types:
 /* ----------------------
  *        __TYPE__
diff --git a/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp b/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp
index f9d1da137..475e775d0 100644
--- a/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp
+++ b/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp
@@ -11,153 +11,96 @@
 
 using namespace state_representation;
 
-
-TEST(CartesianProtoTest, EncodeDecodeSpatialState) {
-  auto send_state = SpatialState(StateType::PARAMETER_BOOL, "A", "B", false);
-  std::string msg = clproto::encode(send_state);
-  EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::SPATIAL_STATE_MESSAGE);
-
-  SpatialState recv_state(StateType::STATE);
-  EXPECT_NO_THROW(clproto::decode<SpatialState>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-
-  EXPECT_EQ(send_state.is_empty(), recv_state.is_empty());
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str());
-}
-
-TEST(CartesianProtoTest, EncodeDecodeCartesianState) {
-  auto send_state = CartesianState::Random("A", "B");
-  std::string msg = clproto::encode(send_state);
-  EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::CARTESIAN_STATE_MESSAGE);
-
-  CartesianState recv_state;
-  EXPECT_NO_THROW(clproto::decode<CartesianState>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-  EXPECT_FALSE(recv_state.is_empty());
-
+template<typename T>
+static void test_cart_state_equal(const T& send_state, const T& recv_state) {
+  EXPECT_EQ(send_state.get_type(), recv_state.get_type());
   EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
   EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str());
   EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5);
 }
 
-TEST(CartesianProtoTest, EncodeDecodeCartesianPose) {
-  auto send_state = CartesianPose::Random("A", "B");
+template<typename T>
+static void test_encode_decode_cartesian(const T& send_state, clproto::MessageType type) {
   std::string msg = clproto::encode(send_state);
   EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::CARTESIAN_POSE_MESSAGE);
+  EXPECT_EQ(clproto::check_message_type(msg), type);
 
-  CartesianPose recv_state;
-  EXPECT_NO_THROW(clproto::decode<CartesianPose>(msg));
+  T recv_state;
+  EXPECT_NO_THROW(clproto::decode<T>(msg));
   EXPECT_TRUE(clproto::decode(msg, recv_state));
   EXPECT_FALSE(recv_state.is_empty());
 
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str());
-  EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5);
-}
+  test_cart_state_equal(send_state, recv_state);
 
-TEST(CartesianProtoTest, EncodeDecodeCartesianTwist) {
-  auto send_state = CartesianTwist::Random("A", "B");
-  std::string msg = clproto::encode(send_state);
+  auto send_state_ptr = make_shared_state(send_state);
+  msg = clproto::encode(send_state_ptr);
   EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::CARTESIAN_TWIST_MESSAGE);
+  EXPECT_EQ(clproto::check_message_type(msg), type);
 
-  CartesianTwist recv_state;
-  EXPECT_NO_THROW(clproto::decode<CartesianTwist>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-  EXPECT_FALSE(recv_state.is_empty());
+  T recv_state_2;
+  auto recv_state_ptr = make_shared_state(recv_state_2);
+  EXPECT_NO_THROW(clproto::decode<std::shared_ptr<State>>(msg));
+  EXPECT_TRUE(clproto::decode(msg, recv_state_ptr));
 
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str());
-  EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5);
+  recv_state_2 = *std::dynamic_pointer_cast<T>(recv_state_ptr);
+  test_cart_state_equal(send_state, recv_state_2);
 }
 
-TEST(CartesianProtoTest, EncodeDecodeCartesianAcceleration) {
-  auto send_state = CartesianAcceleration::Random("A", "B");
-  std::string msg = clproto::encode(send_state);
-  EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::CARTESIAN_ACCELERATION_MESSAGE);
-
-  CartesianAcceleration recv_state;
-  EXPECT_NO_THROW(clproto::decode<CartesianAcceleration>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-  EXPECT_FALSE(recv_state.is_empty());
+template<typename T>
+static void test_encode_decode_empty_cartesian(const T& state) {
+  EXPECT_TRUE(state.is_empty());
+  std::string msg;
+  EXPECT_NO_THROW(msg = clproto::encode(state));
 
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str());
-  EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5);
+  T recv_state;
+  EXPECT_NO_THROW(recv_state = clproto::decode<T>(msg));
+  EXPECT_TRUE(recv_state.is_empty());
 }
 
-TEST(CartesianProtoTest, EncodeDecodeCartesianWrench) {
-  auto send_state = CartesianWrench::Random("A", "B");
+TEST(CartesianProtoTest, EncodeDecodeSpatialState) {
+  auto send_state = SpatialState(StateType::SPATIAL_STATE, "A", "B", false);
   std::string msg = clproto::encode(send_state);
   EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::CARTESIAN_WRENCH_MESSAGE);
+  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::SPATIAL_STATE_MESSAGE);
 
-  CartesianWrench recv_state;
-  EXPECT_NO_THROW(clproto::decode<CartesianWrench>(msg));
+  SpatialState recv_state(StateType::SPATIAL_STATE);
+  EXPECT_NO_THROW(clproto::decode<SpatialState>(msg));
   EXPECT_TRUE(clproto::decode(msg, recv_state));
-  EXPECT_FALSE(recv_state.is_empty());
 
+  EXPECT_EQ(send_state.get_type(), recv_state.get_type());
+  EXPECT_EQ(send_state.is_empty(), recv_state.is_empty());
   EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
   EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str());
-  EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5);
-}
-
-TEST(CartesianProtoTest, EncodeDecodeEmptyCartesianState) {
-  CartesianState empty_state;
-  EXPECT_TRUE(empty_state.is_empty());
-  std::string msg;
-  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
 
-  CartesianState recv_state;
-  EXPECT_NO_THROW(recv_state = clproto::decode<CartesianState>(msg));
-  EXPECT_TRUE(recv_state.is_empty());
-}
+  std::shared_ptr<State> send_state_ptr = std::make_shared<SpatialState>(send_state);
+  msg = clproto::encode(send_state_ptr);
+  EXPECT_TRUE(clproto::is_valid(msg));
+  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::SPATIAL_STATE_MESSAGE);
 
-TEST(CartesianProtoTest, EncodeDecodeEmptyCartesianPose) {
-  CartesianPose empty_state;
-  EXPECT_TRUE(empty_state.is_empty());
-  std::string msg;
-  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
+  SpatialState recv_state_2(StateType::SPATIAL_STATE);
+  auto recv_state_ptr = make_shared_state(recv_state_2);
+  EXPECT_NO_THROW(clproto::decode<std::shared_ptr<State>>(msg));
+  EXPECT_TRUE(clproto::decode(msg, recv_state_ptr));
 
-  CartesianPose recv_state;
-  EXPECT_NO_THROW(recv_state = clproto::decode<CartesianPose>(msg));
-  EXPECT_TRUE(recv_state.is_empty());
+  recv_state_2 = *std::dynamic_pointer_cast<SpatialState>(recv_state_ptr);
+  EXPECT_EQ(send_state.get_type(), recv_state_2.get_type());
+  EXPECT_EQ(send_state.is_empty(), recv_state_2.is_empty());
+  EXPECT_STREQ(send_state.get_name().c_str(), recv_state_2.get_name().c_str());
+  EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state_2.get_reference_frame().c_str());
 }
 
-TEST(CartesianProtoTest, EncodeDecodeEmptyCartesianTwist) {
-  CartesianTwist empty_state;
-  EXPECT_TRUE(empty_state.is_empty());
-  std::string msg;
-  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
-
-  CartesianTwist recv_state;
-  EXPECT_NO_THROW(recv_state = clproto::decode<CartesianTwist>(msg));
-  EXPECT_TRUE(recv_state.is_empty());
+TEST(CartesianProtoTest, EncodeDecodeRandomCartesian) {
+  test_encode_decode_cartesian(CartesianState::Random("A", "B"), clproto::CARTESIAN_STATE_MESSAGE);
+  test_encode_decode_cartesian(CartesianPose::Random("A", "B"), clproto::CARTESIAN_POSE_MESSAGE);
+  test_encode_decode_cartesian(CartesianTwist::Random("A", "B"), clproto::CARTESIAN_TWIST_MESSAGE);
+  test_encode_decode_cartesian(CartesianAcceleration::Random("A", "B"), clproto::CARTESIAN_ACCELERATION_MESSAGE);
+  test_encode_decode_cartesian(CartesianWrench::Random("A", "B"), clproto::CARTESIAN_WRENCH_MESSAGE);
 }
 
-TEST(CartesianProtoTest, EncodeDecodeEmptyCartesianAcceleration) {
-  CartesianAcceleration empty_state;
-  EXPECT_TRUE(empty_state.is_empty());
-  std::string msg;
-  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
-
-  CartesianAcceleration recv_state;
-  EXPECT_NO_THROW(recv_state = clproto::decode<CartesianAcceleration>(msg));
-  EXPECT_TRUE(recv_state.is_empty());
+TEST(CartesianProtoTest, EncodeDecodeEmptyCartesian) {
+  test_encode_decode_empty_cartesian(CartesianState());
+  test_encode_decode_empty_cartesian(CartesianPose());
+  test_encode_decode_empty_cartesian(CartesianTwist());
+  test_encode_decode_empty_cartesian(CartesianAcceleration());
+  test_encode_decode_empty_cartesian(CartesianWrench());
 }
-
-TEST(CartesianProtoTest, EncodeDecodeEmptyCartesianWrench) {
-  CartesianWrench empty_state;
-  EXPECT_TRUE(empty_state.is_empty());
-  std::string msg;
-  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
-
-  CartesianWrench recv_state;
-  EXPECT_NO_THROW(recv_state = clproto::decode<CartesianWrench>(msg));
-  EXPECT_TRUE(recv_state.is_empty());
-}
\ No newline at end of file
diff --git a/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp b/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp
index 20aa85505..b2294704f 100644
--- a/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp
+++ b/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp
@@ -11,17 +11,19 @@
 
 using namespace state_representation;
 
-TEST(JointProtoTest, EncodeDecodeJacobian) {
-  auto send_state = Jacobian::Random("robot", {"one", "two", "three"}, "A", "B");
-  std::string msg = clproto::encode(send_state);
-  EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::JACOBIAN_MESSAGE);
-
-  Jacobian recv_state;
-  EXPECT_NO_THROW(clproto::decode<Jacobian>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-  EXPECT_FALSE(recv_state.is_empty());
+template<typename T>
+static void test_joint_state_equal(const T& send_state, const T& recv_state) {
+  EXPECT_EQ(send_state.get_type(), recv_state.get_type());
+  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
+  ASSERT_EQ(send_state.get_size(), recv_state.get_size());
+  for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) {
+    EXPECT_STREQ(send_state.get_names().at(ind).c_str(), recv_state.get_names().at(ind).c_str());
+  }
+  EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5);
+}
 
+static void test_jacobian_equal(const Jacobian& send_state, const Jacobian& recv_state) {
+  EXPECT_EQ(send_state.get_type(), recv_state.get_type());
   EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
   EXPECT_STREQ(send_state.get_frame().c_str(), recv_state.get_frame().c_str());
   EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str());
@@ -34,100 +36,87 @@ TEST(JointProtoTest, EncodeDecodeJacobian) {
   EXPECT_NEAR(send_state.data().norm(), recv_state.data().norm(), 1e-5);
 }
 
-TEST(JointProtoTest, EncodeDecodeJointState) {
-  std::vector<std::string> joint_names = {"apple", "orange", "banana", "prune"};
-  auto send_state = JointState::Random("zeiss", joint_names);
+template<typename T>
+static void test_encode_decode(const T& send_state, clproto::MessageType type) {
   std::string msg = clproto::encode(send_state);
   EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::JOINT_STATE_MESSAGE);
+  EXPECT_EQ(clproto::check_message_type(msg), type);
 
-  JointState recv_state;
-  EXPECT_NO_THROW(clproto::decode<JointState>(msg));
+  T recv_state;
+  EXPECT_NO_THROW(clproto::decode<T>(msg));
   EXPECT_TRUE(clproto::decode(msg, recv_state));
   EXPECT_FALSE(recv_state.is_empty());
 
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  ASSERT_EQ(send_state.get_size(), recv_state.get_size());
-  for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) {
-    EXPECT_STREQ(send_state.get_names().at(ind).c_str(), recv_state.get_names().at(ind).c_str());
-  }
-  EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5);
-}
+  test_joint_state_equal(send_state, recv_state);
 
-TEST(JointProtoTest, EncodeDecodeJointPositions) {
-  auto send_state = JointPositions::Random("robot", 3);
-  std::string msg = clproto::encode(send_state);
+  auto send_state_ptr = make_shared_state(send_state);
+  msg = clproto::encode(send_state_ptr);
   EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::JOINT_POSITIONS_MESSAGE);
+  EXPECT_EQ(clproto::check_message_type(msg), type);
 
-  JointPositions recv_state;
-  EXPECT_NO_THROW(clproto::decode<JointPositions>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-  EXPECT_FALSE(recv_state.is_empty());
+  T recv_state_2;
+  auto recv_state_ptr = make_shared_state(recv_state_2);
+  EXPECT_NO_THROW(clproto::decode<std::shared_ptr<State>>(msg));
+  EXPECT_TRUE(clproto::decode(msg, recv_state_ptr));
 
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  ASSERT_EQ(send_state.get_size(), recv_state.get_size());
-  for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) {
-    EXPECT_STREQ(send_state.get_names().at(ind).c_str(), recv_state.get_names().at(ind).c_str());
-  }
-  EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5);
+  recv_state_2 = *std::dynamic_pointer_cast<T>(recv_state_ptr);
+  test_joint_state_equal(send_state, recv_state_2);
 }
 
-TEST(JointProtoTest, EncodeDecodeJointVelocities) {
-  auto send_state = JointVelocities::Random("robot", 3);
-  std::string msg = clproto::encode(send_state);
-  EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::JOINT_VELOCITIES_MESSAGE);
+template<typename T>
+static void test_encode_decode_empty_joint(const T& state) {
+  EXPECT_TRUE(state.is_empty());
+  std::string msg;
+  EXPECT_NO_THROW(msg = clproto::encode(state));
 
-  JointVelocities recv_state;
-  EXPECT_NO_THROW(clproto::decode<JointVelocities>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-  EXPECT_FALSE(recv_state.is_empty());
+  T recv_state;
+  EXPECT_NO_THROW(recv_state = clproto::decode<T>(msg));
+  EXPECT_TRUE(recv_state.is_empty());
+}
 
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  ASSERT_EQ(send_state.get_size(), recv_state.get_size());
-  for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) {
-    EXPECT_STREQ(send_state.get_names().at(ind).c_str(), recv_state.get_names().at(ind).c_str());
-  }
-  EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5);
+TEST(JointProtoTest, EncodeDecodeRandomJoint) {
+  std::vector<std::string> joint_names = {"apple", "orange", "banana", "prune"};
+  auto send_state = JointState::Random("zeiss", joint_names);
+  test_encode_decode(send_state, clproto::JOINT_STATE_MESSAGE);
+  test_encode_decode(JointPositions::Random("robot", 3), clproto::JOINT_POSITIONS_MESSAGE);
+  test_encode_decode(JointVelocities::Random("robot", 3), clproto::JOINT_VELOCITIES_MESSAGE);
+  test_encode_decode(JointAccelerations::Random("robot", 3), clproto::JOINT_ACCELERATIONS_MESSAGE);
+  test_encode_decode(JointTorques::Random("robot", 3), clproto::JOINT_TORQUES_MESSAGE);
+}
+
+TEST(CartesianProtoTest, EncodeDecodeEmptyJoint) {
+  test_encode_decode_empty_joint(JointState());
+  test_encode_decode_empty_joint(JointPositions());
+  test_encode_decode_empty_joint(JointVelocities());
+  test_encode_decode_empty_joint(JointAccelerations());
+  test_encode_decode_empty_joint(JointTorques());
 }
 
-TEST(JointProtoTest, EncodeDecodeJointAccelerations) {
-  auto send_state = JointAccelerations::Random("robot", 3);
+TEST(JointProtoTest, EncodeDecodeJacobian) {
+  auto send_state = Jacobian::Random("robot", {"one", "two", "three"}, "A", "B");
   std::string msg = clproto::encode(send_state);
   EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::JOINT_ACCELERATIONS_MESSAGE);
+  EXPECT_EQ(clproto::check_message_type(msg), clproto::JACOBIAN_MESSAGE);
 
-  JointAccelerations recv_state;
-  EXPECT_NO_THROW(clproto::decode<JointAccelerations>(msg));
+  Jacobian recv_state;
+  EXPECT_NO_THROW(clproto::decode<Jacobian>(msg));
   EXPECT_TRUE(clproto::decode(msg, recv_state));
   EXPECT_FALSE(recv_state.is_empty());
 
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  ASSERT_EQ(send_state.get_size(), recv_state.get_size());
-  for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) {
-    EXPECT_STREQ(send_state.get_names().at(ind).c_str(), recv_state.get_names().at(ind).c_str());
-  }
-  EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5);
-}
+  test_jacobian_equal(send_state, recv_state);
 
-TEST(JointProtoTest, EncodeDecodeJointTorques) {
-  auto send_state = JointTorques::Random("robot", 3);
-  std::string msg = clproto::encode(send_state);
+  auto send_state_ptr = make_shared_state(send_state);
+  msg = clproto::encode(send_state_ptr);
   EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::JOINT_TORQUES_MESSAGE);
+  EXPECT_EQ(clproto::check_message_type(msg), clproto::JACOBIAN_MESSAGE);
 
-  JointTorques recv_state;
-  EXPECT_NO_THROW(clproto::decode<JointTorques>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-  EXPECT_FALSE(recv_state.is_empty());
+  Jacobian recv_state_2;
+  auto recv_state_ptr = make_shared_state(recv_state_2);
+  EXPECT_NO_THROW(clproto::decode<std::shared_ptr<State>>(msg));
+  EXPECT_TRUE(clproto::decode(msg, recv_state_ptr));
 
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  ASSERT_EQ(send_state.get_size(), recv_state.get_size());
-  for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) {
-    EXPECT_STREQ(send_state.get_names().at(ind).c_str(), recv_state.get_names().at(ind).c_str());
-  }
-  EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5);
+  recv_state_2 = *std::dynamic_pointer_cast<Jacobian>(recv_state_ptr);
+  test_jacobian_equal(send_state, recv_state_2);
 }
 
 TEST(JointProtoTest, EncodeDecodeEmptyJacobian) {
@@ -140,58 +129,3 @@ TEST(JointProtoTest, EncodeDecodeEmptyJacobian) {
   EXPECT_NO_THROW(recv_state = clproto::decode<Jacobian>(msg));
   EXPECT_TRUE(recv_state.is_empty());
 }
-
-TEST(JointProtoTest, EncodeDecodeEmptyJointPositions) {
-  JointPositions empty_state;
-  EXPECT_TRUE(empty_state.is_empty());
-  std::string msg;
-  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
-
-  JointPositions recv_state;
-  EXPECT_NO_THROW(recv_state = clproto::decode<JointPositions>(msg));
-  EXPECT_TRUE(recv_state.is_empty());
-}
-
-TEST(JointProtoTest, EncodeDecodeEmptyJointState) {
-  JointState empty_state;
-  EXPECT_TRUE(empty_state.is_empty());
-  std::string msg;
-  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
-
-  JointState recv_state;
-  EXPECT_NO_THROW(recv_state = clproto::decode<JointState>(msg));
-  EXPECT_TRUE(recv_state.is_empty());
-}
-
-TEST(JointProtoTest, EncodeDecodeEmptyJointVelocities) {
-  JointVelocities empty_state;
-  EXPECT_TRUE(empty_state.is_empty());
-  std::string msg;
-  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
-
-  JointVelocities recv_state;
-  EXPECT_NO_THROW(recv_state = clproto::decode<JointVelocities>(msg));
-  EXPECT_TRUE(recv_state.is_empty());
-}
-
-TEST(JointProtoTest, EncodeDecodeEmptyJointAccelerations) {
-  JointAccelerations empty_state;
-  EXPECT_TRUE(empty_state.is_empty());
-  std::string msg;
-  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
-
-  JointAccelerations recv_state;
-  EXPECT_NO_THROW(recv_state = clproto::decode<JointAccelerations>(msg));
-  EXPECT_TRUE(recv_state.is_empty());
-}
-
-TEST(JointProtoTest, EncodeDecodeEmptyJointTorques) {
-  JointTorques empty_state;
-  EXPECT_TRUE(empty_state.is_empty());
-  std::string msg;
-  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
-
-  JointTorques recv_state;
-  EXPECT_NO_THROW(recv_state = clproto::decode<JointTorques>(msg));
-  EXPECT_TRUE(recv_state.is_empty());
-}
diff --git a/protocol/clproto_cpp/test/tests/test_messages.cpp b/protocol/clproto_cpp/test/tests/test_messages.cpp
index 2c6a05d5a..9a9625f10 100644
--- a/protocol/clproto_cpp/test/tests/test_messages.cpp
+++ b/protocol/clproto_cpp/test/tests/test_messages.cpp
@@ -1,6 +1,7 @@
 #include <gtest/gtest.h>
 
 #include <state_representation/State.hpp>
+#include <state_representation/geometry/Ellipsoid.hpp>
 #include <state_representation/space/cartesian/CartesianState.hpp>
 #include <state_representation/space/cartesian/CartesianPose.hpp>
 
@@ -14,7 +15,7 @@ TEST(MessageProtoTest, EncodeDecodeState) {
   EXPECT_TRUE(clproto::is_valid(msg));
   EXPECT_TRUE(clproto::check_message_type(msg) == clproto::STATE_MESSAGE);
 
-  State recv_state(StateType::PARAMETER_MATRIX);
+  State recv_state(StateType::STATE);
   EXPECT_NO_THROW(clproto::decode<State>(msg));
   EXPECT_TRUE(clproto::decode(msg, recv_state));
 
@@ -25,6 +26,19 @@ TEST(MessageProtoTest, EncodeDecodeState) {
             recv_state.get_timestamp().time_since_epoch().count());
 }
 
+TEST(MessageProtoTest, EncodeDecodeInvalidState) {
+  auto send_state = Ellipsoid("ellipsoid");
+  auto send_state_ptr = make_shared_state(send_state);
+  EXPECT_THROW(clproto::encode(send_state_ptr), std::invalid_argument);
+
+  auto send_state_2 = State(StateType::STATE, "A", false);
+  std::string msg = clproto::encode(send_state_2);
+
+  Ellipsoid recv_state;
+  auto recv_state_ptr = make_shared_state(recv_state);
+  EXPECT_FALSE(clproto::decode(msg, recv_state_ptr));
+}
+
 TEST(MessageProtoTest, DecodeInvalidString) {
   std::string dummy_msg = "hello world";
 
diff --git a/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp b/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp
index 0d3ce1a14..c66d388b7 100644
--- a/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp
+++ b/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp
@@ -1,242 +1,127 @@
 #include <gtest/gtest.h>
 
 #include <state_representation/parameters/Parameter.hpp>
+#include <state_representation/space/cartesian/CartesianState.hpp>
 
 #include "clproto.h"
 
 using namespace state_representation;
 
-TEST(ParameterProtoTest, EncodeDecodeParameterInt) {
-  int value = 1;
-
-  auto send_state = Parameter("A", value);
-  std::string msg = clproto::encode(send_state);
-  EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE);
-  EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::INT);
-
-  Parameter<int> recv_state("");
-  EXPECT_NO_THROW(clproto::decode<Parameter<int>>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  EXPECT_EQ(send_state.get_value(), recv_state.get_value());
-  EXPECT_EQ(send_state.get_type(), recv_state.get_type());
-}
-
-TEST(ParameterProtoTest, EncodeDecodeParameterIntArray) {
-  std::vector<int> value = {1, 2, 3};
-
-  auto send_state = Parameter("A", value);
-  std::string msg = clproto::encode(send_state);
-  EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE);
-  EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::INT_ARRAY);
-
-  Parameter<std::vector<int>> recv_state("");
-  EXPECT_NO_THROW(clproto::decode<Parameter<std::vector<int>>>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  EXPECT_EQ(send_state.get_value().size(), recv_state.get_value().size());
-  for(std::size_t index = 0; index < send_state.get_value().size(); ++index) {
-    EXPECT_EQ(send_state.get_value().at(index), recv_state.get_value().at(index));
-  }
-  EXPECT_EQ(send_state.get_type(), recv_state.get_type());
-}
-
-TEST(ParameterProtoTest, EncodeDecodeParameterDouble) {
-  double value = 1.0;
-
-  auto send_state = Parameter("A", value);
-  std::string msg = clproto::encode(send_state);
-  EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE);
-  EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::DOUBLE);
-
-  Parameter<double> recv_state("");
-  EXPECT_NO_THROW(clproto::decode<Parameter<double>>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  EXPECT_EQ(send_state.get_value(), recv_state.get_value());
-  EXPECT_EQ(send_state.get_type(), recv_state.get_type());
-}
-
-TEST(ParameterProtoTest, EncodeDecodeParameterDoubleArray) {
-  std::vector<double> value = {1.0, 2.0, 3.0};
-
-  auto send_state = Parameter("A", value);
-  std::string msg = clproto::encode(send_state);
-  EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE);
-  EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::DOUBLE_ARRAY);
-
-  Parameter<std::vector<double>> recv_state("");
-  EXPECT_NO_THROW(clproto::decode<Parameter<std::vector<double>>>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  EXPECT_EQ(send_state.get_value().size(), recv_state.get_value().size());
-  for(std::size_t index = 0; index < send_state.get_value().size(); ++index) {
-    EXPECT_EQ(send_state.get_value().at(index), recv_state.get_value().at(index));
-  }
-  EXPECT_EQ(send_state.get_type(), recv_state.get_type());
-}
-
-TEST(ParameterProtoTest, EncodeDecodeParameterBool) {
-  bool value = true;
-
-  auto send_state = Parameter("A", value);
-  std::string msg = clproto::encode(send_state);
-  EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE);
-  EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::BOOL);
-
-  Parameter<bool> recv_state("");
-  EXPECT_NO_THROW(clproto::decode<Parameter<bool>>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-
+template<typename T>
+static void test_parameter_equal(const T& send_state, const T& recv_state) {
   EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
   EXPECT_EQ(send_state.get_value(), recv_state.get_value());
   EXPECT_EQ(send_state.get_type(), recv_state.get_type());
 }
 
-TEST(ParameterProtoTest, EncodeDecodeParameterBoolArray) {
-  std::vector<bool> value = {true, false, true, false};
-
-  auto send_state = Parameter("A", value);
+template<typename T>
+static void test_encode_decode_parameter(
+    const T& send_state, clproto::MessageType msg_type, clproto::ParameterMessageType param_type
+) {
   std::string msg = clproto::encode(send_state);
   EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE);
-  EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::BOOL_ARRAY);
+  EXPECT_EQ(clproto::check_message_type(msg), msg_type);
+  EXPECT_EQ(clproto::check_parameter_message_type(msg), param_type);
 
-  Parameter<std::vector<bool>> recv_state("");
-  EXPECT_NO_THROW(clproto::decode<Parameter<std::vector<bool>>>(msg));
+  T recv_state("");
+  EXPECT_NO_THROW(clproto::decode<T>(msg));
   EXPECT_TRUE(clproto::decode(msg, recv_state));
+  EXPECT_FALSE(recv_state.is_empty());
 
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  EXPECT_EQ(send_state.get_value().size(), recv_state.get_value().size());
-  for(std::size_t index = 0; index < send_state.get_value().size(); ++index) {
-    EXPECT_EQ(send_state.get_value().at(index), recv_state.get_value().at(index));
-  }
-  EXPECT_EQ(send_state.get_type(), recv_state.get_type());
-}
-
-TEST(ParameterProtoTest, EncodeDecodeParameterString) {
-  std::string value = "value";
+  test_parameter_equal(send_state, recv_state);
 
-  auto send_state = Parameter("A", value);
-  std::string msg = clproto::encode(send_state);
+  auto send_state_ptr = make_shared_state(send_state);
+  msg = clproto::encode(send_state_ptr);
   EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE);
-  EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::STRING);
+  EXPECT_EQ(clproto::check_message_type(msg), msg_type);
+  EXPECT_EQ(clproto::check_parameter_message_type(msg), param_type);
 
-  Parameter<std::string> recv_state("");
-  EXPECT_NO_THROW(clproto::decode<Parameter<std::string>>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
+  T recv_state_2("");
+  auto recv_state_ptr = make_shared_state(recv_state_2);
+  EXPECT_NO_THROW(clproto::decode<std::shared_ptr<State>>(msg));
+  EXPECT_TRUE(clproto::decode(msg, recv_state_ptr));
 
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  EXPECT_STREQ(send_state.get_value().c_str(), recv_state.get_value().c_str());
-  EXPECT_EQ(send_state.get_type(), recv_state.get_type());
+  recv_state_2 = *std::dynamic_pointer_cast<T>(recv_state_ptr);
+  test_parameter_equal(send_state, recv_state_2);
 }
 
-TEST(ParameterProtoTest, EncodeDecodeParameterStringArray) {
-  std::vector<std::string> value = {"value 1", "value 2", "value 3"};
-
-  auto send_state = Parameter("A", value);
-  std::string msg = clproto::encode(send_state);
-  EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE);
-  EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::STRING_ARRAY);
-
-  Parameter<std::vector<std::string>> recv_state("");
-  EXPECT_NO_THROW(clproto::decode<Parameter<std::vector<std::string>>>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
+template<typename T>
+static void test_encode_decode_empty_parameter(const T& state) {
+  EXPECT_TRUE(state.is_empty());
+  std::string msg;
+  EXPECT_NO_THROW(msg = clproto::encode(state));
 
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  EXPECT_EQ(send_state.get_value().size(), recv_state.get_value().size());
-  for(std::size_t index = 0; index < send_state.get_value().size(); ++index) {
-    EXPECT_STREQ(send_state.get_value().at(index).c_str(), recv_state.get_value().at(index).c_str());
-  }
-  EXPECT_EQ(send_state.get_type(), recv_state.get_type());
+  T recv_state("");
+  EXPECT_NO_THROW(recv_state = clproto::decode<T>(msg));
+  EXPECT_TRUE(recv_state.is_empty());
 }
 
-TEST(ParameterProtoTest, EncodeDecodeParameterVector) {
-  Eigen::VectorXd value(5);
-  value << 1.0, 2.0, 3.0, 4.0, 5.0;
-
-  auto send_state = Parameter("A", value);
-  std::string msg = clproto::encode(send_state);
-  EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE);
-  EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::VECTOR);
-
-  Parameter<Eigen::VectorXd> recv_state("");
-  EXPECT_NO_THROW(clproto::decode<Parameter<Eigen::VectorXd>>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  EXPECT_EQ(send_state.get_value().size(), recv_state.get_value().size());
-  for(std::size_t index = 0; index < send_state.get_value().size(); ++index) {
-    EXPECT_EQ(send_state.get_value()(index), recv_state.get_value()(index));
-  }
-  EXPECT_EQ(send_state.get_type(), recv_state.get_type());
+TEST(ParameterProtoTest, EncodeDecodeParameterInt) {
+  test_encode_decode_parameter(
+      Parameter<int>("A", 1), clproto::MessageType::PARAMETER_MESSAGE, clproto::ParameterMessageType::INT
+  );
+  test_encode_decode_parameter(
+      Parameter<std::vector<int>>("A", {1, 2, 3}), clproto::MessageType::PARAMETER_MESSAGE,
+      clproto::ParameterMessageType::INT_ARRAY
+  );
+  test_encode_decode_parameter(
+      Parameter<double>("A", 1.0), clproto::MessageType::PARAMETER_MESSAGE, clproto::ParameterMessageType::DOUBLE
+  );
+  test_encode_decode_parameter(
+      Parameter<std::vector<double>>("A", {1.0, 2.0, 3.0}), clproto::MessageType::PARAMETER_MESSAGE,
+      clproto::ParameterMessageType::DOUBLE_ARRAY
+  );
+  test_encode_decode_parameter(
+      Parameter<bool>("A", true), clproto::MessageType::PARAMETER_MESSAGE, clproto::ParameterMessageType::BOOL
+  );
+  test_encode_decode_parameter(
+      Parameter<std::vector<bool>>("A", {true, false, true, false}), clproto::MessageType::PARAMETER_MESSAGE,
+      clproto::ParameterMessageType::BOOL_ARRAY
+  );
+  test_encode_decode_parameter(
+      Parameter<std::string>("A", "value"), clproto::MessageType::PARAMETER_MESSAGE,
+      clproto::ParameterMessageType::STRING
+  );
+  test_encode_decode_parameter(
+      Parameter<std::vector<std::string>>("A", {"value 1", "value 2", "value 3"}),
+      clproto::MessageType::PARAMETER_MESSAGE, clproto::ParameterMessageType::STRING_ARRAY
+  );
+  Eigen::VectorXd vector_value(5);
+  vector_value << 1.0, 2.0, 3.0, 4.0, 5.0;
+  test_encode_decode_parameter(
+      Parameter<Eigen::VectorXd>("A", vector_value), clproto::MessageType::PARAMETER_MESSAGE,
+      clproto::ParameterMessageType::VECTOR
+  );
+  Eigen::MatrixXd matrix_value(2, 3);
+  matrix_value << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0;
+  test_encode_decode_parameter(
+      Parameter<Eigen::MatrixXd>("A", matrix_value), clproto::MessageType::PARAMETER_MESSAGE,
+      clproto::ParameterMessageType::MATRIX
+  );
 }
 
-TEST(ParameterProtoTest, EncodeDecodeParameterMatrix) {
-  Eigen::MatrixXd value(2, 3);
-  value << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0;
-
-  auto send_state = Parameter("A", value);
-  std::string msg = clproto::encode(send_state);
-  EXPECT_TRUE(clproto::is_valid(msg));
-  EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE);
-  EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::MATRIX);
-
-  Parameter<Eigen::MatrixXd> recv_state("");
-  EXPECT_NO_THROW(clproto::decode<Parameter<Eigen::MatrixXd>>(msg));
-  EXPECT_TRUE(clproto::decode(msg, recv_state));
-
-  EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
-  EXPECT_EQ(send_state.get_value().size(), recv_state.get_value().size());
-  EXPECT_EQ(send_state.get_value().rows(), recv_state.get_value().rows());
-  EXPECT_EQ(send_state.get_value().cols(), recv_state.get_value().cols());
-  for(std::size_t index = 0; index < send_state.get_value().size(); ++index) {
-    EXPECT_EQ(send_state.get_value()(index), recv_state.get_value()(index));
-  }
-  EXPECT_EQ(send_state.get_type(), recv_state.get_type());
+TEST(ParameterProtoTest, EncodeDecodeEmptyParameter) {
+  test_encode_decode_empty_parameter(Parameter<int>(""));
+  test_encode_decode_empty_parameter(Parameter<std::vector<int>>(""));
+  test_encode_decode_empty_parameter(Parameter<double>(""));
+  test_encode_decode_empty_parameter(Parameter<std::vector<double>>(""));
+  test_encode_decode_empty_parameter(Parameter<bool>(""));
+  test_encode_decode_empty_parameter(Parameter<std::vector<bool>>(""));
+  test_encode_decode_empty_parameter(Parameter<std::string>(""));
+  test_encode_decode_empty_parameter(Parameter<std::vector<std::string>>(""));
+  test_encode_decode_empty_parameter(Parameter<Eigen::VectorXd>(""));
+  test_encode_decode_empty_parameter(Parameter<Eigen::MatrixXd>(""));
 }
 
-TEST(ParameterProtoTest, EncodeDecodeEmptyParameterDouble) {
-  Parameter<double> empty_state("");
-  EXPECT_TRUE(empty_state.is_empty());
-  std::string msg;
-  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
-
-  Parameter<double> recv_state("");
-  EXPECT_NO_THROW(recv_state = clproto::decode<Parameter<double>>(msg));
-  EXPECT_TRUE(recv_state.is_empty());
-}
+TEST(MessageProtoTest, EncodeDecodeInvalidParameter) {
+  auto send_state = Parameter<CartesianState>("state", CartesianState::Random("state"));
+  auto send_state_ptr = make_shared_state(send_state);
+  EXPECT_THROW(clproto::encode(send_state_ptr), std::invalid_argument);
 
-TEST(ParameterProtoTest, EncodeDecodeEmptyParameterBool) {
-  Parameter<bool> empty_state("");
-  EXPECT_TRUE(empty_state.is_empty());
-  std::string msg;
-  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
+  auto send_state_2 = State(StateType::STATE, "A", false);
+  std::string msg = clproto::encode(send_state_2);
 
-  Parameter<bool> recv_state("");
-  EXPECT_NO_THROW(recv_state = clproto::decode<Parameter<bool>>(msg));
-  EXPECT_TRUE(recv_state.is_empty());
+  Parameter<CartesianState> recv_state("");
+  auto recv_state_ptr = make_shared_state(recv_state);
+  EXPECT_FALSE(clproto::decode(msg, recv_state_ptr));
 }
-
-TEST(ParameterProtoTest, EncodeDecodeEmptyParameterString) {
-  Parameter<std::string> empty_state("");
-  EXPECT_TRUE(empty_state.is_empty());
-  std::string msg;
-  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
-
-  Parameter<std::string> recv_state("");
-  EXPECT_NO_THROW(recv_state = clproto::decode<Parameter<std::string>>(msg));
-  EXPECT_TRUE(recv_state.is_empty());
-}
\ No newline at end of file
diff --git a/protocol/dev-server.sh b/protocol/dev-server.sh
index 1d2164665..5270534e9 100755
--- a/protocol/dev-server.sh
+++ b/protocol/dev-server.sh
@@ -7,7 +7,7 @@ BRANCH=$(git branch --show-current)
 SSH_PORT=2244
 SSH_KEY_FILE="${HOME}/.ssh/id_rsa.pub"
 
-HELP_MESSAGE="Usage: ./dev-server.sh [-b <branch>] [-p <port>] [-k <file>]
+HELP_MESSAGE="Usage: ./dev-server.sh [-b <branch>] [-p <port>] [-k <file>] [-r]
 
 Build and run a docker container as an SSH toolchain server for remote development.
 
@@ -32,13 +32,16 @@ Options:
   -k, --key-file [path]    Specify the path of the RSA
                            public key file.
                            (default: ${SSH_KEY_FILE})
+  -r, --rebuild            Rebuild the image with the --no-cache option.
   -h, --help               Show this help message."
 
+BUILD_FLAGS=()
 while [ "$#" -gt 0 ]; do
   case "$1" in
   -p|--port) SSH_PORT=$2; shift 2;;
   -k|--key-file) SSH_KEY_FILE=$2; shift 2;;
   -h|--help) echo "${HELP_MESSAGE}"; exit 0;;
+  -r|--rebuild) BUILD_FLAGS+=(--no-cache); shift 1;;
   *) echo 'Error in command line parsing' >&2
      echo -e "\n${HELP_MESSAGE}"
      exit 1
diff --git a/protocol/protobuf/proto/state_representation/state.proto b/protocol/protobuf/proto/state_representation/state.proto
index 81d88c892..81fe33817 100644
--- a/protocol/protobuf/proto/state_representation/state.proto
+++ b/protocol/protobuf/proto/state_representation/state.proto
@@ -5,29 +5,24 @@ package state_representation.proto;
 // The values and order of this enumeration must match the original
 // state_representation::StateType in C++.
 enum StateType {
-  STATE = 0;
-  CARTESIANSTATE = 1;
-  DUALQUATERNIONSTATE = 2;
-  JOINTSTATE = 3;
-  JACOBIANMATRIX = 4;
-  TRAJECTORY = 5;
-  GEOMETRY_SHAPE = 6;
-  GEOMETRY_ELLIPSOID = 7;
-  PARAMETER_DOUBLE = 8;
-  PARAMETER_DOUBLE_ARRAY = 9;
-  PARAMETER_BOOL = 10;
-  PARAMETER_BOOL_ARRAY = 11;
-  PARAMETER_STRING = 12;
-  PARAMETER_STRING_ARRAY = 13;
-  PARAMETER_CARTESIANSTATE = 14;
-  PARAMETER_CARTESIANPOSE = 15;
-  PARAMETER_JOINTSTATE = 16;
-  PARAMETER_JOINTPOSITIONS = 17;
-  PARAMETER_ELLIPSOID = 18;
-  PARAMETER_MATRIX = 19;
-  PARAMETER_VECTOR = 20;
-  PARAMETER_INT = 21;
-  PARAMETER_INT_ARRAY = 22;
+  NONE = 0;
+  STATE = 1;
+  SPATIAL_STATE = 2;
+  CARTESIAN_STATE = 3;
+  CARTESIAN_POSE = 4;
+  CARTESIAN_TWIST = 5;
+  CARTESIAN_ACCELERATION = 6;
+  CARTESIAN_WRENCH = 7;
+  JOINT_STATE = 8;
+  JOINT_POSITIONS = 9;
+  JOINT_VELOCITIES = 10;
+  JOINT_ACCELERATIONS = 11;
+  JOINT_TORQUES = 12;
+  JACOBIAN = 13;
+  PARAMETER = 14;
+  GEOMETRY_SHAPE = 15;
+  GEOMETRY_ELLIPSOID = 16;
+  TRAJECTORY = 17;
 };
 
 message State {
diff --git a/python/Dockerfile.python b/python/Dockerfile.python
index 14f236063..787571ec1 100644
--- a/python/Dockerfile.python
+++ b/python/Dockerfile.python
@@ -6,6 +6,7 @@ RUN git clone --depth 1 --branch $BRANCH https://github.com/epfl-lasa/control-li
 RUN bash control-libraries/source/install.sh --auto
 RUN bash control-libraries/protocol/install.sh --auto
 
+RUN rm -rf control-libraries/python/include control-libraries/python/source
 COPY include control-libraries/python/include
 COPY source control-libraries/python/source
 COPY pyproject.toml setup.py control-libraries/python/
diff --git a/python/README.md b/python/README.md
index aa33660ff..16d84d729 100644
--- a/python/README.md
+++ b/python/README.md
@@ -18,6 +18,9 @@ git clone https://github.com/epfl-lasa/control-libraries
 ## install control-libraries (skip this stage if already done)
 sudo control-libraries/source/install.sh
 
+## install protocol (skip this stage if already done or not desired)
+sudo control-libraries/protocol/install.sh
+
 ## install the bindings using the pip installer
 pip3 install control-libraries/python
 ```
@@ -49,7 +52,7 @@ import dynamical_systems
 print(sr.__version__)
 print(dynamical_systems.__version__)
 
-A = sr.CartesianState.Random("A")
+A = sr.CartesianState().Random("A")
 print(A)
 ```
 
@@ -57,10 +60,10 @@ Or, directly import specific classes from the module.
 ```python
 #!/usr/bin/env python
 from state_representation import JointState
-from dynamical_systems import create_cartesian_ds, DYNAMICAL_SYSTEM
+from dynamical_systems import create_cartesian_ds, DYNAMICAL_SYSTEM_TYPE
 
-B = JointState.Random("B", 3)
-ds = create_cartesian_ds(DYNAMICAL_SYSTEM.POINT_ATTRACTOR)
+B = JointState().Random("B", 3)
+ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR)
 ```
 
 If the `clproto` C++ library is installed, the installation steps above will automatically install the `clproto`
@@ -70,7 +73,7 @@ Python module which can be used to encode and decode objects into bytes of seria
 from state_representation import JointState
 import clproto
 
-B = JointState.Random("B", 3)
+B = JointState().Random("B", 3)
 encoded_msg = clproto.encode(B, clproto.MessageType.JOINT_STATE_MESSAGE)
 
 decoded_object = clproto.decode(encoded_msg)
diff --git a/python/include/controllers_bindings.h b/python/include/controllers_bindings.h
index bdce1c281..954b381b9 100644
--- a/python/include/controllers_bindings.h
+++ b/python/include/controllers_bindings.h
@@ -11,7 +11,7 @@ namespace py = pybind11;
 using namespace pybind11::literals;
 using namespace controllers;
 
-void bind_type(py::module_& m);
+void bind_controller_type(py::module_& m);
 void bind_computational_space(py::module_& m);
 void bind_cartesian_controllers(py::module_& m);
 void bind_joint_controllers(py::module_& m);
diff --git a/python/include/dynamical_systems_bindings.h b/python/include/dynamical_systems_bindings.h
index 5d5e59517..2c30985c3 100644
--- a/python/include/dynamical_systems_bindings.h
+++ b/python/include/dynamical_systems_bindings.h
@@ -11,6 +11,6 @@ namespace py = pybind11;
 using namespace pybind11::literals;
 using namespace dynamical_systems;
 
-void bind_type(py::module_& m);
-void bind_cartesian(py::module_& m);
-void bind_joint(py::module_& m);
+void bind_ds_type(py::module_& m);
+void bind_cartesian_ds(py::module_& m);
+void bind_joint_ds(py::module_& m);
diff --git a/python/include/parameter_container.h b/python/include/parameter_container.h
index 463fa04a1..10b6c522f 100644
--- a/python/include/parameter_container.h
+++ b/python/include/parameter_container.h
@@ -5,8 +5,6 @@
 #include <state_representation/State.hpp>
 #include <state_representation/geometry/Ellipsoid.hpp>
 #include <state_representation/parameters/Parameter.hpp>
-#include <state_representation/space/cartesian/CartesianPose.hpp>
-#include <state_representation/space/joint/JointPositions.hpp>
 
 namespace py_parameter {
 
@@ -19,19 +17,20 @@ struct ParameterValues {
   std::vector<bool> bool_array_value;
   std::string string_value;
   std::vector<std::string> string_array_value;
-  CartesianState cartesian_state;
-  CartesianPose cartesian_pose;
-  JointState joint_state;
-  JointPositions joint_positions;
-  Ellipsoid ellipsoid;
+  std::shared_ptr<State> state_pointer;
   Eigen::MatrixXd matrix_value;
   Eigen::VectorXd vector_value;
 };
 
 class ParameterContainer : public ParameterInterface {
 public:
-  ParameterContainer(const std::string& name, const StateType& type);
-  ParameterContainer(const std::string& name, const py::object& value, const StateType& type);
+  ParameterContainer(
+      const std::string& name, const ParameterType& type, const StateType& parameter_state_type = StateType::NONE
+  );
+  ParameterContainer(
+      const std::string& name, const py::object& value, const ParameterType& type,
+      const StateType& parameter_state_type = StateType::NONE
+  );
   ParameterContainer(const ParameterContainer& parameter);
 
   void set_value(const py::object& value);
diff --git a/python/setup.py b/python/setup.py
index 49b1536e2..f9aea9fd6 100644
--- a/python/setup.py
+++ b/python/setup.py
@@ -10,7 +10,7 @@
 osqp_path_var = 'OSQP_INCLUDE_DIR'
 openrobots_path_var = 'OPENROBOTS_INCLUDE_DIR'
 
-__version__ = "5.2.0"
+__version__ = "6.0.0"
 __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model']
 __include_dirs__ = ['include']
 
@@ -28,13 +28,6 @@
     else:
         raise Exception('Could not find Eigen3 package!')
 
-    if __install_robot_model_module__:
-        osqp_path = os.environ[osqp_path_var] if osqp_path_var in os.environ.keys() else '/usr/local/include/osqp'
-        __include_dirs__.append(osqp_path)
-        openrobots_path = os.environ[
-            openrobots_path_var] if openrobots_path_var in os.environ.keys() else '/opt/openrobots/include'
-        __include_dirs__.append('/opt/openrobots/include')
-
     for lib in __libraries__:
         status = os.popen(f'ldconfig -p | grep {lib}').read().strip()
         if len(status) == 0:
@@ -42,18 +35,25 @@
             if lib == 'clproto':
                 warnings.warn(f'{msg} The clproto module will not be installed.')
                 __install_clproto_module__ = False
-            if lib == 'dynamical_systems':
+            elif lib == 'dynamical_systems':
                 warnings.warn(f'{msg} The dynamical_systems module will not be installed.')
                 __install_dynamical_systems_module__ = False
-            if lib == 'robot_model':
+            elif lib == 'robot_model':
                 warnings.warn(f'{msg} The robot_model module will not be installed.')
                 __install_robot_model_module__ = False
-            if lib == 'controllers':
+            elif lib == 'controllers':
                 warnings.warn(f'{msg} The controllers module will not be installed.')
                 __install_controllers_module__ = False
             else:
                 raise Exception(msg)
 
+    if __install_robot_model_module__:
+        osqp_path = os.environ[osqp_path_var] if osqp_path_var in os.environ.keys() else '/usr/local/include/osqp'
+        __include_dirs__.append(osqp_path)
+        openrobots_path = os.environ[
+            openrobots_path_var] if openrobots_path_var in os.environ.keys() else '/opt/openrobots/include'
+        __include_dirs__.append('/opt/openrobots/include')
+
     if __install_controllers_module__ and not __install_robot_model_module__:
         warnings.warn(
             'The robot model module is required to build the controllers module! '
@@ -131,6 +131,9 @@
     ext_modules=ext_modules,
     test_suite='tests',
     python_requires='>=3',
+    install_requires=[
+        'pyquaternion>=0.9.9'
+    ],
     license='GNU GPL v3',
     zip_safe=False,
 )
diff --git a/python/source/clproto/bind_clproto.cpp b/python/source/clproto/bind_clproto.cpp
index 8a95f6d29..34a58c83f 100644
--- a/python/source/clproto/bind_clproto.cpp
+++ b/python/source/clproto/bind_clproto.cpp
@@ -28,26 +28,26 @@ inline py::bytes encode_bytes(const T& object) {
 }
 
 py::bytes encode_parameter_container(const ParameterContainer& container) {
-  switch (container.get_type()) {
-    case StateType::PARAMETER_INT:
+  switch (container.get_parameter_type()) {
+    case ParameterType::INT:
       return py::bytes(encode(Parameter(container.get_name(), container.values.int_value)));
-    case StateType::PARAMETER_INT_ARRAY:
+    case ParameterType::INT_ARRAY:
       return py::bytes(encode(Parameter(container.get_name(), container.values.int_array_value)));
-    case StateType::PARAMETER_DOUBLE:
+    case ParameterType::DOUBLE:
       return py::bytes(encode(Parameter(container.get_name(), container.values.double_value)));
-    case StateType::PARAMETER_DOUBLE_ARRAY:
+    case ParameterType::DOUBLE_ARRAY:
       return py::bytes(encode(Parameter(container.get_name(), container.values.double_array_value)));
-    case StateType::PARAMETER_BOOL:
+    case ParameterType::BOOL:
       return py::bytes(encode(Parameter(container.get_name(), container.values.bool_value)));
-    case StateType::PARAMETER_BOOL_ARRAY:
+    case ParameterType::BOOL_ARRAY:
       return py::bytes(encode(Parameter(container.get_name(), container.values.bool_array_value)));
-    case StateType::PARAMETER_STRING:
+    case ParameterType::STRING:
       return py::bytes(encode(Parameter(container.get_name(), container.values.string_value)));
-    case StateType::PARAMETER_STRING_ARRAY:
+    case ParameterType::STRING_ARRAY:
       return py::bytes(encode(Parameter(container.get_name(), container.values.string_array_value)));
-    case StateType::PARAMETER_MATRIX:
+    case ParameterType::MATRIX:
       return py::bytes(encode(Parameter(container.get_name(), container.values.matrix_value)));
-    case StateType::PARAMETER_VECTOR:
+    case ParameterType::VECTOR:
       return py::bytes(encode(Parameter(container.get_name(), container.values.vector_value)));
     default:
       throw std::invalid_argument("This StateType is not a valid Parameter.");
@@ -60,43 +60,43 @@ py::object decode_parameter(const std::string& msg) {
   switch (check_parameter_message_type(msg)) {
     case ParameterMessageType::INT: {
       auto param = decode<Parameter<int>>(msg);
-      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type());
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type());
     }
     case ParameterMessageType::INT_ARRAY: {
       auto param = decode<Parameter<std::vector<int>>>(msg);
-      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type());
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type());
     }
     case ParameterMessageType::DOUBLE: {
       auto param = decode<Parameter<double>>(msg);
-      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type());
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type());
     }
     case ParameterMessageType::DOUBLE_ARRAY: {
       auto param = decode<Parameter<std::vector<double>>>(msg);
-      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type());
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type());
     }
     case ParameterMessageType::BOOL: {
       auto param = decode<Parameter<bool>>(msg);
-      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type());
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type());
     }
     case ParameterMessageType::BOOL_ARRAY: {
       auto param = decode<Parameter<std::vector<bool>>>(msg);
-      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type());
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type());
     }
     case ParameterMessageType::STRING: {
       auto param = decode<Parameter<std::string>>(msg);
-      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type());
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type());
     }
     case ParameterMessageType::STRING_ARRAY: {
       auto param = decode<Parameter<std::vector<std::string>>>(msg);
-      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type());
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type());
     }
     case ParameterMessageType::VECTOR: {
       auto param = decode<Parameter<Eigen::VectorXd>>(msg);
-      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type());
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type());
     }
     case ParameterMessageType::MATRIX: {
       auto param = decode<Parameter<Eigen::MatrixXd>>(msg);
-      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type());
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type());
     }
     default:
       throw std::invalid_argument("The message is not a valid encoded Parameter.");
diff --git a/python/source/common/parameter_container.cpp b/python/source/common/parameter_container.cpp
index 879bede17..7b33d8fe3 100644
--- a/python/source/common/parameter_container.cpp
+++ b/python/source/common/parameter_container.cpp
@@ -1,180 +1,260 @@
 #include "parameter_container.h"
 
+#include <state_representation/space/cartesian/CartesianState.hpp>
+#include <state_representation/space/cartesian/CartesianPose.hpp>
+#include <state_representation/space/joint/JointState.hpp>
+#include <state_representation/space/joint/JointPositions.hpp>
+#include <state_representation/geometry/Ellipsoid.hpp>
+
 namespace py_parameter {
 
-ParameterContainer::ParameterContainer(const std::string& name, const StateType& type) :
-    ParameterInterface(type, name) {}
+ParameterContainer::ParameterContainer(
+    const std::string& name, const ParameterType& type, const StateType& parameter_state_type
+) : ParameterInterface(name, type, parameter_state_type) {}
 
-ParameterContainer::ParameterContainer(const std::string& name, const py::object& value, const StateType& type) :
-    ParameterInterface(type, name) {
+ParameterContainer::ParameterContainer(
+    const std::string& name, const py::object& value, const ParameterType& type, const StateType& parameter_state_type
+) : ParameterInterface(name, type, parameter_state_type) {
   set_value(value);
 }
 
 ParameterContainer::ParameterContainer(const ParameterContainer& parameter) :
-    ParameterInterface(parameter.get_type(), parameter.get_name()), values(parameter.values) {}
+    ParameterInterface(parameter.get_name(), parameter.get_parameter_type(), parameter.get_parameter_state_type()),
+    values(parameter.values) {}
 
 void ParameterContainer::set_value(const py::object& value) {
-  switch (this->get_type()) {
-    case StateType::PARAMETER_INT:
+  switch (this->get_parameter_type()) {
+    case ParameterType::INT:
       values.int_value = value.cast<int>();
       break;
-    case StateType::PARAMETER_INT_ARRAY:
+    case ParameterType::INT_ARRAY:
       values.int_array_value = value.cast<std::vector<int>>();
       break;
-    case StateType::PARAMETER_DOUBLE:
+    case ParameterType::DOUBLE:
       values.double_value = value.cast<double>();
       break;
-    case StateType::PARAMETER_DOUBLE_ARRAY:
+    case ParameterType::DOUBLE_ARRAY:
       values.double_array_value = value.cast<std::vector<double>>();
       break;
-    case StateType::PARAMETER_BOOL:
+    case ParameterType::BOOL:
       values.bool_value = value.cast<bool>();
       break;
-    case StateType::PARAMETER_BOOL_ARRAY:
+    case ParameterType::BOOL_ARRAY:
       values.bool_array_value = value.cast<std::vector<bool>>();
       break;
-    case StateType::PARAMETER_STRING:
+    case ParameterType::STRING:
       values.string_value = value.cast<std::string>();
       break;
-    case StateType::PARAMETER_STRING_ARRAY:
+    case ParameterType::STRING_ARRAY:
       values.string_array_value = value.cast<std::vector<std::string>>();
       break;
-    case StateType::PARAMETER_CARTESIANSTATE:
-      values.cartesian_state = value.cast<CartesianState>();
-      break;
-    case StateType::PARAMETER_CARTESIANPOSE:
-      values.cartesian_pose = value.cast<CartesianPose>();
-      break;
-    case StateType::PARAMETER_JOINTSTATE:
-      values.joint_state = value.cast<JointState>();
-      break;
-    case StateType::PARAMETER_JOINTPOSITIONS:
-      values.joint_positions = value.cast<JointPositions>();
+    case ParameterType::STATE:
+      switch (this->get_parameter_state_type()) {
+        case StateType::CARTESIAN_STATE:
+          values.state_pointer = std::make_shared<CartesianState>(value.cast<CartesianState>());
+          break;
+        case StateType::CARTESIAN_POSE:
+          values.state_pointer = std::make_shared<CartesianPose>(value.cast<CartesianPose>());
+          break;
+        case StateType::JOINT_STATE:
+          values.state_pointer = std::make_shared<JointState>(value.cast<JointState>());
+          break;
+        case StateType::JOINT_POSITIONS:
+          values.state_pointer = std::make_shared<JointPositions>(value.cast<JointPositions>());
+          break;
+        case StateType::GEOMETRY_ELLIPSOID:
+          values.state_pointer = std::make_shared<Ellipsoid>(value.cast<Ellipsoid>());
+          break;
+        default:
+          throw std::invalid_argument("The StateType contained by parameter " + this->get_name() + " is unsupported.");
+          break;
+      }
       break;
-    case StateType::PARAMETER_ELLIPSOID:
-      values.ellipsoid = value.cast<Ellipsoid>();
-      break;
-    case StateType::PARAMETER_MATRIX:
+    case ParameterType::MATRIX:
       values.matrix_value = value.cast<Eigen::MatrixXd>();
       break;
-    case StateType::PARAMETER_VECTOR:
+    case ParameterType::VECTOR:
       values.vector_value = value.cast<Eigen::VectorXd>();
       break;
     default:
-      throw std::invalid_argument("This StateType is not a valid Parameter.");
-      break;
+      throw std::invalid_argument("The ParameterType of parameter " + this->get_name() + " is invalid.");
   }
 }
 
 py::object ParameterContainer::get_value() {
-  switch (this->get_type()) {
-    case StateType::PARAMETER_INT:
+  switch (this->get_parameter_type()) {
+    case ParameterType::INT:
       return py::cast(values.int_value);
-    case StateType::PARAMETER_INT_ARRAY:
+    case ParameterType::INT_ARRAY:
       return py::cast(values.int_array_value);
-    case StateType::PARAMETER_DOUBLE:
+    case ParameterType::DOUBLE:
       return py::cast(values.double_value);
-    case StateType::PARAMETER_DOUBLE_ARRAY:
+    case ParameterType::DOUBLE_ARRAY:
       return py::cast(values.double_array_value);
-    case StateType::PARAMETER_BOOL:
+    case ParameterType::BOOL:
       return py::cast(values.bool_value);
-    case StateType::PARAMETER_BOOL_ARRAY:
+    case ParameterType::BOOL_ARRAY:
       return py::cast(values.bool_array_value);
-    case StateType::PARAMETER_STRING:
+    case ParameterType::STRING:
       return py::cast(values.string_value);
-    case StateType::PARAMETER_STRING_ARRAY:
+    case ParameterType::STRING_ARRAY:
       return py::cast(values.string_array_value);
-    case StateType::PARAMETER_CARTESIANSTATE:
-      return py::cast(values.cartesian_state);
-    case StateType::PARAMETER_CARTESIANPOSE:
-      return py::cast(values.cartesian_pose);
-    case StateType::PARAMETER_JOINTSTATE:
-      return py::cast(values.joint_state);
-    case StateType::PARAMETER_JOINTPOSITIONS:
-      return py::cast(values.joint_positions);
-    case StateType::PARAMETER_ELLIPSOID:
-      return py::cast(values.ellipsoid);
-    case StateType::PARAMETER_MATRIX:
+    case ParameterType::STATE:
+      try {
+        switch (this->get_parameter_state_type()) {
+          case StateType::CARTESIAN_STATE:
+            return py::cast(*std::dynamic_pointer_cast<CartesianState>(values.state_pointer));
+          case StateType::CARTESIAN_POSE:
+            return py::cast(*std::dynamic_pointer_cast<CartesianPose>(values.state_pointer));
+          case StateType::JOINT_STATE:
+            return py::cast(*std::dynamic_pointer_cast<JointState>(values.state_pointer));
+          case StateType::JOINT_POSITIONS:
+            return py::cast(*std::dynamic_pointer_cast<JointPositions>(values.state_pointer));
+          case StateType::GEOMETRY_ELLIPSOID:
+            return py::cast(*std::dynamic_pointer_cast<Ellipsoid>(values.state_pointer));
+          default:
+            throw std::invalid_argument(
+                "The StateType contained by parameter " + this->get_name() + " is unsupported.");
+        }
+      } catch (const std::exception&) {
+        throw std::runtime_error("The ParameterType of parameter " + this->get_name() + " is invalid.");
+      }
+      break;
+    case ParameterType::MATRIX:
       return py::cast(values.matrix_value);
-    case StateType::PARAMETER_VECTOR:
+    case ParameterType::VECTOR:
       return py::cast(values.vector_value);
     default:
-      return py::none();
+      break;
   }
+  throw std::invalid_argument("Could not get the value of parameter " + this->get_name());
 }
 
 ParameterContainer interface_ptr_to_container(const std::shared_ptr<ParameterInterface>& parameter) {
-  switch (parameter->get_type()) {
-    case StateType::PARAMETER_INT:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<int>()), parameter->get_type());
-    case StateType::PARAMETER_INT_ARRAY:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<std::vector<int>>()), parameter->get_type());
-    case StateType::PARAMETER_DOUBLE:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<double>()), parameter->get_type());
-    case StateType::PARAMETER_DOUBLE_ARRAY:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<std::vector<double>>()), parameter->get_type());
-    case StateType::PARAMETER_BOOL:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<bool>()), parameter->get_type());
-    case StateType::PARAMETER_BOOL_ARRAY:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<std::vector<bool>>()), parameter->get_type());
-    case StateType::PARAMETER_STRING:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<std::string>()), parameter->get_type());
-    case StateType::PARAMETER_STRING_ARRAY:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<std::vector<std::string>>()), parameter->get_type());
-    case StateType::PARAMETER_CARTESIANSTATE:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<CartesianState>()), parameter->get_type());
-    case StateType::PARAMETER_CARTESIANPOSE:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<CartesianPose>()), parameter->get_type());
-    case StateType::PARAMETER_JOINTSTATE:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<JointState>()), parameter->get_type());
-    case StateType::PARAMETER_JOINTPOSITIONS:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<JointPositions>()), parameter->get_type());
-    case StateType::PARAMETER_ELLIPSOID:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<Ellipsoid>()), parameter->get_type());
-    case StateType::PARAMETER_MATRIX:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<Eigen::MatrixXd>()), parameter->get_type());
-    case StateType::PARAMETER_VECTOR:
-      return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value<Eigen::VectorXd>()), parameter->get_type());
+  switch (parameter->get_parameter_type()) {
+    case ParameterType::INT:
+      return ParameterContainer(
+          parameter->get_name(), py::cast(parameter->get_parameter_value<int>()), ParameterType::INT);
+    case ParameterType::INT_ARRAY:
+      return ParameterContainer(
+          parameter->get_name(), py::cast(parameter->get_parameter_value<std::vector<int>>()),
+          ParameterType::INT_ARRAY);
+    case ParameterType::DOUBLE:
+      return ParameterContainer(
+          parameter->get_name(), py::cast(parameter->get_parameter_value<double>()), ParameterType::DOUBLE);
+    case ParameterType::DOUBLE_ARRAY:
+      return ParameterContainer(
+          parameter->get_name(), py::cast(parameter->get_parameter_value<std::vector<double>>()),
+          ParameterType::DOUBLE_ARRAY);
+    case ParameterType::BOOL:
+      return ParameterContainer(
+          parameter->get_name(), py::cast(parameter->get_parameter_value<bool>()), ParameterType::BOOL);
+    case ParameterType::BOOL_ARRAY:
+      return ParameterContainer(
+          parameter->get_name(), py::cast(parameter->get_parameter_value<std::vector<bool>>()),
+          ParameterType::BOOL_ARRAY);
+    case ParameterType::STRING:
+      return ParameterContainer(
+          parameter->get_name(), py::cast(parameter->get_parameter_value<std::string>()), ParameterType::STRING);
+    case ParameterType::STRING_ARRAY:
+      return ParameterContainer(
+          parameter->get_name(), py::cast(parameter->get_parameter_value<std::vector<std::string>>()),
+          ParameterType::STRING_ARRAY);
+    case ParameterType::STATE:
+      try {
+        switch (parameter->get_parameter_state_type()) {
+          case StateType::CARTESIAN_STATE:
+            return ParameterContainer(
+                parameter->get_name(), py::cast(parameter->get_parameter_value<CartesianState>()), ParameterType::STATE,
+                StateType::CARTESIAN_STATE);
+          case StateType::CARTESIAN_POSE:
+            return ParameterContainer(
+                parameter->get_name(), py::cast(parameter->get_parameter_value<CartesianPose>()), ParameterType::STATE,
+                StateType::CARTESIAN_POSE);
+          case StateType::JOINT_STATE:
+            return ParameterContainer(
+                parameter->get_name(), py::cast(parameter->get_parameter_value<JointState>()), ParameterType::STATE,
+                StateType::JOINT_STATE);
+          case StateType::JOINT_POSITIONS:
+            return ParameterContainer(
+                parameter->get_name(), py::cast(parameter->get_parameter_value<JointPositions>()), ParameterType::STATE,
+                StateType::JOINT_POSITIONS);
+          case StateType::GEOMETRY_ELLIPSOID:
+            return ParameterContainer(
+                parameter->get_name(), py::cast(parameter->get_parameter_value<Ellipsoid>()), ParameterType::STATE,
+                StateType::GEOMETRY_ELLIPSOID);
+          default:
+            throw std::invalid_argument(
+                "The StateType contained by parameter " + parameter->get_name() + " is unsupported.");
+        }
+      } catch (const std::exception&) {
+        throw std::runtime_error("The ParameterType of parameter " + parameter->get_name() + " is invalid.");
+      }
+      break;
+    case ParameterType::MATRIX:
+      return ParameterContainer(
+          parameter->get_name(), py::cast(parameter->get_parameter_value<Eigen::MatrixXd>()), ParameterType::MATRIX);
+    case ParameterType::VECTOR:
+      return ParameterContainer(
+          parameter->get_name(), py::cast(parameter->get_parameter_value<Eigen::VectorXd>()), ParameterType::VECTOR);
     default:
-      throw std::invalid_argument("The conversion from this StateType to ParameterContainer is not supported yet.");
+      break;
   }
+  throw std::invalid_argument("The conversion from this Parameter to a ParameterContainer is not supported.");
 }
 
 std::shared_ptr<ParameterInterface> container_to_interface_ptr(const ParameterContainer& parameter) {
-  switch (parameter.get_type()) {
-    case StateType::PARAMETER_INT:
+  switch (parameter.get_parameter_type()) {
+    case ParameterType::INT:
       return make_shared_parameter(parameter.get_name(), parameter.values.int_value);
-    case StateType::PARAMETER_INT_ARRAY:
+    case ParameterType::INT_ARRAY:
       return make_shared_parameter(parameter.get_name(), parameter.values.int_array_value);
-    case StateType::PARAMETER_DOUBLE:
+    case ParameterType::DOUBLE:
       return make_shared_parameter(parameter.get_name(), parameter.values.double_value);
-    case StateType::PARAMETER_DOUBLE_ARRAY:
+    case ParameterType::DOUBLE_ARRAY:
       return make_shared_parameter(parameter.get_name(), parameter.values.double_array_value);
-    case StateType::PARAMETER_BOOL:
+    case ParameterType::BOOL:
       return make_shared_parameter(parameter.get_name(), parameter.values.bool_value);
-    case StateType::PARAMETER_BOOL_ARRAY:
+    case ParameterType::BOOL_ARRAY:
       return make_shared_parameter(parameter.get_name(), parameter.values.bool_array_value);
-    case StateType::PARAMETER_STRING:
+    case ParameterType::STRING:
       return make_shared_parameter(parameter.get_name(), parameter.values.string_value);
-    case StateType::PARAMETER_STRING_ARRAY:
+    case ParameterType::STRING_ARRAY:
       return make_shared_parameter(parameter.get_name(), parameter.values.string_array_value);
-    case StateType::PARAMETER_CARTESIANSTATE:
-      return make_shared_parameter(parameter.get_name(), parameter.values.cartesian_state);
-    case StateType::PARAMETER_CARTESIANPOSE:
-      return make_shared_parameter(parameter.get_name(), parameter.values.cartesian_pose);
-    case StateType::PARAMETER_JOINTSTATE:
-      return make_shared_parameter(parameter.get_name(), parameter.values.joint_state);
-    case StateType::PARAMETER_JOINTPOSITIONS:
-      return make_shared_parameter(parameter.get_name(), parameter.values.joint_positions);
-    case StateType::PARAMETER_ELLIPSOID:
-      return make_shared_parameter(parameter.get_name(), parameter.values.ellipsoid);
-    case StateType::PARAMETER_MATRIX:
+    case ParameterType::STATE:
+      try {
+        switch (parameter.get_parameter_state_type()) {
+          case StateType::CARTESIAN_STATE:
+            return make_shared_parameter(
+                parameter.get_name(), *std::dynamic_pointer_cast<CartesianState>(parameter.values.state_pointer));
+          case StateType::CARTESIAN_POSE:
+            return make_shared_parameter(
+                parameter.get_name(), *std::dynamic_pointer_cast<CartesianPose>(parameter.values.state_pointer));
+          case StateType::JOINT_STATE:
+            return make_shared_parameter(
+                parameter.get_name(), *std::dynamic_pointer_cast<JointState>(parameter.values.state_pointer));
+          case StateType::JOINT_POSITIONS:
+            return make_shared_parameter(
+                parameter.get_name(), *std::dynamic_pointer_cast<JointPositions>(parameter.values.state_pointer));
+          case StateType::GEOMETRY_ELLIPSOID:
+            return make_shared_parameter(
+                parameter.get_name(), *std::dynamic_pointer_cast<Ellipsoid>(parameter.values.state_pointer));
+          default:
+            throw std::invalid_argument(
+                "The StateType contained by parameter " + parameter.get_name() + " is unsupported.");
+        }
+      } catch (const std::exception&) {
+        throw std::runtime_error("The ParameterType of parameter " + parameter.get_name() + " is invalid.");
+      }
+      break;
+    case ParameterType::MATRIX:
       return make_shared_parameter(parameter.get_name(), parameter.values.matrix_value);
-    case StateType::PARAMETER_VECTOR:
-        return make_shared_parameter(parameter.get_name(), parameter.values.vector_value);
+    case ParameterType::VECTOR:
+      return make_shared_parameter(parameter.get_name(), parameter.values.vector_value);
     default:
-      return {};
+      break;
   }
+  throw std::invalid_argument("The conversion from this ParameterContainer to a Parameter is not supported.");
 }
 
 std::map<std::string, ParameterContainer>
@@ -191,7 +271,11 @@ std::map<std::string, std::shared_ptr<ParameterInterface>>
 container_to_interface_ptr_map(const std::map<std::string, ParameterContainer>& parameters) {
   std::map<std::string, std::shared_ptr<ParameterInterface>> parameter_list;
   for (const auto& param_it: parameters) {
-    parameter_list.insert(std::pair<std::string, std::shared_ptr<ParameterInterface>>(param_it.first, container_to_interface_ptr(param_it.second)));
+    parameter_list.insert(
+        std::pair<std::string, std::shared_ptr<ParameterInterface>>(
+            param_it.first, container_to_interface_ptr(param_it.second)
+        )
+    );
   }
   return parameter_list;
 }
diff --git a/python/source/controllers/bind_type.cpp b/python/source/controllers/bind_controller_type.cpp
similarity index 93%
rename from python/source/controllers/bind_type.cpp
rename to python/source/controllers/bind_controller_type.cpp
index 46540dddc..e671bff17 100644
--- a/python/source/controllers/bind_type.cpp
+++ b/python/source/controllers/bind_controller_type.cpp
@@ -2,7 +2,7 @@
 
 #include <controllers/ControllerType.hpp>
 
-void bind_type(py::module_& m) {
+void bind_controller_type(py::module_& m) {
   py::enum_<CONTROLLER_TYPE>(m, "CONTROLLER_TYPE")
       .value("NONE", CONTROLLER_TYPE::NONE)
       .value("IMPEDANCE", CONTROLLER_TYPE::IMPEDANCE)
diff --git a/python/source/controllers/controllers_bindings.cpp b/python/source/controllers/controllers_bindings.cpp
index 40841efe1..5b42b9717 100644
--- a/python/source/controllers/controllers_bindings.cpp
+++ b/python/source/controllers/controllers_bindings.cpp
@@ -14,7 +14,7 @@ PYBIND11_MODULE(controllers, m) {
 
   py::module_::import("state_representation");
 
-  bind_type(m);
+  bind_controller_type(m);
   bind_computational_space(m);
   bind_cartesian_controllers(m);
   bind_joint_controllers(m);
diff --git a/python/source/dynamical_systems/bind_cartesian.cpp b/python/source/dynamical_systems/bind_cartesian.cpp
deleted file mode 100644
index 8c0c1d300..000000000
--- a/python/source/dynamical_systems/bind_cartesian.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-#include "dynamical_systems_bindings.h"
-
-#include <dynamical_systems/Circular.hpp>
-#include <dynamical_systems/DefaultDynamicalSystem.hpp>
-#include <dynamical_systems/DynamicalSystemFactory.hpp>
-#include <dynamical_systems/IDynamicalSystem.hpp>
-#include <dynamical_systems/PointAttractor.hpp>
-#include <dynamical_systems/Ring.hpp>
-#include <state_representation/space/cartesian/CartesianState.hpp>
-
-#include "py_dynamical_system.h"
-
-using namespace state_representation;
-
-void cartesian(py::module_& m) {
-  py::object parameter_map = py::module_::import("state_representation").attr("ParameterMap");
-  py::class_<IDynamicalSystem<CartesianState>, std::shared_ptr<IDynamicalSystem<CartesianState>>, PyDynamicalSystem<CartesianState>> c(m, "ICartesianDS", parameter_map);
-
-  c.def(py::init<>());
-
-  c.def("is_compatible", &IDynamicalSystem<CartesianState>::is_compatible);
-  c.def("evaluate", &IDynamicalSystem<CartesianState>::evaluate);
-  c.def("get_base_frame", &IDynamicalSystem<CartesianState>::get_base_frame);
-  c.def("set_base_frame", &IDynamicalSystem<CartesianState>::set_base_frame);
-}
-
-void bind_cartesian(py::module_& m) {
-  cartesian(m);
-  py::class_<Circular, std::shared_ptr<Circular>, IDynamicalSystem<CartesianState>>(m, "CartesianCircularDS").def(py::init<>());
-  py::class_<DefaultDynamicalSystem<CartesianState>, std::shared_ptr<DefaultDynamicalSystem<CartesianState>>, IDynamicalSystem<CartesianState>>(m, "CartesianDefaultDS").def(py::init<>());
-  py::class_<PointAttractor<CartesianState>, std::shared_ptr<PointAttractor<CartesianState>>, IDynamicalSystem<CartesianState>>(m, "CartesianPointAttractorDS").def(py::init<>());
-  py::class_<Ring, std::shared_ptr<Ring>, IDynamicalSystem<CartesianState>>(m, "CartesianRingDS").def(py::init<>());
-
-  m.def("create_cartesian_ds", [](DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM type) -> py::object {
-    switch (type) {
-      case DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::CIRCULAR:
-        return py::cast(Circular());
-        break;
-      case DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR:
-        return py::cast(PointAttractor<CartesianState>());
-        break;
-      case DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::RING:
-        return py::cast(Ring());
-        break;
-      default:
-      case DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::NONE:
-        return py::cast(DefaultDynamicalSystem<CartesianState>());
-        break;
-    }
-  });
-}
diff --git a/python/source/dynamical_systems/bind_cartesian_ds.cpp b/python/source/dynamical_systems/bind_cartesian_ds.cpp
new file mode 100644
index 000000000..6358292ab
--- /dev/null
+++ b/python/source/dynamical_systems/bind_cartesian_ds.cpp
@@ -0,0 +1,32 @@
+#include "dynamical_systems_bindings.h"
+
+#include <dynamical_systems/DynamicalSystemFactory.hpp>
+#include <dynamical_systems/IDynamicalSystem.hpp>
+#include <state_representation/space/cartesian/CartesianState.hpp>
+
+#include "py_dynamical_system.h"
+
+using namespace state_representation;
+using namespace py_parameter;
+
+void cartesian(py::module_& m) {
+  py::object parameter_map = py::module_::import("state_representation").attr("ParameterMap");
+  py::class_<IDynamicalSystem<CartesianState>, std::shared_ptr<IDynamicalSystem<CartesianState>>, PyDynamicalSystem<CartesianState>> c(m, "ICartesianDS", parameter_map);
+
+  c.def("is_compatible", &IDynamicalSystem<CartesianState>::is_compatible);
+  c.def("evaluate", &IDynamicalSystem<CartesianState>::evaluate);
+  c.def("get_base_frame", &IDynamicalSystem<CartesianState>::get_base_frame);
+  c.def("set_base_frame", &IDynamicalSystem<CartesianState>::set_base_frame);
+}
+
+void bind_cartesian_ds(py::module_& m) {
+  cartesian(m);
+
+  m.def("create_cartesian_ds", [](DYNAMICAL_SYSTEM_TYPE type, const std::list<ParameterContainer>& parameters) -> py::object {
+    return py::cast(CartesianDynamicalSystemFactory::create_dynamical_system(type, container_to_interface_ptr_list(parameters)));
+  }, "Create a dynamical system of the desired type with initial parameters.", "type"_a, "parameters"_a);
+
+  m.def("create_cartesian_ds", [](DYNAMICAL_SYSTEM_TYPE type) -> py::object {
+    return py::cast(CartesianDynamicalSystemFactory::create_dynamical_system(type, std::list<std::shared_ptr<ParameterInterface>>()));
+  }, "Create a dynamical system of the desired type.", "type"_a);
+}
diff --git a/python/source/dynamical_systems/bind_ds_type.cpp b/python/source/dynamical_systems/bind_ds_type.cpp
new file mode 100644
index 000000000..1416ec256
--- /dev/null
+++ b/python/source/dynamical_systems/bind_ds_type.cpp
@@ -0,0 +1,14 @@
+#include "dynamical_systems_bindings.h"
+
+#include <dynamical_systems/DynamicalSystemType.hpp>
+
+using namespace state_representation;
+
+void bind_ds_type(py::module_& m) {
+  py::enum_<DYNAMICAL_SYSTEM_TYPE>(m, "DYNAMICAL_SYSTEM_TYPE")
+      .value("NONE", DYNAMICAL_SYSTEM_TYPE::NONE)
+      .value("CIRCULAR", DYNAMICAL_SYSTEM_TYPE::CIRCULAR)
+      .value("POINT_ATTRACTOR", DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR)
+      .value("RING", DYNAMICAL_SYSTEM_TYPE::RING)
+      .export_values();
+}
diff --git a/python/source/dynamical_systems/bind_joint.cpp b/python/source/dynamical_systems/bind_joint.cpp
deleted file mode 100644
index 5283235b1..000000000
--- a/python/source/dynamical_systems/bind_joint.cpp
+++ /dev/null
@@ -1,41 +0,0 @@
-#include "dynamical_systems_bindings.h"
-
-#include <dynamical_systems/DefaultDynamicalSystem.hpp>
-#include <dynamical_systems/DynamicalSystemFactory.hpp>
-#include <dynamical_systems/IDynamicalSystem.hpp>
-#include <dynamical_systems/PointAttractor.hpp>
-#include <state_representation/space/joint/JointState.hpp>
-
-#include "py_dynamical_system.h"
-
-using namespace state_representation;
-
-void joint(py::module_& m) {
-  py::object parameter_map = py::module_::import("state_representation").attr("ParameterMap");
-  py::class_<IDynamicalSystem<JointState>, std::shared_ptr<IDynamicalSystem<JointState>>, PyDynamicalSystem<JointState>> c(m, "IJointDS", parameter_map);
-
-  c.def(py::init<>());
-
-  c.def("is_compatible", &IDynamicalSystem<JointState>::is_compatible);
-  c.def("evaluate", &IDynamicalSystem<JointState>::evaluate);
-  c.def("get_base_frame", &IDynamicalSystem<JointState>::get_base_frame);
-  c.def("set_base_frame", &IDynamicalSystem<JointState>::set_base_frame);
-}
-
-void bind_joint(py::module_& m) {
-  joint(m);
-  py::class_<DefaultDynamicalSystem<JointState>, std::shared_ptr<DefaultDynamicalSystem<JointState>>, IDynamicalSystem<JointState>>(m, "JointDefaultDS").def(py::init<>());
-  py::class_<PointAttractor<JointState>, std::shared_ptr<PointAttractor<JointState>>, IDynamicalSystem<JointState>>(m, "JointPointAttractorDS").def(py::init<>());
-
-  m.def("create_joint_ds", [](DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM type) -> py::object {
-    switch (type) {
-      case DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR:
-        return py::cast(PointAttractor<JointState>());
-        break;
-      default:
-      case DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::NONE:
-        return py::cast(DefaultDynamicalSystem<JointState>());
-        break;
-    }
-  });
-}
diff --git a/python/source/dynamical_systems/bind_joint_ds.cpp b/python/source/dynamical_systems/bind_joint_ds.cpp
new file mode 100644
index 000000000..ff6a3a0c6
--- /dev/null
+++ b/python/source/dynamical_systems/bind_joint_ds.cpp
@@ -0,0 +1,33 @@
+#include "dynamical_systems_bindings.h"
+
+#include <dynamical_systems/DynamicalSystemFactory.hpp>
+#include <dynamical_systems/IDynamicalSystem.hpp>
+#include <state_representation/space/joint/JointState.hpp>
+
+#include "py_dynamical_system.h"
+
+using namespace state_representation;
+using namespace py_parameter;
+
+void joint(py::module_& m) {
+  py::object parameter_map = py::module_::import("state_representation").attr("ParameterMap");
+  py::class_<IDynamicalSystem<JointState>, std::shared_ptr<IDynamicalSystem<JointState>>, PyDynamicalSystem<JointState>> c(m, "IJointDS", parameter_map);
+
+  c.def("is_compatible", &IDynamicalSystem<JointState>::is_compatible);
+  c.def("evaluate", &IDynamicalSystem<JointState>::evaluate);
+  c.def("get_base_frame", &IDynamicalSystem<JointState>::get_base_frame);
+  c.def("set_base_frame", &IDynamicalSystem<JointState>::set_base_frame);
+}
+
+void bind_joint_ds(py::module_& m) {
+  joint(m);
+
+  m.def("create_joint_ds", [](DYNAMICAL_SYSTEM_TYPE type, const std::list<ParameterContainer>& parameters) -> py::object {
+    return py::cast(JointDynamicalSystemFactory::create_dynamical_system(type, container_to_interface_ptr_list(parameters)));
+  }, "Create a dynamical system of the desired type with initial parameters.", "type"_a, "parameters"_a);
+
+
+  m.def("create_joint_ds", [](DYNAMICAL_SYSTEM_TYPE type) -> py::object {
+    return py::cast(JointDynamicalSystemFactory::create_dynamical_system(type, std::list<std::shared_ptr<ParameterInterface>>()));
+  }, "Create a dynamical system of the desired type.", "type"_a);
+}
diff --git a/python/source/dynamical_systems/bind_type.cpp b/python/source/dynamical_systems/bind_type.cpp
deleted file mode 100644
index cfe122fe5..000000000
--- a/python/source/dynamical_systems/bind_type.cpp
+++ /dev/null
@@ -1,15 +0,0 @@
-#include "dynamical_systems_bindings.h"
-
-#include <dynamical_systems/DynamicalSystemFactory.hpp>
-#include <state_representation/space/cartesian/CartesianState.hpp>
-
-using namespace state_representation;
-
-void bind_type(py::module_& m) {
-  py::enum_<DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM>(m, "DYNAMICAL_SYSTEM")
-      .value("NONE", DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::NONE)
-      .value("CIRCULAR", DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::CIRCULAR)
-      .value("POINT_ATTRACTOR", DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR)
-      .value("RING", DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::RING)
-      .export_values();
-}
diff --git a/python/source/dynamical_systems/dynamical_systems_bindings.cpp b/python/source/dynamical_systems/dynamical_systems_bindings.cpp
index 5b852bba6..c590c2aa0 100644
--- a/python/source/dynamical_systems/dynamical_systems_bindings.cpp
+++ b/python/source/dynamical_systems/dynamical_systems_bindings.cpp
@@ -14,7 +14,7 @@ PYBIND11_MODULE(dynamical_systems, m) {
 
   py::module_::import("state_representation");
 
-  bind_type(m);
-  bind_cartesian(m);
-  bind_joint(m);
+  bind_ds_type(m);
+  bind_cartesian_ds(m);
+  bind_joint_ds(m);
 }
diff --git a/python/source/state_representation/bind_cartesian_space.cpp b/python/source/state_representation/bind_cartesian_space.cpp
index f821d519a..6d33c98a8 100644
--- a/python/source/state_representation/bind_cartesian_space.cpp
+++ b/python/source/state_representation/bind_cartesian_space.cpp
@@ -12,6 +12,7 @@
 void spatial_state(py::module_& m) {
   py::class_<SpatialState, std::shared_ptr<SpatialState>, State> c(m, "SpatialState");
 
+  c.def(py::init(), "Empty constructor.");
   c.def(py::init<const StateType&>(), "Constructor only specifying the type.", "type"_a);
   c.def(py::init<const StateType&, const std::string&, const std::string&, const bool&>(), "Constructor with name and reference frame specification.", "type"_a, "name"_a, "reference_frame"_a=std::string("world"), "empty"_a=true);
   c.def(py::init<const SpatialState&>(), "Copy constructor from another SpatialState.", "state"_a);
@@ -63,7 +64,11 @@ void cartesian_state(py::module_& m) {
   c.def_static("Random", &CartesianState::Random, "Constructor for a random state", "name"_a, "reference"_a=std::string("world"));
 
   c.def("get_position", &CartesianState::get_position, "Getter of the position attribute");
-  c.def("get_orientation", &CartesianState::get_orientation_coefficients, "Getter of the orientation attribute as quaternion coefficients (w, x, y, z)");
+  c.def("get_orientation", [](const CartesianState &state) -> py::object {
+    py::object PyQuaternion = py::module_::import("pyquaternion").attr("Quaternion");
+    return PyQuaternion(state.get_orientation_coefficients());;
+  }, "Getter of the orientation attribute as pyquaternion object");
+  c.def("get_orientation_coefficients", &CartesianState::get_orientation_coefficients, "Getter of the orientation attribute as quaternion coefficients (w, x, y, z)");
   c.def("get_pose", &CartesianState::get_pose, "Getter of a 7d pose vector from position and orientation coefficients");
   c.def("get_transformation_matrix", &CartesianState::get_transformation_matrix, "Getter of a 4x4 transformation matrix of the pose");
 
@@ -82,22 +87,48 @@ void cartesian_state(py::module_& m) {
   c.def("set_position", py::overload_cast<const Eigen::Vector3d&>(&CartesianState::set_position), "Setter of the position");
   c.def("set_position", py::overload_cast<const std::vector<double>&>(&CartesianState::set_position), "Setter of the position from a list");
   c.def("set_position", py::overload_cast<const double&, const double&, const double&>(&CartesianState::set_position), "Setter of the position from three scalar coordinates", "x"_a, "y"_a, "z"_a);
-  c.def("set_orientation", py::overload_cast<const Eigen::Vector4d&>(&CartesianState::set_orientation), "Setter of the orientation from a 4d vector of quaternion coeffiecients (w, x, y, z)");
-  c.def("set_orientation", py::overload_cast<const std::vector<double>&>(&CartesianState::set_orientation), "Setter of the orientation from a 4d list of quaternion coeffiecients (w, x, y, z)");
-  c.def("set_pose", py::overload_cast<const Eigen::Matrix<double, 7, 1>&>(&CartesianState::set_pose), "Setter of the pose from a 7d vector of position and orientation coefficients (x, y, z, qw, qx, qy, qz");
-  c.def("set_pose", py::overload_cast<const std::vector<double>&>(&CartesianState::set_pose), "Setter of the pose from a 7d list of position and orientation coefficients (x, y, z, qw, qx, qy, qz");
-
-  c.def("set_linear_velocity", &CartesianState::set_linear_velocity, "Setter of the linear velocity attribute");
-  c.def("set_angular_velocity", &CartesianState::set_angular_velocity, "Setter of the angular velocity attribute");
-  c.def("set_twist", &CartesianState::set_twist, "Setter of the linear and angular velocities from a 6d twist vector");
-
-  c.def("set_linear_acceleration", &CartesianState::set_linear_acceleration, "Setter of the linear acceleration attribute");
-  c.def("set_angular_acceleration", &CartesianState::set_angular_acceleration, "Setter of the angular acceleration attribute");
-  c.def("set_acceleration", &CartesianState::set_acceleration, "Setter of the linear and angular acceleration from a 6d acceleration vector");
-
-  c.def("set_force", &CartesianState::set_force, "Setter of the force attribute");
-  c.def("set_torque", &CartesianState::set_torque, "Setter of the torque attribute");
-  c.def("set_wrench", &CartesianState::set_wrench, "Setter of the force and torque from a 6d wrench vector");
+  c.def("set_orientation", [](CartesianState &state, const py::object& quaternion) {
+    py::object PyQuaternion = py::module_::import("pyquaternion").attr("Quaternion");
+    if (py::isinstance<py::list>(quaternion)) {
+      state.set_orientation(py::cast<std::vector<double>>(quaternion));
+    } else if (py::isinstance<py::array>(quaternion)) {
+      state.set_orientation(py::cast<Eigen::Vector4d>(quaternion));
+    } else if (py::isinstance(quaternion, PyQuaternion)) {
+      state.set_orientation(py::cast<Eigen::Vector4d>(quaternion.attr("elements")));
+    } else {
+      throw std::invalid_argument("Type of input argument quaternion is not supported. "
+                                  "Supported types are: pyquaternion.Quaternion, numpy.array, list");
+    }
+  }, "Setter of the orientation attribute from a pyquaternion.Quaternion, numpy.array(w, x, y, z), or list(w, x, y, z)");
+  c.def("set_pose", py::overload_cast<const Eigen::Matrix<double, 7, 1>&>(&CartesianState::set_pose), "Setter of the pose from a 7d vector of position and orientation coefficients (x, y, z, qw, qx, qy, qz)");
+  c.def("set_pose", py::overload_cast<const std::vector<double>&>(&CartesianState::set_pose), "Setter of the pose from a 7d list of position and orientation coefficients (x, y, z, qw, qx, qy, qz)");
+
+  c.def("set_linear_velocity", py::overload_cast<const Eigen::Vector3d&>(&CartesianState::set_linear_velocity), "Setter of the linear velocity attribute");
+  c.def("set_linear_velocity", py::overload_cast<const std::vector<double>&>(&CartesianState::set_linear_velocity), "Setter of the linear velocity from a list");
+  c.def("set_linear_velocity", py::overload_cast<const double&, const double&, const double&>(&CartesianState::set_linear_velocity), "Setter of the linear velocity from three scalar coordinates", "x"_a, "y"_a, "z"_a);
+  c.def("set_angular_velocity", py::overload_cast<const Eigen::Vector3d&>(&CartesianState::set_angular_velocity), "Setter of the angular velocity attribute");
+  c.def("set_angular_velocity", py::overload_cast<const std::vector<double>&>(&CartesianState::set_angular_velocity), "Setter of the angular velocity from a list");
+  c.def("set_angular_velocity", py::overload_cast<const double&, const double&, const double&>(&CartesianState::set_angular_velocity), "Setter of the angular velocity from three scalar coordinates", "x"_a, "y"_a, "z"_a);
+  c.def("set_twist", py::overload_cast<const Eigen::Matrix<double, 6, 1>&>(&CartesianState::set_twist), "Setter of the linear and angular velocities from a 6d twist vector");
+  c.def("set_twist", py::overload_cast<const std::vector<double>&>(&CartesianState::set_twist), "Setter of the linear and angular velocities from a list");
+
+  c.def("set_linear_acceleration", py::overload_cast<const Eigen::Vector3d&>(&CartesianState::set_linear_acceleration), "Setter of the linear acceleration attribute");
+  c.def("set_linear_acceleration", py::overload_cast<const std::vector<double>&>(&CartesianState::set_linear_acceleration), "Setter of the linear acceleration from a list");
+  c.def("set_linear_acceleration", py::overload_cast<const double&, const double&, const double&>(&CartesianState::set_linear_acceleration), "Setter of the linear acceleration from three scalar coordinates", "x"_a, "y"_a, "z"_a);
+  c.def("set_angular_acceleration", py::overload_cast<const Eigen::Vector3d&>(&CartesianState::set_angular_acceleration), "Setter of the angular acceleration attribute");
+  c.def("set_angular_acceleration", py::overload_cast<const std::vector<double>&>(&CartesianState::set_angular_acceleration), "Setter of the angular acceleration from a list");
+  c.def("set_angular_acceleration", py::overload_cast<const double&, const double&, const double&>(&CartesianState::set_angular_acceleration), "Setter of the angular acceleration from three scalar coordinates", "x"_a, "y"_a, "z"_a);
+  c.def("set_acceleration", py::overload_cast<const Eigen::Matrix<double, 6, 1>&>(&CartesianState::set_acceleration), "Setter of the linear and angular accelerations from a 6d acceleration vector");
+  c.def("set_acceleration", py::overload_cast<const std::vector<double>&>(&CartesianState::set_acceleration), "Setter of the linear and angular accelerations from a list");
+
+  c.def("set_force", py::overload_cast<const Eigen::Vector3d&>(&CartesianState::set_force), "Setter of the force attribute");
+  c.def("set_force", py::overload_cast<const std::vector<double>&>(&CartesianState::set_force), "Setter of the force from a list");
+  c.def("set_force", py::overload_cast<const double&, const double&, const double&>(&CartesianState::set_force), "Setter of the force from three scalar coordinates", "x"_a, "y"_a, "z"_a);
+  c.def("set_torque", py::overload_cast<const Eigen::Vector3d&>(&CartesianState::set_torque), "Setter of the torque attribute");
+  c.def("set_torque", py::overload_cast<const std::vector<double>&>(&CartesianState::set_torque), "Setter of the torque from a list");
+  c.def("set_torque", py::overload_cast<const double&, const double&, const double&>(&CartesianState::set_torque), "Setter of the torque from three scalar coordinates", "x"_a, "y"_a, "z"_a);
+  c.def("set_wrench", py::overload_cast<const Eigen::Matrix<double, 6, 1>&>(&CartesianState::set_wrench), "Setter of the force and torque from a 6d wrench vector");
+  c.def("set_wrench", py::overload_cast<const std::vector<double>&>(&CartesianState::set_wrench), "Setter of the force and torque velocities from a list");
 
   c.def("set_zero", &CartesianState::set_zero, "Set the CartesianState to a zero value");
   c.def("clamp_state_variable", &CartesianState::clamp_state_variable, "Clamp inplace the magnitude of the a specific state variable (velocity, acceleration or force)", "max_value"_a, "state_variable_type"_a, "noise_ratio"_a=double(0));
@@ -251,6 +282,7 @@ void cartesian_twist(py::module_& m) {
     c.def(std::string("get_" + attr).c_str(), [](const CartesianTwist&) -> void {}, "Deleted method from parent class.");
     c.def(std::string("set_" + attr).c_str(), [](const CartesianTwist& twist) -> CartesianTwist { return twist; }, "Deleted method from parent class.");
   }
+  c.def(std::string("get_orientation_coefficients").c_str(), [](const CartesianTwist&) -> void {}, "Deleted method from parent class.");
 
   c.def(py::self += py::self);
   c.def(py::self + py::self);
@@ -324,6 +356,7 @@ void cartesian_acceleration(py::module_& m) {
     c.def(std::string("get_" + attr).c_str(), [](const CartesianAcceleration&) -> void {}, "Deleted method from parent class.");
     c.def(std::string("set_" + attr).c_str(), [](const CartesianAcceleration& acceleration) -> CartesianAcceleration { return acceleration; }, "Deleted method from parent class.");
   }
+  c.def(std::string("get_orientation_coefficients").c_str(), [](const CartesianAcceleration&) -> void {}, "Deleted method from parent class.");
 
   c.def(py::self += py::self);
   c.def(py::self + py::self);
@@ -395,6 +428,7 @@ void cartesian_wrench(py::module_& m) {
     c.def(std::string("get_" + attr).c_str(), [](const CartesianWrench&) -> void {}, "Deleted method from parent class.");
     c.def(std::string("set_" + attr).c_str(), [](const CartesianWrench& wrench) -> CartesianWrench { return wrench; }, "Deleted method from parent class.");
   }
+  c.def(std::string("get_orientation_coefficients").c_str(), [](const CartesianWrench&) -> void {}, "Deleted method from parent class.");
 
   c.def(py::self += py::self);
   c.def(py::self + py::self);
diff --git a/python/source/state_representation/bind_parameters.cpp b/python/source/state_representation/bind_parameters.cpp
index 62d644af6..22a94a74f 100644
--- a/python/source/state_representation/bind_parameters.cpp
+++ b/python/source/state_representation/bind_parameters.cpp
@@ -1,25 +1,51 @@
 #include "state_representation_bindings.h"
 
+#include <state_representation/parameters/ParameterType.hpp>
 #include <state_representation/parameters/Parameter.hpp>
 #include <state_representation/parameters/ParameterMap.hpp>
 
+#include <state_representation/space/cartesian/CartesianState.hpp>
+#include <state_representation/space/cartesian/CartesianPose.hpp>
+#include <state_representation/space/joint/JointState.hpp>
+#include <state_representation/space/joint/JointPositions.hpp>
+#include <state_representation/geometry/Ellipsoid.hpp>
+
 #include "parameter_container.h"
 #include "py_parameter_map.h"
 
 using namespace py_parameter;
 
+void parameter_type(py::module_& m) {
+  py::enum_<ParameterType>(m, "ParameterType")
+      .value("INT", ParameterType::INT)
+      .value("INT_ARRAY", ParameterType::INT_ARRAY)
+      .value("DOUBLE", ParameterType::DOUBLE)
+      .value("DOUBLE_ARRAY", ParameterType::DOUBLE_ARRAY)
+      .value("BOOL", ParameterType::BOOL)
+      .value("BOOL_ARRAY", ParameterType::BOOL_ARRAY)
+      .value("STRING", ParameterType::STRING)
+      .value("STRING_ARRAY", ParameterType::STRING_ARRAY)
+      .value("STATE", ParameterType::STATE)
+      .value("VECTOR", ParameterType::VECTOR)
+      .value("MATRIX", ParameterType::MATRIX)
+      .export_values();
+}
+
 void parameter_interface(py::module_& m) {
   py::class_<ParameterInterface, std::shared_ptr<ParameterInterface>, State> c(m, "ParameterInterface");
 
-  c.def(py::init<const StateType&, const std::string&>(), "Constructor with parameter name and type of the parameter", "type"_a, "name"_a);
+  c.def(py::init<const std::string&, const ParameterType&, const StateType&>(), "Constructor of a ParameterInterface with name, parameter type and parameter state type", "name"_a, "type"_a, "parameter_state_type"_a=StateType::NONE);
   c.def(py::init<const ParameterInterface&>(), "Copy constructor from another ParameterInterface", "parameter"_a);
+
+  c.def("get_parameter_type", &ParameterInterface::get_parameter_type, "Get the parameter type.");
+  c.def("get_parameter_state_type", &ParameterInterface::get_parameter_state_type, "Get the state type of the parameter.");
 }
 
 void parameter(py::module_& m) {
   py::class_<ParameterContainer, std::shared_ptr<ParameterContainer>, ParameterInterface> c(m, "Parameter");
 
-  c.def(py::init<const std::string&, const StateType&>(), "Constructor of a parameter with name and type", "name"_a, "type"_a);
-  c.def(py::init<const std::string&, const py::object&, const StateType&>(), "Constructor of a parameter with name, value and type", "name"_a, "value"_a, "type"_a);
+  c.def(py::init<const std::string&, const ParameterType&, const StateType&>(), "Constructor of a parameter with name, parameter type and parameter state type", "name"_a, "type"_a, "parameter_state_type"_a=StateType::NONE);
+  c.def(py::init<const std::string&, const py::object&, const ParameterType&, const StateType&>(), "Constructor of a parameter with name, value, parameter type and parameter state type", "name"_a, "value"_a, "type"_a, "parameter_state_type"_a=StateType::NONE);
   c.def(py::init<const ParameterContainer&>(), "Copy constructor from another Parameter", "parameter"_a);
   c.def(py::init([](const std::shared_ptr<ParameterInterface>& parameter) { return interface_ptr_to_container(parameter); }), "Constructor from a parameter interface pointer", "parameter"_a);
 
@@ -34,73 +60,96 @@ void parameter(py::module_& m) {
   }, "memo"_a);
   c.def("__repr__", [](const ParameterContainer& parameter) {
     std::stringstream buffer;
-    switch (parameter.get_type()) {
-      case StateType::PARAMETER_INT: {
+    switch (parameter.get_parameter_type()) {
+      case ParameterType::INT: {
         Parameter<int> param(parameter.get_name(), parameter.values.int_value);
         buffer << param;
         break;
       }
-      case StateType::PARAMETER_INT_ARRAY: {
+      case ParameterType::INT_ARRAY: {
         Parameter<std::vector<int>> param(parameter.get_name(), parameter.values.int_array_value);
         buffer << param;
         break;
       }
-      case StateType::PARAMETER_DOUBLE: {
+      case ParameterType::DOUBLE: {
         Parameter<double> param(parameter.get_name(), parameter.values.double_value);
         buffer << param;
         break;
       }
-      case StateType::PARAMETER_DOUBLE_ARRAY: {
+      case ParameterType::DOUBLE_ARRAY: {
         Parameter<std::vector<double>> param(parameter.get_name(), parameter.values.double_array_value);
         buffer << param;
         break;
       }
-      case StateType::PARAMETER_BOOL: {
+      case ParameterType::BOOL: {
         Parameter<bool> param(parameter.get_name(), parameter.values.bool_value);
         buffer << param;
         break;
       }
-      case StateType::PARAMETER_BOOL_ARRAY: {
+      case ParameterType::BOOL_ARRAY: {
         Parameter<std::vector<bool>> param(parameter.get_name(), parameter.values.bool_array_value);
         buffer << param;
         break;
       }
-      case StateType::PARAMETER_STRING: {
+      case ParameterType::STRING: {
         Parameter<std::string> param(parameter.get_name(), parameter.values.string_value);
         buffer << param;
         break;
       }
-      case StateType::PARAMETER_STRING_ARRAY: {
+      case ParameterType::STRING_ARRAY: {
         Parameter<std::vector<std::string>> param(parameter.get_name(), parameter.values.string_array_value);
         buffer << param;
         break;
       }
-      case StateType::PARAMETER_CARTESIANSTATE: {
-        Parameter<CartesianState> param(parameter.get_name(), parameter.values.cartesian_state);
-        buffer << param;
-        break;
-      }
-      case StateType::PARAMETER_CARTESIANPOSE: {
-        Parameter<CartesianPose> param(parameter.get_name(), parameter.values.cartesian_pose);
-        buffer << param;
-        break;
-      }
-      case StateType::PARAMETER_JOINTSTATE: {
-        Parameter<JointState> param(parameter.get_name(), parameter.values.joint_state);
-        buffer << param;
-        break;
-      }
-      case StateType::PARAMETER_JOINTPOSITIONS: {
-        Parameter<JointPositions> param(parameter.get_name(), parameter.values.joint_positions);
-        buffer << param;
+      case ParameterType::STATE: {
+        try {
+          switch (parameter.get_parameter_state_type()) {
+            case StateType::CARTESIAN_STATE: {
+              Parameter<CartesianState> param
+                  (parameter.get_name(), *std::dynamic_pointer_cast<CartesianState>(parameter.values.state_pointer));
+              buffer << param;
+              break;
+            }
+            case StateType::CARTESIAN_POSE: {
+              Parameter<CartesianPose> param
+                  (parameter.get_name(), *std::dynamic_pointer_cast<CartesianPose>(parameter.values.state_pointer));
+              buffer << param;
+              break;
+            }
+            case StateType::JOINT_STATE: {
+              Parameter<JointState> param
+                  (parameter.get_name(), *std::dynamic_pointer_cast<JointState>(parameter.values.state_pointer));
+              buffer << param;
+              break;
+            }
+            case StateType::JOINT_POSITIONS: {
+              Parameter<JointPositions> param
+                  (parameter.get_name(), *std::dynamic_pointer_cast<JointPositions>(parameter.values.state_pointer));
+              buffer << param;
+              break;
+            }
+            case StateType::GEOMETRY_ELLIPSOID: {
+              Parameter<Ellipsoid> param
+                  (parameter.get_name(), *std::dynamic_pointer_cast<Ellipsoid>(parameter.values.state_pointer));
+              buffer << param;
+              break;
+            }
+            default:
+              buffer << "Parameter " << parameter.get_name() << " contains an unsupported state type" << std::endl;
+              break;
+          }
+        } catch (const std::exception&) {
+          buffer << "Parameter " << parameter.get_name() << " is invalid" << std::endl;
+          break;
+        }
         break;
       }
-      case StateType::PARAMETER_MATRIX: {
+      case ParameterType::MATRIX: {
         Parameter<Eigen::MatrixXd> param(parameter.get_name(), parameter.values.matrix_value);
         buffer << param;
         break;
       }
-      case StateType::PARAMETER_VECTOR: {
+      case ParameterType::VECTOR: {
         Parameter<Eigen::VectorXd> param(parameter.get_name(), parameter.values.vector_value);
         buffer << param;
         break;
@@ -159,14 +208,15 @@ void parameter_map(py::module_& m) {
     self.set_parameters(container_to_interface_ptr_map(parameters));
   }, "Set parameters from a map with <name, parameter> pairs", "parameters"_a);
   c.def(
-      "set_parameter_value", [](ParameterMap& self, const std::string& name, const py::object& value, const StateType& type) -> void {
-        auto param = ParameterContainer(name, value, type);
+      "set_parameter_value", [](ParameterMap& self, const std::string& name, const py::object& value, const ParameterType& type, const StateType& parameter_state_type) -> void {
+        auto param = ParameterContainer(name, value, type, parameter_state_type);
         self.set_parameter(container_to_interface_ptr(param));
-      }, "Set a parameter value by its name", "name"_a, "value"_a, "type"_a
+      }, "Set a parameter value by its name", "name"_a, "value"_a, "type"_a, "parameter_state_type"_a=StateType::NONE
   );
 }
 
 void bind_parameters(py::module_& m) {
+  parameter_type(m);
   parameter_interface(m);
   parameter(m);
   parameter_map(m);
diff --git a/python/source/state_representation/bind_state.cpp b/python/source/state_representation/bind_state.cpp
index b89737276..b9aed8bd5 100644
--- a/python/source/state_representation/bind_state.cpp
+++ b/python/source/state_representation/bind_state.cpp
@@ -5,28 +5,24 @@
 
 void state_type(py::module_& m) {
   py::enum_<StateType>(m, "StateType")
+      .value("NONE", StateType::NONE)
       .value("STATE", StateType::STATE)
-      .value("CARTESIANSTATE", StateType::CARTESIANSTATE)
-      .value("DUALQUATERNIONSTATE", StateType::DUALQUATERNIONSTATE)
-      .value("JOINTSTATE", StateType::JOINTSTATE)
-      .value("JACOBIANMATRIX", StateType::JACOBIANMATRIX)
+      .value("SPATIAL_STATE", StateType::SPATIAL_STATE)
+      .value("CARTESIAN_STATE", StateType::CARTESIAN_STATE)
+      .value("CARTESIAN_POSE", StateType::CARTESIAN_POSE)
+      .value("CARTESIAN_TWIST", StateType::CARTESIAN_TWIST)
+      .value("CARTESIAN_ACCELERATION", StateType::CARTESIAN_ACCELERATION)
+      .value("CARTESIAN_WRENCH", StateType::CARTESIAN_WRENCH)
+      .value("JOINT_STATE", StateType::JOINT_STATE)
+      .value("JOINT_POSITIONS", StateType::JOINT_POSITIONS)
+      .value("JOINT_VELOCITIES", StateType::JOINT_VELOCITIES)
+      .value("JOINT_ACCELERATIONS", StateType::JOINT_ACCELERATIONS)
+      .value("JOINT_TORQUES", StateType::JOINT_TORQUES)
+      .value("JACOBIAN", StateType::JACOBIAN)
+      .value("PARAMETER", StateType::PARAMETER)
       .value("GEOMETRY_SHAPE", StateType::GEOMETRY_SHAPE)
       .value("GEOMETRY_ELLIPSOID", StateType::GEOMETRY_ELLIPSOID)
-      .value("PARAMETER_INT", StateType::PARAMETER_INT)
-      .value("PARAMETER_INT_ARRAY", StateType::PARAMETER_INT_ARRAY)
-      .value("PARAMETER_DOUBLE", StateType::PARAMETER_DOUBLE)
-      .value("PARAMETER_DOUBLE_ARRAY", StateType::PARAMETER_DOUBLE_ARRAY)
-      .value("PARAMETER_BOOL", StateType::PARAMETER_BOOL)
-      .value("PARAMETER_BOOL_ARRAY", StateType::PARAMETER_BOOL_ARRAY)
-      .value("PARAMETER_STRING", StateType::PARAMETER_STRING)
-      .value("PARAMETER_STRING_ARRAY", StateType::PARAMETER_STRING_ARRAY)
-      .value("PARAMETER_CARTESIANSTATE", StateType::PARAMETER_CARTESIANSTATE)
-      .value("PARAMETER_CARTESIANPOSE", StateType::PARAMETER_CARTESIANPOSE)
-      .value("PARAMETER_JOINTSTATE", StateType::PARAMETER_JOINTSTATE)
-      .value("PARAMETER_JOINTPOSITIONS", StateType::PARAMETER_JOINTPOSITIONS)
-      .value("PARAMETER_ELLIPSOID", StateType::PARAMETER_ELLIPSOID)
-      .value("PARAMETER_MATRIX", StateType::PARAMETER_MATRIX)
-      .value("PARAMETER_VECTOR", StateType::PARAMETER_VECTOR)
+      .value("TRAJECTORY", StateType::TRAJECTORY)
       .export_values();
 }
 
diff --git a/python/test/controllers/test_compliant_twist.py b/python/test/controllers/test_compliant_twist.py
index 5d066b392..af424bd9f 100644
--- a/python/test/controllers/test_compliant_twist.py
+++ b/python/test/controllers/test_compliant_twist.py
@@ -18,10 +18,10 @@ def setUpClass(cls):
 
     @classmethod
     def set_gains(cls, lpd, lod, ast, ad):
-        cls.ctrl.set_parameter_value("linear_principle_damping", lpd, sr.StateType.PARAMETER_DOUBLE)
-        cls.ctrl.set_parameter_value("linear_orthogonal_damping", lod, sr.StateType.PARAMETER_DOUBLE)
-        cls.ctrl.set_parameter_value("angular_stiffness", ast, sr.StateType.PARAMETER_DOUBLE)
-        cls.ctrl.set_parameter_value("angular_damping", ad, sr.StateType.PARAMETER_DOUBLE)
+        cls.ctrl.set_parameter_value("linear_principle_damping", lpd, sr.ParameterType.DOUBLE)
+        cls.ctrl.set_parameter_value("linear_orthogonal_damping", lod, sr.ParameterType.DOUBLE)
+        cls.ctrl.set_parameter_value("angular_stiffness", ast, sr.ParameterType.DOUBLE)
+        cls.ctrl.set_parameter_value("angular_damping", ad, sr.ParameterType.DOUBLE)
 
     # FIXME this line sometimes fails with github actions
     # def test_cartesian_wrench(self):
@@ -50,22 +50,22 @@ def test_get_and_set_parameters(self):
             if param.get_name() == "linear_principle_damping":
                 self.assertTrue(abs(param.get_value() - 1) < 1e-5)
                 self.assertTrue(abs(self.ctrl.get_parameter_value("linear_principle_damping") - 1) < 1e-5)
-                self.ctrl.set_parameter_value("linear_principle_damping", 11, sr.StateType.PARAMETER_DOUBLE)
+                self.ctrl.set_parameter_value("linear_principle_damping", 11, sr.ParameterType.DOUBLE)
                 self.assertTrue(abs(self.ctrl.get_parameter_value("linear_principle_damping") - 11) < 1e-5)
             if param.get_name() == "linear_orthogonal_damping":
                 self.assertTrue(abs(param.get_value() - 2) < 1e-5)
                 self.assertTrue(abs(self.ctrl.get_parameter_value("linear_orthogonal_damping") - 2) < 1e-5)
-                self.ctrl.set_parameter_value("linear_orthogonal_damping", 12, sr.StateType.PARAMETER_DOUBLE)
+                self.ctrl.set_parameter_value("linear_orthogonal_damping", 12, sr.ParameterType.DOUBLE)
                 self.assertTrue(abs(self.ctrl.get_parameter_value("linear_orthogonal_damping") - 12) < 1e-5)
             if param.get_name() == "angular_stiffness":
                 self.assertTrue(abs(param.get_value() - 3) < 1e-5)
                 self.assertTrue(abs(self.ctrl.get_parameter_value("angular_stiffness") - 3) < 1e-5)
-                self.ctrl.set_parameter_value("angular_stiffness", 13, sr.StateType.PARAMETER_DOUBLE)
+                self.ctrl.set_parameter_value("angular_stiffness", 13, sr.ParameterType.DOUBLE)
                 self.assertTrue(abs(self.ctrl.get_parameter_value("angular_stiffness") - 13) < 1e-5)
             if param.get_name() == "angular_damping":
                 self.assertTrue(abs(param.get_value() - 4) < 1e-5)
                 self.assertTrue(abs(self.ctrl.get_parameter_value("angular_damping") - 4) < 1e-5)
-                self.ctrl.set_parameter_value("angular_damping", 14, sr.StateType.PARAMETER_DOUBLE)
+                self.ctrl.set_parameter_value("angular_damping", 14, sr.ParameterType.DOUBLE)
                 self.assertTrue(abs(self.ctrl.get_parameter_value("angular_damping") - 14) < 1e-5)
 
 
diff --git a/python/test/controllers/test_controller.py b/python/test/controllers/test_controller.py
index 987fa623e..b0b58d17e 100644
--- a/python/test/controllers/test_controller.py
+++ b/python/test/controllers/test_controller.py
@@ -109,9 +109,9 @@ def test_joint_controller(self):
         self.assertRaises(RuntimeError, create_joint_controller, CONTROLLER_TYPE.COMPLIANT_TWIST)
 
     def test_controller_with_params(self):
-        param_list = [sr.Parameter("damping", 0.0, sr.StateType.PARAMETER_DOUBLE),
-                      sr.Parameter("stiffness", 5.0, sr.StateType.PARAMETER_DOUBLE),
-                      sr.Parameter("inertia", 10.0, sr.StateType.PARAMETER_DOUBLE)]
+        param_list = [sr.Parameter("damping", 0.0, sr.ParameterType.DOUBLE),
+                      sr.Parameter("stiffness", 5.0, sr.ParameterType.DOUBLE),
+                      sr.Parameter("inertia", 10.0, sr.ParameterType.DOUBLE)]
 
         ctrl = create_cartesian_controller(CONTROLLER_TYPE.IMPEDANCE, param_list)
         self.assertFalse(ctrl is None)
@@ -150,7 +150,7 @@ def test_controller_with_robot(self):
                          robot.get_number_of_joints() * robot.get_number_of_joints())
 
     def test_controller_with_robot_and_params(self):
-        parameters = [sr.Parameter("damping", 5.0, sr.StateType.PARAMETER_DOUBLE)]
+        parameters = [sr.Parameter("damping", 5.0, sr.ParameterType.DOUBLE)]
         robot = Model("robot", os.path.join(os.path.dirname(os.path.realpath(__file__)), "panda_arm.urdf"))
 
         self.assertRaises(RuntimeError, create_joint_controller, CONTROLLER_TYPE.NONE, parameters, robot)
diff --git a/python/test/controllers/test_dissipative_impedance.py b/python/test/controllers/test_dissipative_impedance.py
index ad26ba2d0..badaf65fe 100644
--- a/python/test/controllers/test_dissipative_impedance.py
+++ b/python/test/controllers/test_dissipative_impedance.py
@@ -13,7 +13,7 @@ def test_compute_command_with_colinear_velocity(self):
 
         eigenvalues = ctrl.get_parameter_value("damping_eigenvalues")
         eigenvalues[0] = 10
-        ctrl.set_parameter_value("damping_eigenvalues", eigenvalues, sr.StateType.PARAMETER_VECTOR)
+        ctrl.set_parameter_value("damping_eigenvalues", eigenvalues, sr.ParameterType.VECTOR)
 
         desired_twist = sr.CartesianTwist("test", [1, 0, 0])
         feedback_twist = sr.CartesianTwist("test", [1, 1, 0])
diff --git a/python/test/dynamical_systems/test_cartesian_point_attractor.py b/python/test/dynamical_systems/test_cartesian_point_attractor.py
index f85b239ff..ec6e7681a 100644
--- a/python/test/dynamical_systems/test_cartesian_point_attractor.py
+++ b/python/test/dynamical_systems/test_cartesian_point_attractor.py
@@ -2,7 +2,7 @@
 import state_representation as sr
 import unittest
 from datetime import timedelta
-from dynamical_systems import CartesianPointAttractorDS
+from dynamical_systems import create_cartesian_ds, DYNAMICAL_SYSTEM_TYPE
 
 
 class TestCartesianPointAttractor(unittest.TestCase):
@@ -14,12 +14,12 @@ def assert_np_array_equal(self, a: np.array, b: np.array, places=3):
             self.fail(f'{e}')
 
     def test_empty_constructor(self):
-        ds = CartesianPointAttractorDS()
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR)
         attractor = sr.CartesianState.Identity("CAttractor", "A")
 
         self.assertTrue(ds.get_parameter_value("attractor").is_empty())
         self.assertTrue(ds.get_base_frame().is_empty())
-        ds.set_parameter(sr.Parameter("attractor", attractor, sr.StateType.PARAMETER_CARTESIANSTATE))
+        ds.set_parameter(sr.Parameter("attractor", attractor, sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE))
         self.assertFalse(ds.get_parameter_value("attractor").is_empty())
         self.assertFalse(ds.get_base_frame().is_empty())
         self.assertEqual(ds.get_base_frame().get_name(), attractor.get_reference_frame())
@@ -27,7 +27,7 @@ def test_empty_constructor(self):
         self.assert_np_array_equal(ds.get_base_frame().get_transformation_matrix(), np.eye(4))
 
     def test_is_compatible(self):
-        ds = CartesianPointAttractorDS()
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR)
         state1 = sr.CartesianState.Identity("B", "A")
         state2 = sr.CartesianState.Identity("D", "C")
         state3 = sr.CartesianState.Identity("C", "A")
@@ -45,16 +45,16 @@ def test_is_compatible(self):
             ds.evaluate(state4)
 
         ds.set_parameter(sr.Parameter("attractor", sr.CartesianState.Identity("CAttractor", "A"),
-                                      sr.StateType.PARAMETER_CARTESIANSTATE))
+                                      sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE))
         self.assertTrue(ds.is_compatible(state1))
         self.assertFalse(ds.is_compatible(state2))
         self.assertTrue(ds.is_compatible(state3))
         self.assertTrue(ds.is_compatible(state4))
 
     def test_pose(self):
-        ds = CartesianPointAttractorDS()
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR)
         target = sr.CartesianPose.Random("B")
-        ds.set_parameter(sr.Parameter("attractor", target, sr.StateType.PARAMETER_CARTESIANSTATE))
+        ds.set_parameter(sr.Parameter("attractor", target, sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE))
 
         current_pose = sr.CartesianPose.Identity("B")
         for i in range(100):
@@ -69,8 +69,8 @@ def test_stacked_moving_reference_frames(self):
         BinA = sr.CartesianState.Random("B", "A")
         CinA = sr.CartesianState(sr.CartesianPose.Random("C", "A"))
 
-        ds = CartesianPointAttractorDS()
-        ds.set_parameter(sr.Parameter("attractor", BinA, sr.StateType.PARAMETER_CARTESIANSTATE))
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR)
+        ds.set_parameter(sr.Parameter("attractor", BinA, sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE))
 
         twist = sr.CartesianTwist(ds.evaluate(CinA))
 
diff --git a/python/test/dynamical_systems/test_circular.py b/python/test/dynamical_systems/test_circular.py
index 6ad6af9e9..c1c0fa11f 100644
--- a/python/test/dynamical_systems/test_circular.py
+++ b/python/test/dynamical_systems/test_circular.py
@@ -2,7 +2,7 @@
 import state_representation as sr
 import unittest
 from datetime import timedelta
-from dynamical_systems import CartesianCircularDS
+from dynamical_systems import create_cartesian_ds, DYNAMICAL_SYSTEM_TYPE
 
 
 class TestCircular(unittest.TestCase):
@@ -25,11 +25,11 @@ def assert_np_array_equal(self, a: np.array, b: np.array, places=3):
             self.fail(f'{e}')
 
     def test_empty_constructor(self):
-        ds = CartesianCircularDS()
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.CIRCULAR)
         self.assertTrue(ds.get_parameter_value("limit_cycle").get_center_state().is_empty())
         self.assertTrue(ds.get_base_frame().is_empty())
 
-        ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.StateType.PARAMETER_ELLIPSOID))
+        ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.ParameterType.STATE, sr.StateType.GEOMETRY_ELLIPSOID))
         self.assertFalse(ds.get_parameter_value("limit_cycle").get_center_state().is_empty())
         self.assertFalse(ds.get_base_frame().is_empty())
 
@@ -38,7 +38,7 @@ def test_empty_constructor(self):
         self.assert_np_array_equal(ds.get_base_frame().get_transformation_matrix(), np.eye(4))
 
     def test_is_compatible(self):
-        ds = CartesianCircularDS()
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.CIRCULAR)
         state1 = sr.CartesianState.Identity("world", "A")
         state2 = sr.CartesianState("D", "C")
         state3 = sr.CartesianState("C", "A")
@@ -55,16 +55,16 @@ def test_is_compatible(self):
         with self.assertRaises(RuntimeError):
             ds.evaluate(state4)
 
-        ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.StateType.PARAMETER_ELLIPSOID))
+        ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.ParameterType.STATE, sr.StateType.GEOMETRY_ELLIPSOID))
         self.assertTrue(ds.is_compatible(state1))
         self.assertFalse(ds.is_compatible(state2))
         self.assertTrue(ds.is_compatible(state3))
         self.assertTrue(ds.is_compatible(state4))
 
     def test_points_on_radius_random_center(self):
-        ds = CartesianCircularDS()
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.CIRCULAR)
         self.limit_cycle.set_center_position(np.random.rand(3, 1))
-        ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.StateType.PARAMETER_ELLIPSOID))
+        ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.ParameterType.STATE, sr.StateType.GEOMETRY_ELLIPSOID))
 
         current_pose = sr.CartesianPose("A", 10 * np.random.rand(3, 1))
         for i in range(self.nb_steps):
diff --git a/python/test/dynamical_systems/test_ds.py b/python/test/dynamical_systems/test_ds.py
index 092500e1e..7a34e268e 100644
--- a/python/test/dynamical_systems/test_ds.py
+++ b/python/test/dynamical_systems/test_ds.py
@@ -1,6 +1,6 @@
 import state_representation as sr
 import unittest
-from dynamical_systems import ICartesianDS, DYNAMICAL_SYSTEM, create_cartesian_ds, create_joint_ds
+from dynamical_systems import ICartesianDS, DYNAMICAL_SYSTEM_TYPE, create_cartesian_ds, create_joint_ds
 
 DS_METHOD_EXPECTS = [
     'is_compatible',
@@ -24,46 +24,46 @@ def test_callable_methods(self):
             self.assertIn(expected, methods)
 
     def test_cartesian_factory(self):
-        cart_ds = create_cartesian_ds(DYNAMICAL_SYSTEM.NONE)
+        cart_ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.NONE)
         cart_ds.set_base_frame(sr.CartesianState.Identity("A"))
         cart_ds.evaluate(sr.CartesianState.Identity("C", "A"))
         self.assertTrue(cart_ds.evaluate(sr.CartesianState.Identity("C", "A")).is_empty())
         self.assertTrue(len(cart_ds.get_parameter_list()) == 0)
 
-        param_list = [sr.Parameter("test", 1.0, sr.StateType.PARAMETER_DOUBLE)]
+        param_list = [sr.Parameter("test", 1.0, sr.ParameterType.DOUBLE)]
         with self.assertRaises(RuntimeError):
             cart_ds.set_parameters(param_list)
 
     def test_cartesian_ds(self):
-        ds = create_cartesian_ds(DYNAMICAL_SYSTEM.POINT_ATTRACTOR)
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR)
         expected_params = ["attractor", "gain"]
         for param in list(ds.get_parameters().keys()):
             self.assertIn(param, expected_params)
 
-        ds = create_cartesian_ds(DYNAMICAL_SYSTEM.RING)
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.RING)
         expected_params = ["center", "rotation_offset", "radius", "width", "speed", "field_strength", "normal_gain",
                            "angular_gain"]
         for param in list(ds.get_parameters().keys()):
             self.assertIn(param, expected_params)
 
-        ds = create_cartesian_ds(DYNAMICAL_SYSTEM.CIRCULAR)
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.CIRCULAR)
         expected_params = ["limit_cycle", "planar_gain", "normal_gain", "circular_velocity"]
         for param in list(ds.get_parameters().keys()):
             self.assertIn(param, expected_params)
 
     def test_joint_factory(self):
-        joint_ds = create_joint_ds(DYNAMICAL_SYSTEM.NONE)
+        joint_ds = create_joint_ds(DYNAMICAL_SYSTEM_TYPE.NONE)
         joint_ds.set_base_frame(sr.JointState.Zero("robot", 3))
         joint_ds.evaluate(sr.JointState.Random("robot", 3))
         self.assertTrue(joint_ds.evaluate(sr.JointState.Random("robot", 3)).is_empty())
         self.assertTrue(len(joint_ds.get_parameter_list()) == 0)
 
-        param_list = [sr.Parameter("test", 1.0, sr.StateType.PARAMETER_DOUBLE)]
+        param_list = [sr.Parameter("test", 1.0, sr.ParameterType.DOUBLE)]
         with self.assertRaises(RuntimeError):
             joint_ds.set_parameters(param_list)
 
     def test_joint_ds(self):
-        ds = create_joint_ds(DYNAMICAL_SYSTEM.POINT_ATTRACTOR)
+        ds = create_joint_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR)
         expected_params = ["attractor", "gain"]
         for param in list(ds.get_parameters().keys()):
             self.assertIn(param, expected_params)
diff --git a/python/test/dynamical_systems/test_joint_point_attractor.py b/python/test/dynamical_systems/test_joint_point_attractor.py
index 15a6dc56b..23ff7c959 100644
--- a/python/test/dynamical_systems/test_joint_point_attractor.py
+++ b/python/test/dynamical_systems/test_joint_point_attractor.py
@@ -1,23 +1,27 @@
 import state_representation as sr
 import unittest
 from datetime import timedelta
-from dynamical_systems import JointPointAttractorDS
+from dynamical_systems import create_joint_ds, DYNAMICAL_SYSTEM_TYPE
 
 
 class TestJointPointAttractor(unittest.TestCase):
-    def test_empty_constructor(self):
-        ds = JointPointAttractorDS()
+    def test_constructor(self):
+        ds = create_joint_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR)
         attractor = sr.JointState.Zero("robot", 3)
 
         self.assertTrue(ds.get_parameter_value("attractor").is_empty())
         self.assertTrue(ds.get_base_frame().is_empty())
-        ds.set_parameter(sr.Parameter("attractor", attractor, sr.StateType.PARAMETER_JOINTSTATE))
+        ds.set_parameter(sr.Parameter("attractor", attractor, sr.ParameterType.STATE, sr.StateType.JOINT_STATE))
+        self.assertFalse(ds.get_parameter_value("attractor").is_empty())
+
+        parameters = [sr.Parameter("attractor", attractor, sr.ParameterType.STATE, sr.StateType.JOINT_STATE)]
+        ds = create_joint_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR, parameters)
         self.assertFalse(ds.get_parameter_value("attractor").is_empty())
 
     def test_convergence(self):
-        ds = JointPointAttractorDS()
+        ds = create_joint_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR)
         attractor = sr.JointPositions.Random("robot", 3)
-        ds.set_parameter(sr.Parameter("attractor", attractor, sr.StateType.PARAMETER_JOINTSTATE))
+        ds.set_parameter(sr.Parameter("attractor", attractor, sr.ParameterType.STATE, sr.StateType.JOINT_STATE))
 
         current_state = sr.JointPositions.Random("robot", 3)
         current_state.set_data(10 * current_state.data())
diff --git a/python/test/dynamical_systems/test_ring.py b/python/test/dynamical_systems/test_ring.py
index be22fd5e7..52d5f3e2b 100644
--- a/python/test/dynamical_systems/test_ring.py
+++ b/python/test/dynamical_systems/test_ring.py
@@ -3,7 +3,7 @@
 import state_representation as sr
 import unittest
 from datetime import timedelta
-from dynamical_systems import CartesianRingDS
+from dynamical_systems import create_cartesian_ds, DYNAMICAL_SYSTEM_TYPE
 
 
 class TestRing(unittest.TestCase):
@@ -25,11 +25,11 @@ def assert_np_array_equal(self, a: np.array, b: np.array, places=3):
             self.fail(f'{e}')
 
     def test_empty_constructor(self):
-        ds = CartesianRingDS()
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.RING)
         center = sr.CartesianPose.Identity("CAttractor", "A")
 
         self.assertTrue(ds.get_parameter_value("center").is_empty())
-        ds.set_parameter(sr.Parameter("center", center, sr.StateType.PARAMETER_CARTESIANPOSE))
+        ds.set_parameter(sr.Parameter("center", center, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE))
         self.assertFalse(ds.get_parameter_value("center").is_empty())
 
         self.assertFalse(ds.get_base_frame().is_empty())
@@ -38,7 +38,7 @@ def test_empty_constructor(self):
         self.assert_np_array_equal(ds.get_base_frame().get_transformation_matrix(), np.eye(4))
 
     def test_is_compatible(self):
-        ds = CartesianRingDS()
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.RING)
         center = sr.CartesianPose.Identity("CAttractor", "A")
         state1 = sr.CartesianState.Identity("B", "A")
         state2 = sr.CartesianState.Identity("D", "C")
@@ -56,18 +56,18 @@ def test_is_compatible(self):
         with self.assertRaises(RuntimeError):
             ds.evaluate(state4)
 
-        ds.set_parameter(sr.Parameter("center", center, sr.StateType.PARAMETER_CARTESIANPOSE))
+        ds.set_parameter(sr.Parameter("center", center, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE))
         self.assertTrue(ds.is_compatible(state1))
         self.assertFalse(ds.is_compatible(state2))
         self.assertTrue(ds.is_compatible(state3))
         self.assertTrue(ds.is_compatible(state4))
 
     def test_points_on_radius(self):
-        ds = CartesianRingDS()
-        ds.set_parameter(sr.Parameter("center", self.center, sr.StateType.PARAMETER_CARTESIANPOSE))
-        ds.set_parameter(sr.Parameter("radius", self.radius, sr.StateType.PARAMETER_DOUBLE))
-        ds.set_parameter(sr.Parameter("width", self.width, sr.StateType.PARAMETER_DOUBLE))
-        ds.set_parameter(sr.Parameter("speed", self.speed, sr.StateType.PARAMETER_DOUBLE))
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.RING)
+        ds.set_parameter(sr.Parameter("center", self.center, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE))
+        ds.set_parameter(sr.Parameter("radius", self.radius, sr.ParameterType.DOUBLE))
+        ds.set_parameter(sr.Parameter("width", self.width, sr.ParameterType.DOUBLE))
+        ds.set_parameter(sr.Parameter("speed", self.speed, sr.ParameterType.DOUBLE))
 
         current_pose = copy.deepcopy(self.center)
         twist = sr.CartesianTwist(ds.evaluate(current_pose))
@@ -99,9 +99,9 @@ def test_points_on_radius(self):
 
     def test_convergence_on_radius_random_center(self):
         center = sr.CartesianPose.Random("A")
-        ds = CartesianRingDS()
-        ds.set_parameter(sr.Parameter("center", center, sr.StateType.PARAMETER_CARTESIANPOSE))
-        ds.set_parameter(sr.Parameter("radius", self.radius, sr.StateType.PARAMETER_DOUBLE))
+        ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.RING)
+        ds.set_parameter(sr.Parameter("center", center, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE))
+        ds.set_parameter(sr.Parameter("radius", self.radius, sr.ParameterType.DOUBLE))
 
         current_pose = sr.CartesianPose("B", self.radius * np.random.rand(3, 1))
         for i in range(self.nb_steps):
diff --git a/python/test/state_representation/space/cartesian/test_cartesian_pose.py b/python/test/state_representation/space/cartesian/test_cartesian_pose.py
index 4274a8980..4b5ecec0f 100755
--- a/python/test/state_representation/space/cartesian/test_cartesian_pose.py
+++ b/python/test/state_representation/space/cartesian/test_cartesian_pose.py
@@ -15,15 +15,16 @@ def test_constructors(self):
         CartesianPose("A", "B")
         A = CartesianPose("A", np.array([1, 2, 3]), "B")
         self.assert_np_array_equal(A.get_position(), [1, 2, 3])
-        self.assert_np_array_equal(A.get_orientation(), [1, 0, 0, 0])
+        self.assert_np_array_equal(A.get_orientation().elements, [1, 0, 0, 0])
+        self.assert_np_array_equal(A.get_orientation_coefficients(), [1, 0, 0, 0])
 
         A = CartesianPose("A", np.array([1, 2, 3, 4]), "B")
         self.assert_np_array_equal(A.get_position(), [0, 0, 0])
-        self.assert_np_array_equal(A.get_orientation(), [1, 2, 3, 4] / np.linalg.norm([1, 2, 3, 4]))
+        self.assert_np_array_equal(A.get_orientation_coefficients(), [1, 2, 3, 4] / np.linalg.norm([1, 2, 3, 4]))
 
         A = CartesianPose("A", np.array([1, 2, 3]), np.array([1, 2, 3, 4]), "B")
         self.assert_np_array_equal(A.get_position(), [1, 2, 3])
-        self.assert_np_array_equal(A.get_orientation(), [1, 2, 3, 4] / np.linalg.norm([1, 2, 3, 4]))
+        self.assert_np_array_equal(A.get_orientation().elements, [1, 2, 3, 4] / np.linalg.norm([1, 2, 3, 4]))
 
     def test_copy(self):
         state = CartesianPose().Random("test")
diff --git a/python/test/state_representation/space/cartesian/test_cartesian_state.py b/python/test/state_representation/space/cartesian/test_cartesian_state.py
index 262c55f84..77b6dfdd8 100755
--- a/python/test/state_representation/space/cartesian/test_cartesian_state.py
+++ b/python/test/state_representation/space/cartesian/test_cartesian_state.py
@@ -2,6 +2,7 @@
 import copy
 
 import numpy as np
+from pyquaternion.quaternion import Quaternion
 from numpy.testing import assert_array_equal, assert_array_almost_equal
 from state_representation import State, CartesianState, StateType, CartesianStateVariable
 
@@ -25,6 +26,7 @@
     'get_linear_velocity',
     'get_name',
     'get_orientation',
+    'get_orientation_coefficients',
     'get_pose',
     'get_position',
     'get_reference_frame',
@@ -107,19 +109,19 @@ def test_constructors(self):
         empty1 = CartesianState()
         self.assertTrue(isinstance(empty1, State))
         self.assertEqual(type(empty1), CartesianState)
-        self.assertEqual(empty1.get_type(), StateType.CARTESIANSTATE)
+        self.assertEqual(empty1.get_type(), StateType.CARTESIAN_STATE)
         self.assert_name_empty_frame_equal(empty1, "", True, "world")
         self.assertAlmostEqual(np.linalg.norm(empty1.data()), 1)
 
         empty2 = CartesianState("test")
         self.assertEqual(type(empty1), CartesianState)
-        self.assertEqual(empty2.get_type(), StateType.CARTESIANSTATE)
+        self.assertEqual(empty2.get_type(), StateType.CARTESIAN_STATE)
         self.assert_name_empty_frame_equal(empty2, "test", True, "world")
         self.assertAlmostEqual(np.linalg.norm(empty2.data()), 1)
 
         empty3 = CartesianState("test", "reference")
         self.assertEqual(type(empty1), CartesianState)
-        self.assertEqual(empty3.get_type(), StateType.CARTESIANSTATE)
+        self.assertEqual(empty3.get_type(), StateType.CARTESIAN_STATE)
         self.assert_name_empty_frame_equal(empty3, "test", True, "reference")
         self.assertAlmostEqual(np.linalg.norm(empty3.data()), 1)
 
@@ -128,7 +130,7 @@ def test_identity_initialization(self):
         self.assertTrue(isinstance(identity, State))
         self.assertFalse(identity.is_empty())
         self.assertAlmostEqual(np.linalg.norm(identity.get_position()), 0)
-        self.assertAlmostEqual(np.linalg.norm(identity.get_orientation()), 1)
+        self.assertAlmostEqual(identity.get_orientation().norm, 1)
         self.assertAlmostEqual(identity.get_orientation()[0], 1)
         self.assertAlmostEqual(np.linalg.norm(identity.get_twist()), 0)
         self.assertAlmostEqual(np.linalg.norm(identity.get_acceleration()), 0)
@@ -139,7 +141,7 @@ def test_random_initialization(self):
         self.assertTrue(isinstance(random, State))
         self.assertFalse(random.is_empty())
         self.assertTrue(np.linalg.norm(random.get_position()) > 0)
-        self.assertAlmostEqual(np.linalg.norm(random.get_orientation()), 1)
+        self.assertAlmostEqual(random.get_orientation().norm, 1)
         [self.assertTrue(random.get_orientation()[i] != 0) for i in range(4)]
         self.assertTrue(np.linalg.norm(random.get_twist()) > 0)
         self.assertTrue(np.linalg.norm(random.get_acceleration()) > 0)
@@ -188,14 +190,20 @@ def test_get_set_fields(self):
         with self.assertRaises(RuntimeError):
             cs.set_position([1., 2., 3., 4.])
 
-        # orientation
+        # orientation coefficients
         orientation_vec = np.random.rand(4)
         orientation_vec = orientation_vec / np.linalg.norm(orientation_vec)
         cs.set_orientation(orientation_vec)
         [self.assertAlmostEqual(cs.get_orientation()[i], orientation_vec[i]) for i in range(4)]
-        # TODO what is an Eigen::Quaternion in Python ?
         with self.assertRaises(RuntimeError):
-            cs.set_orientation(position)
+            cs.set_orientation(orientation_vec[:3])
+
+        # orientation quaternion
+        quaternion = Quaternion.random()
+        cs.set_orientation(quaternion)
+        assert_array_almost_equal(cs.get_orientation().elements, quaternion.elements)
+        with self.assertRaises(ValueError):
+            cs.set_orientation(dict())
 
         matrix = cs.get_transformation_matrix()
         trans = matrix[:3, 3]
@@ -320,7 +328,7 @@ def test_norms(self):
         norms = cs.norms()
         self.assertEqual(len(norms), 8)
         self.assertAlmostEqual(norms[0], np.linalg.norm(cs.get_position()))
-        self.assertAlmostEqual(norms[1], np.linalg.norm(cs.get_orientation()))
+        self.assertAlmostEqual(norms[1], cs.get_orientation().norm)
         self.assertAlmostEqual(norms[2], np.linalg.norm(cs.get_linear_velocity()))
         self.assertAlmostEqual(norms[3], np.linalg.norm(cs.get_angular_velocity()))
         self.assertAlmostEqual(norms[4], np.linalg.norm(cs.get_linear_acceleration()))
diff --git a/python/test/state_representation/space/test_spatial_state.py b/python/test/state_representation/space/test_spatial_state.py
index e1cbd781e..0e3b0c3b3 100644
--- a/python/test/state_representation/space/test_spatial_state.py
+++ b/python/test/state_representation/space/test_spatial_state.py
@@ -21,14 +21,14 @@ def test_callable_methods(self):
             self.assertIn(expected, methods)
 
     def test_constructors(self):
-        state1 = SpatialState(StateType.JOINTSTATE)
-        self.assertEqual(state1.get_type(), StateType.JOINTSTATE)
+        state1 = SpatialState(StateType.JOINT_STATE)
+        self.assertEqual(state1.get_type(), StateType.JOINT_STATE)
         self.assertEqual(state1.get_name(), "")
         self.assertEqual(state1.get_reference_frame(), "world")
         self.assertTrue(state1.is_empty())
 
-        state2 = SpatialState(StateType.CARTESIANSTATE, "test", "robot", False)
-        self.assertEqual(state2.get_type(), StateType.CARTESIANSTATE)
+        state2 = SpatialState(StateType.CARTESIAN_STATE, "test", "robot", False)
+        self.assertEqual(state2.get_type(), StateType.CARTESIAN_STATE)
         self.assertEqual(state2.get_name(), "test")
         self.assertEqual(state2.get_reference_frame(), "robot")
         self.assertFalse(state2.is_empty())
@@ -40,7 +40,7 @@ def test_constructors(self):
         self.assertEqual(state2.is_empty(), state3.is_empty())
 
     def test_copy(self):
-        state = SpatialState(StateType.CARTESIANSTATE, "test", "robot", False)
+        state = SpatialState(StateType.CARTESIAN_STATE, "test", "robot", False)
         for state_copy in [copy.copy(state), copy.deepcopy(state)]:
             self.assertEqual(state.get_type(), state_copy.get_type())
             self.assertEqual(state.get_name(), state_copy.get_name())
@@ -48,7 +48,7 @@ def test_copy(self):
             self.assertEqual(state.is_empty(), state_copy.is_empty())
 
     def test_compatibility(self):
-        state1 = SpatialState(StateType.CARTESIANSTATE, "test", "robot", True)
+        state1 = SpatialState(StateType.CARTESIAN_STATE, "test", "robot", True)
         state2 = SpatialState(StateType.STATE, "test")
         self.assertFalse(state1.is_compatible(state2))
         state2.set_reference_frame("robot")
diff --git a/python/test/state_representation/test_parameters.py b/python/test/state_representation/test_parameters.py
index 6fb6ef6db..89f25ce0d 100755
--- a/python/test/state_representation/test_parameters.py
+++ b/python/test/state_representation/test_parameters.py
@@ -23,178 +23,193 @@ def joint_equal(self, joint1, joint2):
         self.assert_np_array_equal(joint1.data(), joint2.data())
 
     def test_param_invalid(self):
-        param = sr.Parameter("test", sr.StateType.CARTESIANSTATE)
-        self.assertRaises(ValueError, param.set_value, 1)
+        param = sr.Parameter("test", sr.ParameterType.STRING)
+        self.assertRaises(RuntimeError, param.set_value, 1)
 
     def test_param_copy(self):
-        param = sr.Parameter("test", 1, sr.StateType.PARAMETER_INT)
+        param = sr.Parameter("test", 1, sr.ParameterType.INT)
         self.assertEqual(param.get_name(), "test")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_INT)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.INT)
         self.assertEqual(param.get_value(), 1)
         param_copy = sr.Parameter(param)
         self.assertEqual(param_copy.get_name(), "test")
-        self.assertEqual(param_copy.get_type(), sr.StateType.PARAMETER_INT)
+        self.assertEqual(param_copy.get_parameter_type(), sr.ParameterType.INT)
         self.assertEqual(param_copy.get_value(), 1)
 
     def test_param_int(self):
-        param = sr.Parameter("int", sr.StateType.PARAMETER_INT)
+        param = sr.Parameter("int", sr.ParameterType.INT)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "int")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_INT)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.INT)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
         param.set_value(1)
         self.assertEqual(param.get_value(), 1)
-        param1 = sr.Parameter("int", 1, sr.StateType.PARAMETER_INT)
+        param1 = sr.Parameter("int", 1, sr.ParameterType.INT)
         self.assertEqual(param1.get_value(), 1)
 
     def test_param_int_array(self):
-        param = sr.Parameter("int_array", sr.StateType.PARAMETER_INT_ARRAY)
+        param = sr.Parameter("int_array", sr.ParameterType.INT_ARRAY)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "int_array")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_INT_ARRAY)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.INT_ARRAY)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
         values = [2, 3, 4]
         param.set_value(values)
         [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))]
-        param1 = sr.Parameter("int_array", values, sr.StateType.PARAMETER_INT_ARRAY)
+        param1 = sr.Parameter("int_array", values, sr.ParameterType.INT_ARRAY)
         [self.assertEqual(param1.get_value()[i], values[i]) for i in range(len(values))]
 
     def test_param_double(self):
-        param = sr.Parameter("double", sr.StateType.PARAMETER_DOUBLE)
+        param = sr.Parameter("double", sr.ParameterType.DOUBLE)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "double")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_DOUBLE)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.DOUBLE)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
         param.set_value(1.5)
         self.assertEqual(param.get_value(), 1.5)
-        param1 = sr.Parameter("double", 1.5, sr.StateType.PARAMETER_DOUBLE)
+        param1 = sr.Parameter("double", 1.5, sr.ParameterType.DOUBLE)
         self.assertEqual(param1.get_value(), 1.5)
 
     def test_param_double_array(self):
-        param = sr.Parameter("double_array", sr.StateType.PARAMETER_DOUBLE_ARRAY)
+        param = sr.Parameter("double_array", sr.ParameterType.DOUBLE_ARRAY)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "double_array")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_DOUBLE_ARRAY)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.DOUBLE_ARRAY)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
         values = [2.2, 3.3, 4.4]
         param.set_value(values)
         [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))]
-        param1 = sr.Parameter("double_array", values, sr.StateType.PARAMETER_DOUBLE_ARRAY)
+        param1 = sr.Parameter("double_array", values, sr.ParameterType.DOUBLE_ARRAY)
         [self.assertEqual(param1.get_value()[i], values[i]) for i in range(len(values))]
 
     def test_param_bool(self):
-        param = sr.Parameter("bool", sr.StateType.PARAMETER_BOOL)
+        param = sr.Parameter("bool", sr.ParameterType.BOOL)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "bool")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_BOOL)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.BOOL)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
         param.set_value(False)
         self.assertEqual(param.get_value(), False)
-        param1 = sr.Parameter("bool", False, sr.StateType.PARAMETER_BOOL)
+        param1 = sr.Parameter("bool", False, sr.ParameterType.BOOL)
         self.assertEqual(param1.get_value(), False)
 
     def test_param_bool_array(self):
-        param = sr.Parameter("bool_array", sr.StateType.PARAMETER_BOOL_ARRAY)
+        param = sr.Parameter("bool_array", sr.ParameterType.BOOL_ARRAY)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "bool_array")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_BOOL_ARRAY)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.BOOL_ARRAY)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
         values = [True, False, False]
         param.set_value(values)
         [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))]
-        param1 = sr.Parameter("bool_array", values, sr.StateType.PARAMETER_BOOL_ARRAY)
+        param1 = sr.Parameter("bool_array", values, sr.ParameterType.BOOL_ARRAY)
         [self.assertEqual(param1.get_value()[i], values[i]) for i in range(len(values))]
 
     def test_param_string(self):
-        param = sr.Parameter("string", sr.StateType.PARAMETER_STRING)
+        param = sr.Parameter("string", sr.ParameterType.STRING)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "string")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_STRING)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.STRING)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
         param.set_value("parameter")
         self.assertEqual(param.get_value(), "parameter")
-        param1 = sr.Parameter("string", "parameter", sr.StateType.PARAMETER_STRING)
+        param1 = sr.Parameter("string", "parameter", sr.ParameterType.STRING)
         self.assertEqual(param1.get_value(), "parameter")
 
     def test_param_string_array(self):
-        param = sr.Parameter("string_array", sr.StateType.PARAMETER_STRING_ARRAY)
+        param = sr.Parameter("string_array", sr.ParameterType.STRING_ARRAY)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "string_array")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_STRING_ARRAY)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.STRING_ARRAY)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
         values = ["test", "parameter", "bindings"]
         param.set_value(values)
         [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))]
-        param1 = sr.Parameter("string_array", values, sr.StateType.PARAMETER_STRING_ARRAY)
+        param1 = sr.Parameter("string_array", values, sr.ParameterType.STRING_ARRAY)
         [self.assertEqual(param1.get_value()[i], values[i]) for i in range(len(values))]
 
     def test_param_cartesian_state(self):
-        param = sr.Parameter("cartesian_state", sr.StateType.PARAMETER_CARTESIANSTATE)
+        param = sr.Parameter("cartesian_state", sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "cartesian_state")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_CARTESIANSTATE)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.CARTESIAN_STATE)
         values = sr.CartesianState.Random("test")
         param.set_value(values)
         self.cartesian_equal(param.get_value(), values)
-        param1 = sr.Parameter("cartesian_state", values, sr.StateType.PARAMETER_CARTESIANSTATE)
+        param1 = sr.Parameter("cartesian_state", values, sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE)
         self.cartesian_equal(param1.get_value(), values)
 
     def test_param_cartesian_pose(self):
-        param = sr.Parameter("cartesian_pose", sr.StateType.PARAMETER_CARTESIANPOSE)
+        param = sr.Parameter("cartesian_pose", sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "cartesian_pose")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_CARTESIANPOSE)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.CARTESIAN_POSE)
         values = sr.CartesianPose.Random("test")
         param.set_value(values)
         self.cartesian_equal(param.get_value(), values)
-        param1 = sr.Parameter("cartesian_pose", values, sr.StateType.PARAMETER_CARTESIANPOSE)
+        param1 = sr.Parameter("cartesian_pose", values, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE)
         self.cartesian_equal(param1.get_value(), values)
 
     def test_param_joint_state(self):
-        param = sr.Parameter("joint_state", sr.StateType.PARAMETER_JOINTSTATE)
+        param = sr.Parameter("joint_state", sr.ParameterType.STATE, sr.StateType.JOINT_STATE)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "joint_state")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_JOINTSTATE)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.JOINT_STATE)
         values = sr.JointState.Random("test", 3)
         param.set_value(values)
         self.joint_equal(param.get_value(), values)
-        param1 = sr.Parameter("joint_state", values, sr.StateType.PARAMETER_JOINTSTATE)
+        param1 = sr.Parameter("joint_state", values, sr.ParameterType.STATE, sr.StateType.JOINT_STATE)
         self.joint_equal(param1.get_value(), values)
 
     def test_param_joint_positions(self):
-        param = sr.Parameter("joint_positions", sr.StateType.PARAMETER_JOINTPOSITIONS)
+        param = sr.Parameter("joint_positions", sr.ParameterType.STATE, sr.StateType.JOINT_POSITIONS)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "joint_positions")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_JOINTPOSITIONS)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.JOINT_POSITIONS)
         values = sr.JointPositions.Random("test", 3)
         param.set_value(values)
         self.joint_equal(param.get_value(), values)
-        param1 = sr.Parameter("joint_positions", values, sr.StateType.PARAMETER_JOINTPOSITIONS)
+        param1 = sr.Parameter("joint_positions", values, sr.ParameterType.STATE, sr.StateType.JOINT_POSITIONS)
         self.joint_equal(param1.get_value(), values)
 
     def test_param_ellipsoid(self):
-        param = sr.Parameter("ellipse", sr.StateType.PARAMETER_ELLIPSOID)
+        param = sr.Parameter("ellipse", sr.ParameterType.STATE, sr.StateType.GEOMETRY_ELLIPSOID)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "ellipse")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_ELLIPSOID)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.GEOMETRY_ELLIPSOID)
         values = sr.Ellipsoid("test")
         param.set_value(values)
         self.assertTrue(param.get_value().get_name(), values.get_name())
-        param1 = sr.Parameter("ellipse", values, sr.StateType.PARAMETER_ELLIPSOID)
+        param1 = sr.Parameter("ellipse", values, sr.ParameterType.STATE, sr.StateType.GEOMETRY_ELLIPSOID)
         self.assertTrue(param1.get_value().get_name(), values.get_name())
 
     def test_param_matrix(self):
-        param = sr.Parameter("matrix", sr.StateType.PARAMETER_MATRIX)
+        param = sr.Parameter("matrix", sr.ParameterType.MATRIX)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "matrix")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_MATRIX)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.MATRIX)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
         values = np.random.rand(3, 2)
         param.set_value(values)
         self.assert_np_array_equal(param.get_value(), values)
-        param1 = sr.Parameter("matrix", values, sr.StateType.PARAMETER_MATRIX)
+        param1 = sr.Parameter("matrix", values, sr.ParameterType.MATRIX)
         self.assert_np_array_equal(param1.get_value(), values)
 
     def test_param_vector(self):
-        param = sr.Parameter("vector", sr.StateType.PARAMETER_VECTOR)
+        param = sr.Parameter("vector", sr.ParameterType.VECTOR)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "vector")
-        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_VECTOR)
+        self.assertEqual(param.get_parameter_type(), sr.ParameterType.VECTOR)
+        self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
         values = np.random.rand(3)
         param.set_value(values)
         self.assert_np_array_equal(param.get_value(), values)
-        param1 = sr.Parameter("vector", values, sr.StateType.PARAMETER_VECTOR)
+        param1 = sr.Parameter("vector", values, sr.ParameterType.VECTOR)
         self.assert_np_array_equal(param1.get_value(), values)
 
     def param_map_equal(self, param_dict, param_map):
@@ -218,9 +233,9 @@ def simple_param_equal(param1, param2):
 
 
     def test_param_map(self):
-        param_dict = {"int": sr.Parameter("int", 1, sr.StateType.PARAMETER_INT),
-                      "double": sr.Parameter("double", 2.2, sr.StateType.PARAMETER_DOUBLE),
-                      "string": sr.Parameter("string", "test", sr.StateType.PARAMETER_STRING)}
+        param_dict = {"int": sr.Parameter("int", 1, sr.ParameterType.INT),
+                      "double": sr.Parameter("double", 2.2, sr.ParameterType.DOUBLE),
+                      "string": sr.Parameter("string", "test", sr.ParameterType.STRING)}
         param_list = [param for name, param in param_dict.items()]
 
         m = sr.ParameterMap()
@@ -230,7 +245,7 @@ def test_param_map(self):
 
         m = sr.ParameterMap()
         for name, param in param_dict.items():
-            m.set_parameter_value(param.get_name(), param.get_value(), param.get_type())
+            m.set_parameter_value(param.get_name(), param.get_value(), param.get_parameter_type(), param.get_parameter_state_type())
         self.param_map_equal(param_dict, m)
 
         m = sr.ParameterMap()
diff --git a/python/test/state_representation/test_state.py b/python/test/state_representation/test_state.py
index 66ce225ce..0b80e54d9 100644
--- a/python/test/state_representation/test_state.py
+++ b/python/test/state_representation/test_state.py
@@ -34,20 +34,20 @@ def test_constructors(self):
         self.assertEqual(empty1.get_name(), "")
         self.assertTrue(empty1.is_empty())
 
-        empty2 = State(StateType.JOINTSTATE)
-        self.assertEqual(empty2.get_type(), StateType.JOINTSTATE)
+        empty2 = State(StateType.JOINT_STATE)
+        self.assertEqual(empty2.get_type(), StateType.JOINT_STATE)
         self.assertEqual(empty2.get_name(), "")
         self.assertTrue(empty2.is_empty())
 
-        empty3 = State(StateType.CARTESIANSTATE, "test", True)
-        self.assertEqual(empty3.get_type(), StateType.CARTESIANSTATE)
+        empty3 = State(StateType.CARTESIAN_STATE, "test", True)
+        self.assertEqual(empty3.get_type(), StateType.CARTESIAN_STATE)
         self.assertEqual(empty3.get_name(), "test")
         self.assertTrue(empty3.is_empty())
         empty3.set_filled()
         self.assertFalse(empty3.is_empty())
 
         state = State(empty3)
-        self.assertEqual(state.get_type(), StateType.CARTESIANSTATE)
+        self.assertEqual(state.get_type(), StateType.CARTESIAN_STATE)
         self.assertEqual(state.get_name(), "test")
         self.assertFalse(state.is_empty())
         state.set_empty()
diff --git a/python/test/test_clproto.py b/python/test/test_clproto.py
index b1dafa672..75a3a87f7 100755
--- a/python/test/test_clproto.py
+++ b/python/test/test_clproto.py
@@ -149,47 +149,47 @@ def param_encode_decode_tester(self, obj1, message_type):
             self.assertEqual(obj1.get_value(), obj2.get_value())
 
     def test_encode_decode_param_int(self):
-        obj1 = sr.Parameter("int", 1, sr.StateType.PARAMETER_INT)
+        obj1 = sr.Parameter("int", 1, sr.ParameterType.INT)
         self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.INT)
 
     def test_encode_decode_param_int_array(self):
-        obj1 = sr.Parameter("int", [1, 2, 3], sr.StateType.PARAMETER_INT_ARRAY)
+        obj1 = sr.Parameter("int", [1, 2, 3], sr.ParameterType.INT_ARRAY)
         self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.INT_ARRAY)
 
     def test_encode_decode_param_double(self):
-        obj1 = sr.Parameter("double", 1.1, sr.StateType.PARAMETER_DOUBLE)
+        obj1 = sr.Parameter("double", 1.1, sr.ParameterType.DOUBLE)
         self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.DOUBLE)
 
     def test_encode_decode_param_double_array(self):
-        obj1 = sr.Parameter("double", [1.1, 2.2, 3.3], sr.StateType.PARAMETER_DOUBLE_ARRAY)
+        obj1 = sr.Parameter("double", [1.1, 2.2, 3.3], sr.ParameterType.DOUBLE_ARRAY)
         self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.DOUBLE_ARRAY)
 
     def test_encode_decode_param_bool(self):
-        obj1 = sr.Parameter("bool", True, sr.StateType.PARAMETER_BOOL)
+        obj1 = sr.Parameter("bool", True, sr.ParameterType.BOOL)
         self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.BOOL)
 
     def test_encode_decode_param_bool_array(self):
-        obj1 = sr.Parameter("bool", [True, False, True], sr.StateType.PARAMETER_BOOL_ARRAY)
+        obj1 = sr.Parameter("bool", [True, False, True], sr.ParameterType.BOOL_ARRAY)
         self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.BOOL_ARRAY)
 
     def test_encode_decode_param_string(self):
-        obj1 = sr.Parameter("string", "test", sr.StateType.PARAMETER_STRING)
+        obj1 = sr.Parameter("string", "test", sr.ParameterType.STRING)
         self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.STRING)
 
     def test_encode_decode_param_string_array(self):
-        obj1 = sr.Parameter("string", ["1", "2", "3"], sr.StateType.PARAMETER_STRING_ARRAY)
+        obj1 = sr.Parameter("string", ["1", "2", "3"], sr.ParameterType.STRING_ARRAY)
         self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.STRING_ARRAY)
 
     def test_encode_decode_param_vector(self):
-        obj1 = sr.Parameter("vector", np.random.rand(3), sr.StateType.PARAMETER_VECTOR)
+        obj1 = sr.Parameter("vector", np.random.rand(3), sr.ParameterType.VECTOR)
         self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.VECTOR)
 
     def test_encode_decode_param_matrix(self):
-        obj1 = sr.Parameter("matrix", np.random.rand(3, 2), sr.StateType.PARAMETER_MATRIX)
+        obj1 = sr.Parameter("matrix", np.random.rand(3, 2), sr.ParameterType.MATRIX)
         self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.MATRIX)
 
     def test_encode_decode_param_invalid(self):
-        obj = sr.Parameter("cartesian_state", sr.StateType.PARAMETER_CARTESIANSTATE)
+        obj = sr.Parameter("cartesian_state", sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE)
         self.assertRaises(ValueError, clproto.encode, obj, clproto.MessageType.PARAMETER_MESSAGE)
 
 if __name__ == '__main__':
diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt
index e784fd7aa..706659025 100644
--- a/source/CMakeLists.txt
+++ b/source/CMakeLists.txt
@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(control_libraries VERSION 5.2.0)
+project(control_libraries VERSION 6.0.0)
 
 # Build options
 option(BUILD_TESTING "Build all tests." OFF)
@@ -39,6 +39,10 @@ else()
   find_package(GTest QUIET)
 endif()
 
+if(EXPERIMENTAL_FEATURES)
+  add_compile_definitions(EXPERIMENTAL_FEATURES)
+endif()
+
 add_subdirectory(state_representation)
 list(APPEND INSTALL_INTERFACE_LINK_LIBRARIES state_representation)
 get_target_property(TARGET_INTERFACE_LINK_LIBRARIES state_representation INTERFACE_LINK_LIBRARIES)
diff --git a/source/README.md b/source/README.md
index 301f090a9..690fed564 100644
--- a/source/README.md
+++ b/source/README.md
@@ -2,7 +2,7 @@
 
 ## `state_representation`
 
-This library provides a set of classes to represent **states** in **Cartesian** or **joint** spaces.
+This library provides a set of classes to represent **states** in **Cartesian** or **joint** space.
 The classes define and combine variables such as position, velocity, acceleration and force into
 a consistent internal representation used across the other libraries.
 
@@ -188,29 +188,3 @@ lines to the CMakeLists file:
 list(APPEND CMAKE_PREFIX_PATH /opt/openrobots)
 include_directories(/opt/openrobots/include)
 ```
-
-## Troubleshooting
-
-This section lists common problems that might come up when using the `control-libraries` modules.
-
-### Boost container limit compile error in ROS
-When using the `robot_model` module in ROS and trying to `catkin_make` the workspace, it might produce the following error:
-```bash
-/opt/openrobots/include/pinocchio/container/boost-container-limits.hpp:29:7: error: #error "BOOST_MPL_LIMIT_LIST_SIZE value is lower than the value of PINOCCHIO_BOOST_MPL_LIMIT_CONTAINER_SIZE"
-#23 2.389    29 |     # error "BOOST_MPL_LIMIT_LIST_SIZE value is lower than the value of PINOCCHIO_BOOST_MPL_LIMIT_CONTAINER_SIZE"
-```
-In order to avoid this error and successfully `catkin_make` the workspace, make sure that the `CMakeList.txt` of the ROS
-package contains all the necessary directives, i.e. on top of
-
-```cmake
-list(APPEND CMAKE_PREFIX_PATH /opt/openrobots)
-include_directories(/opt/openrobots/include)
-find_package(control_libraries REQUIRED)
-```
-it should also have
-```bash
-find_package(Boost REQUIRED COMPONENTS system)
-add_compile_definitions(BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS)
-add_compile_definitions(BOOST_MPL_LIMIT_LIST_SIZE=30)
-```
-For a comprehensive example, please check the [`CMakeLists.txt` of the ROS demos](../demos/ros_examples/CMakeLists.txt).
diff --git a/source/controllers/README.md b/source/controllers/README.md
index e7a3a97c6..f5c37f602 100644
--- a/source/controllers/README.md
+++ b/source/controllers/README.md
@@ -3,7 +3,14 @@
 This library introduces a set of controllers to be used in robotic control-loop schemes. 
 
 All controllers have a common interface inheriting from the `IController<S>` class, which is templated to 
-operate in a particular space `S`, namely joint space or Cartesian space.
+operate in a particular space `S`, namely Cartesian or joint space.
+
+## Table of contents
+* [Constructing a controller](#constructing-a-controller)
+* [Using a controller](#using-a-controller)
+  * [Parameters](#parameters)
+  * [Compute command](#compute-command)
+* [Developing a new controller](#developing-a-new-controller)
 
 ## Constructing a controller
 
@@ -201,7 +208,7 @@ protected:
 // MyCartesianController.cpp
 #include "MyCartesianController.hpp"
 
-#include "controllers/exceptions/InvalidParameterException.hpp"
+#include "state_representation/exceptions/InvalidParameterException.hpp"
 
 using namespace state_representation;
 
diff --git a/source/controllers/include/controllers/impedance/Impedance.hpp b/source/controllers/include/controllers/impedance/Impedance.hpp
index 02bee783c..5cd033a3c 100644
--- a/source/controllers/include/controllers/impedance/Impedance.hpp
+++ b/source/controllers/include/controllers/impedance/Impedance.hpp
@@ -118,6 +118,10 @@ void Impedance<S>::validate_and_set_parameter(
   } else if (parameter->get_name() == "force_limit") {
     auto limit_matrix = this->gain_matrix_from_parameter(parameter);
     this->force_limit_->set_value(limit_matrix.diagonal());
+  } else {
+    throw state_representation::exceptions::InvalidParameterException(
+        "No parameter with name '" + parameter->get_name() + "' found"
+    );
   }
 }
 
@@ -126,10 +130,10 @@ Eigen::MatrixXd Impedance<S>::gain_matrix_from_parameter(
     const std::shared_ptr<state_representation::ParameterInterface>& parameter
 ) {
   Eigen::MatrixXd matrix;
-  if (parameter->get_type() == state_representation::StateType::PARAMETER_DOUBLE) {
+  if (parameter->get_parameter_type() == state_representation::ParameterType::DOUBLE) {
     auto gain = std::static_pointer_cast<state_representation::Parameter<double>>(parameter);
     matrix = gain->get_value() * Eigen::MatrixXd::Identity(this->dimensions_, this->dimensions_);
-  } else if (parameter->get_type() == state_representation::StateType::PARAMETER_DOUBLE_ARRAY) {
+  } else if (parameter->get_parameter_type() == state_representation::ParameterType::DOUBLE_ARRAY) {
     auto gain = std::static_pointer_cast<state_representation::Parameter<std::vector<double>>>(parameter);
     if (gain->get_value().size() != this->dimensions_) {
       throw state_representation::exceptions::IncompatibleSizeException(
@@ -138,7 +142,7 @@ Eigen::MatrixXd Impedance<S>::gain_matrix_from_parameter(
     }
     Eigen::VectorXd diagonal = Eigen::VectorXd::Map(gain->get_value().data(), this->dimensions_);
     matrix = diagonal.asDiagonal();
-  } else if (parameter->get_type() == state_representation::StateType::PARAMETER_VECTOR) {
+  } else if (parameter->get_parameter_type() == state_representation::ParameterType::VECTOR) {
     auto gain = std::static_pointer_cast<state_representation::Parameter<Eigen::VectorXd>>(parameter);
     if (gain->get_value().size() != this->dimensions_) {
       throw state_representation::exceptions::IncompatibleSizeException(
@@ -147,7 +151,7 @@ Eigen::MatrixXd Impedance<S>::gain_matrix_from_parameter(
     }
     Eigen::VectorXd diagonal = gain->get_value();
     matrix = diagonal.asDiagonal();
-  } else if (parameter->get_type() == state_representation::StateType::PARAMETER_MATRIX) {
+  } else if (parameter->get_parameter_type() == state_representation::ParameterType::MATRIX) {
     auto gain = std::static_pointer_cast<state_representation::Parameter<Eigen::MatrixXd>>(parameter);
     if (gain->get_value().rows() != this->dimensions_ || gain->get_value().cols() != this->dimensions_) {
       auto dim = std::to_string(this->dimensions_);
diff --git a/source/controllers/src/impedance/CompliantTwist.cpp b/source/controllers/src/impedance/CompliantTwist.cpp
index 920aa1a39..fe8ed480e 100644
--- a/source/controllers/src/impedance/CompliantTwist.cpp
+++ b/source/controllers/src/impedance/CompliantTwist.cpp
@@ -79,7 +79,7 @@ CartesianState CompliantTwist::compute_command(
 void CompliantTwist::validate_and_set_parameter(
     const std::shared_ptr<state_representation::ParameterInterface>& parameter
 ) {
-  if (parameter->get_type() != StateType::PARAMETER_DOUBLE) {
+  if (parameter->get_parameter_type() != ParameterType::DOUBLE) {
     throw state_representation::exceptions::InvalidParameterException(
         "Parameter " + parameter->get_name() + " must be a double");
   }
@@ -92,6 +92,10 @@ void CompliantTwist::validate_and_set_parameter(
     this->set_angular_stiffness(value);
   } else if (parameter->get_name() == "angular_damping") {
     this->set_angular_damping(value);
+  } else {
+    throw state_representation::exceptions::InvalidParameterException(
+        "No parameter with name '" + parameter->get_name() + "' found"
+    );
   }
 }
 
diff --git a/source/dynamical_systems/README.md b/source/dynamical_systems/README.md
index 5815610d6..4aeeefa37 100644
--- a/source/dynamical_systems/README.md
+++ b/source/dynamical_systems/README.md
@@ -3,11 +3,19 @@
 This library provides a set of classes to represent **Dynamical Systems**: 
 functions which map a state to a state derivative.
 
+All controllers have a common interface inheriting from the `IDynamicalSystem<S>` class, which is templated to operate
+in a particular space `S`, namely Cartesian or joint space.
+
 ## Table of contents:
-* [Base DynamicalSystem](#base-dynamicalsystem)
+* [Constructing a dynamical system](#constructing-a-dynamical-system)
+* [Using a dynamical system](#using-a-dynamical-system)
+  * [Parameters](#parameters)
+  * [Evaluate](#evaluate)
+* [Developing a new dynamical system](#developing-a-new-dynamical-system)
+* [About](#about)
   * [Base frame](#base-frame)
   * [Reference frames](#reference-frames)
-* [Linear](#point-attractor)
+* [Point attractor](#point-attractor)
   * [Configuring the Linear DS](#configuring-the-point-attractor-ds)
   * [Evaluating the Linear DS](#evaluating-the-point-attractor-ds)
 * [Circular](#circular)
@@ -15,53 +23,191 @@ functions which map a state to a state derivative.
 * [Ring](#ring)
   * [Configuring the Ring DS](#configuring-the-ring-ds)
 
-## Base DynamicalSystem
+## Constructing a dynamical system
 
-The base class IDynamicalSystem defines the main public interface pattern which the derived classes follow.
- 
-It is templated for any state type, though the intention is to use the **State Representation** library
-(for example, `CartesianState` or `JointState`).
+The `DynamicalSystemFactory<S>` provides construction helpers for dynamical systems using a factory pattern. Specific
+dynamical systems are created and injected into a common `shared_ptr<IDynamicalSystem<S>>`.
+
+The `CartesianDynamicalSystemFactory` and `JointDynamicalSystemFactory` are shortcuts for the Cartesian and
+joint space dynamical system factories.
+
+The factory provides a static function `create_dynamical_system`, which can take a number of inputs. The first input
+argument is always the controller type (defined in `dynamical_systems/DynamicalSystemType.hpp`).
 
-The main function is:
 ```c++
-template<class S>
-S IDynamicalSystem<S>::evaluate(const S& state) const
+#include "dynamical_systems/DynamicalSystemFactory.hpp"
+
+using namespace dynamical_systems;
+using namespace state_representation;
+
+// create a Cartesian dynamical system
+std::shared_ptr<IDynamicalSystem<CartesianState>> cart_ds;
+cart_ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR);
+
+// create a Cartesian dynamical system using "auto" to avoid verbose typing
+auto cart_ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR);
 ```
-where `S` is the state representation type.
-The purpose of the evaluate function is to calculate a state derivative from a state.
-The current implementations support calculating a velocity from a position in either joint space or Cartesian space,
-though higher derivatives and other space types can be extended in a similar fashion.
 
-In the case of a `CartesianState`, only the position and orientation is evaluated, and the returned object
-updates only the linear and angular velocity properties.
-In the case of `JointState`, only the joint position is evaluated, and the returned object contains only the
-joint velocity.
+Initial dynamical system parameters can be passed to `create_dynamical_system` as a list. See the next section for more
+details on parameter configuration.
 
-### Base frame
+```c++
+#include "dynamical_systems/DynamicalSystemFactory.hpp"
+#include "state_representation/parameters/Parameter.hpp"
 
-The *DynamicalSystem* base class has a private `base_frame` property, which can be thought of as the DS origin. 
-The functions `get_base_frame()` and `set_base_frame(const S& state)` can be used to access or modify this base frame.
+using namespace dynamical_systems;
+using namespace state_representation;
 
-The `DynamicalSystem<CartesianState>` can be constructed with a `state` to set the base frame, or with string frame name. In 
-the latter case the base frame is set as a null / identity frame with the specified name.
-For example, `DynamicalSystem<CartesianState>("base")` will create a dynamical system with the null base frame "base",
-expressed in its own frame "base".
+std::list<std::shared_ptr<ParameterInterface>> parameters;
+parameters.emplace_back(make_shared_parameter("attractor", CartesianPose::Random("target")));
+parameters.emplace_back(make_shared_parameter("gain", 5.0));
 
-The `DynamicalSystem<JointState>` can only be constructed with a `state` to set the base frame. Whilst the term 
-*base frame* makes more sense for a `CartesianState` DS, it rather refers to a specific robot with corresponding 
-joint names in the case of a `JointState` DS.
+auto cart_ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR, parameters);
+```
+
+## Using a dynamical system
+
+The dynamical system factory returns a base class pointer `shared_ptr<IDynamicalSystem<S>>` for the chosen state type
+`S`, pointing to an instance of a specific derived dynamical system. This allows all dynamical systems to share the same
+consistent interface for usage and configuration.
+
+### Parameters
+
+Dynamical systems support custom parametrization through the base interface using `state_representation::ParameterMap`
+methods:
+
+- `get_parameters()`
+- `get_parameter(name)`
+- `get_parameter_value<T>(name)`
+- `set_parameters(parameters)`
+- `set_parameter(parameter)`
+- `set_parameter_value(name, value)`
+
+These methods can be used after construction to get or set dynamical system parameters. Refer to the documentation
+on `dynamical_systems::IDynamicalSystem<S>` and `state_representation::ParameterMap` for more
+information.
+
+### Evaluate
+
+The main purpose of each dynamical system is the `evaluate` function, which calculates the state derivative from the
+current state. For example, the point attractor dynamical system takes the difference between the current pose and
+the desired pose and multiplies it by a linear gain to compute the desired twist.
+
+```c++
+#include "dynamical_systems/DynamicalSystemFactory.hpp"
+#include "state_representation/space/cartesian/CartesianPose.hpp"
+
+using namespace dynamical_systems;
+
+std::list<std::shared_ptr<ParameterInterface>> parameters;
+parameters.emplace_back(make_shared_parameter("attractor", CartesianPose::Random("target")));
+parameters.emplace_back(make_shared_parameter("gain", 5.0));
+
+auto cart_ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR, parameters);
+auto current_state = CartesianPose::Random("state");
+
+// compute the twist
+auto desired_twist = cart_ds->evaluate(current_state);
+```
 
-In most cases, the constructor for the base DynamicalSystem should not be used directly,
-and rather the derived DS classes should construct the base accordingly.
+## Developing a new dynamical system
 
-However, the `set_base_frame()` method remains useful in combination with derived classes.
+To implement a new dynamical system, you need to create a class that derives from the `IDynamicalSystem` base class or
+any derived dynamical system such as `PointAttractor`. This class can be templated to accept different input spaces
+(e.g. `CartesianState` or `JointState`) or specify the desired input state.
+
+The derived dynamical system should override `compute_dynamics` to produce some custom behaviour. in addition, if the dynamical
+system has any parameters, you should:
+- Add parameter pointers as class properties
+- Initialize and declare parameters in the constructor
+- Override the `is_compatible` method, if necessary
+- Override the protected `validate_and_set_parameter` method
+
+```c++
+// MyCartesianDS.hpp
+#include "dynamical_systems/IDynamicalSystem.hpp"
+#include "state_representation/space/cartesian/CartesianState.hpp"
+
+class MyCartesianDS : public dynamical_systems::IDynamicalSystem<state_representation::CartesianState> {
+public:
+  // initialize and declare parameters
+  MyCartesianDS();
+
+protected:
+  // override this method to implement custom logic
+  state_representation::CartesianState compute_dynamics(
+      const state_representation::CartesianState& state
+  ) const override;
+  
+  // override this method to update dynamical system configurations when a parameter is modified
+  void validate_and_set_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter) override;
+  
+  // add any additional parameters as class properties
+  std::shared_ptr<state_representation::Parameter<int>> foo_;
+  std::shared_ptr<state_representation::Parameter<double>> bar_;
+};
+```
+
+```c++
+// MyCartesianDS.cpp
+#include "MyCartesianDS.hpp"
+
+#include "state_representation/exceptions/InvalidParameterException.hpp"
+
+using namespace state_representation;
+
+namespace dynamical_systems {
+
+// initialize parameters
+MyCartesianDS::MyCartesianDS() :
+    foo_(make_shared_parameter<int>(1)),
+    bar_(make_shared_parameter<double>(2.0)) {
+  // "declare" parameters by inserting them into the parameter list with an associated name
+  this->parameters_.insert(std::make_pair("foo", foo_));
+  this->parameters_.insert(std::make_pair("bar", bar_));
+}
+
+// implement the command logic
+CartesianState MyCartesianDS::compute_dynamics(const CartesianState& state) const {
+    CartesianState twist = ...;
+    return twist;
+}
+
+// implement parameter validation
+void MyCartesianController::validate_and_set_parameter(
+    const std::shared_ptr<state_representation::ParameterInterface>& parameter
+) {
+  if (parameter->get_name() == "foo") {
+    auto value = std::static_pointer_cast<state_representation::Parameter<int>>(parameter);
+    // if a parameter value is not supported by the dynamical system, throw an InvalidParameterException
+    if (value < 0 || value > 10) {
+      throw exceptions::InvalidParameterException("Parameter foo must be in range [0 - 10]");
+    }
+    this->foo_->set_value(value);
+  } else if (parameter->get_name() == "bar") {
+    this->bar_->set_value(std::static_pointer_cast<state_representation::Parameter<double>>(parameter));
+  }
+}
+
+}
+```
+
+## About
+
+### Base frame
+
+The `IDynamicalSystem` base class has a private `base_frame` property, which can be thought of as the DS origin. 
+The functions `get_base_frame()` and `set_base_frame(const S& state)` can be used to access or modify this base frame.
+
+Whilst the term *base frame* makes more sense for a `CartesianState` DS, it rather refers to a specific robot with corresponding 
+joint names in the case of a `JointState` DS.
 
 ### Reference frames
 
 The following section applies to derived DS classes using the `CartesianState` type. The 
 [Point Attractor DS](#point-attractor) will be used as an example.
 
-The `evaluate()` function will always return a twist expressed in the same reference frame as the input state,
+The `evaluate` function will always return a twist expressed in the same reference frame as the input state,
 provided that the input state is compatible with the DS.
 
 The input state must be expressed in one of two supported reference frames:
@@ -72,9 +218,7 @@ The following snippet illustrates the difference in these two options.
 ```c++
 // create a point attractor DS with attractor B in frame A
 state_representation::CartesianState BinA = state_representation::CartesianState::Identity("B", "A");
-auto ds = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR
-);
+auto ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR);
 ds->set_attractor_value("attractor", BinA);
 
 ds->get_parameter_value("attractor").get_name();             // "B"
@@ -134,9 +278,7 @@ state_representation::CartesianState EE("end_effector", "robot");
 state_representation::CartesianState attractor = state_representation::CartesianState::Random("attractor", "task");
 state_representation::CartesianState taskInRobot("task", "robot");
 
-auto ds = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR
-);
+auto ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR);
 ds->set_attractor_value("attractor", attractor);
 
 // control loop
@@ -168,9 +310,7 @@ from the attractor. It is currently implemented for the `CartesianState` and `Jo
 
 ```c++
 // construction with the DS factory
-auto ds = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-    DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR
-    );
+auto ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR);
 ```
 
 ### Configuring the Point Attractor DS
@@ -203,9 +343,7 @@ ds->set_parameter_value("attractor", csB);
 To get the velocity from a state, simply call the `evaluate()` function.
 
 ```c++
-auto ds = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR
-);
+auto ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR);
 state_representation::CartesianState csA = state_representation::CartesianState::Identity("A");
 ds->set_parameter_value("attractor", csA);
 
@@ -237,9 +375,7 @@ The internal representation of the limit cycle is a `state_representation::Ellip
 
 ```c++
 // construction with the DS factory
-auto ds = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::CIRCULAR
-);
+auto ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::CIRCULAR);
 ```
 
 ### Configuring the Circular DS
@@ -281,9 +417,7 @@ It only supports the `CartesianState` type, and always acts in a circular ring.
 
 ```c++
 // construction with the DS factory
-auto ds = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::RING
-);
+auto ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::RING);
 ```
 
 ### Configuring the Ring DS
diff --git a/source/dynamical_systems/include/dynamical_systems/Circular.hpp b/source/dynamical_systems/include/dynamical_systems/Circular.hpp
index 235c88e42..cda113200 100644
--- a/source/dynamical_systems/include/dynamical_systems/Circular.hpp
+++ b/source/dynamical_systems/include/dynamical_systems/Circular.hpp
@@ -17,6 +17,12 @@ class Circular : public IDynamicalSystem<state_representation::CartesianState> {
    */
   Circular();
 
+  /**
+   * @brief Constructor from an initial parameter list
+   * @param parameters A parameter list containing initial parameters
+   */
+  explicit Circular(const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters);
+
   /**
    * @copydoc IDynamicalSystem::set_parameter
    */
diff --git a/source/dynamical_systems/include/dynamical_systems/DynamicalSystemFactory.hpp b/source/dynamical_systems/include/dynamical_systems/DynamicalSystemFactory.hpp
index 77db8ce0a..c3ea3652b 100644
--- a/source/dynamical_systems/include/dynamical_systems/DynamicalSystemFactory.hpp
+++ b/source/dynamical_systems/include/dynamical_systems/DynamicalSystemFactory.hpp
@@ -1,7 +1,11 @@
 #pragma once
 
+#include "dynamical_systems/DynamicalSystemType.hpp"
 #include "dynamical_systems/IDynamicalSystem.hpp"
 
+#include "state_representation/space/cartesian/CartesianState.hpp"
+#include "state_representation/space/joint/JointState.hpp"
+
 namespace dynamical_systems {
 
 /**
@@ -12,18 +16,31 @@ namespace dynamical_systems {
 template<class S>
 class DynamicalSystemFactory {
 public:
-  /**
-   * @brief Enumeration of the implemented dynamical systems
-   */
-  enum class DYNAMICAL_SYSTEM {
-    NONE, CIRCULAR, POINT_ATTRACTOR, RING
-  };
-
   /**
    * @brief Create a dynamical system of the desired type.
-   * @param type Underlying state type of the dynamical system
+   * @param type The type of dynamical system
    * @return The shared pointer to the dynamical system
    */
-  static std::shared_ptr<IDynamicalSystem<S>> create_dynamical_system(DYNAMICAL_SYSTEM type);
+  static std::shared_ptr<IDynamicalSystem<S>> create_dynamical_system(DYNAMICAL_SYSTEM_TYPE type);
+
+  /**
+ * @brief Create a dynamical system of the desired type with initial parameters.
+ * @param type The type of dynamical system
+ * @param parameters A list of parameters to set on the dynamical system
+ * @return The shared pointer to the dynamical system
+ */
+  static std::shared_ptr<IDynamicalSystem<S>> create_dynamical_system(
+      DYNAMICAL_SYSTEM_TYPE type, const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters
+  );
 };
+
+template<class S>
+std::shared_ptr<IDynamicalSystem<S>> DynamicalSystemFactory<S>::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE type) {
+  std::list<std::shared_ptr<state_representation::ParameterInterface>> parameters;
+  return DynamicalSystemFactory<S>::create_dynamical_system(type, parameters);
+}
+
+typedef DynamicalSystemFactory<state_representation::CartesianState> CartesianDynamicalSystemFactory;
+typedef DynamicalSystemFactory<state_representation::JointState> JointDynamicalSystemFactory;
+
 }// namespace dynamical_systems
diff --git a/source/dynamical_systems/include/dynamical_systems/DynamicalSystemType.hpp b/source/dynamical_systems/include/dynamical_systems/DynamicalSystemType.hpp
new file mode 100644
index 000000000..bdc367b15
--- /dev/null
+++ b/source/dynamical_systems/include/dynamical_systems/DynamicalSystemType.hpp
@@ -0,0 +1,15 @@
+#pragma once
+
+namespace dynamical_systems {
+
+/**
+ * @brief Enumeration of the implemented dynamical systems
+ */
+enum class DYNAMICAL_SYSTEM_TYPE {
+  NONE,
+  CIRCULAR,
+  POINT_ATTRACTOR,
+  RING
+};
+
+}
\ No newline at end of file
diff --git a/source/dynamical_systems/include/dynamical_systems/PointAttractor.hpp b/source/dynamical_systems/include/dynamical_systems/PointAttractor.hpp
index 7e83b8fcf..29c68438f 100644
--- a/source/dynamical_systems/include/dynamical_systems/PointAttractor.hpp
+++ b/source/dynamical_systems/include/dynamical_systems/PointAttractor.hpp
@@ -18,6 +18,12 @@ class PointAttractor : public IDynamicalSystem<S> {
    */
   PointAttractor();
 
+  /**
+   * @brief Constructor from an initial parameter list
+   * @param parameters A parameter list containing initial attractor and gain values
+   */
+  explicit PointAttractor(const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters);
+
   /**
    * @copydoc IDynamicalSystem::set_base_frame
    */
@@ -54,4 +60,12 @@ class PointAttractor : public IDynamicalSystem<S> {
   std::shared_ptr<state_representation::Parameter<S>> attractor_; ///< attractor of the dynamical system in the space
   std::shared_ptr<state_representation::Parameter<Eigen::MatrixXd>> gain_; ///< gain associate to the system
 };
+
+template<class S>
+PointAttractor<S>::PointAttractor(
+    const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters
+) : PointAttractor<S>() {
+  this->set_parameters(parameters);
+}
+
 }// namespace dynamical_systems
diff --git a/source/dynamical_systems/include/dynamical_systems/Ring.hpp b/source/dynamical_systems/include/dynamical_systems/Ring.hpp
index b62e37ace..6297bf7b9 100644
--- a/source/dynamical_systems/include/dynamical_systems/Ring.hpp
+++ b/source/dynamical_systems/include/dynamical_systems/Ring.hpp
@@ -20,6 +20,12 @@ class Ring : public IDynamicalSystem<state_representation::CartesianState> {
   */
   Ring();
 
+  /**
+   * @brief Constructor from an initial parameter list
+   * @param parameters A parameter list containing initial parameters
+   */
+  explicit Ring(const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters);
+
   /**
    * @copydoc IDynamicalSystem::set_parameter
    */
diff --git a/source/dynamical_systems/include/dynamical_systems/exceptions/InvalidDynamicalSystemException.hpp b/source/dynamical_systems/include/dynamical_systems/exceptions/InvalidDynamicalSystemException.hpp
new file mode 100644
index 000000000..239bc1a72
--- /dev/null
+++ b/source/dynamical_systems/include/dynamical_systems/exceptions/InvalidDynamicalSystemException.hpp
@@ -0,0 +1,11 @@
+#pragma once
+
+#include <stdexcept>
+#include <string>
+
+namespace dynamical_systems::exceptions {
+class InvalidDynamicalSystemException : public std::logic_error {
+public:
+  explicit InvalidDynamicalSystemException(const std::string& msg) : std::logic_error(msg) {};
+};
+}
diff --git a/source/dynamical_systems/src/Circular.cpp b/source/dynamical_systems/src/Circular.cpp
index 2dd6e63e0..ab87903ea 100644
--- a/source/dynamical_systems/src/Circular.cpp
+++ b/source/dynamical_systems/src/Circular.cpp
@@ -21,6 +21,11 @@ Circular::Circular() :
   this->parameters_.insert(std::make_pair("circular_velocity", this->circular_velocity_));
 }
 
+Circular::Circular(const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters) :
+    Circular() {
+  this->set_parameters(parameters);
+}
+
 void Circular::set_limit_cycle(Ellipsoid& limit_cycle) {
   const auto& center = limit_cycle.get_center_state();
   if (center.is_empty()) {
@@ -36,7 +41,8 @@ void Circular::set_limit_cycle(Ellipsoid& limit_cycle) {
       throw state_representation::exceptions::IncompatibleReferenceFramesException(
           "The reference frame of the center " + center.get_name() + " in frame " + center.get_reference_frame()
               + " is incompatible with the base frame of the dynamical system " + this->get_base_frame().get_name()
-              + " in frame " + this->get_base_frame().get_reference_frame() + ".");
+              + " in frame " + this->get_base_frame().get_reference_frame() + "."
+      );
     }
     limit_cycle.set_center_state(this->get_base_frame().inverse() * center);
   }
@@ -71,7 +77,8 @@ void Circular::validate_and_set_parameter(const std::shared_ptr<ParameterInterfa
     this->circular_velocity_->set_value(std::static_pointer_cast<Parameter<double>>(parameter)->get_value());
   } else {
     throw state_representation::exceptions::InvalidParameterException(
-        "No parameter with name '" + parameter->get_name() + "' found");
+        "No parameter with name '" + parameter->get_name() + "' found"
+    );
   }
 }
 
diff --git a/source/dynamical_systems/src/DynamicalSystemFactory.cpp b/source/dynamical_systems/src/DynamicalSystemFactory.cpp
index b5f87eb23..b52703967 100644
--- a/source/dynamical_systems/src/DynamicalSystemFactory.cpp
+++ b/source/dynamical_systems/src/DynamicalSystemFactory.cpp
@@ -4,6 +4,7 @@
 #include "dynamical_systems/DefaultDynamicalSystem.hpp"
 #include "dynamical_systems/PointAttractor.hpp"
 #include "dynamical_systems/Ring.hpp"
+#include "dynamical_systems/exceptions/InvalidDynamicalSystemException.hpp"
 #include "state_representation/space/joint/JointState.hpp"
 
 using namespace state_representation;
@@ -11,29 +12,34 @@ using namespace state_representation;
 namespace dynamical_systems {
 
 template<>
-std::shared_ptr<IDynamicalSystem<CartesianState>>
-DynamicalSystemFactory<CartesianState>::create_dynamical_system(DYNAMICAL_SYSTEM type) {
+std::shared_ptr<IDynamicalSystem<CartesianState>> DynamicalSystemFactory<CartesianState>::create_dynamical_system(
+    DYNAMICAL_SYSTEM_TYPE type, const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters
+) {
   switch (type) {
-    case DYNAMICAL_SYSTEM::POINT_ATTRACTOR:
-      return std::make_shared<PointAttractor<CartesianState>>();
-    case DYNAMICAL_SYSTEM::CIRCULAR:
-      return std::make_shared<Circular>();
-    case DYNAMICAL_SYSTEM::RING:
-      return std::make_shared<Ring>();
+    case DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR:
+      return std::make_shared<PointAttractor<CartesianState>>(parameters);
+    case DYNAMICAL_SYSTEM_TYPE::CIRCULAR:
+      return std::make_shared<Circular>(parameters);
+    case DYNAMICAL_SYSTEM_TYPE::RING:
+      return std::make_shared<Ring>(parameters);
     default:
-    case DYNAMICAL_SYSTEM::NONE:
+    case DYNAMICAL_SYSTEM_TYPE::NONE:
       return std::make_shared<DefaultDynamicalSystem<CartesianState>>();
   }
 }
 
 template<>
-std::shared_ptr<IDynamicalSystem<JointState>>
-DynamicalSystemFactory<JointState>::create_dynamical_system(DYNAMICAL_SYSTEM type) {
+std::shared_ptr<IDynamicalSystem<JointState>> DynamicalSystemFactory<JointState>::create_dynamical_system(
+    DYNAMICAL_SYSTEM_TYPE type, const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters
+) {
   switch (type) {
-    case DYNAMICAL_SYSTEM::POINT_ATTRACTOR:
-      return std::make_shared<PointAttractor<JointState>>();
+    case DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR:
+      return std::make_shared<PointAttractor<JointState>>(parameters);
+    case DYNAMICAL_SYSTEM_TYPE::CIRCULAR:
+    case DYNAMICAL_SYSTEM_TYPE::RING:
+      throw exceptions::InvalidDynamicalSystemException("This JointState DS is not valid");
     default:
-    case DYNAMICAL_SYSTEM::NONE:
+    case DYNAMICAL_SYSTEM_TYPE::NONE:
       return std::make_shared<DefaultDynamicalSystem<JointState>>();
   }
 }
diff --git a/source/dynamical_systems/src/PointAttractor.cpp b/source/dynamical_systems/src/PointAttractor.cpp
index 72cf834b6..8c7e7cda9 100644
--- a/source/dynamical_systems/src/PointAttractor.cpp
+++ b/source/dynamical_systems/src/PointAttractor.cpp
@@ -56,10 +56,10 @@ bool PointAttractor<JointState>::is_compatible(const JointState& state) const {
 
 template<class S>
 void PointAttractor<S>::set_gain(const std::shared_ptr<ParameterInterface>& parameter, unsigned int expected_size) {
-  if (parameter->get_type() == StateType::PARAMETER_DOUBLE) {
+  if (parameter->get_parameter_type() == ParameterType::DOUBLE) {
     auto gain = parameter->get_parameter_value<double>();
     this->gain_->set_value(gain * Eigen::MatrixXd::Identity(expected_size, expected_size));
-  } else if (parameter->get_type() == StateType::PARAMETER_DOUBLE_ARRAY) {
+  } else if (parameter->get_parameter_type() == ParameterType::DOUBLE_ARRAY) {
     auto gain = parameter->get_parameter_value<std::vector<double>>();
     if (gain.size() != expected_size) {
       throw exceptions::IncompatibleSizeException(
@@ -67,7 +67,7 @@ void PointAttractor<S>::set_gain(const std::shared_ptr<ParameterInterface>& para
     }
     Eigen::VectorXd diagonal = Eigen::VectorXd::Map(gain.data(), expected_size);
     this->gain_->set_value(diagonal.asDiagonal());
-  } else if (parameter->get_type() == StateType::PARAMETER_MATRIX) {
+  } else if (parameter->get_parameter_type() == ParameterType::MATRIX) {
     auto gain = parameter->get_parameter_value<Eigen::MatrixXd>();
     if (gain.rows() != expected_size && gain.cols() != expected_size) {
       throw exceptions::IncompatibleSizeException(
@@ -143,9 +143,9 @@ void PointAttractor<CartesianState>::set_base_frame(const CartesianState& base_f
 template<>
 void PointAttractor<CartesianState>::validate_and_set_parameter(const std::shared_ptr<ParameterInterface>& parameter) {
   if (parameter->get_name() == "attractor") {
-    if (parameter->get_type() == StateType::PARAMETER_CARTESIANSTATE) {
+    if (parameter->get_parameter_state_type() == StateType::CARTESIAN_STATE) {
       this->set_attractor(parameter->get_parameter_value<CartesianState>());
-    } else if (parameter->get_type() == StateType::PARAMETER_CARTESIANPOSE) {
+    } else if (parameter->get_parameter_state_type() == StateType::CARTESIAN_POSE) {
       this->set_attractor(parameter->get_parameter_value<CartesianPose>());
     }
   } else if (parameter->get_name() == "gain") {
@@ -159,9 +159,9 @@ void PointAttractor<CartesianState>::validate_and_set_parameter(const std::share
 template<>
 void PointAttractor<JointState>::validate_and_set_parameter(const std::shared_ptr<ParameterInterface>& parameter) {
   if (parameter->get_name() == "attractor") {
-    if (parameter->get_type() == StateType::PARAMETER_JOINTSTATE) {
+    if (parameter->get_parameter_state_type() == StateType::JOINT_STATE) {
     this->set_attractor(parameter->get_parameter_value<JointState>());
-  } else if (parameter->get_type() == StateType::PARAMETER_JOINTPOSITIONS) {
+  } else if (parameter->get_parameter_state_type() == StateType::JOINT_POSITIONS) {
     this->set_attractor(parameter->get_parameter_value<JointPositions>());
   }
   } else if (parameter->get_name() == "gain") {
diff --git a/source/dynamical_systems/src/Ring.cpp b/source/dynamical_systems/src/Ring.cpp
index 6faa00df6..411d4b5c5 100644
--- a/source/dynamical_systems/src/Ring.cpp
+++ b/source/dynamical_systems/src/Ring.cpp
@@ -32,6 +32,10 @@ Ring::Ring() :
   this->parameters_.insert(std::make_pair("angular_gain", this->angular_gain_));
 }
 
+Ring::Ring(const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters) : Ring() {
+  this->set_parameters(parameters);
+}
+
 void Ring::set_center(const CartesianPose& center) {
   if (center.is_empty()) {
     throw state_representation::exceptions::EmptyStateException(center.get_name() + " state is empty");
@@ -46,7 +50,8 @@ void Ring::set_center(const CartesianPose& center) {
       throw state_representation::exceptions::IncompatibleReferenceFramesException(
           "The reference frame of the center " + center.get_name() + " in frame " + center.get_reference_frame()
               + " is incompatible with the base frame of the dynamical system " + this->get_base_frame().get_name()
-              + " in frame " + this->get_base_frame().get_reference_frame() + ".");
+              + " in frame " + this->get_base_frame().get_reference_frame() + "."
+      );
     }
     this->center_->set_value(this->get_base_frame().inverse() * center);
   } else {
@@ -104,7 +109,8 @@ void Ring::validate_and_set_parameter(const std::shared_ptr<ParameterInterface>&
     this->angular_gain_->set_value(std::static_pointer_cast<Parameter<double>>(parameter)->get_value());
   } else {
     throw state_representation::exceptions::InvalidParameterException(
-        "No parameter with name '" + parameter->get_name() + "' found");
+        "No parameter with name '" + parameter->get_name() + "' found"
+    );
   }
 }
 
@@ -212,7 +218,8 @@ CartesianState Ring::compute_dynamics(const CartesianState& state) const {
   twist.set_linear_velocity(this->calculate_local_linear_velocity(pose, local_field_strength));
   twist.set_angular_velocity(
       this->calculate_local_angular_velocity(
-          pose, twist.get_linear_velocity(), local_field_strength));
+          pose, twist.get_linear_velocity(), local_field_strength
+      ));
 
   // transform the twist back to the base reference frame
   return CartesianState(this->center_->get_value()) * twist;
diff --git a/source/dynamical_systems/test/tests/test_circular.cpp b/source/dynamical_systems/test/tests/test_circular.cpp
index efff74860..6f44e8f21 100644
--- a/source/dynamical_systems/test/tests/test_circular.cpp
+++ b/source/dynamical_systems/test/tests/test_circular.cpp
@@ -16,9 +16,7 @@ using namespace std::literals::chrono_literals;
 class CircularDSTest : public testing::Test {
 protected:
   void SetUp() override {
-    ds = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-        DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::CIRCULAR
-    );
+    ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::CIRCULAR);
     current_pose = CartesianPose("A", 10 * Eigen::Vector3d::Random());
     center = CartesianPose::Identity("B");
     limit_cycle.set_center_pose(center);
diff --git a/source/dynamical_systems/test/tests/test_factory.cpp b/source/dynamical_systems/test/tests/test_factory.cpp
index c2a59882c..4f80e7994 100644
--- a/source/dynamical_systems/test/tests/test_factory.cpp
+++ b/source/dynamical_systems/test/tests/test_factory.cpp
@@ -13,8 +13,8 @@ TEST(DSFactoryTest, CreateDS) {
   std::list<std::shared_ptr<state_representation::ParameterInterface>> param_list;
   param_list.emplace_back(make_shared_parameter("test", 1.0));
 
-  auto cart_ds = dynamical_systems::DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-      dynamical_systems::DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::NONE
+  auto cart_ds = dynamical_systems::CartesianDynamicalSystemFactory::create_dynamical_system(
+      dynamical_systems::DYNAMICAL_SYSTEM_TYPE::NONE
   );
   cart_ds->set_base_frame(CartesianState::Identity("A"));
   ASSERT_NO_THROW(auto res = cart_ds->evaluate(CartesianState::Identity("C", "A")));
@@ -22,8 +22,8 @@ TEST(DSFactoryTest, CreateDS) {
   EXPECT_EQ(cart_ds->get_parameters().size(), 0);
   EXPECT_THROW(cart_ds->set_parameters(param_list), exceptions::InvalidParameterException);
 
-  auto joint_ds = dynamical_systems::DynamicalSystemFactory<JointState>::create_dynamical_system(
-      dynamical_systems::DynamicalSystemFactory<JointState>::DYNAMICAL_SYSTEM::NONE
+  auto joint_ds = dynamical_systems::JointDynamicalSystemFactory::create_dynamical_system(
+      dynamical_systems::DYNAMICAL_SYSTEM_TYPE::NONE
   );
   ASSERT_NO_THROW(auto res = joint_ds->evaluate(JointState::Random("robot", 3)));
   EXPECT_TRUE(joint_ds->evaluate(JointState::Random("robot", 3)).is_empty());
diff --git a/source/dynamical_systems/test/tests/test_point_attractor.cpp b/source/dynamical_systems/test/tests/test_point_attractor.cpp
index 9bcb9e58b..f36286077 100644
--- a/source/dynamical_systems/test/tests/test_point_attractor.cpp
+++ b/source/dynamical_systems/test/tests/test_point_attractor.cpp
@@ -13,7 +13,6 @@
 #include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp"
 #include "state_representation/exceptions/IncompatibleStatesException.hpp"
 
-
 using namespace state_representation;
 using namespace dynamical_systems;
 using namespace std::literals::chrono_literals;
@@ -23,9 +22,7 @@ class PointAttractorTest : public testing::Test {
   void SetUp() override {
     current_pose = CartesianPose("A", 10 * Eigen::Vector3d::Random());
     target_pose = CartesianPose("B", 10 * Eigen::Vector3d::Random(), Eigen::Quaterniond::UnitRandom());
-    ds = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-        DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR
-    );
+    ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR);
   }
   void print_current_and_target_pose() {
     std::cout << current_pose << std::endl;
@@ -270,10 +267,8 @@ TEST_F(PointAttractorTest, UpdateAttractorFrame) {
   EXPECT_NO_THROW(ds->evaluate(D));
 }
 
-TEST(JointPointAttractorTest, EmptyConstructor) {
-  auto ds = DynamicalSystemFactory<JointState>::create_dynamical_system(
-      DynamicalSystemFactory<JointState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR
-  );
+TEST(JointPointAttractorTest, Constructor) {
+  auto ds = JointDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR);
   // construct empty cartesian state DS
   auto attractor = JointState::Zero("robot", 3);
 
@@ -283,12 +278,16 @@ TEST(JointPointAttractorTest, EmptyConstructor) {
   ds->set_parameter_value("attractor", attractor);
   EXPECT_FALSE(ds->get_parameter_value<JointState>("attractor").is_empty());
   EXPECT_TRUE(ds->get_base_frame().is_empty());
+
+  std::list<std::shared_ptr<state_representation::ParameterInterface>> parameters;
+  parameters.push_back(make_shared_parameter("attractor", JointState::Zero("robot", 3)));
+  ds = JointDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR, parameters);
+  EXPECT_FALSE(ds->get_parameter_value<JointState>("attractor").is_empty());
+  EXPECT_TRUE(ds->get_base_frame().is_empty());
 }
 
 TEST(JointPointAttractorTest, EmptyCompatible) {
-  auto ds = DynamicalSystemFactory<JointState>::create_dynamical_system(
-      DynamicalSystemFactory<JointState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR
-  );
+  auto ds = JointDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR);
   JointState state1 = JointState::Zero("robot", 3);
   JointState state2 = JointState("test", 3);
   JointState state3 = JointState("robot", 4);
@@ -308,9 +307,7 @@ TEST(JointPointAttractorTest, EmptyCompatible) {
 }
 
 TEST(JointPointAttractorTest, Convergence) {
-  auto ds = DynamicalSystemFactory<JointState>::create_dynamical_system(
-      DynamicalSystemFactory<JointState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR
-  );
+  auto ds = JointDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR);
   auto attractor = JointPositions::Random("robot", 3);
   auto current_state = JointPositions::Random("robot", 3);
   current_state.set_data(10 * current_state.data());
diff --git a/source/dynamical_systems/test/tests/test_ring.cpp b/source/dynamical_systems/test/tests/test_ring.cpp
index 0e209315f..445731890 100644
--- a/source/dynamical_systems/test/tests/test_ring.cpp
+++ b/source/dynamical_systems/test/tests/test_ring.cpp
@@ -6,7 +6,6 @@
 
 #include "state_representation/space/cartesian/CartesianState.hpp"
 #include "state_representation/space/cartesian/CartesianPose.hpp"
-#include "state_representation/parameters/Parameter.hpp"
 #include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp"
 #include "state_representation/exceptions/EmptyStateException.hpp"
 
@@ -17,9 +16,7 @@ using namespace std::literals::chrono_literals;
 class RingDSTest : public ::testing::Test {
 protected:
   RingDSTest() {
-    ds = DynamicalSystemFactory<CartesianState>::create_dynamical_system(
-        DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::RING
-    );
+    ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::RING);
     center = CartesianPose::Identity("A");
     current_pose = CartesianPose("B", radius * Eigen::Vector3d::Random());
   }
diff --git a/source/state_representation/README.md b/source/state_representation/README.md
index 26c4d2286..95e33319c 100644
--- a/source/state_representation/README.md
+++ b/source/state_representation/README.md
@@ -1,6 +1,6 @@
 # State Representation
 
-This library provides a set of classes to represent **states** in **Cartesian** or **joint** spaces, **parameters**, 
+This library provides a set of classes to represent **states** in **Cartesian** or **joint** space, **parameters**, 
 or **geometrical shapes** that can be used as obstacles.
 Those are a set of helpers functions to handle common concepts in robotics such as transformations between frames and 
 the link between them and the robot state.
diff --git a/source/state_representation/include/state_representation/State.hpp b/source/state_representation/include/state_representation/State.hpp
index 5d72f4ced..49a32f7fd 100644
--- a/source/state_representation/include/state_representation/State.hpp
+++ b/source/state_representation/include/state_representation/State.hpp
@@ -9,6 +9,7 @@
 #include <eigen3/Eigen/Core>
 #include <eigen3/Eigen/Dense>
 
+#include "state_representation/StateType.hpp"
 #include "state_representation/MathTools.hpp"
 
 /**
@@ -16,31 +17,6 @@
  * @brief Core state variables and objects
  */
 namespace state_representation {
-enum class StateType {
-  STATE,
-  CARTESIANSTATE,
-  DUALQUATERNIONSTATE,
-  JOINTSTATE,
-  JACOBIANMATRIX,
-  TRAJECTORY,
-  GEOMETRY_SHAPE,
-  GEOMETRY_ELLIPSOID,
-  PARAMETER_DOUBLE,
-  PARAMETER_DOUBLE_ARRAY,
-  PARAMETER_BOOL,
-  PARAMETER_BOOL_ARRAY,
-  PARAMETER_STRING,
-  PARAMETER_STRING_ARRAY,
-  PARAMETER_CARTESIANSTATE,
-  PARAMETER_CARTESIANPOSE,
-  PARAMETER_JOINTSTATE,
-  PARAMETER_JOINTPOSITIONS,
-  PARAMETER_ELLIPSOID,
-  PARAMETER_MATRIX,
-  PARAMETER_VECTOR,
-  PARAMETER_INT,
-  PARAMETER_INT_ARRAY
-};
 
 /**
  * @class State
@@ -178,6 +154,13 @@ class State : public std::enable_shared_from_this<State> {
    */
   friend std::ostream& operator<<(std::ostream& os, const State& state);
 
+protected:
+  /**
+   * @brief Override the state type
+   * @param type the type of the state to set
+   */
+  void set_type(const StateType& type);
+
 private:
   StateType type_;                                              ///< type of the State
   std::string name_;                                            ///< name of the state
@@ -198,54 +181,14 @@ inline State& State::operator=(const State& state) {
   return *this;
 }
 
-inline const StateType& State::get_type() const {
-  return this->type_;
-}
-
-inline bool State::is_empty() const {
-  return this->empty_;
-}
-
-inline void State::set_empty(bool empty) {
-  this->empty_ = empty;
-}
-
-inline void State::set_filled() {
-  this->empty_ = false;
-  this->reset_timestamp();
-}
-
-inline const std::chrono::time_point<std::chrono::steady_clock>& State::get_timestamp() const {
-  return this->timestamp_;
-}
-
-inline void State::set_timestamp(const std::chrono::time_point<std::chrono::steady_clock>& timepoint) {
-  this->timestamp_ = timepoint;
-}
-
-inline void State::reset_timestamp() {
-  this->timestamp_ = std::chrono::steady_clock::now();
-}
-
 template<typename DurationT>
 inline bool State::is_deprecated(const std::chrono::duration<int64_t, DurationT>& time_delay) {
   return ((std::chrono::steady_clock::now() - this->timestamp_) > time_delay);
 }
 
-inline const std::string& State::get_name() const {
-  return this->name_;
+template<typename T>
+std::shared_ptr<State> make_shared_state(const T& state) {
+  return std::make_shared<T>(state);
 }
 
-inline void State::set_name(const std::string& name) {
-  this->name_ = name;
-}
-
-inline bool State::is_compatible(const State& state) const {
-  bool compatible = (this->name_ == state.name_);
-  return compatible;
-}
-
-inline void State::initialize() {
-  this->empty_ = true;
-}
 }// namespace state_representation
diff --git a/source/state_representation/include/state_representation/StateType.hpp b/source/state_representation/include/state_representation/StateType.hpp
new file mode 100644
index 000000000..0f8ed9b47
--- /dev/null
+++ b/source/state_representation/include/state_representation/StateType.hpp
@@ -0,0 +1,38 @@
+#pragma once
+
+/**
+ * @namespace state_representation
+ * @brief Core state variables and objects
+ */
+namespace state_representation {
+
+/**
+ * @enum StateType
+ * @brief The class types inheriting from State.
+ */
+enum class StateType {
+  NONE,
+  STATE,
+  SPATIAL_STATE,
+  CARTESIAN_STATE,
+  CARTESIAN_POSE,
+  CARTESIAN_TWIST,
+  CARTESIAN_ACCELERATION,
+  CARTESIAN_WRENCH,
+  JOINT_STATE,
+  JOINT_POSITIONS,
+  JOINT_VELOCITIES,
+  JOINT_ACCELERATIONS,
+  JOINT_TORQUES,
+  JACOBIAN,
+  PARAMETER,
+  GEOMETRY_SHAPE,
+  GEOMETRY_ELLIPSOID,
+  TRAJECTORY,
+#ifdef EXPERIMENTAL_FEATURES
+  DUAL_QUATERNION_STATE,
+  DUAL_QUATERNION_POSE,
+  DUAL_QUATERNION_TWIST
+#endif
+};
+}
\ No newline at end of file
diff --git a/source/state_representation/include/state_representation/parameters/Parameter.hpp b/source/state_representation/include/state_representation/parameters/Parameter.hpp
index 561823e41..82057fd3a 100644
--- a/source/state_representation/include/state_representation/parameters/Parameter.hpp
+++ b/source/state_representation/include/state_representation/parameters/Parameter.hpp
@@ -25,6 +25,11 @@ class Parameter : public ParameterInterface {
    */
   explicit Parameter(const std::string& name, const T& value);
 
+  /**
+   * @brief Default virtual destructor
+   */
+  virtual ~Parameter() = default;
+
   /**
    * @brief Copy constructor
    * @param parameter The parameter to copy
@@ -44,7 +49,7 @@ class Parameter : public ParameterInterface {
    * @return The value attribute
    */
   template<typename U>
-  U get_value();
+  U get_value() const;
 
   /**
    * @brief Getter of the value attribute.
@@ -89,7 +94,7 @@ Parameter<T>& Parameter<T>::operator=(const Parameter<U>& parameter) {
 
 template<typename T>
 template<typename U>
-U Parameter<T>::get_value() {
+U Parameter<T>::get_value() const {
   return static_cast<U>(this->value_);
 }
 
diff --git a/source/state_representation/include/state_representation/parameters/ParameterInterface.hpp b/source/state_representation/include/state_representation/parameters/ParameterInterface.hpp
index 8b6459967..62d28b854 100644
--- a/source/state_representation/include/state_representation/parameters/ParameterInterface.hpp
+++ b/source/state_representation/include/state_representation/parameters/ParameterInterface.hpp
@@ -2,6 +2,7 @@
 
 #include <memory>
 
+#include "state_representation/parameters/ParameterType.hpp"
 #include "state_representation/exceptions/InvalidParameterCastException.hpp"
 #include "state_representation/exceptions/InvalidPointerException.hpp"
 #include "state_representation/State.hpp"
@@ -15,11 +16,14 @@ class Parameter;
 class ParameterInterface : public State {
 public:
   /**
-   * @brief Constructor with parameter name and type of the parameter.
-   * @param type The type of the parameter
+   * @brief Constructor with parameter name and type.
    * @param name The name of the parameter
+   * @param type The type of the parameter
+   * @param parameter_state_type The state type of the parameter, if applicable
    */
-  explicit ParameterInterface(const StateType& type, const std::string& name);
+  ParameterInterface(
+      const std::string& name, const ParameterType& type, const StateType& parameter_state_type = StateType::NONE
+  );
 
   /**
    * @brief Copy constructor
@@ -27,6 +31,11 @@ class ParameterInterface : public State {
    */
   ParameterInterface(const ParameterInterface& parameter);
 
+  /**
+   * @brief Default virtual destructor
+   */
+  virtual ~ParameterInterface() = default;
+
   /**
    * @brief Copy assignment operator that has to be defined
    * to the custom assignment operator.
@@ -50,7 +59,7 @@ class ParameterInterface : public State {
    * if downcasting failed and validate_pointer was set to false.
    */
   template<typename T>
-  std::shared_ptr<Parameter<T>> get_parameter(bool validate_pointer = true);
+  std::shared_ptr<Parameter<T>> get_parameter(bool validate_pointer = true) const;
 
   /**
    * @brief Get the parameter value of a derived Parameter instance through the ParameterInterface pointer.
@@ -61,10 +70,10 @@ class ParameterInterface : public State {
    * @return The value contained in the underlying Parameter instance
    */
   template<typename T>
-  T get_parameter_value();
+  T get_parameter_value() const;
 
   /**
-   * @brief Set the  parameter value of a derived Parameter instance through the ParameterInterface pointer.
+   * @brief Set the parameter value of a derived Parameter instance through the ParameterInterface pointer.
    * @details This throws an InvalidParameterCastException if the ParameterInterface does not point to
    * a valid Parameter instance or if the specified type does not match the type of the Parameter instance.
    * @see ParameterInterface::get_parameter()
@@ -73,13 +82,31 @@ class ParameterInterface : public State {
    */
   template<typename T>
   void set_parameter_value(const T& value);
+
+  /**
+   * @brief Get the parameter type.
+   * @return The type of the underlying parameter
+   */
+  ParameterType get_parameter_type() const;
+
+  /**
+   * @brief Get the state type of the parameter.
+   * @details If the parameter type from get_parameter_type() is not ParameterType::STATE,
+   * this will return StateType::NONE.
+   * @return The state type of the underlying parameter
+   */
+  StateType get_parameter_state_type() const;
+
+private:
+  ParameterType parameter_type_;
+  StateType parameter_state_type_;
 };
 
 template<typename T>
-inline std::shared_ptr<Parameter<T>> ParameterInterface::get_parameter(bool validate_pointer) {
+inline std::shared_ptr<Parameter<T>> ParameterInterface::get_parameter(bool validate_pointer) const {
   std::shared_ptr<Parameter<T>> parameter_ptr;
   try {
-    parameter_ptr = std::dynamic_pointer_cast<Parameter<T>>(shared_from_this());
+    parameter_ptr = std::dynamic_pointer_cast<Parameter<T>>(std::const_pointer_cast<State>(shared_from_this()));
   } catch (const std::exception&) {
     if (validate_pointer) {
       throw exceptions::InvalidPointerException(
@@ -96,7 +123,7 @@ inline std::shared_ptr<Parameter<T>> ParameterInterface::get_parameter(bool vali
 }
 
 template<typename T>
-inline T ParameterInterface::get_parameter_value() {
+inline T ParameterInterface::get_parameter_value() const {
   return get_parameter<T>(true)->get_value();
 }
 
diff --git a/source/state_representation/include/state_representation/parameters/ParameterType.hpp b/source/state_representation/include/state_representation/parameters/ParameterType.hpp
new file mode 100644
index 000000000..97b374399
--- /dev/null
+++ b/source/state_representation/include/state_representation/parameters/ParameterType.hpp
@@ -0,0 +1,22 @@
+#pragma once
+
+namespace state_representation {
+
+/**
+ * @enum ParameterType
+ * @brief The parameter value types.
+ */
+enum class ParameterType {
+  BOOL,
+  BOOL_ARRAY,
+  INT,
+  INT_ARRAY,
+  DOUBLE,
+  DOUBLE_ARRAY,
+  STRING,
+  STRING_ARRAY,
+  STATE,
+  VECTOR,
+  MATRIX
+};
+}
\ No newline at end of file
diff --git a/source/state_representation/include/state_representation/space/SpatialState.hpp b/source/state_representation/include/state_representation/space/SpatialState.hpp
index e1f06a572..60c1fd1d1 100644
--- a/source/state_representation/include/state_representation/space/SpatialState.hpp
+++ b/source/state_representation/include/state_representation/space/SpatialState.hpp
@@ -9,13 +9,19 @@ class SpatialState : public State {
 
 public:
   /**
-   * @brief Empty constructor only specifying the type.
+   * @brief Empty constructor.
+   */
+  SpatialState();
+
+  /**
+   * @brief Empty constructor with a specific state type.
+   * @param type The type of the State
    */
   explicit SpatialState(const StateType& type);
 
   /**
    * @brief Constructor with name and reference frame specification.
-   * @param type The type of SpatialState (Cartesian or DualQuaternion)
+   * @param type The type of the State
    * @param name The name of the State
    * @param reference_frame The reference frame in which the state is expressed, by default world
    * @param empty Specify if the state is initialized as empty, default true
diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp
index abc545b88..47f58810d 100644
--- a/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp
+++ b/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp
@@ -33,15 +33,26 @@ class CartesianAcceleration : public CartesianState {
   void set_orientation(const Eigen::Quaterniond& orientation) = delete;
   void set_orientation(const Eigen::Vector4d& orientation) = delete;
   void set_orientation(const std::vector<double>& orientation) = delete;
+  void set_orientation(const double& w, const double& x, const double& y, const double& z) = delete;
   void set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) = delete;
   void set_pose(const Eigen::Matrix<double, 7, 1>& pose) = delete;
   void set_pose(const std::vector<double>& pose) = delete;
-  void set_linear_velocity(const Eigen::Vector3d& linear_acceleration) = delete;
-  void set_angular_velocity(const Eigen::Vector3d& angular_acceleration) = delete;
-  void set_twist(const Eigen::Matrix<double, 6, 1>& acceleration) = delete;
+  void set_linear_velocity(const Eigen::Vector3d& linear_velocity) = delete;
+  void set_linear_velocity(const std::vector<double>& linear_velocity) = delete;
+  void set_linear_velocity(const double& x, const double& y, const double& z) = delete;
+  void set_angular_velocity(const Eigen::Vector3d& angular_velocity) = delete;
+  void set_angular_velocity(const std::vector<double>& angular_velocity) = delete;
+  void set_angular_velocity(const double& x, const double& y, const double& z) = delete;
+  void set_twist(const Eigen::Matrix<double, 6, 1>& twist) = delete;
+  void set_twist(const std::vector<double>& twist) = delete;
   void set_force(const Eigen::Vector3d& force) = delete;
+  void set_force(const std::vector<double>& force) = delete;
+  void set_force(const double& x, const double& y, const double& z) = delete;
   void set_torque(const Eigen::Vector3d& torque) = delete;
+  void set_torque(const std::vector<double>& torque) = delete;
+  void set_torque(const double& x, const double& y, const double& z) = delete;
   void set_wrench(const Eigen::Matrix<double, 6, 1>& wrench) = delete;
+  void set_wrench(const std::vector<double>& wrench) = delete;
   CartesianState operator*=(const CartesianState& state) = delete;
   CartesianState operator*(const CartesianState& state) = delete;
   friend CartesianState operator*=(const CartesianState& state, const CartesianAcceleration& acceleration) = delete;
@@ -49,7 +60,7 @@ class CartesianAcceleration : public CartesianState {
   /**
    * @brief Empty constructor
    */
-  explicit CartesianAcceleration() = default;
+  explicit CartesianAcceleration();
 
   /**
    * @brief Constructor with name and reference frame provided
diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp
index b9031ef39..c718c81d7 100644
--- a/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp
+++ b/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp
@@ -30,21 +30,36 @@ class CartesianPose : public CartesianState {
   const Eigen::Vector3d& get_torque() const = delete;
   Eigen::Matrix<double, 6, 1> get_wrench() const = delete;
   void set_linear_velocity(const Eigen::Vector3d& linear_velocity) = delete;
+  void set_linear_velocity(const std::vector<double>& linear_velocity) = delete;
+  void set_linear_velocity(const double& x, const double& y, const double& z) = delete;
   void set_angular_velocity(const Eigen::Vector3d& angular_velocity) = delete;
+  void set_angular_velocity(const std::vector<double>& angular_velocity) = delete;
+  void set_angular_velocity(const double& x, const double& y, const double& z) = delete;
   void set_twist(const Eigen::Matrix<double, 6, 1>& twist) = delete;
+  void set_twist(const std::vector<double>& twist) = delete;
   void set_linear_acceleration(const Eigen::Vector3d& linear_acceleration) = delete;
+  void set_linear_acceleration(const std::vector<double>& linear_acceleration) = delete;
+  void set_linear_acceleration(const double& x, const double& y, const double& z) = delete;
   void set_angular_acceleration(const Eigen::Vector3d& angular_acceleration) = delete;
+  void set_angular_acceleration(const std::vector<double>& angular_acceleration) = delete;
+  void set_angular_acceleration(const double& x, const double& y, const double& z) = delete;
   void set_acceleration(const Eigen::Matrix<double, 6, 1>& acceleration) = delete;
+  void set_acceleration(const std::vector<double>& acceleration) = delete;
   void set_force(const Eigen::Vector3d& force) = delete;
+  void set_force(const std::vector<double>& force) = delete;
+  void set_force(const double& x, const double& y, const double& z) = delete;
   void set_torque(const Eigen::Vector3d& torque) = delete;
+  void set_torque(const std::vector<double>& torque) = delete;
+  void set_torque(const double& x, const double& y, const double& z) = delete;
   void set_wrench(const Eigen::Matrix<double, 6, 1>& wrench) = delete;
+  void set_wrench(const std::vector<double>& wrench) = delete;
   CartesianState operator*=(const CartesianState& state) = delete;
   friend CartesianState operator*=(const CartesianState& state, const CartesianPose& pose) = delete;
 
   /**
    * @brief Empty constructor
    */
-  explicit CartesianPose() = default;
+  explicit CartesianPose();
 
   /**
    * @brief Constructor with name and reference frame provided
diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp
index 65aea975b..3e67ce2f2 100644
--- a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp
+++ b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp
@@ -109,8 +109,8 @@ class CartesianState : public SpatialState {
 
   /**
    * @brief Constructor with name and reference frame provided
-   * @brief name the name of the state
-   * @brief reference the name of the reference frame
+   * @param name the name of the state
+   * @param reference the name of the reference frame
    */
   explicit CartesianState(const std::string& name, const std::string& reference = "world");
 
@@ -257,6 +257,11 @@ class CartesianState : public SpatialState {
    */
   void set_orientation(const std::vector<double>& orientation);
 
+  /**
+   * @brief Setter of the orientation from four scalar coefficients (w, x, y, z)
+   */
+  void set_orientation(const double& w, const double& x, const double& y, const double& z);
+
   /**
    * @brief Setter of the pose from both position and orientation
    * @param position the position
@@ -283,46 +288,121 @@ class CartesianState : public SpatialState {
    */
   void set_linear_velocity(const Eigen::Vector3d& linear_velocity);
 
+  /**
+   * @brief Setter of the linear velocity from a std vector
+   */
+  void set_linear_velocity(const std::vector<double>& linear_velocity);
+
+  /**
+   * @brief Setter of the linear velocity from three scalar coordinates
+   */
+  void set_linear_velocity(const double& x, const double& y, const double& z);
+
   /**
    * @brief Setter of the angular velocity attribute
    */
   void set_angular_velocity(const Eigen::Vector3d& angular_velocity);
 
+  /**
+   * @brief Setter of the angular velocity from a std vector
+   */
+  void set_angular_velocity(const std::vector<double>& angular_velocity);
+
+  /**
+   * @brief Setter of the angular velocity from three scalar coordinates
+   */
+  void set_angular_velocity(const double& x, const double& y, const double& z);
+
   /**
    * @brief Setter of the linear and angular velocities from a 6d twist vector
    */
   void set_twist(const Eigen::Matrix<double, 6, 1>& twist);
 
+  /**
+   * @brief Setter of the linear and angular velocities from a std vector
+   */
+  void set_twist(const std::vector<double>& twist);
+
   /**
    * @brief Setter of the linear acceleration attribute
    */
   void set_linear_acceleration(const Eigen::Vector3d& linear_acceleration);
 
+  /**
+   * @brief Setter of the linear acceleration from a std vector
+   */
+  void set_linear_acceleration(const std::vector<double>& linear_acceleration);
+
+  /**
+   * @brief Setter of the linear acceleration from three scalar coordinates
+   */
+  void set_linear_acceleration(const double& x, const double& y, const double& z);
+
   /**
    * @brief Setter of the angular velocity attribute
    */
   void set_angular_acceleration(const Eigen::Vector3d& angular_acceleration);
 
+  /**
+   * @brief Setter of the angular acceleration from a std vector
+   */
+  void set_angular_acceleration(const std::vector<double>& angular_acceleration);
+
+  /**
+   * @brief Setter of the angular acceleration from three scalar coordinates
+   */
+  void set_angular_acceleration(const double& x, const double& y, const double& z);
+
   /**
    * @brief Setter of the linear and angular acceleration from a 6d acceleration vector
    */
   void set_acceleration(const Eigen::Matrix<double, 6, 1>& acceleration);
 
+  /**
+   * @brief Setter of the linear and angular acceleration from a std vector
+   */
+  void set_acceleration(const std::vector<double>& acceleration);
+
   /**
    * @brief Setter of the force attribute
    */
   void set_force(const Eigen::Vector3d& force);
 
   /**
-   * @brief Setter of the force attribute
+   * @brief Setter of the force from a std vector
+   */
+  void set_force(const std::vector<double>& force);
+
+  /**
+   * @brief Setter of the force from three scalar coordinates
+   */
+  void set_force(const double& x, const double& y, const double& z);
+
+  /**
+   * @brief Setter of the torque attribute
    */
   void set_torque(const Eigen::Vector3d& torque);
 
+  /**
+   * @brief Setter of the torque from a std vector
+   */
+  void set_torque(const std::vector<double>& torque);
+
+  /**
+   * @brief Setter of the torque from three scalar coordinates
+   */
+  void set_torque(const double& x, const double& y, const double& z);
+
   /**
    * @brief Setter of the force and torque from a 6d wrench vector
    */
   void set_wrench(const Eigen::Matrix<double, 6, 1>& wrench);
 
+  /**
+   * @brief Setter of the force and torque from a std vector
+   */
+  void set_wrench(const std::vector<double>& wrench);
+
   /**
    * @brief Initialize the CartesianState to a zero value
    */
@@ -535,74 +615,6 @@ inline CartesianState& CartesianState::operator=(const CartesianState& state) {
   return *this;
 }
 
-inline const Eigen::Vector3d& CartesianState::get_position() const {
-  return this->position_;
-}
-
-inline const Eigen::Quaterniond& CartesianState::get_orientation() const {
-  return this->orientation_;
-}
-
-inline Eigen::Vector4d CartesianState::get_orientation_coefficients() const {
-  return Eigen::Vector4d(
-      this->get_orientation().w(), this->get_orientation().x(), this->get_orientation().y(),
-      this->get_orientation().z());
-}
-
-inline Eigen::Matrix<double, 7, 1> CartesianState::get_pose() const {
-  Eigen::Matrix<double, 7, 1> pose;
-  pose << this->get_position(), this->get_orientation_coefficients();
-  return pose;
-}
-
-inline Eigen::Matrix4d CartesianState::get_transformation_matrix() const {
-  Eigen::Matrix4d pose;
-  pose << this->orientation_.toRotationMatrix(), this->position_, 0., 0., 0., 1;
-  return pose;
-}
-
-inline const Eigen::Vector3d& CartesianState::get_linear_velocity() const {
-  return this->linear_velocity_;
-}
-
-inline const Eigen::Vector3d& CartesianState::get_angular_velocity() const {
-  return this->angular_velocity_;
-}
-
-inline Eigen::Matrix<double, 6, 1> CartesianState::get_twist() const {
-  Eigen::Matrix<double, 6, 1> twist;
-  twist << this->get_linear_velocity(), this->get_angular_velocity();
-  return twist;
-}
-
-inline const Eigen::Vector3d& CartesianState::get_linear_acceleration() const {
-  return this->linear_acceleration_;
-}
-
-inline const Eigen::Vector3d& CartesianState::get_angular_acceleration() const {
-  return this->angular_acceleration_;
-}
-
-inline Eigen::Matrix<double, 6, 1> CartesianState::get_acceleration() const {
-  Eigen::Matrix<double, 6, 1> acceleration;
-  acceleration << this->get_linear_acceleration(), this->get_angular_acceleration();
-  return acceleration;
-}
-
-inline const Eigen::Vector3d& CartesianState::get_force() const {
-  return this->force_;
-}
-
-inline const Eigen::Vector3d& CartesianState::get_torque() const {
-  return this->torque_;
-}
-
-inline Eigen::Matrix<double, 6, 1> CartesianState::get_wrench() const {
-  Eigen::Matrix<double, 6, 1> wrench;
-  wrench << this->get_force(), this->get_torque();
-  return wrench;
-}
-
 inline Eigen::VectorXd CartesianState::get_state_variable(const CartesianStateVariable& state_variable_type) const {
   switch (state_variable_type) {
     case CartesianStateVariable::POSITION:
@@ -682,88 +694,6 @@ inline void CartesianState::set_state_variable(
   this->set_state_variable(angular_state_variable, new_value.tail(3));
 }
 
-inline void CartesianState::set_position(const Eigen::Vector3d& position) {
-  this->set_state_variable(this->position_, position);
-}
-
-inline void CartesianState::set_position(const std::vector<double>& position) {
-  this->set_state_variable(this->position_, position);
-}
-
-inline void CartesianState::set_position(const double& x, const double& y, const double& z) {
-  this->set_position(Eigen::Vector3d(x, y, z));
-}
-
-inline void CartesianState::set_orientation(const Eigen::Quaterniond& orientation) {
-  this->set_filled();
-  this->orientation_ = orientation.normalized();
-}
-
-inline void CartesianState::set_orientation(const Eigen::Vector4d& orientation) {
-  this->set_orientation(Eigen::Quaterniond(orientation(0), orientation(1), orientation(2), orientation(3)));
-}
-
-inline void CartesianState::set_orientation(const std::vector<double>& orientation) {
-  if (orientation.size() != 4) {
-    throw exceptions::IncompatibleSizeException("The input vector is not of size 4 required for orientation");
-  }
-  this->set_orientation(Eigen::Vector4d::Map(orientation.data(), orientation.size()));
-}
-
-inline void CartesianState::set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) {
-  this->set_position(position);
-  this->set_orientation(orientation);
-}
-
-inline void CartesianState::set_pose(const Eigen::Matrix<double, 7, 1>& pose) {
-  this->set_position(pose.head(3));
-  this->set_orientation(pose.tail(4));
-}
-
-inline void CartesianState::set_pose(const std::vector<double>& pose) {
-  if (pose.size() != 7) {
-    throw exceptions::IncompatibleSizeException("The input vector is not of size 7 required for pose");
-  }
-  this->set_position(std::vector<double>(pose.begin(), pose.begin() + 3));
-  this->set_orientation(std::vector<double>(pose.begin() + 3, pose.end()));
-}
-
-inline void CartesianState::set_linear_velocity(const Eigen::Vector3d& linear_velocity) {
-  this->set_state_variable(this->linear_velocity_, linear_velocity);
-}
-
-inline void CartesianState::set_angular_velocity(const Eigen::Vector3d& angular_velocity) {
-  this->set_state_variable(this->angular_velocity_, angular_velocity);
-}
-
-inline void CartesianState::set_twist(const Eigen::Matrix<double, 6, 1>& twist) {
-  this->set_state_variable(this->linear_velocity_, this->angular_velocity_, twist);
-}
-
-inline void CartesianState::set_linear_acceleration(const Eigen::Vector3d& linear_acceleration) {
-  this->set_state_variable(this->linear_acceleration_, linear_acceleration);
-}
-
-inline void CartesianState::set_angular_acceleration(const Eigen::Vector3d& angular_acceleration) {
-  this->set_state_variable(this->angular_acceleration_, angular_acceleration);
-}
-
-inline void CartesianState::set_acceleration(const Eigen::Matrix<double, 6, 1>& acceleration) {
-  this->set_state_variable(this->linear_acceleration_, this->angular_acceleration_, acceleration);
-}
-
-inline void CartesianState::set_force(const Eigen::Vector3d& force) {
-  this->set_state_variable(this->force_, force);
-}
-
-inline void CartesianState::set_torque(const Eigen::Vector3d& torque) {
-  this->set_state_variable(this->torque_, torque);
-}
-
-inline void CartesianState::set_wrench(const Eigen::Matrix<double, 6, 1>& wrench) {
-  this->set_state_variable(this->force_, this->torque_, wrench);
-}
-
 inline void CartesianState::set_state_variable(
     const Eigen::VectorXd& new_value, const CartesianStateVariable& state_variable_type
 ) {
diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp
index 8efa82f6d..f0943406c 100644
--- a/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp
+++ b/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp
@@ -35,15 +35,26 @@ class CartesianTwist : public CartesianState {
   void set_orientation(const Eigen::Quaterniond& orientation) = delete;
   void set_orientation(const Eigen::Vector4d& orientation) = delete;
   void set_orientation(const std::vector<double>& orientation) = delete;
+  void set_orientation(const double& w, const double& x, const double& y, const double& z) = delete;
   void set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) = delete;
   void set_pose(const Eigen::Matrix<double, 7, 1>& pose) = delete;
   void set_pose(const std::vector<double>& pose) = delete;
   void set_linear_acceleration(const Eigen::Vector3d& linear_acceleration) = delete;
+  void set_linear_acceleration(const std::vector<double>& linear_acceleration) = delete;
+  void set_linear_acceleration(const double& x, const double& y, const double& z) = delete;
   void set_angular_acceleration(const Eigen::Vector3d& angular_acceleration) = delete;
+  void set_angular_acceleration(const std::vector<double>& angular_acceleration) = delete;
+  void set_angular_acceleration(const double& x, const double& y, const double& z) = delete;
   void set_acceleration(const Eigen::Matrix<double, 6, 1>& acceleration) = delete;
+  void set_acceleration(const std::vector<double>& acceleration) = delete;
   void set_force(const Eigen::Vector3d& force) = delete;
+  void set_force(const std::vector<double>& force) = delete;
+  void set_force(const double& x, const double& y, const double& z) = delete;
   void set_torque(const Eigen::Vector3d& torque) = delete;
+  void set_torque(const std::vector<double>& torque) = delete;
+  void set_torque(const double& x, const double& y, const double& z) = delete;
   void set_wrench(const Eigen::Matrix<double, 6, 1>& wrench) = delete;
+  void set_wrench(const std::vector<double>& wrench) = delete;
   CartesianState operator*=(const CartesianState& state) = delete;
   CartesianState operator*(const CartesianState& state) = delete;
   friend CartesianState operator*=(const CartesianState& state, const CartesianTwist& twist) = delete;
@@ -51,7 +62,7 @@ class CartesianTwist : public CartesianState {
   /**
    * @brief Empty constructor
    */
-  explicit CartesianTwist() = default;
+  explicit CartesianTwist();
 
   /**
    * @brief Constructor with name and reference frame provided
diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp
index 182e88fd2..862eec2f2 100644
--- a/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp
+++ b/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp
@@ -34,15 +34,26 @@ class CartesianWrench : public CartesianState {
   void set_orientation(const Eigen::Quaterniond& orientation) = delete;
   void set_orientation(const Eigen::Vector4d& orientation) = delete;
   void set_orientation(const std::vector<double>& orientation) = delete;
+  void set_orientation(const double& w, const double& x, const double& y, const double& z) = delete;
   void set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) = delete;
   void set_pose(const Eigen::Matrix<double, 7, 1>& pose) = delete;
   void set_pose(const std::vector<double>& pose) = delete;
   void set_linear_velocity(const Eigen::Vector3d& linear_velocity) = delete;
+  void set_linear_velocity(const std::vector<double>& linear_velocity) = delete;
+  void set_linear_velocity(const double& x, const double& y, const double& z) = delete;
   void set_angular_velocity(const Eigen::Vector3d& angular_velocity) = delete;
+  void set_angular_velocity(const std::vector<double>& angular_velocity) = delete;
+  void set_angular_velocity(const double& x, const double& y, const double& z) = delete;
   void set_twist(const Eigen::Matrix<double, 6, 1>& twist) = delete;
+  void set_twist(const std::vector<double>& twist) = delete;
   void set_linear_acceleration(const Eigen::Vector3d& linear_acceleration) = delete;
+  void set_linear_acceleration(const std::vector<double>& linear_acceleration) = delete;
+  void set_linear_acceleration(const double& x, const double& y, const double& z) = delete;
   void set_angular_acceleration(const Eigen::Vector3d& angular_acceleration) = delete;
+  void set_angular_acceleration(const std::vector<double>& angular_acceleration) = delete;
+  void set_angular_acceleration(const double& x, const double& y, const double& z) = delete;
   void set_acceleration(const Eigen::Matrix<double, 6, 1>& acceleration) = delete;
+  void set_acceleration(const std::vector<double>& acceleration) = delete;
   CartesianState operator*=(const CartesianState& state) = delete;
   CartesianState operator*(const CartesianState& state) = delete;
   friend CartesianState operator*=(const CartesianState& state, const CartesianWrench& wrench) = delete;
@@ -50,7 +61,7 @@ class CartesianWrench : public CartesianState {
   /**
    * @brief Empty constructor
    */
-  explicit CartesianWrench() = default;
+  explicit CartesianWrench();
 
   /**
     * @brief Constructor with name and reference frame provided
diff --git a/source/state_representation/include/state_representation/space/joint/JointAccelerations.hpp b/source/state_representation/include/state_representation/space/joint/JointAccelerations.hpp
index d63703754..696d1b378 100644
--- a/source/state_representation/include/state_representation/space/joint/JointAccelerations.hpp
+++ b/source/state_representation/include/state_representation/space/joint/JointAccelerations.hpp
@@ -40,7 +40,7 @@ class JointAccelerations : public JointState {
   /**
    * @brief Empty constructor
    */
-  explicit JointAccelerations() = default;
+  explicit JointAccelerations();
 
   /**
    * @brief Constructor with name and number of joints provided
diff --git a/source/state_representation/include/state_representation/space/joint/JointPositions.hpp b/source/state_representation/include/state_representation/space/joint/JointPositions.hpp
index 3ed0042ef..9912ff229 100644
--- a/source/state_representation/include/state_representation/space/joint/JointPositions.hpp
+++ b/source/state_representation/include/state_representation/space/joint/JointPositions.hpp
@@ -40,7 +40,7 @@ class JointPositions : public JointState {
   /**
    * @brief Empty constructor
    */
-  explicit JointPositions() = default;
+  explicit JointPositions();
 
   /**
    * @brief Constructor with name and number of joints provided
diff --git a/source/state_representation/include/state_representation/space/joint/JointTorques.hpp b/source/state_representation/include/state_representation/space/joint/JointTorques.hpp
index 561826229..fdceb1637 100644
--- a/source/state_representation/include/state_representation/space/joint/JointTorques.hpp
+++ b/source/state_representation/include/state_representation/space/joint/JointTorques.hpp
@@ -37,7 +37,7 @@ class JointTorques : public JointState {
   /**
    * @brief Empty constructor
    */
-  explicit JointTorques() = default;
+  explicit JointTorques();
 
   /**
    * @brief Constructor with name and number of joints provided
diff --git a/source/state_representation/include/state_representation/space/joint/JointVelocities.hpp b/source/state_representation/include/state_representation/space/joint/JointVelocities.hpp
index 3a35ee979..1a1b25250 100644
--- a/source/state_representation/include/state_representation/space/joint/JointVelocities.hpp
+++ b/source/state_representation/include/state_representation/space/joint/JointVelocities.hpp
@@ -42,7 +42,7 @@ class JointVelocities : public JointState {
   /**
    * @brief Empty constructor
    */
-  explicit JointVelocities() = default;
+  explicit JointVelocities();
 
   /**
    * @brief Constructor with name and number of joints provided
diff --git a/source/state_representation/src/State.cpp b/source/state_representation/src/State.cpp
index 1e50c9cc5..242c5ee34 100644
--- a/source/state_representation/src/State.cpp
+++ b/source/state_representation/src/State.cpp
@@ -16,6 +16,52 @@ State::State(const State& state) :
     empty_(state.empty_),
     timestamp_(std::chrono::steady_clock::now()) {}
 
+const StateType& State::get_type() const {
+  return this->type_;
+}
+
+bool State::is_empty() const {
+  return this->empty_;
+}
+
+void State::set_empty(bool empty) {
+  this->empty_ = empty;
+}
+
+void State::set_filled() {
+  this->empty_ = false;
+  this->reset_timestamp();
+}
+
+const std::chrono::time_point<std::chrono::steady_clock>& State::get_timestamp() const {
+  return this->timestamp_;
+}
+
+void State::set_timestamp(const std::chrono::time_point<std::chrono::steady_clock>& timepoint) {
+  this->timestamp_ = timepoint;
+}
+
+void State::reset_timestamp() {
+  this->timestamp_ = std::chrono::steady_clock::now();
+}
+
+const std::string& State::get_name() const {
+  return this->name_;
+}
+
+void State::set_name(const std::string& name) {
+  this->name_ = name;
+}
+
+bool State::is_compatible(const State& state) const {
+  bool compatible = (this->name_ == state.name_);
+  return compatible;
+}
+
+void State::initialize() {
+  this->empty_ = true;
+}
+
 void State::set_data(const Eigen::VectorXd&) {
   throw exceptions::NotImplementedException("set_data() is not implemented for the base State class");
 }
@@ -35,4 +81,9 @@ std::ostream& operator<<(std::ostream& os, const State& state) {
   os << " State: " << state.get_name();
   return os;
 }
+
+void State::set_type(const StateType& type) {
+  this->type_ = type;
+}
+
 }// namespace state_representation
diff --git a/source/state_representation/src/parameters/Parameter.cpp b/source/state_representation/src/parameters/Parameter.cpp
index f6e3bc05d..b0c13f0bb 100644
--- a/source/state_representation/src/parameters/Parameter.cpp
+++ b/source/state_representation/src/parameters/Parameter.cpp
@@ -8,147 +8,148 @@ namespace state_representation {
 
 template<>
 Parameter<int>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_INT, name) {}
+    ParameterInterface(name, ParameterType::INT) {}
 
 template<>
 Parameter<int>::Parameter(const std::string& name, const int& value) :
-    ParameterInterface(StateType::PARAMETER_INT, name), value_(value) {
+    ParameterInterface(name, ParameterType::INT), value_(value) {
   this->set_filled();
 }
 
 template<>
 Parameter<std::vector<int>>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_INT_ARRAY, name) {}
+    ParameterInterface(name, ParameterType::INT_ARRAY) {}
 
 template<>
 Parameter<std::vector<int>>::Parameter(const std::string& name, const std::vector<int>& value) :
-    ParameterInterface(StateType::PARAMETER_INT_ARRAY, name), value_(value) {
+    ParameterInterface(name, ParameterType::INT_ARRAY), value_(value) {
   this->set_filled();
 }
 
 template<>
 Parameter<double>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_DOUBLE, name) {}
+    ParameterInterface(name, ParameterType::DOUBLE) {}
 
 template<>
 Parameter<double>::Parameter(const std::string& name, const double& value) :
-    ParameterInterface(StateType::PARAMETER_DOUBLE, name), value_(value) {
+    ParameterInterface(name, ParameterType::DOUBLE), value_(value) {
   this->set_filled();
 }
 
 template<>
 Parameter<std::vector<double>>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_DOUBLE_ARRAY, name) {}
+    ParameterInterface(name, ParameterType::DOUBLE_ARRAY) {}
 
 template<>
 Parameter<std::vector<double>>::Parameter(const std::string& name, const std::vector<double>& value) :
-    ParameterInterface(StateType::PARAMETER_DOUBLE_ARRAY, name), value_(value) {
+    ParameterInterface(name, ParameterType::DOUBLE_ARRAY), value_(value) {
   this->set_filled();
 }
 
 template<>
-Parameter<bool>::Parameter(const std::string& name) : ParameterInterface(StateType::PARAMETER_BOOL, name) {}
+Parameter<bool>::Parameter(const std::string& name) :
+    ParameterInterface(name, ParameterType::BOOL) {}
 
 template<>
 Parameter<bool>::Parameter(const std::string& name, const bool& value) :
-    ParameterInterface(StateType::PARAMETER_BOOL, name), value_(value) {
+    ParameterInterface(name, ParameterType::BOOL), value_(value) {
   this->set_filled();
 }
 
 template<>
 Parameter<std::vector<bool>>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_BOOL_ARRAY, name) {
+    ParameterInterface(name, ParameterType::BOOL_ARRAY) {
 }
 
 template<>
 Parameter<std::vector<bool>>::Parameter(const std::string& name, const std::vector<bool>& value) :
-    ParameterInterface(StateType::PARAMETER_BOOL_ARRAY, name), value_(value) {
+    ParameterInterface(name, ParameterType::BOOL_ARRAY), value_(value) {
   this->set_filled();
 }
 
 template<>
 Parameter<std::string>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_STRING, name) {}
+    ParameterInterface(name, ParameterType::STRING) {}
 
 template<>
 Parameter<std::string>::Parameter(const std::string& name, const std::string& value) :
-    ParameterInterface(StateType::PARAMETER_STRING, name), value_(value) {
+    ParameterInterface(name, ParameterType::STRING), value_(value) {
   this->set_filled();
 }
 
 template<>
 Parameter<std::vector<std::string>>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_STRING_ARRAY, name) {}
+    ParameterInterface(name, ParameterType::STRING_ARRAY) {}
 
 template<>
 Parameter<std::vector<std::string>>::Parameter(const std::string& name, const std::vector<std::string>& value) :
-    ParameterInterface(StateType::PARAMETER_STRING_ARRAY, name), value_(value) {
+    ParameterInterface(name, ParameterType::STRING_ARRAY), value_(value) {
   this->set_filled();
 }
 
 template<>
 Parameter<CartesianState>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_CARTESIANSTATE, name) {}
+    ParameterInterface(name, ParameterType::STATE, StateType::CARTESIAN_STATE) {}
 
 template<>
 Parameter<CartesianState>::Parameter(const std::string& name, const CartesianState& value) :
-    ParameterInterface(StateType::PARAMETER_CARTESIANSTATE, name), value_(value) {
+    ParameterInterface(name, ParameterType::STATE, StateType::CARTESIAN_STATE), value_(value) {
   this->set_filled();
 }
 
 template<>
 Parameter<CartesianPose>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_CARTESIANPOSE, name) {}
+    ParameterInterface(name, ParameterType::STATE, StateType::CARTESIAN_POSE) {}
 
 template<>
 Parameter<CartesianPose>::Parameter(const std::string& name, const CartesianPose& value) :
-    ParameterInterface(StateType::PARAMETER_CARTESIANPOSE, name), value_(value) {
+    ParameterInterface(name, ParameterType::STATE, StateType::CARTESIAN_POSE), value_(value) {
   this->set_filled();
 }
 
 template<>
 Parameter<JointState>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_JOINTSTATE, name) {}
+    ParameterInterface(name, ParameterType::STATE, StateType::JOINT_STATE) {}
 
 template<>
 Parameter<JointState>::Parameter(const std::string& name, const JointState& value) :
-    ParameterInterface(StateType::PARAMETER_JOINTSTATE, name), value_(value) {
+    ParameterInterface(name, ParameterType::STATE, StateType::JOINT_STATE), value_(value) {
   this->set_filled();
 }
 
 template<>
 Parameter<JointPositions>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_JOINTPOSITIONS, name) {}
+    ParameterInterface(name, ParameterType::STATE, StateType::JOINT_POSITIONS) {}
 
 template<>
 Parameter<JointPositions>::Parameter(const std::string& name, const JointPositions& value) :
-    ParameterInterface(StateType::PARAMETER_JOINTPOSITIONS, name), value_(value) {
+    ParameterInterface(name, ParameterType::STATE, StateType::JOINT_POSITIONS), value_(value) {
   this->set_filled();
 }
 
 template<>
 Parameter<Ellipsoid>::Parameter(const std::string& name, const Ellipsoid& value) :
-    ParameterInterface(StateType::PARAMETER_ELLIPSOID, name), value_(value) {
+    ParameterInterface(name, ParameterType::STATE, StateType::GEOMETRY_ELLIPSOID), value_(value) {
   this->set_filled();
 }
 
 template<>
 Parameter<Eigen::MatrixXd>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_MATRIX, name) {}
+    ParameterInterface(name, ParameterType::MATRIX) {}
 
 template<>
 Parameter<Eigen::MatrixXd>::Parameter(const std::string& name, const Eigen::MatrixXd& value) :
-    ParameterInterface(StateType::PARAMETER_MATRIX, name), value_(value) {
+    ParameterInterface(name, ParameterType::MATRIX), value_(value) {
   this->set_filled();
 }
 
 template<>
 Parameter<Eigen::VectorXd>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_VECTOR, name) {}
+    ParameterInterface(name, ParameterType::VECTOR) {}
 
 template<>
 Parameter<Eigen::VectorXd>::Parameter(const std::string& name, const Eigen::VectorXd& value) :
-    ParameterInterface(StateType::PARAMETER_VECTOR, name), value_(value) {
+    ParameterInterface(name, ParameterType::VECTOR), value_(value) {
   this->set_filled();
 }
 
diff --git a/source/state_representation/src/parameters/ParameterInterface.cpp b/source/state_representation/src/parameters/ParameterInterface.cpp
index ce22e7e3c..dfb7a47d4 100644
--- a/source/state_representation/src/parameters/ParameterInterface.cpp
+++ b/source/state_representation/src/parameters/ParameterInterface.cpp
@@ -2,12 +2,28 @@
 
 namespace state_representation {
 
-ParameterInterface::ParameterInterface(const StateType& type, const std::string& name) : State(type, name) {}
+ParameterInterface::ParameterInterface(
+    const std::string& name, const ParameterType& type, const StateType& parameter_state_type
+) : State(StateType::PARAMETER, name), parameter_type_(type), parameter_state_type_(parameter_state_type) {}
 
-ParameterInterface::ParameterInterface(const ParameterInterface& parameter) : State(parameter) {}
+ParameterInterface::ParameterInterface(const ParameterInterface& parameter) :
+    State(parameter),
+    parameter_type_(parameter.get_parameter_type()),
+    parameter_state_type_(parameter.get_parameter_state_type()) {}
 
 ParameterInterface& ParameterInterface::operator=(const ParameterInterface& state) {
   State::operator=(state);
+  this->parameter_type_ = state.get_parameter_type();
+  this->parameter_state_type_ = state.get_parameter_state_type();
   return (*this);
 }
+
+ParameterType ParameterInterface::get_parameter_type() const {
+  return parameter_type_;
+}
+
+StateType ParameterInterface::get_parameter_state_type() const {
+  return parameter_state_type_;
+}
+
 }// namespace state_representation
diff --git a/source/state_representation/src/parameters/ParameterMap.cpp b/source/state_representation/src/parameters/ParameterMap.cpp
index baae32c07..68f7a311e 100644
--- a/source/state_representation/src/parameters/ParameterMap.cpp
+++ b/source/state_representation/src/parameters/ParameterMap.cpp
@@ -52,9 +52,8 @@ void ParameterMap::assert_parameter_valid(const std::shared_ptr<ParameterInterfa
   }
 }
 
-void
-ParameterMap::validate_and_set_parameter(const std::shared_ptr<ParameterInterface>& parameter) {
-  this->parameters_[parameter->get_name()] = parameter;
+void ParameterMap::validate_and_set_parameter(const std::shared_ptr<ParameterInterface>& parameter) {
+  this->parameters_.insert_or_assign(parameter->get_name(), parameter);
 }
 
 }
\ No newline at end of file
diff --git a/source/state_representation/src/space/Jacobian.cpp b/source/state_representation/src/space/Jacobian.cpp
index 90653920b..dab175f62 100644
--- a/source/state_representation/src/space/Jacobian.cpp
+++ b/source/state_representation/src/space/Jacobian.cpp
@@ -4,7 +4,7 @@
 #include "state_representation/exceptions/IncompatibleStatesException.hpp"
 
 namespace state_representation {
-Jacobian::Jacobian() : State(StateType::JACOBIANMATRIX) {
+Jacobian::Jacobian() : State(StateType::JACOBIAN) {
   this->State::initialize();
 }
 
@@ -12,7 +12,7 @@ Jacobian::Jacobian(const std::string& robot_name,
                    unsigned int nb_joints,
                    const std::string& frame,
                    const std::string& reference_frame) :
-    State(StateType::JACOBIANMATRIX, robot_name),
+    State(StateType::JACOBIAN, robot_name),
     joint_names_(nb_joints),
     frame_(frame),
     reference_frame_(reference_frame),
@@ -26,7 +26,7 @@ Jacobian::Jacobian(const std::string& robot_name,
                    const std::vector<std::string>& joint_names,
                    const std::string& frame,
                    const std::string& reference_frame) :
-    State(StateType::JACOBIANMATRIX, robot_name),
+    State(StateType::JACOBIAN, robot_name),
     joint_names_(joint_names),
     frame_(frame),
     reference_frame_(reference_frame),
@@ -88,7 +88,7 @@ Jacobian Jacobian::Random(const std::string& robot_name,
 bool Jacobian::is_compatible(const State& state) const {
   bool compatible = false;
   switch (state.get_type()) {
-    case StateType::JACOBIANMATRIX:
+    case StateType::JACOBIAN:
       // compatibility is assured through the vector of joint names
       compatible = (this->get_name() == state.get_name())
           && (this->cols_ == dynamic_cast<const Jacobian&>(state).get_joint_names().size());
@@ -101,7 +101,11 @@ bool Jacobian::is_compatible(const State& state) const {
             && (this->frame_ == dynamic_cast<const Jacobian&>(state).get_frame())));
       }
       break;
-    case StateType::JOINTSTATE:
+    case StateType::JOINT_STATE:
+    case StateType::JOINT_POSITIONS:
+    case StateType::JOINT_VELOCITIES:
+    case StateType::JOINT_ACCELERATIONS:
+    case StateType::JOINT_TORQUES:
       // compatibility is assured through the vector of joint names
       compatible = (this->get_name() == state.get_name())
           && (this->cols_ == dynamic_cast<const JointState&>(state).get_size());
@@ -111,7 +115,11 @@ bool Jacobian::is_compatible(const State& state) const {
         }
       }
       break;
-    case StateType::CARTESIANSTATE:
+    case StateType::CARTESIAN_STATE:
+    case StateType::CARTESIAN_POSE:
+    case StateType::CARTESIAN_TWIST:
+    case StateType::CARTESIAN_ACCELERATION:
+    case StateType::CARTESIAN_WRENCH:
       // compatibility is assured through the reference frame and the name of the frame
       compatible = (this->reference_frame_ == dynamic_cast<const CartesianState&>(state).get_reference_frame())
           && (this->frame_ == dynamic_cast<const CartesianState&>(state).get_name());
diff --git a/source/state_representation/src/space/SpatialState.cpp b/source/state_representation/src/space/SpatialState.cpp
index f75ef1da0..185babced 100644
--- a/source/state_representation/src/space/SpatialState.cpp
+++ b/source/state_representation/src/space/SpatialState.cpp
@@ -1,6 +1,10 @@
 #include "state_representation/space/SpatialState.hpp"
 
 namespace state_representation {
+
+SpatialState::SpatialState() :
+    State(StateType::SPATIAL_STATE), reference_frame_("world") {}
+
 SpatialState::SpatialState(const StateType& type) :
     State(type), reference_frame_("world") {}
 
diff --git a/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp b/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp
index 55074f1cd..8eae5eb17 100644
--- a/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp
+++ b/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp
@@ -4,12 +4,19 @@
 using namespace state_representation::exceptions;
 
 namespace state_representation {
+CartesianAcceleration::CartesianAcceleration() {
+  this->set_type(StateType::CARTESIAN_ACCELERATION);
+}
+
 CartesianAcceleration::CartesianAcceleration(const std::string& name, const std::string& reference) :
-    CartesianState(name, reference) {}
+    CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_ACCELERATION);
+}
 
 CartesianAcceleration::CartesianAcceleration(
     const std::string& name, const Eigen::Vector3d& linear_acceleration, const std::string& reference
 ) : CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_ACCELERATION);
   this->set_linear_acceleration(linear_acceleration);
 }
 
@@ -17,6 +24,7 @@ CartesianAcceleration::CartesianAcceleration(
     const std::string& name, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_acceleration,
     const std::string& reference
 ) : CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_ACCELERATION);
   this->set_linear_acceleration(linear_acceleration);
   this->set_angular_acceleration(angular_acceleration);
 }
@@ -24,11 +32,13 @@ CartesianAcceleration::CartesianAcceleration(
 CartesianAcceleration::CartesianAcceleration(
     const std::string& name, const Eigen::Matrix<double, 6, 1>& acceleration, const std::string& reference
 ) : CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_ACCELERATION);
   this->set_acceleration(acceleration);
 }
 
 CartesianAcceleration::CartesianAcceleration(const CartesianState& state) : CartesianState(state) {
   // set all the state variables to 0 except linear and angular velocities
+  this->set_type(StateType::CARTESIAN_ACCELERATION);
   this->set_zero();
   this->set_acceleration(state.get_acceleration());
   this->set_empty(state.is_empty());
diff --git a/source/state_representation/src/space/cartesian/CartesianPose.cpp b/source/state_representation/src/space/cartesian/CartesianPose.cpp
index 2259c3c52..3d832b73a 100644
--- a/source/state_representation/src/space/cartesian/CartesianPose.cpp
+++ b/source/state_representation/src/space/cartesian/CartesianPose.cpp
@@ -5,22 +5,31 @@
 using namespace state_representation::exceptions;
 
 namespace state_representation {
-CartesianPose::CartesianPose(const std::string& name, const std::string& reference) : CartesianState(name, reference) {}
+CartesianPose::CartesianPose() {
+  this->set_type(StateType::CARTESIAN_POSE);
+}
+
+CartesianPose::CartesianPose(const std::string& name, const std::string& reference) : CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_POSE);
+}
 
 CartesianPose::CartesianPose(const std::string& name, const Eigen::Vector3d& position, const std::string& reference) :
     CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_POSE);
   this->set_position(position);
 }
 
 CartesianPose::CartesianPose(
     const std::string& name, double x, double y, double z, const std::string& reference
 ) : CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_POSE);
   this->set_position(x, y, z);
 }
 
 CartesianPose::CartesianPose(
     const std::string& name, const Eigen::Quaterniond& orientation, const std::string& reference
 ) : CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_POSE);
   this->set_orientation(orientation);
 }
 
@@ -28,12 +37,14 @@ CartesianPose::CartesianPose(
     const std::string& name, const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation,
     const std::string& reference
 ) : CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_POSE);
   this->set_position(position);
   this->set_orientation(orientation);
 }
 
 CartesianPose::CartesianPose(const CartesianState& state) : CartesianState(state) {
   // set all the state variables to 0 except position and orientation
+  this->set_type(StateType::CARTESIAN_POSE);
   this->set_zero();
   this->set_pose(state.get_pose());
   this->set_empty(state.is_empty());
diff --git a/source/state_representation/src/space/cartesian/CartesianState.cpp b/source/state_representation/src/space/cartesian/CartesianState.cpp
index a593c4659..2376608bb 100644
--- a/source/state_representation/src/space/cartesian/CartesianState.cpp
+++ b/source/state_representation/src/space/cartesian/CartesianState.cpp
@@ -6,12 +6,12 @@
 using namespace state_representation::exceptions;
 
 namespace state_representation {
-CartesianState::CartesianState() : SpatialState(StateType::CARTESIANSTATE) {
+CartesianState::CartesianState() : SpatialState(StateType::CARTESIAN_STATE) {
   this->initialize();
 }
 
-CartesianState::CartesianState(const std::string& robot_name, const std::string& reference) :
-    SpatialState(StateType::CARTESIANSTATE, robot_name, reference) {
+CartesianState::CartesianState(const std::string& name, const std::string& reference) :
+    SpatialState(StateType::CARTESIAN_STATE, name, reference) {
   this->initialize();
 }
 
@@ -45,6 +45,232 @@ CartesianState CartesianState::Random(const std::string& name, const std::string
   return random;
 }
 
+const Eigen::Vector3d& CartesianState::get_position() const {
+  return this->position_;
+}
+
+const Eigen::Quaterniond& CartesianState::get_orientation() const {
+  return this->orientation_;
+}
+
+Eigen::Vector4d CartesianState::get_orientation_coefficients() const {
+  return Eigen::Vector4d(
+      this->get_orientation().w(), this->get_orientation().x(), this->get_orientation().y(),
+      this->get_orientation().z());
+}
+
+Eigen::Matrix<double, 7, 1> CartesianState::get_pose() const {
+  Eigen::Matrix<double, 7, 1> pose;
+  pose << this->get_position(), this->get_orientation_coefficients();
+  return pose;
+}
+
+Eigen::Matrix4d CartesianState::get_transformation_matrix() const {
+  Eigen::Matrix4d pose;
+  pose << this->orientation_.toRotationMatrix(), this->position_, 0., 0., 0., 1;
+  return pose;
+}
+
+const Eigen::Vector3d& CartesianState::get_linear_velocity() const {
+  return this->linear_velocity_;
+}
+
+const Eigen::Vector3d& CartesianState::get_angular_velocity() const {
+  return this->angular_velocity_;
+}
+
+Eigen::Matrix<double, 6, 1> CartesianState::get_twist() const {
+  Eigen::Matrix<double, 6, 1> twist;
+  twist << this->get_linear_velocity(), this->get_angular_velocity();
+  return twist;
+}
+
+const Eigen::Vector3d& CartesianState::get_linear_acceleration() const {
+  return this->linear_acceleration_;
+}
+
+const Eigen::Vector3d& CartesianState::get_angular_acceleration() const {
+  return this->angular_acceleration_;
+}
+
+Eigen::Matrix<double, 6, 1> CartesianState::get_acceleration() const {
+  Eigen::Matrix<double, 6, 1> acceleration;
+  acceleration << this->get_linear_acceleration(), this->get_angular_acceleration();
+  return acceleration;
+}
+
+const Eigen::Vector3d& CartesianState::get_force() const {
+  return this->force_;
+}
+
+const Eigen::Vector3d& CartesianState::get_torque() const {
+  return this->torque_;
+}
+
+Eigen::Matrix<double, 6, 1> CartesianState::get_wrench() const {
+  Eigen::Matrix<double, 6, 1> wrench;
+  wrench << this->get_force(), this->get_torque();
+  return wrench;
+}
+
+void CartesianState::set_position(const Eigen::Vector3d& position) {
+  this->set_state_variable(this->position_, position);
+}
+
+void CartesianState::set_position(const std::vector<double>& position) {
+  this->set_state_variable(this->position_, position);
+}
+
+void CartesianState::set_position(const double& x, const double& y, const double& z) {
+  this->set_position(Eigen::Vector3d(x, y, z));
+}
+
+void CartesianState::set_orientation(const Eigen::Quaterniond& orientation) {
+  this->set_filled();
+  this->orientation_ = orientation.normalized();
+}
+
+void CartesianState::set_orientation(const Eigen::Vector4d& orientation) {
+  this->set_orientation(Eigen::Quaterniond(orientation(0), orientation(1), orientation(2), orientation(3)));
+}
+
+void CartesianState::set_orientation(const std::vector<double>& orientation) {
+  if (orientation.size() != 4) {
+    throw exceptions::IncompatibleSizeException("The input vector is not of size 4 required for orientation");
+  }
+  this->set_orientation(Eigen::Vector4d::Map(orientation.data(), orientation.size()));
+}
+
+void CartesianState::set_orientation(const double& w, const double& x, const double& y, const double& z) {
+  this->set_orientation(Eigen::Vector4d(w, x, y, z));
+}
+
+void CartesianState::set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) {
+  this->set_position(position);
+  this->set_orientation(orientation);
+}
+
+void CartesianState::set_pose(const Eigen::Matrix<double, 7, 1>& pose) {
+  this->set_position(pose.head(3));
+  this->set_orientation(pose.tail(4));
+}
+
+void CartesianState::set_pose(const std::vector<double>& pose) {
+  if (pose.size() != 7) {
+    throw exceptions::IncompatibleSizeException("The input vector is not of size 7 required for pose");
+  }
+  this->set_position(std::vector<double>(pose.begin(), pose.begin() + 3));
+  this->set_orientation(std::vector<double>(pose.begin() + 3, pose.end()));
+}
+
+void CartesianState::set_linear_velocity(const Eigen::Vector3d& linear_velocity) {
+  this->set_state_variable(this->linear_velocity_, linear_velocity);
+}
+
+void CartesianState::set_linear_velocity(const std::vector<double>& linear_velocity) {
+  this->set_state_variable(this->linear_velocity_, linear_velocity);
+}
+
+void CartesianState::set_linear_velocity(const double& x, const double& y, const double& z) {
+  this->set_linear_velocity(Eigen::Vector3d(x, y, z));
+}
+
+void CartesianState::set_angular_velocity(const Eigen::Vector3d& angular_velocity) {
+  this->set_state_variable(this->angular_velocity_, angular_velocity);
+}
+
+void CartesianState::set_angular_velocity(const std::vector<double>& angular_velocity) {
+  this->set_state_variable(this->angular_velocity_, angular_velocity);
+}
+
+void CartesianState::set_angular_velocity(const double& x, const double& y, const double& z) {
+  this->set_angular_velocity(Eigen::Vector3d(x, y, z));
+}
+
+void CartesianState::set_twist(const Eigen::Matrix<double, 6, 1>& twist) {
+  this->set_state_variable(this->linear_velocity_, this->angular_velocity_, twist);
+}
+
+void CartesianState::set_twist(const std::vector<double>& twist) {
+  if (twist.size() != 6) {
+    throw exceptions::IncompatibleSizeException("The input vector is not of size 6 required for twist");
+  }
+  this->set_linear_velocity(std::vector<double>(twist.begin(), twist.begin() + 3));
+  this->set_angular_velocity(std::vector<double>(twist.begin() + 3, twist.end()));
+}
+
+void CartesianState::set_linear_acceleration(const Eigen::Vector3d& linear_acceleration) {
+  this->set_state_variable(this->linear_acceleration_, linear_acceleration);
+}
+
+void CartesianState::set_linear_acceleration(const std::vector<double>& linear_acceleration) {
+  this->set_state_variable(this->linear_acceleration_, linear_acceleration);
+}
+
+void CartesianState::set_linear_acceleration(const double& x, const double& y, const double& z) {
+  this->set_linear_acceleration(Eigen::Vector3d(x, y, z));
+}
+
+void CartesianState::set_angular_acceleration(const Eigen::Vector3d& angular_acceleration) {
+  this->set_state_variable(this->angular_acceleration_, angular_acceleration);
+}
+
+void CartesianState::set_angular_acceleration(const std::vector<double>& angular_acceleration) {
+  this->set_state_variable(this->angular_acceleration_, angular_acceleration);
+}
+
+void CartesianState::set_angular_acceleration(const double& x, const double& y, const double& z) {
+  this->set_angular_acceleration(Eigen::Vector3d(x, y, z));
+}
+
+void CartesianState::set_acceleration(const Eigen::Matrix<double, 6, 1>& acceleration) {
+  this->set_state_variable(this->linear_acceleration_, this->angular_acceleration_, acceleration);
+}
+
+void CartesianState::set_acceleration(const std::vector<double>& acceleration) {
+  if (acceleration.size() != 6) {
+    throw exceptions::IncompatibleSizeException("The input vector is not of size 6 required for acceleration");
+  }
+  this->set_linear_acceleration(std::vector<double>(acceleration.begin(), acceleration.begin() + 3));
+  this->set_angular_acceleration(std::vector<double>(acceleration.begin() + 3, acceleration.end()));
+}
+
+void CartesianState::set_force(const Eigen::Vector3d& force) {
+  this->set_state_variable(this->force_, force);
+}
+
+void CartesianState::set_force(const std::vector<double>& force) {
+  this->set_state_variable(this->force_, force);
+}
+
+void CartesianState::set_force(const double& x, const double& y, const double& z) {
+  this->set_force(Eigen::Vector3d(x, y, z));
+}
+
+void CartesianState::set_torque(const Eigen::Vector3d& torque) {
+  this->set_state_variable(this->torque_, torque);
+}
+
+void CartesianState::set_torque(const std::vector<double>& torque) {
+  this->set_state_variable(this->torque_, torque);
+}
+
+void CartesianState::set_torque(const double& x, const double& y, const double& z) {
+  this->set_torque(Eigen::Vector3d(x, y, z));
+}
+
+void CartesianState::set_wrench(const Eigen::Matrix<double, 6, 1>& wrench) {
+  this->set_state_variable(this->force_, this->torque_, wrench);
+}
+
+void CartesianState::set_wrench(const std::vector<double>& wrench) {
+  if (wrench.size() != 6) {
+    throw exceptions::IncompatibleSizeException("The input vector is not of size 6 required for wrench");
+  }
+  this->set_force(std::vector<double>(wrench.begin(), wrench.begin() + 3));
+  this->set_torque(std::vector<double>(wrench.begin() + 3, wrench.end()));
+}
+
 CartesianState& CartesianState::operator*=(double lambda) {
   // sanity check
   if (this->is_empty()) {
@@ -125,8 +351,9 @@ CartesianState& CartesianState::operator*=(const CartesianState& state) {
   Eigen::Vector3d f_alpha_b = this->get_angular_acceleration();
   // intermediate variables for b_S_c
   Eigen::Vector3d b_P_c = state.get_position();
-  Eigen::Quaterniond b_R_c = (this->get_orientation().dot(state.get_orientation()) > 0) ? state.get_orientation()
-                                                                                        : Eigen::Quaterniond(-state.get_orientation().coeffs());
+  Eigen::Quaterniond b_R_c =
+      (this->get_orientation().dot(state.get_orientation()) > 0) ? state.get_orientation() : Eigen::Quaterniond(
+          -state.get_orientation().coeffs());
   Eigen::Vector3d b_v_c = state.get_linear_velocity();
   Eigen::Vector3d b_omega_c = state.get_angular_velocity();
   Eigen::Vector3d b_a_c = state.get_linear_acceleration();
@@ -138,10 +365,9 @@ CartesianState& CartesianState::operator*=(const CartesianState& state) {
   this->set_linear_velocity(f_v_b + f_R_b * b_v_c + f_omega_b.cross(f_R_b * b_P_c));
   this->set_angular_velocity(f_omega_b + f_R_b * b_omega_c);
   // acceleration
-  this->set_linear_acceleration(f_a_b + f_R_b * b_a_c
-                                + f_alpha_b.cross(f_R_b * b_P_c)
-                                + 2 * f_omega_b.cross(f_R_b * b_v_c)
-                                + f_omega_b.cross(f_omega_b.cross(f_R_b * b_P_c)));
+  this->set_linear_acceleration(
+      f_a_b + f_R_b * b_a_c + f_alpha_b.cross(f_R_b * b_P_c) + 2 * f_omega_b.cross(f_R_b * b_v_c)
+          + f_omega_b.cross(f_omega_b.cross(f_R_b * b_P_c)));
   this->set_angular_acceleration(f_alpha_b + f_R_b * b_alpha_c + f_omega_b.cross(f_R_b * b_omega_c));
   // wrench
   //TODO
@@ -168,8 +394,9 @@ CartesianState& CartesianState::operator+=(const CartesianState& state) {
   // operation on pose
   this->set_position(this->get_position() + state.get_position());
   // specific operation on quaternion using Hamilton product
-  Eigen::Quaterniond orientation = (this->get_orientation().dot(state.get_orientation()) > 0) ? state.get_orientation()
-                                                                                              : Eigen::Quaterniond(-state.get_orientation().coeffs());
+  Eigen::Quaterniond orientation =
+      (this->get_orientation().dot(state.get_orientation()) > 0) ? state.get_orientation() : Eigen::Quaterniond(
+          -state.get_orientation().coeffs());
   this->set_orientation(this->get_orientation() * orientation);
   // operation on twist
   this->set_twist(this->get_twist() + state.get_twist());
@@ -200,8 +427,9 @@ CartesianState& CartesianState::operator-=(const CartesianState& state) {
   // operation on pose
   this->set_position(this->get_position() - state.get_position());
   // specific operation on quaternion using Hamilton product
-  Eigen::Quaterniond orientation = (this->get_orientation().dot(state.get_orientation()) > 0) ? state.get_orientation()
-                                                                                              : Eigen::Quaterniond(-state.get_orientation().coeffs());
+  Eigen::Quaterniond orientation =
+      (this->get_orientation().dot(state.get_orientation()) > 0) ? state.get_orientation() : Eigen::Quaterniond(
+          -state.get_orientation().coeffs());
   this->set_orientation(this->get_orientation() * orientation.conjugate());
   // operation on twist
   this->set_twist(this->get_twist() - state.get_twist());
diff --git a/source/state_representation/src/space/cartesian/CartesianTwist.cpp b/source/state_representation/src/space/cartesian/CartesianTwist.cpp
index f63745de6..49cc49f24 100644
--- a/source/state_representation/src/space/cartesian/CartesianTwist.cpp
+++ b/source/state_representation/src/space/cartesian/CartesianTwist.cpp
@@ -4,12 +4,19 @@
 using namespace state_representation::exceptions;
 
 namespace state_representation {
+CartesianTwist::CartesianTwist() {
+  this->set_type(StateType::CARTESIAN_TWIST);
+}
+
 CartesianTwist::CartesianTwist(const std::string& name, const std::string& reference) :
-    CartesianState(name, reference) {}
+    CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_TWIST);
+}
 
 CartesianTwist::CartesianTwist(
     const std::string& name, const Eigen::Vector3d& linear_velocity, const std::string& reference
 ) : CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_TWIST);
   this->set_linear_velocity(linear_velocity);
 }
 
@@ -17,6 +24,7 @@ CartesianTwist::CartesianTwist(
     const std::string& name, const Eigen::Vector3d& linear_velocity, const Eigen::Vector3d& angular_velocity,
     const std::string& reference
 ) : CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_TWIST);
   this->set_linear_velocity(linear_velocity);
   this->set_angular_velocity(angular_velocity);
 }
@@ -24,11 +32,13 @@ CartesianTwist::CartesianTwist(
 CartesianTwist::CartesianTwist(
     const std::string& name, const Eigen::Matrix<double, 6, 1>& twist, const std::string& reference
 ) : CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_TWIST);
   this->set_twist(twist);
 }
 
 CartesianTwist::CartesianTwist(const CartesianState& state) : CartesianState(state) {
   // set all the state variables to 0 except linear and angular velocities
+  this->set_type(StateType::CARTESIAN_TWIST);
   this->set_zero();
   this->set_twist(state.get_twist());
   this->set_empty(state.is_empty());
diff --git a/source/state_representation/src/space/cartesian/CartesianWrench.cpp b/source/state_representation/src/space/cartesian/CartesianWrench.cpp
index 4e5dc889a..e3a1714d1 100644
--- a/source/state_representation/src/space/cartesian/CartesianWrench.cpp
+++ b/source/state_representation/src/space/cartesian/CartesianWrench.cpp
@@ -3,17 +3,25 @@
 using namespace state_representation::exceptions;
 
 namespace state_representation {
+CartesianWrench::CartesianWrench() {
+  this->set_type(StateType::CARTESIAN_WRENCH);
+}
+
 CartesianWrench::CartesianWrench(const std::string& name, const std::string& reference) :
-    CartesianState(name, reference) {}
+    CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_WRENCH);
+}
 
 CartesianWrench::CartesianWrench(const std::string& name, const Eigen::Vector3d& force, const std::string& reference) :
     CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_WRENCH);
   this->set_force(force);
 }
 
 CartesianWrench::CartesianWrench(
     const std::string& name, const Eigen::Vector3d& force, const Eigen::Vector3d& torque, const std::string& reference
 ) : CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_WRENCH);
   this->set_force(force);
   this->set_torque(torque);
 }
@@ -21,11 +29,13 @@ CartesianWrench::CartesianWrench(
 CartesianWrench::CartesianWrench(
     const std::string& name, const Eigen::Matrix<double, 6, 1>& wrench, const std::string& reference
 ) : CartesianState(name, reference) {
+  this->set_type(StateType::CARTESIAN_WRENCH);
   this->set_wrench(wrench);
 }
 
 CartesianWrench::CartesianWrench(const CartesianState& state) : CartesianState(state) {
   // set all the state variables to 0 except force and torque
+  this->set_type(StateType::CARTESIAN_WRENCH);
   this->set_zero();
   this->set_wrench(state.get_wrench());
   this->set_empty(state.is_empty());
diff --git a/source/state_representation/src/space/joint/JointAccelerations.cpp b/source/state_representation/src/space/joint/JointAccelerations.cpp
index 52b99ffe8..4fd7f2c02 100644
--- a/source/state_representation/src/space/joint/JointAccelerations.cpp
+++ b/source/state_representation/src/space/joint/JointAccelerations.cpp
@@ -4,25 +4,35 @@
 using namespace state_representation::exceptions;
 
 namespace state_representation {
+JointAccelerations::JointAccelerations() {
+  this->set_type(StateType::JOINT_ACCELERATIONS);
+}
+
 JointAccelerations::JointAccelerations(const std::string& robot_name, unsigned int nb_joints) :
-    JointState(robot_name, nb_joints) {}
+    JointState(robot_name, nb_joints) {
+  this->set_type(StateType::JOINT_ACCELERATIONS);
+}
 
 JointAccelerations::JointAccelerations(const std::string& robot_name, const Eigen::VectorXd& accelerations) :
     JointState(robot_name, accelerations.size()) {
+  this->set_type(StateType::JOINT_ACCELERATIONS);
   this->set_accelerations(accelerations);
 }
 
 JointAccelerations::JointAccelerations(const std::string& robot_name, const std::vector<std::string>& joint_names) :
-    JointState(robot_name, joint_names) {}
+    JointState(robot_name, joint_names) {
+  this->set_type(StateType::JOINT_ACCELERATIONS);}
 
 JointAccelerations::JointAccelerations(const std::string& robot_name,
                                        const std::vector<std::string>& joint_names,
                                        const Eigen::VectorXd& accelerations) : JointState(robot_name, joint_names) {
+  this->set_type(StateType::JOINT_ACCELERATIONS);
   this->set_accelerations(accelerations);
 }
 
 JointAccelerations::JointAccelerations(const JointState& state) : JointState(state) {
   // set all the state variables to 0 except accelerations
+  this->set_type(StateType::JOINT_ACCELERATIONS);
   this->set_zero();
   this->set_accelerations(state.get_accelerations());
   this->set_empty(state.is_empty());
diff --git a/source/state_representation/src/space/joint/JointPositions.cpp b/source/state_representation/src/space/joint/JointPositions.cpp
index 767222cf3..7e23f38a8 100644
--- a/source/state_representation/src/space/joint/JointPositions.cpp
+++ b/source/state_representation/src/space/joint/JointPositions.cpp
@@ -5,25 +5,36 @@
 using namespace state_representation::exceptions;
 
 namespace state_representation {
+JointPositions::JointPositions() {
+  this->set_type(StateType::JOINT_POSITIONS);
+}
+
 JointPositions::JointPositions(const std::string& robot_name, unsigned int nb_joints) :
-    JointState(robot_name, nb_joints) {}
+    JointState(robot_name, nb_joints) {
+  this->set_type(StateType::JOINT_POSITIONS);
+}
 
 JointPositions::JointPositions(const std::string& robot_name, const Eigen::VectorXd& positions) :
     JointState(robot_name, positions.size()) {
+  this->set_type(StateType::JOINT_POSITIONS);
   this->set_positions(positions);
 }
 
 JointPositions::JointPositions(const std::string& robot_name, const std::vector<std::string>& joint_names) :
-    JointState(robot_name, joint_names) {}
+    JointState(robot_name, joint_names) {
+  this->set_type(StateType::JOINT_POSITIONS);
+}
 
 JointPositions::JointPositions(
     const std::string& robot_name, const std::vector<std::string>& joint_names, const Eigen::VectorXd& positions
 ) : JointState(robot_name, joint_names) {
+  this->set_type(StateType::JOINT_POSITIONS);
   this->set_positions(positions);
 }
 
 JointPositions::JointPositions(const JointState& state) : JointState(state) {
   // set all the state variables to 0 except positions
+  this->set_type(StateType::JOINT_POSITIONS);
   this->set_zero();
   this->set_positions(state.get_positions());
   this->set_empty(state.is_empty());
diff --git a/source/state_representation/src/space/joint/JointState.cpp b/source/state_representation/src/space/joint/JointState.cpp
index fbf14d639..136a6ac06 100644
--- a/source/state_representation/src/space/joint/JointState.cpp
+++ b/source/state_representation/src/space/joint/JointState.cpp
@@ -13,18 +13,18 @@ static void assert_index_in_range(unsigned int joint_index, unsigned int size) {
   }
 }
 
-JointState::JointState() : State(StateType::JOINTSTATE) {
+JointState::JointState() : State(StateType::JOINT_STATE) {
   this->initialize();
 }
 
 JointState::JointState(const std::string& robot_name, unsigned int nb_joints) :
-    State(StateType::JOINTSTATE, robot_name), names_(nb_joints) {
+    State(StateType::JOINT_STATE, robot_name), names_(nb_joints) {
   this->set_names(nb_joints);
   this->initialize();
 }
 
 JointState::JointState(const std::string& robot_name, const std::vector<std::string>& joint_names) :
-    State(StateType::JOINTSTATE, robot_name), names_(joint_names) {
+    State(StateType::JOINT_STATE, robot_name), names_(joint_names) {
   this->initialize();
 }
 
diff --git a/source/state_representation/src/space/joint/JointTorques.cpp b/source/state_representation/src/space/joint/JointTorques.cpp
index bf6f8a1b8..e5e394ecb 100644
--- a/source/state_representation/src/space/joint/JointTorques.cpp
+++ b/source/state_representation/src/space/joint/JointTorques.cpp
@@ -3,23 +3,34 @@
 using namespace state_representation::exceptions;
 
 namespace state_representation {
-JointTorques::JointTorques(const std::string& robot_name, unsigned int nb_joints) : JointState(robot_name, nb_joints) {}
+JointTorques::JointTorques() {
+  this->set_type(StateType::JOINT_TORQUES);
+}
+
+JointTorques::JointTorques(const std::string& robot_name, unsigned int nb_joints) : JointState(robot_name, nb_joints) {
+  this->set_type(StateType::JOINT_TORQUES);
+}
 
 JointTorques::JointTorques(const std::string& robot_name, const Eigen::VectorXd& torques) :
     JointState(robot_name, torques.size()) {
+  this->set_type(StateType::JOINT_TORQUES);
   this->set_torques(torques);
 }
 
 JointTorques::JointTorques(const std::string& robot_name, const std::vector<std::string>& joint_names) :
-    JointState(robot_name, joint_names) {}
+    JointState(robot_name, joint_names) {
+  this->set_type(StateType::JOINT_TORQUES);
+}
 
 JointTorques::JointTorques(const std::string& robot_name, const std::vector<std::string>& joint_names,
                            const Eigen::VectorXd& torques) : JointState(robot_name, joint_names) {
+  this->set_type(StateType::JOINT_TORQUES);
   this->set_torques(torques);
 }
 
 JointTorques::JointTorques(const JointState& state) : JointState(state) {
   // set all the state variables to 0 except torques
+  this->set_type(StateType::JOINT_TORQUES);
   this->set_zero();
   this->set_torques(state.get_torques());
   this->set_empty(state.is_empty());
diff --git a/source/state_representation/src/space/joint/JointVelocities.cpp b/source/state_representation/src/space/joint/JointVelocities.cpp
index 33fdd9759..737c69a4b 100644
--- a/source/state_representation/src/space/joint/JointVelocities.cpp
+++ b/source/state_representation/src/space/joint/JointVelocities.cpp
@@ -4,25 +4,36 @@
 using namespace state_representation::exceptions;
 
 namespace state_representation {
+JointVelocities::JointVelocities() {
+  this->set_type(StateType::JOINT_VELOCITIES);
+}
+
 JointVelocities::JointVelocities(const std::string& robot_name, unsigned int nb_joints) :
-    JointState(robot_name, nb_joints) {}
+    JointState(robot_name, nb_joints) {
+  this->set_type(StateType::JOINT_VELOCITIES);
+}
 
 JointVelocities::JointVelocities(const std::string& robot_name, const Eigen::VectorXd& velocities) :
     JointState(robot_name, velocities.size()) {
+  this->set_type(StateType::JOINT_VELOCITIES);
   this->set_velocities(velocities);
 }
 
 JointVelocities::JointVelocities(const std::string& robot_name, const std::vector<std::string>& joint_names) :
-    JointState(robot_name, joint_names) {}
+    JointState(robot_name, joint_names) {
+  this->set_type(StateType::JOINT_VELOCITIES);
+}
 
 JointVelocities::JointVelocities(
     const std::string& robot_name, const std::vector<std::string>& joint_names, const Eigen::VectorXd& velocities
 ) : JointState(robot_name, joint_names) {
+  this->set_type(StateType::JOINT_VELOCITIES);
   this->set_velocities(velocities);
 }
 
 JointVelocities::JointVelocities(const JointState& state) : JointState(state) {
   // set all the state variables to 0 except velocities
+  this->set_type(StateType::JOINT_VELOCITIES);
   this->set_zero();
   this->set_velocities(state.get_velocities());
   this->set_empty(state.is_empty());
diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_acceleration.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_acceleration.cpp
index 667a166bf..262e543f7 100644
--- a/source/state_representation/test/tests/space/cartesian/test_cartesian_acceleration.cpp
+++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_acceleration.cpp
@@ -12,8 +12,14 @@ static void expect_only_acceleration(CartesianAcceleration& acceleration) {
   EXPECT_EQ(static_cast<CartesianState&>(acceleration).get_wrench().norm(), 0);
 }
 
+TEST(CartesianAccelerationTest, EmptyConstructor) {
+  CartesianAcceleration empty;
+  EXPECT_EQ(empty.get_type(), StateType::CARTESIAN_ACCELERATION);
+}
+
 TEST(CartesianAccelerationTest, RandomAccelerationInitialization) {
   CartesianAcceleration random = CartesianAcceleration::Random("test");
+  EXPECT_EQ(random.get_type(), StateType::CARTESIAN_ACCELERATION);
   EXPECT_GT(random.get_acceleration().norm(), 0);
   expect_only_acceleration(random);
 }
@@ -21,12 +27,14 @@ TEST(CartesianAccelerationTest, RandomAccelerationInitialization) {
 TEST(CartesianAccelerationTest, CopyAcceleration) {
   CartesianAcceleration acc1 = CartesianAcceleration::Random("test");
   CartesianAcceleration acc2(acc1);
+  EXPECT_EQ(acc1.get_type(), acc2.get_type());
   EXPECT_EQ(acc1.get_name(), acc2.get_name());
   EXPECT_EQ(acc1.get_reference_frame(), acc2.get_reference_frame());
   EXPECT_EQ(acc1.data(), acc2.data());
   expect_only_acceleration(acc2);
 
   CartesianAcceleration acc3 = acc1;
+  EXPECT_EQ(acc1.get_type(), acc3.get_type());
   EXPECT_EQ(acc1.get_name(), acc3.get_name());
   EXPECT_EQ(acc1.get_reference_frame(), acc3.get_reference_frame());
   EXPECT_EQ(acc1.data(), acc3.data());
diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp
index 80a1ea4dd..8d4721598 100644
--- a/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp
+++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp
@@ -34,8 +34,14 @@ class CartesianPoseTestClass : public testing::Test {
   double tol = 1e-5;
 };
 
+TEST(CartesianPoseTest, EmptyConstructor) {
+  CartesianPose empty;
+  EXPECT_EQ(empty.get_type(), StateType::CARTESIAN_POSE);
+}
+
 TEST(CartesianPoseTest, RandomPoseInitialization) {
   CartesianPose random = CartesianPose::Random("test");
+  EXPECT_EQ(random.get_type(), StateType::CARTESIAN_POSE);
   EXPECT_NE(random.get_position().norm(), 0);
   EXPECT_NE(random.get_orientation().w(), 0);
   EXPECT_NE(random.get_orientation().x(), 0);
@@ -47,12 +53,14 @@ TEST(CartesianPoseTest, RandomPoseInitialization) {
 TEST(CartesianPoseTest, CopyPose) {
   CartesianPose pose1 = CartesianPose::Random("test");
   CartesianPose pose2(pose1);
+  EXPECT_EQ(pose1.get_type(), pose2.get_type());
   EXPECT_EQ(pose1.get_name(), pose2.get_name());
   EXPECT_EQ(pose1.get_reference_frame(), pose2.get_reference_frame());
   EXPECT_EQ(pose1.data(), pose2.data());
   expect_only_pose(pose2);
 
   CartesianPose pose3 = pose1;
+  EXPECT_EQ(pose1.get_type(), pose3.get_type());
   EXPECT_EQ(pose1.get_name(), pose3.get_name());
   EXPECT_EQ(pose1.get_reference_frame(), pose3.get_reference_frame());
   EXPECT_EQ(pose1.data(), pose3.data());
@@ -152,6 +160,7 @@ TEST(CartesianPoseTest, MultiplyPoseAndState) {
   CartesianState s = CartesianPose::Random("test2", "test");
   CartesianState res = p * s;
   CartesianPose res2 = p * static_cast<CartesianPose>(s);
+  EXPECT_EQ(res2.get_type(), StateType::CARTESIAN_POSE);
   expect_near(res.get_position(), res2.get_position());
   EXPECT_GT(abs(res.get_orientation().dot(res2.get_orientation())), 1 - 1e-5);
 }
@@ -162,6 +171,7 @@ TEST_F(CartesianPoseTestClass, TestAddTwoPoses) {
   Eigen::Quaterniond rot2(0, 1, 0, 0);
   tf2 = CartesianPose("t1", pos2, rot2);
   auto tf3 = tf1 + tf2;
+  EXPECT_EQ(tf3.get_type(), StateType::CARTESIAN_POSE);
   Eigen::Vector3d pos_truth(1, 0, 0);
   Eigen::Quaterniond rot_truth(0, -1, 0, 0);
   expect_near(tf3.get_position(), pos_truth);
@@ -173,18 +183,22 @@ TEST_F(CartesianPoseTestClass, TestPoseToVelocity) {
   std::chrono::seconds dt1(1);
   std::chrono::milliseconds dt2(100);
   auto res1 = tf1 / dt1;
+  EXPECT_EQ(res1.get_type(), StateType::CARTESIAN_TWIST);
   EXPECT_EQ(res1.get_linear_velocity(), tf1.get_position());
   EXPECT_EQ(res1.get_angular_velocity(), Eigen::Vector3d(M_PI, 0, 0));
   auto res2 = tf1 / dt2;
+  EXPECT_EQ(res2.get_type(), StateType::CARTESIAN_TWIST);
   EXPECT_EQ(res2.get_linear_velocity(), 10 * tf1.get_position());
   EXPECT_EQ(res2.get_angular_velocity(), 10 * Eigen::Vector3d(M_PI, 0, 0));
 }
 
 TEST_F(CartesianPoseTestClass, TestImplicitConversion) {
   CartesianTwist vel("t1");
+  EXPECT_EQ(vel.get_type(), StateType::CARTESIAN_TWIST);
   vel.set_linear_velocity(Eigen::Vector3d(0.1, 0.1, 0.1));
   vel.set_angular_velocity(Eigen::Vector3d(M_PI, 0, 0));
   tf1 += vel;
+  EXPECT_EQ(tf1.get_type(), StateType::CARTESIAN_POSE);
   Eigen::Vector3d pos_truth(1.1, 2.1, 3.1);
   Eigen::Quaterniond rot_truth(0, 1, 0, 0);
   EXPECT_EQ(tf1.get_position(), pos_truth);
diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp
index 2773610ad..6999744bf 100644
--- a/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp
+++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp
@@ -12,7 +12,7 @@ static void assert_name_empty_frame_equal(
     const CartesianState& state1, const std::string& name, bool empty, const std::string& reference_frame
 ) {
   EXPECT_EQ(state1.get_name(), name);
-  EXPECT_EQ(state1.get_type(), StateType::CARTESIANSTATE);
+  EXPECT_EQ(state1.get_type(), StateType::CARTESIAN_STATE);
   EXPECT_EQ(state1.is_empty(), empty);
   EXPECT_EQ(state1.get_reference_frame(), reference_frame);
 }
@@ -45,6 +45,7 @@ void test_clamping(
 
 TEST(CartesianStateTest, Constructors) {
   CartesianState empty1;
+  EXPECT_EQ(empty1.get_type(), StateType::CARTESIAN_STATE);
   assert_name_empty_frame_equal(empty1, "", true, "world");
   EXPECT_FLOAT_EQ(empty1.data().norm(), 1);
 
@@ -103,6 +104,8 @@ TEST(CartesianStateTest, CopyConstructor) {
 
 TEST(CartesianStateTest, GetSetFields) {
   CartesianState cs("test");
+  static Eigen::Vector3d data;
+  static std::vector<double> std_data(3);
 
   // name
   cs.set_name("robot");
@@ -112,10 +115,10 @@ TEST(CartesianStateTest, GetSetFields) {
   EXPECT_EQ(cs.get_reference_frame(), "base");
 
   // position
-  std::vector<double> position{1, 2, 3};
-  cs.set_position(position);
-  for (std::size_t i = 0; i < position.size(); ++i) {
-    EXPECT_FLOAT_EQ(cs.get_position()(i), position.at(i));
+  std_data = {1, 2, 3};
+  cs.set_position(std_data);
+  for (std::size_t i = 0; i < std_data.size(); ++i) {
+    EXPECT_FLOAT_EQ(cs.get_position()(i), std_data.at(i));
   }
   cs.set_position(1.1, 2.2, 3.3);
   EXPECT_TRUE(Eigen::Vector3d(1.1, 2.2, 3.3).isApprox(cs.get_position()));
@@ -132,7 +135,10 @@ TEST(CartesianStateTest, GetSetFields) {
       orientation{random_orientation.w(), random_orientation.x(), random_orientation.y(), random_orientation.z()};
   cs.set_orientation(orientation);
   EXPECT_TRUE(random_orientation.coeffs().isApprox(cs.get_orientation().coeffs()));
-  EXPECT_THROW(cs.set_orientation(position), exceptions::IncompatibleSizeException);
+  random_orientation = Eigen::Quaterniond::UnitRandom();
+  cs.set_orientation(random_orientation.w(), random_orientation.x(), random_orientation.y(), random_orientation.z());
+  EXPECT_TRUE(random_orientation.coeffs().isApprox(cs.get_orientation().coeffs()));
+  EXPECT_THROW(cs.set_orientation(std_data), exceptions::IncompatibleSizeException);
 
   auto matrix = cs.get_transformation_matrix();
   Eigen::Vector3d trans = matrix.topRightCorner<3, 1>();
@@ -145,38 +151,116 @@ TEST(CartesianStateTest, GetSetFields) {
   // pose
   EXPECT_THROW(cs.set_pose(std::vector<double>(8)), exceptions::IncompatibleSizeException);
 
+  // linear velocity
+  data = Eigen::Vector3d::Random();
+  cs.set_linear_velocity(data);
+  EXPECT_TRUE(cs.get_linear_velocity().isApprox(data));
+  std_data = {2, 3, 4};
+  cs.set_linear_velocity(std_data);
+  for (std::size_t i = 0; i < std_data.size(); ++i) {
+    EXPECT_FLOAT_EQ(cs.get_linear_velocity()(i), std_data.at(i));
+  }
+  cs.set_linear_velocity(2.1, 3.2, 4.3);
+  EXPECT_TRUE(Eigen::Vector3d(2.1, 3.2, 4.3).isApprox(cs.get_linear_velocity()));
+  EXPECT_THROW(cs.set_linear_velocity(std::vector<double>(4)), exceptions::IncompatibleSizeException);
+
+  // angular velocity
+  data = Eigen::Vector3d::Random();
+  cs.set_angular_velocity(data);
+  EXPECT_TRUE(cs.get_angular_velocity().isApprox(data));
+  std_data = {3, 4, 5};
+  cs.set_angular_velocity(std_data);
+  for (std::size_t i = 0; i < std_data.size(); ++i) {
+    EXPECT_FLOAT_EQ(cs.get_angular_velocity()(i), std_data.at(i));
+  }
+  cs.set_angular_velocity(3.1, 4.2, 5.3);
+  EXPECT_TRUE(Eigen::Vector3d(3.1, 4.2, 5.3).isApprox(cs.get_angular_velocity()));
+  EXPECT_THROW(cs.set_angular_velocity(std::vector<double>(4)), exceptions::IncompatibleSizeException);
+
   // twist
-  Eigen::Vector3d linear_velocity = Eigen::Vector3d::Random();
-  cs.set_linear_velocity(linear_velocity);
-  EXPECT_TRUE(cs.get_linear_velocity().isApprox(linear_velocity));
-  Eigen::Vector3d angular_velocity = Eigen::Vector3d::Random();
-  cs.set_angular_velocity(angular_velocity);
-  EXPECT_TRUE(cs.get_angular_velocity().isApprox(angular_velocity));
-  Eigen::VectorXd twist = Eigen::VectorXd::Random(6);
+  Eigen::VectorXd twist_eigen = Eigen::VectorXd::Random(6);
+  cs.set_twist(twist_eigen);
+  EXPECT_TRUE(cs.get_twist().isApprox(twist_eigen));
+  std::vector<double> twist{4, 5, 6, 7, 8, 9};
   cs.set_twist(twist);
-  EXPECT_TRUE(cs.get_twist().isApprox(twist));
+  for (std::size_t i = 0; i < twist.size(); ++i) {
+    EXPECT_FLOAT_EQ(cs.get_twist()(i), twist.at(i));
+  }
+  EXPECT_THROW(cs.set_twist(std::vector<double>(4)), exceptions::IncompatibleSizeException);
+
+  // linear acceleration
+  data = Eigen::Vector3d::Random();
+  cs.set_linear_acceleration(data);
+  EXPECT_TRUE(cs.get_linear_acceleration().isApprox(data));
+  std_data = {5, 6, 7};
+  cs.set_linear_acceleration(std_data);
+  for (std::size_t i = 0; i < std_data.size(); ++i) {
+    EXPECT_FLOAT_EQ(cs.get_linear_acceleration()(i), std_data.at(i));
+  }
+  cs.set_linear_acceleration(5.1, 6.2, 7.3);
+  EXPECT_TRUE(Eigen::Vector3d(5.1, 6.2, 7.3).isApprox(cs.get_linear_acceleration()));
+  EXPECT_THROW(cs.set_linear_acceleration(std::vector<double>(4)), exceptions::IncompatibleSizeException);
+
+  // angular acceleration
+  data = Eigen::Vector3d::Random();
+  cs.set_angular_acceleration(data);
+  EXPECT_TRUE(cs.get_angular_acceleration().isApprox(data));
+  std_data = {6, 7, 8};
+  cs.set_angular_acceleration(std_data);
+  for (std::size_t i = 0; i < std_data.size(); ++i) {
+    EXPECT_FLOAT_EQ(cs.get_angular_acceleration()(i), std_data.at(i));
+  }
+  cs.set_angular_acceleration(6.1, 7.2, 8.3);
+  EXPECT_TRUE(Eigen::Vector3d(6.1, 7.2, 8.3).isApprox(cs.get_angular_acceleration()));
+  EXPECT_THROW(cs.set_angular_acceleration(std::vector<double>(4)), exceptions::IncompatibleSizeException);
 
   // acceleration
-  Eigen::Vector3d linear_acceleration = Eigen::Vector3d::Random();
-  cs.set_linear_acceleration(linear_acceleration);
-  EXPECT_TRUE(cs.get_linear_acceleration().isApprox(linear_acceleration));
-  Eigen::Vector3d angular_acceleration = Eigen::Vector3d::Random();
-  cs.set_angular_acceleration(angular_acceleration);
-  EXPECT_TRUE(cs.get_angular_acceleration().isApprox(angular_acceleration));
-  Eigen::VectorXd accerlerations = Eigen::VectorXd::Random(6);
-  cs.set_acceleration(accerlerations);
-  EXPECT_TRUE(cs.get_acceleration().isApprox(accerlerations));
+  Eigen::VectorXd acceleration_eigen = Eigen::VectorXd::Random(6);
+  cs.set_acceleration(acceleration_eigen);
+  EXPECT_TRUE(cs.get_acceleration().isApprox(acceleration_eigen));
+  std::vector<double> acceleration{7, 8, 9, 10, 11, 12};
+  cs.set_acceleration(acceleration);
+  for (std::size_t i = 0; i < acceleration.size(); ++i) {
+    EXPECT_FLOAT_EQ(cs.get_acceleration()(i), acceleration.at(i));
+  }
+  EXPECT_THROW(cs.set_acceleration(std::vector<double>(4)), exceptions::IncompatibleSizeException);
+
+  // force
+  data = Eigen::Vector3d::Random();
+  cs.set_force(data);
+  EXPECT_TRUE(cs.get_force().isApprox(data));
+  std_data = {8, 9, 10};
+  cs.set_force(std_data);
+  for (std::size_t i = 0; i < std_data.size(); ++i) {
+    EXPECT_FLOAT_EQ(cs.get_force()(i), std_data.at(i));
+  }
+  cs.set_force(8.1, 9.2, 10.3);
+  EXPECT_TRUE(Eigen::Vector3d(8.1, 9.2, 10.3).isApprox(cs.get_force()));
+  EXPECT_THROW(cs.set_force(std::vector<double>(4)), exceptions::IncompatibleSizeException);
+
+  // torque
+  data = Eigen::Vector3d::Random();
+  cs.set_torque(data);
+  EXPECT_TRUE(cs.get_torque().isApprox(data));
+  std_data = {9, 10, 11};
+  cs.set_torque(std_data);
+  for (std::size_t i = 0; i < std_data.size(); ++i) {
+    EXPECT_FLOAT_EQ(cs.get_torque()(i), std_data.at(i));
+  }
+  cs.set_torque(9.1, 10.2, 11.3);
+  EXPECT_TRUE(Eigen::Vector3d(9.1, 10.2, 11.3).isApprox(cs.get_torque()));
+  EXPECT_THROW(cs.set_torque(std::vector<double>(4)), exceptions::IncompatibleSizeException);
 
   // wrench
-  Eigen::Vector3d force = Eigen::Vector3d::Random();
-  cs.set_force(force);
-  EXPECT_TRUE(cs.get_force().isApprox(force));
-  Eigen::Vector3d torque = Eigen::Vector3d::Random();
-  cs.set_torque(torque);
-  EXPECT_TRUE(cs.get_torque().isApprox(torque));
-  Eigen::VectorXd wrench = Eigen::VectorXd::Random(6);
+  Eigen::VectorXd wrench_eigen = Eigen::VectorXd::Random(6);
+  cs.set_wrench(wrench_eigen);
+  EXPECT_TRUE(cs.get_wrench().isApprox(wrench_eigen));
+  std::vector<double> wrench{10, 11, 12, 13, 14, 15};
   cs.set_wrench(wrench);
-  EXPECT_TRUE(cs.get_wrench().isApprox(wrench));
+  for (std::size_t i = 0; i < wrench.size(); ++i) {
+    EXPECT_FLOAT_EQ(cs.get_wrench()(i), wrench.at(i));
+  }
+  EXPECT_THROW(cs.set_wrench(std::vector<double>(4)), exceptions::IncompatibleSizeException);
 
   cs.set_zero();
   EXPECT_FLOAT_EQ(cs.data().norm(), 1);
diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_twist.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_twist.cpp
index b630bc2f2..c390787e8 100644
--- a/source/state_representation/test/tests/space/cartesian/test_cartesian_twist.cpp
+++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_twist.cpp
@@ -12,8 +12,14 @@ static void expect_only_twist(CartesianTwist& twist) {
   EXPECT_EQ(static_cast<CartesianState&>(twist).get_wrench().norm(), 0);
 }
 
+TEST(CartesianTwistTest, EmptyConstructor) {
+  CartesianTwist empty;
+  EXPECT_EQ(empty.get_type(), StateType::CARTESIAN_TWIST);
+}
+
 TEST(CartesianTwistTest, RandomTwistInitialization) {
   CartesianTwist random = CartesianTwist::Random("test");
+  EXPECT_EQ(random.get_type(), StateType::CARTESIAN_TWIST);
   EXPECT_GT(random.get_twist().norm(), 0);
   expect_only_twist(random);
 }
@@ -21,12 +27,14 @@ TEST(CartesianTwistTest, RandomTwistInitialization) {
 TEST(CartesianTwistTest, CopyTwist) {
   CartesianTwist twist1 = CartesianTwist::Random("test");
   CartesianTwist twist2(twist1);
+  EXPECT_EQ(twist1.get_type(), twist2.get_type());
   EXPECT_EQ(twist1.get_name(), twist2.get_name());
   EXPECT_EQ(twist1.get_reference_frame(), twist2.get_reference_frame());
   EXPECT_EQ(twist1.data(), twist2.data());
   expect_only_twist(twist2);
 
   CartesianTwist twist3 = twist1;
+  EXPECT_EQ(twist1.get_type(), twist3.get_type());
   EXPECT_EQ(twist1.get_name(), twist3.get_name());
   EXPECT_EQ(twist1.get_reference_frame(), twist3.get_reference_frame());
   EXPECT_EQ(twist1.data(), twist3.data());
@@ -115,9 +123,11 @@ TEST(CartesianTwistTest, TestVelocityToAcceleration) {
   std::chrono::seconds dt1(1);
   std::chrono::milliseconds dt2(100);
   auto res1 = twist / dt1;
+  EXPECT_EQ(res1.get_type(), StateType::CARTESIAN_ACCELERATION);
   EXPECT_EQ(res1.get_linear_acceleration(), twist.get_linear_velocity());
   EXPECT_EQ(res1.get_angular_acceleration(), twist.get_angular_velocity());
   auto res2 = twist / dt2;
+  EXPECT_EQ(res2.get_type(), StateType::CARTESIAN_ACCELERATION);
   EXPECT_TRUE(res2.get_linear_acceleration().isApprox(10 * twist.get_linear_velocity()));
   EXPECT_TRUE(res2.get_angular_acceleration().isApprox(10 * twist.get_angular_velocity()));
 }
@@ -127,6 +137,7 @@ TEST(CartesianTwistTest, TestImplicitConversion) {
   acc.set_linear_acceleration(Eigen::Vector3d(0.1, 0.1, 0.1));
   acc.set_linear_acceleration(Eigen::Vector3d(M_PI, 0, 0));
   CartesianTwist twist(acc);
+  EXPECT_EQ(twist.get_type(), StateType::CARTESIAN_TWIST);
   EXPECT_EQ(twist.get_linear_velocity(), acc.get_linear_acceleration());
   EXPECT_EQ(twist.get_angular_velocity(), acc.get_angular_acceleration());
 }
diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_wrench.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_wrench.cpp
index 6a002dc1a..92caf2051 100644
--- a/source/state_representation/test/tests/space/cartesian/test_cartesian_wrench.cpp
+++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_wrench.cpp
@@ -12,8 +12,14 @@ static void expect_only_wrench(CartesianWrench& wrench) {
   EXPECT_EQ(static_cast<CartesianState&>(wrench).get_acceleration().norm(), 0);
 }
 
+TEST(CartesianWrenchTest, EmptyConstructor) {
+  CartesianWrench empty;
+  EXPECT_EQ(empty.get_type(), StateType::CARTESIAN_WRENCH);
+}
+
 TEST(CartesianWrenchTest, RandomWrenchInitialization) {
   CartesianWrench random = CartesianWrench::Random("test");
+  EXPECT_EQ(random.get_type(), StateType::CARTESIAN_WRENCH);
   EXPECT_NE(random.get_wrench().norm(), 0);
   expect_only_wrench(random);
 }
@@ -21,12 +27,14 @@ TEST(CartesianWrenchTest, RandomWrenchInitialization) {
 TEST(CartesianWrenchTest, CopyWrench) {
   CartesianWrench wrench1 = CartesianWrench::Random("test");
   CartesianWrench wrench2(wrench1);
+  EXPECT_EQ(wrench1.get_type(), wrench2.get_type());
   EXPECT_EQ(wrench1.get_name(), wrench2.get_name());
   EXPECT_EQ(wrench1.get_reference_frame(), wrench2.get_reference_frame());
   EXPECT_EQ(wrench1.data(), wrench2.data());
   expect_only_wrench(wrench2);
 
   CartesianWrench wrench3 = wrench1;
+  EXPECT_EQ(wrench1.get_type(), wrench3.get_type());
   EXPECT_EQ(wrench1.get_name(), wrench3.get_name());
   EXPECT_EQ(wrench1.get_reference_frame(), wrench3.get_reference_frame());
   EXPECT_EQ(wrench1.data(), wrench3.data());
diff --git a/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp b/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp
index 272c28638..f44c61152 100644
--- a/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp
+++ b/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp
@@ -11,9 +11,13 @@ static void expect_only_accelerations(JointAccelerations& acc) {
 }
 
 TEST(JointAccelerationsTest, Constructors) {
+  JointAccelerations empty;
+  EXPECT_EQ(empty.get_type(), StateType::JOINT_ACCELERATIONS);
+
   std::vector<std::string> joint_names{"joint_10", "joint_20"};
   Eigen::Vector2d accelerations = Eigen::Vector2d::Random();
   JointAccelerations ja1("test", accelerations);
+  EXPECT_EQ(ja1.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_EQ(ja1.get_name(), "test");
   EXPECT_FALSE(ja1.is_empty());
   EXPECT_EQ(ja1.get_size(), accelerations.size());
@@ -23,6 +27,7 @@ TEST(JointAccelerationsTest, Constructors) {
   EXPECT_EQ(ja1.data(), accelerations);
 
   JointAccelerations ja2("test", joint_names, accelerations);
+  EXPECT_EQ(ja2.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_EQ(ja2.get_name(), "test");
   EXPECT_FALSE(ja2.is_empty());
   EXPECT_EQ(ja2.get_size(), joint_names.size());
@@ -34,7 +39,9 @@ TEST(JointAccelerationsTest, Constructors) {
 
 TEST(JointAccelerationsTest, StateCopyConstructor) {
   JointState random_state = JointState::Random("test", 3);
+  EXPECT_EQ(random_state.get_type(), StateType::JOINT_STATE);
   JointAccelerations copy1(random_state);
+  EXPECT_EQ(copy1.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_EQ(random_state.get_name(), copy1.get_name());
   EXPECT_EQ(random_state.get_names(), copy1.get_names());
   EXPECT_EQ(random_state.get_size(), copy1.get_size());
@@ -42,6 +49,7 @@ TEST(JointAccelerationsTest, StateCopyConstructor) {
   expect_only_accelerations(copy1);
 
   JointAccelerations copy2 = random_state;
+  EXPECT_EQ(copy2.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_EQ(random_state.get_name(), copy2.get_name());
   EXPECT_EQ(random_state.get_names(), copy2.get_names());
   EXPECT_EQ(random_state.get_size(), copy2.get_size());
@@ -50,16 +58,21 @@ TEST(JointAccelerationsTest, StateCopyConstructor) {
 
   JointState empty_state;
   JointAccelerations copy3(empty_state);
+  EXPECT_EQ(copy3.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_TRUE(copy3.is_empty());
   JointAccelerations copy4 = empty_state;
+  EXPECT_EQ(copy4.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_TRUE(copy4.is_empty());
   JointAccelerations copy5 = empty_state.copy();
+  EXPECT_EQ(copy5.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_TRUE(copy5.is_empty());
 }
 
 TEST(JointAccelerationsTest, VelocitiesCopyConstructor) {
   JointVelocities random_velocities = JointVelocities::Random("test", 3);
+  EXPECT_EQ(random_velocities.get_type(), StateType::JOINT_VELOCITIES);
   JointAccelerations copy1(random_velocities);
+  EXPECT_EQ(copy1.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_EQ(random_velocities.get_name(), copy1.get_name());
   EXPECT_EQ(random_velocities.get_names(), copy1.get_names());
   EXPECT_EQ(random_velocities.get_size(), copy1.get_size());
@@ -67,6 +80,7 @@ TEST(JointAccelerationsTest, VelocitiesCopyConstructor) {
   expect_only_accelerations(copy1);
 
   JointAccelerations copy2 = random_velocities;
+  EXPECT_EQ(copy2.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_EQ(random_velocities.get_name(), copy1.get_name());
   EXPECT_EQ(random_velocities.get_names(), copy1.get_names());
   EXPECT_EQ(random_velocities.get_size(), copy1.get_size());
@@ -80,10 +94,12 @@ TEST(JointAccelerationsTest, VelocitiesCopyConstructor) {
 
 TEST(JointAccelerationsTest, RandomInitialization) {
   JointAccelerations random1 = JointAccelerations::Random("test", 3);
+  EXPECT_EQ(random1.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_NE(random1.get_accelerations().norm(), 0);
   expect_only_accelerations(random1);
 
   JointAccelerations random2 = JointAccelerations::Random("test", std::vector<std::string>{"j0", "j1"});
+  EXPECT_EQ(random2.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_NE(random2.get_accelerations().norm(), 0);
   expect_only_accelerations(random2);
 }
@@ -107,6 +123,7 @@ TEST(JointAccelerationsTest, Clamping) {
   EXPECT_EQ(ja2.data(), accelerations);
   ja2.clamp(3 * Eigen::ArrayXd::Ones(ja2.get_size()), 0.5 * Eigen::ArrayXd::Ones(ja2.get_size()));
   EXPECT_EQ(ja2.data(), result);
+  EXPECT_EQ(ja2.get_type(), StateType::JOINT_ACCELERATIONS);
 }
 
 TEST(JointAccelerationsTest, GetSetData) {
@@ -128,11 +145,13 @@ TEST(JointAccelerationsTest, GetSetData) {
     EXPECT_EQ(state_vec.at(i), ja1.data()(i));
   }
   EXPECT_THROW(ja1.set_data(Eigen::Vector2d::Zero()), exceptions::IncompatibleSizeException);
+  EXPECT_EQ(ja1.get_type(), StateType::JOINT_ACCELERATIONS);
 }
 
 TEST(JointAccelerationsTest, ScalarMultiplication) {
   JointAccelerations ja = JointAccelerations::Random("test", 3);
   JointAccelerations jscaled = 0.5 * ja;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_EQ(jscaled.data(), 0.5 * ja.data());
 
   JointAccelerations empty;
@@ -144,9 +163,11 @@ TEST(JointAccelerationsTest, MatrixMultiplication) {
   Eigen::MatrixXd gains = Eigen::VectorXd::Random(ja.get_size()).asDiagonal();
 
   JointAccelerations jscaled = gains * ja;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_EQ(jscaled.data(), gains * ja.data());
   EXPECT_EQ((ja * gains).data(), jscaled.data());
   ja *= gains;
+  EXPECT_EQ(ja.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_EQ(jscaled.data(), ja.data());
   JointAccelerations jscaled2 = ja * gains;
 
@@ -159,9 +180,11 @@ TEST(JointAccelerationsTest, ArrayMultiplication) {
   Eigen::ArrayXd gains = Eigen::ArrayXd::Random(ja.get_size());
 
   JointAccelerations jscaled = gains * ja;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_EQ(jscaled.data(), (gains * ja.array()).matrix());
   EXPECT_EQ((ja * gains).data(), jscaled.data());
   ja *= gains;
+  EXPECT_EQ(ja.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_EQ(jscaled.data(), ja.data());
 
   gains = Eigen::ArrayXd::Random(2 * ja.get_size());
@@ -172,8 +195,10 @@ TEST(JointAccelerationsTest, ChronoMultiplication) {
   JointAccelerations ja = JointAccelerations::Random("test", 3);
   auto time = std::chrono::seconds(2);
   JointVelocities jv1 = ja * time;
+  EXPECT_EQ(jv1.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(jv1.get_velocities(), ja.get_accelerations() * time.count());
   JointVelocities jv2 = time * ja;
+  EXPECT_EQ(jv2.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(jv2.get_velocities(), ja.get_accelerations() * time.count());
 
   JointAccelerations empty;
diff --git a/source/state_representation/test/tests/space/joint/test_joint_positions.cpp b/source/state_representation/test/tests/space/joint/test_joint_positions.cpp
index b5bed40b2..220c76c27 100644
--- a/source/state_representation/test/tests/space/joint/test_joint_positions.cpp
+++ b/source/state_representation/test/tests/space/joint/test_joint_positions.cpp
@@ -11,9 +11,13 @@ static void expect_only_positions(JointPositions& pos) {
 }
 
 TEST(JointPositionsTest, Constructors) {
+  JointPositions empty;
+  EXPECT_EQ(empty.get_type(), StateType::JOINT_POSITIONS);
+
   std::vector<std::string> joint_names{"joint_10", "joint_20"};
   Eigen::Vector2d positions = Eigen::Vector2d::Random();
   JointPositions jp1("test", positions);
+  EXPECT_EQ(jp1.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_EQ(jp1.get_name(), "test");
   EXPECT_FALSE(jp1.is_empty());
   EXPECT_EQ(jp1.get_size(), positions.size());
@@ -23,6 +27,7 @@ TEST(JointPositionsTest, Constructors) {
   EXPECT_EQ(jp1.data(), positions);
 
   JointPositions jp2("test", joint_names, positions);
+  EXPECT_EQ(jp2.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_EQ(jp2.get_name(), "test");
   EXPECT_FALSE(jp2.is_empty());
   EXPECT_EQ(jp2.get_size(), joint_names.size());
@@ -34,7 +39,9 @@ TEST(JointPositionsTest, Constructors) {
 
 TEST(JointPositionsTest, StateCopyConstructor) {
   JointState random_state = JointState::Random("test", 3);
+  EXPECT_EQ(random_state.get_type(), StateType::JOINT_STATE);
   JointPositions copy1(random_state);
+  EXPECT_EQ(copy1.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_EQ(random_state.get_name(), copy1.get_name());
   EXPECT_EQ(random_state.get_names(), copy1.get_names());
   EXPECT_EQ(random_state.get_size(), copy1.get_size());
@@ -42,6 +49,7 @@ TEST(JointPositionsTest, StateCopyConstructor) {
   expect_only_positions(copy1);
 
   JointPositions copy2 = random_state;
+  EXPECT_EQ(copy2.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_EQ(random_state.get_name(), copy2.get_name());
   EXPECT_EQ(random_state.get_names(), copy2.get_names());
   EXPECT_EQ(random_state.get_size(), copy2.get_size());
@@ -50,16 +58,21 @@ TEST(JointPositionsTest, StateCopyConstructor) {
 
   JointState empty_state;
   JointPositions copy3(empty_state);
+  EXPECT_EQ(copy3.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_TRUE(copy3.is_empty());
   JointPositions copy4 = empty_state;
+  EXPECT_EQ(copy4.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_TRUE(copy4.is_empty());
   JointPositions copy5 = empty_state.copy();
+  EXPECT_EQ(copy5.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_TRUE(copy5.is_empty());
 }
 
 TEST(JointPositionsTest, VelocitiesCopyConstructor) {
   JointVelocities random_velocities = JointVelocities::Random("test", 3);
+  EXPECT_EQ(random_velocities.get_type(), StateType::JOINT_VELOCITIES);
   JointPositions copy1(random_velocities);
+  EXPECT_EQ(copy1.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_EQ(random_velocities.get_name(), copy1.get_name());
   EXPECT_EQ(random_velocities.get_names(), copy1.get_names());
   EXPECT_EQ(random_velocities.get_size(), copy1.get_size());
@@ -67,6 +80,7 @@ TEST(JointPositionsTest, VelocitiesCopyConstructor) {
   expect_only_positions(copy1);
 
   JointPositions copy2 = random_velocities;
+  EXPECT_EQ(copy2.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_EQ(random_velocities.get_name(), copy1.get_name());
   EXPECT_EQ(random_velocities.get_names(), copy1.get_names());
   EXPECT_EQ(random_velocities.get_size(), copy1.get_size());
@@ -80,10 +94,12 @@ TEST(JointPositionsTest, VelocitiesCopyConstructor) {
 
 TEST(JointPositionsTest, RandomInitialization) {
   JointPositions random1 = JointPositions::Random("test", 3);
+  EXPECT_EQ(random1.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_NE(random1.get_positions().norm(), 0);
   expect_only_positions(random1);
 
   JointPositions random2 = JointPositions::Random("test", std::vector<std::string>{"j0", "j1"});
+  EXPECT_EQ(random2.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_NE(random2.get_positions().norm(), 0);
   expect_only_positions(random2);
 }
@@ -133,6 +149,7 @@ TEST(JointPositionsTest, GetSetData) {
 TEST(JointPositionsTest, ScalarMultiplication) {
   JointPositions jp = JointPositions::Random("test", 3);
   JointPositions jscaled = 0.5 * jp;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_EQ(jscaled.data(), 0.5 * jp.data());
 
   JointPositions empty;
@@ -144,9 +161,11 @@ TEST(JointPositionsTest, MatrixMultiplication) {
   Eigen::MatrixXd gains = Eigen::VectorXd::Random(jp.get_size()).asDiagonal();
 
   JointPositions jscaled = gains * jp;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_EQ(jscaled.data(), gains * jp.data());
   EXPECT_EQ((jp * gains).data(), jscaled.data());
   jp *= gains;
+  EXPECT_EQ(jp.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_EQ(jscaled.data(), jp.data());
   JointPositions jscaled2 = jp * gains;
 
@@ -159,9 +178,11 @@ TEST(JointPositionsTest, ArrayMultiplication) {
   Eigen::ArrayXd gains = Eigen::ArrayXd::Random(jp.get_size());
 
   JointPositions jscaled = gains * jp;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_EQ(jscaled.data(), (gains * jp.array()).matrix());
   EXPECT_EQ((jp * gains).data(), jscaled.data());
   jp *= gains;
+  EXPECT_EQ(jp.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_EQ(jscaled.data(), jp.data());
 
   gains = Eigen::ArrayXd::Random(2 * jp.get_size());
@@ -170,8 +191,10 @@ TEST(JointPositionsTest, ArrayMultiplication) {
 
 TEST(JointStateTest, ChronoDivision) {
   JointPositions jp = JointPositions::Random("test", 3);
+  EXPECT_EQ(jp.get_type(), StateType::JOINT_POSITIONS);
   auto time = std::chrono::seconds(1);
   JointVelocities jv = jp / time;
+  EXPECT_EQ(jv.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(jv.get_velocities(), jp.get_positions() / time.count());
 
   JointPositions empty;
diff --git a/source/state_representation/test/tests/space/joint/test_joint_state.cpp b/source/state_representation/test/tests/space/joint/test_joint_state.cpp
index 3ca46a4b3..91d6e63d2 100644
--- a/source/state_representation/test/tests/space/joint/test_joint_state.cpp
+++ b/source/state_representation/test/tests/space/joint/test_joint_state.cpp
@@ -10,13 +10,14 @@ using namespace state_representation;
 
 TEST(JointStateTest, Constructors) {
   JointState empty;
+  EXPECT_EQ(empty.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(empty.get_name(), "");
-  EXPECT_EQ(empty.get_type(), StateType::JOINTSTATE);
   EXPECT_TRUE(empty.is_empty());
   EXPECT_EQ(empty.get_size(), 0);
   EXPECT_EQ(empty.data().norm(), 0);
 
   JointState js1("test", 3);
+  EXPECT_EQ(js1.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(js1.get_name(), "test");
   EXPECT_TRUE(js1.is_empty());
   EXPECT_EQ(js1.get_size(), 3);
@@ -27,6 +28,7 @@ TEST(JointStateTest, Constructors) {
 
   std::vector<std::string> joint_names{"joint_10", "joint_20"};
   JointState js2("test", joint_names);
+  EXPECT_EQ(js2.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(js2.get_name(), "test");
   EXPECT_TRUE(js2.is_empty());
   EXPECT_EQ(js2.get_size(), joint_names.size());
@@ -38,22 +40,26 @@ TEST(JointStateTest, Constructors) {
 
 TEST(JointStateTest, ZeroInitialization) {
   JointState zero = JointState::Zero("test", 3);
+  EXPECT_EQ(zero.get_type(), StateType::JOINT_STATE);
   EXPECT_FALSE(zero.is_empty());
   EXPECT_EQ(zero.data().norm(), 0);
 
   JointState zero2 = JointState::Zero("test", std::vector<std::string>{"j0", "j1"});
+  EXPECT_EQ(zero2.get_type(), StateType::JOINT_STATE);
   EXPECT_FALSE(zero2.is_empty());
   EXPECT_EQ(zero2.data().norm(), 0);
 }
 
 TEST(JointStateTest, RandomStateInitialization) {
   JointState random = JointState::Random("test", 3);
+  EXPECT_EQ(random.get_type(), StateType::JOINT_STATE);
   EXPECT_NE(random.get_positions().norm(), 0);
   EXPECT_NE(random.get_velocities().norm(), 0);
   EXPECT_NE(random.get_accelerations().norm(), 0);
   EXPECT_NE(random.get_torques().norm(), 0);
 
   JointState random2 = JointState::Random("test", std::vector<std::string>{"j0", "j1"});
+  EXPECT_EQ(random2.get_type(), StateType::JOINT_STATE);
   EXPECT_NE(random2.get_positions().norm(), 0);
   EXPECT_NE(random2.get_velocities().norm(), 0);
   EXPECT_NE(random2.get_accelerations().norm(), 0);
@@ -63,18 +69,21 @@ TEST(JointStateTest, RandomStateInitialization) {
 TEST(JointStateTest, CopyConstructor) {
   JointState random = JointState::Random("test", 3);
   JointState copy1(random);
+  EXPECT_EQ(copy1.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(random.get_name(), copy1.get_name());
   EXPECT_EQ(random.get_names(), copy1.get_names());
   EXPECT_EQ(random.get_size(), copy1.get_size());
   EXPECT_EQ(random.data(), copy1.data());
 
   JointState copy2 = random;
+  EXPECT_EQ(copy2.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(random.get_name(), copy2.get_name());
   EXPECT_EQ(random.get_names(), copy2.get_names());
   EXPECT_EQ(random.get_size(), copy2.get_size());
   EXPECT_EQ(random.data(), copy2.data());
 
   JointState copy3 = random.copy();
+  EXPECT_EQ(copy3.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(random.get_name(), copy3.get_name());
   EXPECT_EQ(random.get_names(), copy3.get_names());
   EXPECT_EQ(random.get_size(), copy3.get_size());
@@ -82,10 +91,13 @@ TEST(JointStateTest, CopyConstructor) {
 
   JointState empty;
   JointState copy4(empty);
+  EXPECT_EQ(copy4.get_type(), StateType::JOINT_STATE);
   EXPECT_TRUE(copy4.is_empty());
   JointState copy5 = empty;
+  EXPECT_EQ(copy5.get_type(), StateType::JOINT_STATE);
   EXPECT_TRUE(copy5.is_empty());
   JointState copy6 = empty.copy();
+  EXPECT_EQ(copy6.get_type(), StateType::JOINT_STATE);
   EXPECT_TRUE(copy6.is_empty());
 }
 
@@ -152,9 +164,11 @@ TEST(JointStateTest, GetSetFields) {
   EXPECT_THROW(js.set_torques(Eigen::VectorXd::Zero(7)), exceptions::IncompatibleSizeException);
 
   js.set_zero();
+  EXPECT_EQ(js.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(js.data().norm(), 0);
   EXPECT_EQ(js.is_empty(), false);
   js.set_empty();
+  EXPECT_EQ(js.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(js.is_empty(), true);
 }
 
@@ -213,6 +227,7 @@ TEST(JointStateTest, SetZero) {
 
   JointState random2 = JointState::Random("test", 3);
   random2.set_zero();
+  EXPECT_EQ(random2.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(random2.data().norm(), 0);
 }
 
@@ -327,8 +342,10 @@ TEST(JointStateTest, Addition) {
   JointState js3 = JointState::Random("test", 4);
   EXPECT_THROW(js1 + js3, exceptions::IncompatibleStatesException);
   JointState jsum = js1 + js2;
+  EXPECT_EQ(jsum.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(jsum.data(), js1.data() + js2.data());
   js2 += js1;
+  EXPECT_EQ(js2.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(jsum.data(), js2.data());
 }
 
@@ -338,17 +355,21 @@ TEST(JointStateTest, Subtraction) {
   JointState js3 = JointState::Random("test", 4);
   EXPECT_THROW(js1 - js3, exceptions::IncompatibleStatesException);
   JointState jdiff = js1 - js2;
+  EXPECT_EQ(jdiff.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(jdiff.data(), js1.data() - js2.data());
   js1 -= js2;
+  EXPECT_EQ(js1.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(jdiff.data(), js1.data());
 }
 
 TEST(JointStateTest, ScalarMultiplication) {
   JointState js = JointState::Random("test", 3);
   JointState jscaled = 0.5 * js;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(jscaled.data(), 0.5 * js.data());
   EXPECT_EQ((js * 0.5).data(), jscaled.data());
   js *= 0.5;
+  EXPECT_EQ(js.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(jscaled.data(), js.data());
 
   JointState empty;
@@ -358,8 +379,10 @@ TEST(JointStateTest, ScalarMultiplication) {
 TEST(JointStateTest, ScalarDivision) {
   JointState js = JointState::Random("test", 3);
   JointState jscaled = js / 0.5;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(jscaled.data(), js.data() / 0.5);
   js /= 0.5;
+  EXPECT_EQ(js.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(jscaled.data(), js.data());
 
   JointState empty;
@@ -371,9 +394,11 @@ TEST(JointStateTest, MatrixMultiplication) {
   Eigen::MatrixXd gains = Eigen::VectorXd::Random(4 * js.get_size()).asDiagonal();
 
   JointState jscaled = gains * js;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(jscaled.data(), gains * js.data());
   EXPECT_EQ((js * gains).data(), jscaled.data());
   js *= gains;
+  EXPECT_EQ(js.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(jscaled.data(), js.data());
   JointState jscaled2 = js * gains;
 
@@ -386,9 +411,11 @@ TEST(JointStateTest, ArrayMultiplication) {
   Eigen::ArrayXd gains = Eigen::ArrayXd::Random(4 * js.get_size());
 
   JointState jscaled = gains * js;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(jscaled.data(), (gains * js.array()).matrix());
   EXPECT_EQ((js * gains).data(), jscaled.data());
   js *= gains;
+  EXPECT_EQ(js.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(jscaled.data(), js.data());
 
   gains = Eigen::ArrayXd::Random(js.get_size());
diff --git a/source/state_representation/test/tests/space/joint/test_joint_torques.cpp b/source/state_representation/test/tests/space/joint/test_joint_torques.cpp
index 9f60846e7..2ea564cfe 100644
--- a/source/state_representation/test/tests/space/joint/test_joint_torques.cpp
+++ b/source/state_representation/test/tests/space/joint/test_joint_torques.cpp
@@ -11,9 +11,13 @@ static void expect_only_torques(JointTorques& tor) {
 }
 
 TEST(JointTorquesTest, Constructors) {
+  JointTorques empty;
+  EXPECT_EQ(empty.get_type(), StateType::JOINT_TORQUES);
+
   std::vector<std::string> joint_names{"joint_10", "joint_20"};
   Eigen::Vector2d torques = Eigen::Vector2d::Random();
   JointTorques jt1("test", torques);
+  EXPECT_EQ(jt1.get_type(), StateType::JOINT_TORQUES);
   EXPECT_EQ(jt1.get_name(), "test");
   EXPECT_FALSE(jt1.is_empty());
   EXPECT_EQ(jt1.get_size(), torques.size());
@@ -23,6 +27,7 @@ TEST(JointTorquesTest, Constructors) {
   EXPECT_EQ(jt1.data(), torques);
 
   JointTorques jt2("test", joint_names, torques);
+  EXPECT_EQ(jt2.get_type(), StateType::JOINT_TORQUES);
   EXPECT_EQ(jt2.get_name(), "test");
   EXPECT_FALSE(jt2.is_empty());
   EXPECT_EQ(jt2.get_size(), joint_names.size());
@@ -35,6 +40,7 @@ TEST(JointTorquesTest, Constructors) {
 TEST(JointTorquesTest, StateCopyConstructor) {
   JointState random_state = JointState::Random("test", 3);
   JointTorques copy1(random_state);
+  EXPECT_EQ(copy1.get_type(), StateType::JOINT_TORQUES);
   EXPECT_EQ(random_state.get_name(), copy1.get_name());
   EXPECT_EQ(random_state.get_names(), copy1.get_names());
   EXPECT_EQ(random_state.get_size(), copy1.get_size());
@@ -42,6 +48,7 @@ TEST(JointTorquesTest, StateCopyConstructor) {
   expect_only_torques(copy1);
 
   JointTorques copy2 = random_state;
+  EXPECT_EQ(copy2.get_type(), StateType::JOINT_TORQUES);
   EXPECT_EQ(random_state.get_name(), copy2.get_name());
   EXPECT_EQ(random_state.get_names(), copy2.get_names());
   EXPECT_EQ(random_state.get_size(), copy2.get_size());
@@ -50,19 +57,24 @@ TEST(JointTorquesTest, StateCopyConstructor) {
 
   JointState empty_state;
   JointTorques copy3(empty_state);
+  EXPECT_EQ(copy3.get_type(), StateType::JOINT_TORQUES);
   EXPECT_TRUE(copy3.is_empty());
   JointTorques copy4 = empty_state;
+  EXPECT_EQ(copy4.get_type(), StateType::JOINT_TORQUES);
   EXPECT_TRUE(copy4.is_empty());
   JointTorques copy5 = empty_state.copy();
+  EXPECT_EQ(copy5.get_type(), StateType::JOINT_TORQUES);
   EXPECT_TRUE(copy5.is_empty());
 }
 
 TEST(JointTorquesTest, RandomInitialization) {
   JointTorques random1 = JointTorques::Random("test", 3);
+  EXPECT_EQ(random1.get_type(), StateType::JOINT_TORQUES);
   EXPECT_NE(random1.get_torques().norm(), 0);
   expect_only_torques(random1);
 
   JointTorques random2 = JointTorques::Random("test", std::vector<std::string>{"j0", "j1"});
+  EXPECT_EQ(random2.get_type(), StateType::JOINT_TORQUES);
   EXPECT_NE(random2.get_torques().norm(), 0);
   expect_only_torques(random2);
 }
@@ -112,6 +124,7 @@ TEST(JointTorquesTest, GetSetData) {
 TEST(JointTorquesTest, ScalarMultiplication) {
   JointTorques jt = JointTorques::Random("test", 3);
   JointTorques jscaled = 0.5 * jt;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_TORQUES);
   EXPECT_EQ(jscaled.data(), 0.5 * jt.data());
 
   JointTorques empty;
@@ -123,9 +136,11 @@ TEST(JointTorquesTest, MatrixMultiplication) {
   Eigen::MatrixXd gains = Eigen::VectorXd::Random(jt.get_size()).asDiagonal();
 
   JointTorques jscaled = gains * jt;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_TORQUES);
   EXPECT_EQ(jscaled.data(), gains * jt.data());
   EXPECT_EQ((jt * gains).data(), jscaled.data());
   jt *= gains;
+  EXPECT_EQ(jt.get_type(), StateType::JOINT_TORQUES);
   EXPECT_EQ(jscaled.data(), jt.data());
   JointTorques jscaled2 = jt * gains;
 
@@ -138,9 +153,11 @@ TEST(JointTorquesTest, ArrayMultiplication) {
   Eigen::ArrayXd gains = Eigen::ArrayXd::Random(jt.get_size());
 
   JointTorques jscaled = gains * jt;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_TORQUES);
   EXPECT_EQ(jscaled.data(), (gains * jt.array()).matrix());
   EXPECT_EQ((jt * gains).data(), jscaled.data());
   jt *= gains;
+  EXPECT_EQ(jt.get_type(), StateType::JOINT_TORQUES);
   EXPECT_EQ(jscaled.data(), jt.data());
 
   gains = Eigen::ArrayXd::Random(2 * jt.get_size());
diff --git a/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp b/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp
index 14d640e96..deb8cbfb6 100644
--- a/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp
+++ b/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp
@@ -11,9 +11,13 @@ static void expect_only_velocities(JointVelocities& vel) {
 }
 
 TEST(JointVelocitiesTest, Constructors) {
+  JointVelocities empty;
+  EXPECT_EQ(empty.get_type(), StateType::JOINT_VELOCITIES);
+
   std::vector<std::string> joint_names{"joint_10", "joint_20"};
   Eigen::Vector2d velocities = Eigen::Vector2d::Random();
   JointVelocities jv1("test", velocities);
+  EXPECT_EQ(jv1.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(jv1.get_name(), "test");
   EXPECT_FALSE(jv1.is_empty());
   EXPECT_EQ(jv1.get_size(), velocities.size());
@@ -23,6 +27,7 @@ TEST(JointVelocitiesTest, Constructors) {
   EXPECT_EQ(jv1.data(), velocities);
 
   JointVelocities jv2("test", joint_names, velocities);
+  EXPECT_EQ(jv2.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(jv2.get_name(), "test");
   EXPECT_FALSE(jv2.is_empty());
   EXPECT_EQ(jv2.get_size(), joint_names.size());
@@ -35,6 +40,7 @@ TEST(JointVelocitiesTest, Constructors) {
 TEST(JointVelocitiesTest, StateCopyConstructor) {
   JointState random_state = JointState::Random("test", 3);
   JointVelocities copy1(random_state);
+  EXPECT_EQ(copy1.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(random_state.get_name(), copy1.get_name());
   EXPECT_EQ(random_state.get_names(), copy1.get_names());
   EXPECT_EQ(random_state.get_size(), copy1.get_size());
@@ -42,6 +48,7 @@ TEST(JointVelocitiesTest, StateCopyConstructor) {
   expect_only_velocities(copy1);
 
   JointVelocities copy2 = random_state;
+  EXPECT_EQ(copy2.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(random_state.get_name(), copy2.get_name());
   EXPECT_EQ(random_state.get_names(), copy2.get_names());
   EXPECT_EQ(random_state.get_size(), copy2.get_size());
@@ -50,16 +57,20 @@ TEST(JointVelocitiesTest, StateCopyConstructor) {
 
   JointState empty_state;
   JointVelocities copy3(empty_state);
+  EXPECT_EQ(copy3.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_TRUE(copy3.is_empty());
   JointVelocities copy4 = empty_state;
+  EXPECT_EQ(copy4.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_TRUE(copy4.is_empty());
   JointVelocities copy5 = empty_state.copy();
+  EXPECT_EQ(copy5.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_TRUE(copy5.is_empty());
 }
 
 TEST(JointVelocitiesTest, AccelerationsCopyConstructor) {
   JointAccelerations random_accelerations = JointAccelerations::Random("test", 3);
   JointVelocities copy1(random_accelerations);
+  EXPECT_EQ(copy1.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(random_accelerations.get_name(), copy1.get_name());
   EXPECT_EQ(random_accelerations.get_names(), copy1.get_names());
   EXPECT_EQ(random_accelerations.get_size(), copy1.get_size());
@@ -67,6 +78,7 @@ TEST(JointVelocitiesTest, AccelerationsCopyConstructor) {
   expect_only_velocities(copy1);
 
   JointVelocities copy2 = random_accelerations;
+  EXPECT_EQ(copy2.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(random_accelerations.get_name(), copy1.get_name());
   EXPECT_EQ(random_accelerations.get_names(), copy1.get_names());
   EXPECT_EQ(random_accelerations.get_size(), copy1.get_size());
@@ -81,6 +93,7 @@ TEST(JointVelocitiesTest, AccelerationsCopyConstructor) {
 TEST(JointVelocitiesTest, PositionsCopyConstructor) {
   JointPositions random_positions = JointPositions::Random("test", 3);
   JointVelocities copy1(random_positions);
+  EXPECT_EQ(copy1.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(random_positions.get_name(), copy1.get_name());
   EXPECT_EQ(random_positions.get_names(), copy1.get_names());
   EXPECT_EQ(random_positions.get_size(), copy1.get_size());
@@ -88,6 +101,7 @@ TEST(JointVelocitiesTest, PositionsCopyConstructor) {
   expect_only_velocities(copy1);
 
   JointVelocities copy2 = random_positions;
+  EXPECT_EQ(copy2.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(random_positions.get_name(), copy1.get_name());
   EXPECT_EQ(random_positions.get_names(), copy1.get_names());
   EXPECT_EQ(random_positions.get_size(), copy1.get_size());
@@ -101,10 +115,12 @@ TEST(JointVelocitiesTest, PositionsCopyConstructor) {
 
 TEST(JointVelocitiesTest, RandomInitialization) {
   JointVelocities random1 = JointVelocities::Random("test", 3);
+  EXPECT_EQ(random1.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_NE(random1.get_velocities().norm(), 0);
   expect_only_velocities(random1);
 
   JointVelocities random2 = JointVelocities::Random("test", std::vector<std::string>{"j0", "j1"});
+  EXPECT_EQ(random2.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_NE(random2.get_velocities().norm(), 0);
   expect_only_velocities(random2);
 }
@@ -154,6 +170,7 @@ TEST(JointVelocitiesTest, GetSetData) {
 TEST(JointVelocitiesTest, ScalarMultiplication) {
   JointVelocities jv = JointVelocities::Random("test", 3);
   JointVelocities jscaled = 0.5 * jv;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(jscaled.data(), 0.5 * jv.data());
 
   JointVelocities empty;
@@ -165,9 +182,11 @@ TEST(JointVelocitiesTest, MatrixMultiplication) {
   Eigen::MatrixXd gains = Eigen::VectorXd::Random(jv.get_size()).asDiagonal();
 
   JointVelocities jscaled = gains * jv;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(jscaled.data(), gains * jv.data());
   EXPECT_EQ((jv * gains).data(), jscaled.data());
   jv *= gains;
+  EXPECT_EQ(jv.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(jscaled.data(), jv.data());
   JointVelocities jscaled2 = jv * gains;
 
@@ -180,9 +199,11 @@ TEST(JointVelocitiesTest, ArrayMultiplication) {
   Eigen::ArrayXd gains = Eigen::ArrayXd::Random(jv.get_size());
 
   JointVelocities jscaled = gains * jv;
+  EXPECT_EQ(jscaled.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(jscaled.data(), (gains * jv.array()).matrix());
   EXPECT_EQ((jv * gains).data(), jscaled.data());
   jv *= gains;
+  EXPECT_EQ(jv.get_type(), StateType::JOINT_VELOCITIES);
   EXPECT_EQ(jscaled.data(), jv.data());
 
   gains = Eigen::ArrayXd::Random(2 * jv.get_size());
@@ -191,10 +212,13 @@ TEST(JointVelocitiesTest, ArrayMultiplication) {
 
 TEST(JointVelocitiesTest, ChronoMultiplication) {
   JointVelocities jv = JointVelocities::Random("test", 3);
+  EXPECT_EQ(jv.get_type(), StateType::JOINT_VELOCITIES);
   auto time = std::chrono::seconds(2);
   JointPositions jp1 = jv * time;
+  EXPECT_EQ(jp1.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_EQ(jp1.get_positions(), jv.get_velocities() * time.count());
   JointPositions jp2 = time * jv;
+  EXPECT_EQ(jp1.get_type(), StateType::JOINT_POSITIONS);
   EXPECT_EQ(jp2.get_positions(), jv.get_velocities() * time.count());
 
   JointVelocities empty;
@@ -203,8 +227,10 @@ TEST(JointVelocitiesTest, ChronoMultiplication) {
 
 TEST(JointVelocitiesTest, ChronoDivision) {
   JointVelocities jv = JointVelocities::Random("test", 3);
+  EXPECT_EQ(jv.get_type(), StateType::JOINT_VELOCITIES);
   auto time = std::chrono::seconds(2);
   JointAccelerations ja = jv / time;
+  EXPECT_EQ(ja.get_type(), StateType::JOINT_ACCELERATIONS);
   EXPECT_EQ(ja.get_accelerations(), jv.get_velocities() / time.count());
 
   JointVelocities empty;
diff --git a/source/state_representation/test/tests/space/test_jacobian.cpp b/source/state_representation/test/tests/space/test_jacobian.cpp
index a9926446b..a94902b1b 100644
--- a/source/state_representation/test/tests/space/test_jacobian.cpp
+++ b/source/state_representation/test/tests/space/test_jacobian.cpp
@@ -7,6 +7,7 @@ using namespace state_representation::exceptions;
 
 TEST(JacobianTest, TestCreate) {
   Jacobian jac("robot", 7, "test");
+  EXPECT_EQ(jac.get_type(), StateType::JACOBIAN);
   EXPECT_EQ(jac.rows(), 6);
   EXPECT_EQ(jac.cols(), 7);
   EXPECT_TRUE(jac.is_empty());
@@ -22,6 +23,7 @@ TEST(JacobianTest, TestCreate) {
 
 TEST(JacobianTest, TestCreateWithVectorOfJoints) {
   Jacobian jac("robot", std::vector<std::string>{"j1", "j2"}, "test", "test_ref");
+  EXPECT_EQ(jac.get_type(), StateType::JACOBIAN);
   EXPECT_EQ(jac.get_joint_names().at(0), "j1");
   EXPECT_EQ(jac.get_joint_names().at(1), "j2");
   EXPECT_EQ(jac.get_reference_frame(), "test_ref");
@@ -45,6 +47,7 @@ TEST(JacobianTest, TestSetData) {
 
 TEST(JacobianTest, TestRandomCreate) {
   Jacobian jac = Jacobian::Random("robot", 7, "test");
+  EXPECT_EQ(jac.get_type(), StateType::JACOBIAN);
   EXPECT_FALSE(jac.is_empty());
   for (std::size_t i = 0; i < jac.cols(); ++i) {
     EXPECT_GT(jac.col(i).norm(), 0);
@@ -54,6 +57,7 @@ TEST(JacobianTest, TestRandomCreate) {
 TEST(JacobianTest, TestTranspose) {
   Jacobian jac = Jacobian::Random("robot", 7, "test");
   Jacobian jact = jac.transpose();
+  EXPECT_EQ(jact.get_type(), StateType::JACOBIAN);
 
   EXPECT_EQ(jact.rows(), 7);
   EXPECT_EQ(jact.cols(), 6);
diff --git a/source/state_representation/test/tests/space/test_spatial_state.cpp b/source/state_representation/test/tests/space/test_spatial_state.cpp
index 73d2ef87c..e6682fce2 100644
--- a/source/state_representation/test/tests/space/test_spatial_state.cpp
+++ b/source/state_representation/test/tests/space/test_spatial_state.cpp
@@ -5,28 +5,34 @@
 using namespace state_representation;
 
 TEST(SpatialStateTest, Constructors) {
-  SpatialState state1(StateType::JOINTSTATE);
-  EXPECT_EQ(state1.get_type(), StateType::JOINTSTATE);
+  SpatialState state0;
+  EXPECT_EQ(state0.get_type(), StateType::SPATIAL_STATE);
+  EXPECT_EQ(state0.get_name(), "");
+  EXPECT_EQ(state0.get_reference_frame(), "world");
+  EXPECT_TRUE(state0.is_empty());
+  
+  SpatialState state1(StateType::JOINT_STATE);
+  EXPECT_EQ(state1.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(state1.get_name(), "");
   EXPECT_EQ(state1.get_reference_frame(), "world");
   EXPECT_TRUE(state1.is_empty());
 
-  SpatialState state2(StateType::CARTESIANSTATE, "test", "robot", false);
-  EXPECT_EQ(state2.get_type(), StateType::CARTESIANSTATE);
+  SpatialState state2(StateType::CARTESIAN_STATE, "test", "robot", false);
+  EXPECT_EQ(state2.get_type(), StateType::CARTESIAN_STATE);
   EXPECT_EQ(state2.get_name(), "test");
   EXPECT_EQ(state2.get_reference_frame(), "robot");
   EXPECT_FALSE(state2.is_empty());
 
   SpatialState state3(state2);
-  EXPECT_EQ(state3.get_type(), StateType::CARTESIANSTATE);
+  EXPECT_EQ(state3.get_type(), StateType::CARTESIAN_STATE);
   EXPECT_EQ(state3.get_name(), "test");
   EXPECT_EQ(state3.get_reference_frame(), "robot");
   EXPECT_FALSE(state3.is_empty());
 }
 
 TEST(SpatialStateTest, Compatibility) {
-  SpatialState state1(StateType::CARTESIANSTATE, "test", "robot", true);
-  SpatialState state2(StateType::CARTESIANSTATE, "robot");
+  SpatialState state1(StateType::CARTESIAN_STATE, "test", "robot", true);
+  SpatialState state2(StateType::CARTESIAN_STATE, "robot");
   EXPECT_FALSE(state1.is_compatible(state2));
   state2.set_reference_frame("robot");
   EXPECT_FALSE(state1.is_compatible(state2));
@@ -35,15 +41,15 @@ TEST(SpatialStateTest, Compatibility) {
 }
 
 TEST(SpatialStateTest, Swap) {
-  SpatialState state1(StateType::CARTESIANSTATE, "cartesian");
-  SpatialState state2(StateType::JOINTSTATE, "joint", "robot", false);
+  SpatialState state1(StateType::CARTESIAN_STATE, "cartesian");
+  SpatialState state2(StateType::JOINT_STATE, "joint", "robot", false);
   swap(state1, state2);
-  EXPECT_EQ(state1.get_type(), StateType::JOINTSTATE);
+  EXPECT_EQ(state1.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(state1.get_name(), "joint");
   EXPECT_EQ(state1.get_reference_frame(), "robot");
   EXPECT_FALSE(state1.is_empty());
 
-  EXPECT_EQ(state2.get_type(), StateType::CARTESIANSTATE);
+  EXPECT_EQ(state2.get_type(), StateType::CARTESIAN_STATE);
   EXPECT_EQ(state2.get_name(), "cartesian");
   EXPECT_EQ(state2.get_reference_frame(), "world");
   EXPECT_TRUE(state2.is_empty());
diff --git a/source/state_representation/test/tests/test_ellipsoid.cpp b/source/state_representation/test/tests/test_ellipsoid.cpp
index 9c373f1a6..56d79c931 100644
--- a/source/state_representation/test/tests/test_ellipsoid.cpp
+++ b/source/state_representation/test/tests/test_ellipsoid.cpp
@@ -38,6 +38,7 @@ TEST(EllipsoidTest, Sampling) {
 }
 
 TEST(EllipsoidTest, EllipsoidFitting) {
+  GTEST_SKIP() << "Skipping Ellipsoid fit test to reduce computational burden";
   Ellipsoid ellipse("test");
 
   // sample from the parameterization
diff --git a/source/state_representation/test/tests/test_parameter.cpp b/source/state_representation/test/tests/test_parameter.cpp
index e5b393071..03f8a1a04 100644
--- a/source/state_representation/test/tests/test_parameter.cpp
+++ b/source/state_representation/test/tests/test_parameter.cpp
@@ -10,34 +10,51 @@
 
 using namespace state_representation;
 
+TEST(ParameterTest, CopyConstructor) {
+  Parameter<int> int_param("test", 1);
+  ParameterInterface int_param_interface(int_param);
+  EXPECT_EQ(int_param_interface.get_parameter_type(), int_param.get_parameter_type());
+}
+
 TEST(ParameterTest, Conversion) {
   Parameter<int> int_param("test");
   EXPECT_TRUE(int_param.is_empty());
-  EXPECT_EQ(int_param.get_type(), StateType::PARAMETER_INT);
+  EXPECT_EQ(int_param.get_type(), StateType::PARAMETER);
+  EXPECT_EQ(int_param.get_parameter_type(), ParameterType::INT);
+  EXPECT_EQ(int_param.get_parameter_state_type(), StateType::NONE);
   int_param.set_value(2);
   EXPECT_EQ(int_param.get_value(), 2);
   EXPECT_EQ(typeid(int_param.get_value<double>()).name(), typeid(2.0).name());
   Parameter<double> double_param("double");
   EXPECT_NO_THROW(double_param = int_param);
-  EXPECT_EQ(double_param.get_type(), StateType::PARAMETER_DOUBLE);
+  EXPECT_EQ(double_param.get_type(), StateType::PARAMETER);
+  EXPECT_EQ(double_param.get_parameter_type(), ParameterType::DOUBLE);
+  EXPECT_EQ(double_param.get_parameter_state_type(), StateType::NONE);
   EXPECT_EQ(double_param.get_name(), int_param.get_name());
   EXPECT_EQ(double_param.get_value(), 2.0);
 
   std::vector<int> values{1, 2, 3};
   Parameter<std::vector<int>> int_array_param("test", values);
   EXPECT_FALSE(int_array_param.is_empty());
-  EXPECT_EQ(int_array_param.get_type(), StateType::PARAMETER_INT_ARRAY);
+  EXPECT_EQ(int_array_param.get_type(), StateType::PARAMETER);
+  EXPECT_EQ(int_array_param.get_parameter_type(), ParameterType::INT_ARRAY);
+  EXPECT_EQ(int_array_param.get_parameter_state_type(), StateType::NONE);
   for (std::size_t i = 0; i < values.size(); ++i) {
     EXPECT_EQ(int_array_param.get_value().at(i), values.at(i));
   }
 
   Parameter<CartesianPose> test1("test", CartesianPose::Random("test"));
+  EXPECT_EQ(test1.get_parameter_state_type(), StateType::CARTESIAN_POSE);
   Parameter<CartesianState> test2(test1);
-  EXPECT_EQ(test2.get_type(), StateType::PARAMETER_CARTESIANSTATE);
+  EXPECT_EQ(test2.get_type(), StateType::PARAMETER);
+  EXPECT_EQ(test2.get_parameter_type(), ParameterType::STATE);
+  EXPECT_EQ(test2.get_parameter_state_type(), StateType::CARTESIAN_STATE);
 
   std::shared_ptr<Parameter<CartesianState>> test3 =
       std::make_shared<Parameter<CartesianState>>(Parameter<CartesianPose>("test", CartesianPose::Random("test")));
-  EXPECT_EQ(test3->get_type(), StateType::PARAMETER_CARTESIANSTATE);
+  EXPECT_EQ(test3->get_type(), StateType::PARAMETER);
+  EXPECT_EQ(test3->get_parameter_type(), ParameterType::STATE);
+  EXPECT_EQ(test3->get_parameter_state_type(), StateType::CARTESIAN_STATE);
 }
 
 TEST(ParameterTest, Event) {
@@ -82,7 +99,9 @@ TEST(ParameterTest, MakeShared) {
   auto pose = CartesianPose::Random("A", "B");
   auto param = make_shared_parameter("name", pose);
   EXPECT_EQ(param->get_name(), "name");
-  EXPECT_EQ(param->get_type(), StateType::PARAMETER_CARTESIANPOSE);
+  EXPECT_EQ(param->get_type(), StateType::PARAMETER);
+  EXPECT_EQ(param->get_parameter_type(), ParameterType::STATE);
+  EXPECT_EQ(param->get_parameter_state_type(), StateType::CARTESIAN_POSE);
   EXPECT_EQ(param->is_empty(), false);
   EXPECT_EQ(param->get_value().get_name(), "A");
   EXPECT_EQ(param->get_value().get_reference_frame(), "B");
@@ -95,13 +114,16 @@ TEST(ParameterTest, ParameterThroughInterface) {
 
   auto param = param_interface->get_parameter<CartesianPose>();
   EXPECT_EQ(param->get_name(), "name");
-  EXPECT_EQ(param->get_type(), StateType::PARAMETER_CARTESIANPOSE);
+  EXPECT_EQ(param->get_type(), StateType::PARAMETER);
+  EXPECT_EQ(param->get_parameter_type(), ParameterType::STATE);
+  EXPECT_EQ(param->get_parameter_state_type(), StateType::CARTESIAN_POSE);
   EXPECT_EQ(param->is_empty(), false);
   EXPECT_EQ(param->get_value().get_name(), "A");
   EXPECT_EQ(param->get_value().get_reference_frame(), "B");
   EXPECT_TRUE(param->get_value().data().isApprox(pose.data()));
 
   auto param_value = param_interface->get_parameter_value<CartesianPose>();
+  EXPECT_EQ(param_value.get_type(), StateType::CARTESIAN_POSE);
   EXPECT_EQ(param_value.get_name(), "A");
   EXPECT_EQ(param_value.get_reference_frame(), "B");
   EXPECT_TRUE(param_value.data().isApprox(pose.data()));
@@ -115,7 +137,7 @@ TEST(ParameterTest, ParameterThroughInterface) {
 }
 
 TEST(ParameterTest, ParameterInterfaceBadPointer) {
-  ParameterInterface parameter_interface(StateType::PARAMETER_INT, "name");
+  ParameterInterface parameter_interface("name", ParameterType::INT);
 
   // by default (validate_pointer = true), throw when the ParameterInterface instance is not managed by any pointer
   EXPECT_THROW(parameter_interface.get_parameter<int>(), exceptions::InvalidPointerException);
@@ -127,7 +149,7 @@ TEST(ParameterTest, ParameterInterfaceBadPointer) {
 }
 
 TEST(ParameterTest, ParameterInterfaceNullCast) {
-  auto parameter_interface_ptr = std::make_shared<ParameterInterface>(StateType::PARAMETER_INT, "name");
+  auto parameter_interface_ptr = std::make_shared<ParameterInterface>("name", ParameterType::INT);
   std::shared_ptr<Parameter<int>> parameter;
 
   // by default (validate_pointer = true), throw when the pointer does not address a Parameter instance
diff --git a/source/state_representation/test/tests/test_state.cpp b/source/state_representation/test/tests/test_state.cpp
index 4da2a32ea..ef3c86992 100644
--- a/source/state_representation/test/tests/test_state.cpp
+++ b/source/state_representation/test/tests/test_state.cpp
@@ -12,20 +12,20 @@ TEST(StateTest, Constructors) {
   EXPECT_EQ(empty1.get_name(), "");
   EXPECT_TRUE(empty1.is_empty());
 
-  State empty2(StateType::JOINTSTATE);
-  EXPECT_EQ(empty2.get_type(), StateType::JOINTSTATE);
+  State empty2(StateType::JOINT_STATE);
+  EXPECT_EQ(empty2.get_type(), StateType::JOINT_STATE);
   EXPECT_EQ(empty2.get_name(), "");
   EXPECT_TRUE(empty2.is_empty());
 
-  State empty3(StateType::CARTESIANSTATE, "test", true);
-  EXPECT_EQ(empty3.get_type(), StateType::CARTESIANSTATE);
+  State empty3(StateType::CARTESIAN_STATE, "test", true);
+  EXPECT_EQ(empty3.get_type(), StateType::CARTESIAN_STATE);
   EXPECT_EQ(empty3.get_name(), "test");
   EXPECT_TRUE(empty3.is_empty());
   empty3.set_filled();
   EXPECT_FALSE(empty3.is_empty());
 
   State state(empty3);
-  EXPECT_EQ(state.get_type(), StateType::CARTESIANSTATE);
+  EXPECT_EQ(state.get_type(), StateType::CARTESIAN_STATE);
   EXPECT_EQ(state.get_name(), "test");
   EXPECT_FALSE(state.is_empty());
   state.set_empty();
@@ -59,13 +59,13 @@ TEST(StateTest, Timestamp) {
 }
 
 TEST(StateTest, Swap) {
-  State state1(StateType::CARTESIANSTATE, "cartesian", true);
+  State state1(StateType::CARTESIAN_STATE, "cartesian", true);
   State state2(StateType::STATE, "state", false);
   swap(state1, state2);
   EXPECT_EQ(state1.get_type(), StateType::STATE);
   EXPECT_EQ(state1.get_name(), "state");
   EXPECT_FALSE(state1.is_empty());
-  EXPECT_EQ(state2.get_type(), StateType::CARTESIANSTATE);
+  EXPECT_EQ(state2.get_type(), StateType::CARTESIAN_STATE);
   EXPECT_EQ(state2.get_name(), "cartesian");
   EXPECT_TRUE(state2.is_empty());
 }
\ No newline at end of file