From d00290f781e774d32050c2abfdd81763e2f1383f Mon Sep 17 00:00:00 2001
From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>
Date: Wed, 8 Sep 2021 11:20:19 +0200
Subject: [PATCH 01/22] Release/v4.0.0 (#202)

* Set version 4.0.0

* Rename control-libraries

* Update all references to
control_libraries to the new
repo name control-libraries

* Update CHANGELOG
---
 CHANGELOG.md                          | 103 +++++++++++++++++++-------
 CONTRIBUTING.md                       |  13 ++--
 README.md                             |  18 ++---
 demos/control_loop_examples/README.md |   2 +-
 demos/ros_examples/README.md          |   4 +-
 doxygen/README.md                     |   2 +-
 doxygen/docs/README.md                |   4 +-
 doxygen/docs/index.html               |   2 +-
 doxygen/doxygen.conf                  |   2 +-
 protocol/README.md                    |   4 +-
 protocol/clproto_cpp/CMakeLists.txt   |   2 +-
 python/Dockerfile.python              |  10 +--
 python/README.md                      |  12 +--
 python/run.sh                         |   2 +-
 python/setup.py                       |   4 +-
 source/CMakeLists.txt                 |   2 +-
 source/README.md                      |   4 +-
 17 files changed, 121 insertions(+), 69 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 3d066c7df..7d4881b30 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,46 +1,97 @@
 # CHANGELOG
 
 Release Versions:
+- [4.0.0](#400)
 - [3.1.0](#310)
 - [3.0.0](#300)
 - [2.0.0](#200)
 - [1.0.0](#100)
 
-## Upcoming changes (in development)
+## 4.0.0
 
-- Protobuf message protocol and C++ binding library `clproto` for
-serializing and deserializing control library objects
-(#168, #175, #177, #179, #180, #190)
-- Methods for packing and unpacking multiple encoded state messages
-into serialized message packets (#182)
-- Add set_data function (#163)
-- Move set_data declaration to State and add it for Ellipsoid (#166)
+Version 4.0.0 introduces some powerful new features for using `state_representation` objects
+in real applications, including the brand new `clproto` C++ serialization library on the basis of Protobuf.
+
+Many additional fixes and improvements have been made across the modules, and some deprecated methods
+have now been removed with this major version release.
+
+This release also marks the repository being renamed to `control-libraries` (formerly `control_libraries`). 
+
+See the following notes for more details.
+
+### Breaking changes
+
+This release contains the following breaking changes:
+- Rename repository to control-libraries
+- Remove previously deprecated from_std_vector function (#186)
+- Remove invalid multiplication operators for CartesianState and its derived classes (#195)
+
+**Repository namespace**
+
+The repository and all associated references have been renamed from `epfl-lasa/control_libraries` to
+`epfl-lasa/control-libraries`. This is to better match the GitHub repository standard style and to match
+the pattern in the container names. This breaking change will necessitate downstream users to update their 
+installation paths when cloning from GitHub.
+
+**state_representation**
+
+The following functions have been removed:
+- `CartesianState::from_std_vector`
+- `CartesianPose::from_std_vector`
+- `Ellipsoid::from_std_vector`
+- `JointState::from_std_vector`
+- `JointPositions::from_std_vector`
+
+The following multiplication operations are no longer permitted:
+- `CartesianState *= CartesianPose | CartesianTwist | CartesianWrench`
+- `CartesianPose *= CartesianTwist | CartesianWrench | CartesianState`
+- `CartesianTwist *= CartesianPose | CartesianTwist | CartesianWrench | CartesianState`
+- `CartesianTwist * CartesianPose | CartesianTwist | CartesianWrench | CartesianState`
+- `CartesianWrench *= CartesianPose | CartesianTwist | CartesianWrench | CartesianState`
+- `CartesianWrench * CartesianPose | CartesianTwist | CartesianWrench | CartesianState`
+
+The permitted multiplication operations are:
+- `CartesianState *= CartesianState`
+- `CartesianState * CartesianPose | CartesianTwist | CartesianWrench | CartesianState`
+- `CartesianPose *= CartesianPose`
+- `CartesianPose * CartesianPose | CartesianTwist | CartesianWrench | CartesianState`
+
+### Features
+
+The biggest feature to come with 4.0.0 is the Protobuf schema for `state_representation` types and the 
+associated `clproto` C++ serialization library. See the documentation for more information.
+
+Speaking of documentation, documentation is now generated and hosted for each release and for the main and develop
+branches at:
+[https://epfl-lasa.github.io/control-libraries](https://epfl-lasa.github.io/control-libraries)
+
+Similarly, docker images for development and downstream use are now also built and hosted automatically.
+
+The `state_representation` API has seen a number of smaller features to introduce new classes, methods and operators.
+
+- Protobuf message protocol and C++ binding library `clproto` for serializing and deserializing control library objects
+  (#168, #175, #177, #179, #180, #190)
+- Build and push development dependencies image in CI and related restructuring of Docker resources (#169)
 - Add automatic documentation generation and deployment to GitHub Pages (#170, #199)
-- Build and push development dependencies image in CI and related 
-  restructuring of Docker resources (#169)
-- Correct an error in the `makefile` of the protobuf bindings and remove
-  the generated bindings from the repository, while providing installation
-  scripts (#174)
+- Add set_data function declaration to all State objects (#163, #166)
 - Add class JointAccelerations (#173)
+- Methods for packing and unpacking multiple encoded state messages
+  into serialized message packets (#182)
+- Add scalar division operator for CartesianState and its derived classes (#192)
+
+### Fixes and improvements
+
+- Correct an error in the `makefile` of the protobuf bindings and remove
+  the generated bindings from the repository, while providing installation scripts (#174)
 - Fix clamp_state_variable function for CartesianState and JointState (#176, #191)
 - Install tagged versions of osqp and osqpEigen (#184)
-- Refactor JointState tests and split them into separate test suites (#183, #187)
 - Add missing integration constructor from JointAccelerations for JointVelocities (#185)
-- Remove previously deprecated from_std_vector function (#186)
-- Refactor CartesianState tests and split them into separate test suites (#188)
-- Add scalar division operator for CartesianState and its derived classes (#192)
-- Remove invalid multiplication operators for CartesianState and its derived classes (#195)
 - Fix path to Dockerfile in demos (#197)
 
-### Pending TODOs for the next release
+### Behind the scenes
 
-- Revise `*=` and `*` operators in Cartesian types before the next release with
-  breaking changes (some are marked *deprecated*, and some are left as is, but
-  they should be deleted) (#156)
-- Add the wrench computation in the `*` operator and `inverse` function (#134)
-- Refactor and improve unittests for state_representation (especially JointState
-  and CartesianState)
-- Rename repository
+- Refactor JointState tests and split them into separate test suites (#183, #187)
+- Refactor CartesianState tests and split them into separate test suites (#188)
 
 ## 3.1.0
 
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
index 76ed4efb0..41728d8b9 100644
--- a/CONTRIBUTING.md
+++ b/CONTRIBUTING.md
@@ -4,8 +4,8 @@ This repository is still under development, and we welcome user engagement to
 find bugs, resolve issues and add useful features.  
 
 Before contributing to this repository, please first discuss the change you wish to make
-by using the repository [Discussions](https://github.com/epfl-lasa/control_libraries/discussions),
-opening an [issue](https://github.com/epfl-lasa/control_libraries/issues),
+by using the repository [Discussions](https://github.com/epfl-lasa/control-libraries/discussions),
+opening an [issue](https://github.com/epfl-lasa/control-libraries/issues),
 or by contacting the maintainers of this repository directly. 
 
 
@@ -96,9 +96,10 @@ they can be marked as `EXPERIMENTAL` to exclude them from compilation.
 
 At the time of release, a release branch should be made from development. In the release branch,
 the project version number should be updated in the following locations:
-- The [top-level CMakeLists](source/CMakeLists.txt) 
-- The [python bindings setup.py](python/setup.py)
-- The PROJECT_NUMBER in the [doxygen config](doxygen/doxygen.conf)
+- The [top-level CMakeLists](./source/CMakeLists.txt) 
+- The [python bindings setup.py](./python/setup.py)
+- The [clproto bindings CMakeLists](./protocol/clproto_cpp/CMakeLists.txt)
+- The PROJECT_NUMBER in the [doxygen config](./doxygen/doxygen.conf)
 
 In addition, the release branch should be used to finalize the [changelog](CHANGELOG.md), which includes
 moving all content from the "Upcoming changes (in development)" header under a new header with the corresponding
@@ -108,4 +109,4 @@ Once the changes specific to the release have been approved, a linear GitFlow st
 merge this release branch into `main`, and then additionally squash and rebase the release branch back into `develop`.
 
 View and join the full discussion surrounding release workflow and strategy here: \
-https://github.com/epfl-lasa/control_libraries/discussions/77
\ No newline at end of file
+https://github.com/epfl-lasa/control-libraries/discussions/77
\ No newline at end of file
diff --git a/README.md b/README.md
index d0f8261ab..0e2dec760 100644
--- a/README.md
+++ b/README.md
@@ -4,16 +4,16 @@
         <td width="75%">Status</td>
     </tr>
     <tr>
-        <td width="25%"><a href="https://github.com/epfl-lasa/control_libraries/tree/main">Main</a></td>
+        <td width="25%"><a href="https://github.com/epfl-lasa/control-libraries/tree/main">Main</a></td>
         <td width="75%">
-            <img src="https://github.com/epfl-lasa/control_libraries/actions/workflows/build-test.yml/badge.svg?branch=main">
+            <img src="https://github.com/epfl-lasa/control-libraries/actions/workflows/build-test.yml/badge.svg?branch=main">
             <br>
-            <img src="https://github.com/epfl-lasa/control_libraries/actions/workflows/build-push.yml/badge.svg?branch=main">
+            <img src="https://github.com/epfl-lasa/control-libraries/actions/workflows/build-push.yml/badge.svg?branch=main">
         </td>
     </tr>
     <tr>
-        <td width="25%"><a href="https://github.com/epfl-lasa/control_libraries/tree/develop">Development</a></td>
-        <td width="75%"><img src="https://github.com/epfl-lasa/control_libraries/actions/workflows/build-test.yml/badge.svg?branch=develop"></td>
+        <td width="25%"><a href="https://github.com/epfl-lasa/control-libraries/tree/develop">Development</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,18 @@
 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/main/">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.
+<a href="https://github.com/epfl-lasa/control-libraries/tree/main/source">source</a> 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>
+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.
 
 ## 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 plain C++, ROS, and ROS2, refer to the <a href="https://github.com/epfl-lasa/control-libraries/tree/main/demos">demos</a> folder.
diff --git a/demos/control_loop_examples/README.md b/demos/control_loop_examples/README.md
index c5c1108d1..cef1e1a5e 100644
--- a/demos/control_loop_examples/README.md
+++ b/demos/control_loop_examples/README.md
@@ -7,7 +7,7 @@
 
 ## Running demonstration scripts
 This package contains a set of demonstration scripts that showcase the functionalities introduced in the different
-libraries of `control_libraries`.
+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:
diff --git a/demos/ros_examples/README.md b/demos/ros_examples/README.md
index cf113314f..3daf4b3c1 100644
--- a/demos/ros_examples/README.md
+++ b/demos/ros_examples/README.md
@@ -46,14 +46,14 @@ To run this script and visualize the robot, follow the steps below to launch a *
 
 - in a first terminal, run:
     ```bash
-    cd control_libraries/demos/ros_examples
+    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
     ```
 - in a second terminal, run:
     ```bash
-    # in a directory of your choice (preferably not in the control_libraries directory)
+    # in a directory of your choice (preferably not in the control-libraries directory)
     git clone https://github.com/domire8/pybullet_ros.git --branch control-libraries-demo --single-branch
     cd pybullet_ros/docker
     bash build-run.sh # this will build a docker image and spin up a container
diff --git a/doxygen/README.md b/doxygen/README.md
index add6cc7fa..0111d2db7 100644
--- a/doxygen/README.md
+++ b/doxygen/README.md
@@ -17,4 +17,4 @@ A GitHub workflow automatically generates the html documentation and commits it
 to the `docs` branch of this repo. GitHub Pages is configured to read the static
 page content from this branch and serves it from the following URL:
 
-https://epfl-lasa.github.io/control_libraries
+https://epfl-lasa.github.io/control-libraries
diff --git a/doxygen/docs/README.md b/doxygen/docs/README.md
index ee8f54662..895f72e34 100644
--- a/doxygen/docs/README.md
+++ b/doxygen/docs/README.md
@@ -1,4 +1,4 @@
-# https://epfl-lasa.github.io/control_libraries
+# https://epfl-lasa.github.io/control-libraries
 
 This branch hosts html documentation auto-generated from the control libraries source code
 using doxygen and hosted using GitHub Pages.
@@ -6,4 +6,4 @@ using doxygen and hosted using GitHub Pages.
 The content of this branch is not intended to be viewed or edited directly.
 
 Instead, please refer to the following page to view the documentation:
-https://epfl-lasa.github.io/control_libraries
+https://epfl-lasa.github.io/control-libraries
diff --git a/doxygen/docs/index.html b/doxygen/docs/index.html
index 8a56fb905..db1c849ae 100644
--- a/doxygen/docs/index.html
+++ b/doxygen/docs/index.html
@@ -8,7 +8,7 @@ <h1>Documented branches and tags</h1>
 <ul id="version_list"/>
 <script>
 // discover all documented versions and link them in the version list
-var root_path = "https://api.github.com/repos/epfl-lasa/control_libraries/git/trees/docs";
+var root_path = "https://api.github.com/repos/epfl-lasa/control-libraries/git/trees/docs";
 fetch(root_path).then((resp) => resp.json()).then(function(data) {
   var versions_dir = data.tree.filter(f=>/versions/.test(f.path));
   fetch(versions_dir[0].url).then((resp) => resp.json()).then(function(data) {
diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf
index cfad74e3d..c3223e0e1 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         = 3.1.0
+PROJECT_NUMBER         = 4.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/README.md b/protocol/README.md
index e0e2734e1..900fb9f60 100644
--- a/protocol/README.md
+++ b/protocol/README.md
@@ -33,8 +33,8 @@ The easiest way to install `clproto` is to use the included install script in au
 By supplying the `--auto` flag to this script, it will automatically and recursively install any dependencies.
 If Protobuf is not yet installed, this step will take some time.
 ```shell
-git clone https://github.com/epfl-lasa/control_libraries.git
-sudo control_libraries/clproto/install.sh --auto
+git clone https://github.com/epfl-lasa/control-libraries.git
+sudo control-libraries/clproto/install.sh --auto
 ```
 
 ### Copying protobuf dependencies
diff --git a/protocol/clproto_cpp/CMakeLists.txt b/protocol/clproto_cpp/CMakeLists.txt
index c966bea9f..230df7120 100644
--- a/protocol/clproto_cpp/CMakeLists.txt
+++ b/protocol/clproto_cpp/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.9)
-project(clproto VERSION 3.1.0)
+project(clproto VERSION 4.0.0)
 
 set(CMAKE_CXX_STANDARD 17)
 
diff --git a/python/Dockerfile.python b/python/Dockerfile.python
index ba4e65c36..053e936da 100644
--- a/python/Dockerfile.python
+++ b/python/Dockerfile.python
@@ -2,13 +2,13 @@ FROM ghcr.io/epfl-lasa/control-libraries/development-dependencies:latest
 ARG BRANCH=develop
 
 WORKDIR /source
-RUN git clone --single-branch --branch $BRANCH https://github.com/epfl-lasa/control_libraries
-RUN bash control_libraries/source/install.sh --auto --no-controllers --no-dynamical-systems --no-robot-model
+RUN git clone --single-branch --branch $BRANCH https://github.com/epfl-lasa/control-libraries
+RUN bash control-libraries/source/install.sh --auto --no-controllers --no-dynamical-systems --no-robot-model
 RUN ldconfig
 
-COPY source control_libraries/python/source
-COPY pyproject.toml setup.py control_libraries/python/
-RUN pip3 install control_libraries/python
+COPY source control-libraries/python/source
+COPY pyproject.toml setup.py control-libraries/python/
+RUN pip3 install control-libraries/python
 
 RUN useradd --create-home --shell /bin/bash dev
 USER dev
diff --git a/python/README.md b/python/README.md
index 27d89c897..db879bd41 100644
--- a/python/README.md
+++ b/python/README.md
@@ -4,7 +4,7 @@ This directory defines Python bindings for the control libraries.
 
 ## Installation
 
-You must first install `control_libraries` before you can install the Python bindings.
+You must first install `control-libraries` before you can install the Python bindings.
 Refer to the installation instructions in the top-level [README](../README.md) for more information.
 
 Additionally, the installation of the bindings requires the following prerequisites:
@@ -13,13 +13,13 @@ Additionally, the installation of the bindings requires the following prerequisi
 
 The installation itself is then quite straightforward:
 ```shell script
-git clone https://github.com/epfl-lasa/control_libraries
+git clone https://github.com/epfl-lasa/control-libraries
 
-## install control_libraries (skip this stage if already done)
-bash control_libraries/source/install.sh
+## install control-libraries (skip this stage if already done)
+bash control-libraries/source/install.sh
 
 ## install the bindings using the pip installer
-pip3 install control_libraries/python
+pip3 install control-libraries/python
 ```
 
 The example above installs the module to the default dist-packages location.
@@ -29,7 +29,7 @@ The process also works with Python virtual environments. For example, with `pipe
 ```shell script
 ## pip3 install pipenv
 
-pipenv install control_libraries/python
+pipenv install control-libraries/python
 ```
 
 Once installed, you can simply import the module with an optional short alias:
diff --git a/python/run.sh b/python/run.sh
index ae80a98dc..b9d6061bd 100755
--- a/python/run.sh
+++ b/python/run.sh
@@ -9,6 +9,6 @@ echo "Using control libraries branch ${BRANCH}"
 docker build . --file ./Dockerfile.python --build-arg BRANCH="${BRANCH}" --tag control-libraries/python/test || exit 1
 
 docker run -it --rm \
-  --volume "$(pwd)":/source/control_libraries/python \
+  --volume "$(pwd)":/source/control-libraries/python \
   --name control-libraries-python-test \
   control-libraries/python/test
diff --git a/python/setup.py b/python/setup.py
index 6491fa150..44926f2b2 100644
--- a/python/setup.py
+++ b/python/setup.py
@@ -4,7 +4,7 @@
 from pybind11.setup_helpers import ParallelCompile, naive_recompile
 import os
 
-__version__ = "3.1.0"
+__version__ = "4.0.0"
 __libraries__ = ['state_representation']
 __include_dirs__ = []
 
@@ -40,7 +40,7 @@
     version=__version__,
     author="Enrico Eberhard",
     author_email="enrico.eberhard@epfl.ch",
-    url="https://github.com/epfl-lasa/control_libraries",
+    url="https://github.com/epfl-lasa/control-libraries",
     description="Python bindings for the C++ control libraries",
     long_description="",
     ext_modules=ext_modules,
diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt
index a91bd2398..9f764cbfe 100644
--- a/source/CMakeLists.txt
+++ b/source/CMakeLists.txt
@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.9)
 
-project(control_libraries VERSION 3.1.0)
+project(control-libraries VERSION 4.0.0)
 
 # Build options
 option(BUILD_TESTING "Build all tests." OFF)
diff --git a/source/README.md b/source/README.md
index 91d5389a9..29f232f6a 100644
--- a/source/README.md
+++ b/source/README.md
@@ -97,7 +97,7 @@ The library files are installed to `usr/local/lib`, and the library header files
 are copied to `/usr/local/include`.
 
 ```shell script
-cd control_libraries/source
+cd control-libraries/source
 mkdir build && cd build
 cmake -DCMAKE_BUILD_TYPE=Release ..
 make -j
@@ -124,7 +124,7 @@ using the CMake directive `add_subdirectory(...)` to link it with your project.
 
 ## Troubleshooting
 
-This section lists common problems that might come up when using the `control_libraries` modules.
+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:

From b1e2c7f019436ddc014757d68c36bbd64b6e23ec Mon Sep 17 00:00:00 2001
From: domire8 <71256590+domire8@users.noreply.github.com>
Date: Wed, 8 Sep 2021 13:55:36 +0200
Subject: [PATCH 02/22] Remove -j option from make_bindings (#203)

---
 protocol/install.sh | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/protocol/install.sh b/protocol/install.sh
index 505d42c02..ce8f343c3 100755
--- a/protocol/install.sh
+++ b/protocol/install.sh
@@ -35,7 +35,7 @@ Options:
   -h, --help               Show this help message."
 
 function make_bindings() {
-  cd "${PROTOBUF_DIR}" && make -j all || exit 1
+  cd "${PROTOBUF_DIR}" && make all || exit 1
 }
 
 function clean_bindings() {

From d7078d9c20f2093ed1fc80d51138f231548618b8 Mon Sep 17 00:00:00 2001
From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>
Date: Thu, 9 Sep 2021 10:11:15 +0200
Subject: [PATCH 03/22] Python bindings for clproto (#204)

* Initial clproto python binding structure

* Add clproto module to setup.py

* Add clproto src dir

* Add dummy clproto test

* Bind and test all clproto methods

* Add Python binding for all clproto
methods

* Add tests for all clproto methods

* Specify clproto library requirement
in setup.py and only install module
if clproto library is found

* Fix missing invocation of binding
code for JointAccelerations

* Touchup clproto README

* Update CHANGELOG and README

* Fix setup and CI install

* Install clproto in python test
workflow

* Remove clproto library link from
state representation module, in case
clproto is not installed

Co-authored-by: Dominic Reber <dominic.reber@epfl.ch>
---
 .../actions/build-test-python/entrypoint.sh   |   2 +-
 CHANGELOG.md                                  |   4 +
 protocol/clproto_cpp/README.md                |  10 +-
 python/Dockerfile.python                      |   2 +-
 python/README.md                              |  25 ++-
 python/run.sh                                 |   1 +
 python/setup.py                               |  32 +++-
 python/source/clproto/bind_clproto.cpp        | 153 ++++++++++++++++++
 python/source/clproto/clproto_bindings.cpp    |  17 ++
 python/source/clproto/clproto_bindings.h      |  21 +++
 .../state_representation/bind_joint_space.cpp |   1 +
 python/test/test_clproto.py                   | 112 +++++++++++++
 12 files changed, 366 insertions(+), 14 deletions(-)
 create mode 100644 python/source/clproto/bind_clproto.cpp
 create mode 100644 python/source/clproto/clproto_bindings.cpp
 create mode 100644 python/source/clproto/clproto_bindings.h
 create mode 100755 python/test/test_clproto.py

diff --git a/.github/actions/build-test-python/entrypoint.sh b/.github/actions/build-test-python/entrypoint.sh
index d6f3e43de..7933628ec 100755
--- a/.github/actions/build-test-python/entrypoint.sh
+++ b/.github/actions/build-test-python/entrypoint.sh
@@ -2,7 +2,7 @@
 
 echo ">>> Installing control libraries..."
 bash /github/workspace/source/install.sh --auto --no-controllers --no-dynamical-systems --no-robot-model || exit 1
-ldconfig
+bash /github/workspace/protocol/install.sh --auto || exit 1
 
 echo ">>> Building Python bindings..."
 pip3 install /github/workspace/python || (echo ">>> [ERROR] Build stage failed!" && exit 2) || exit $?
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 7d4881b30..783a6e1b8 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -7,6 +7,10 @@ Release Versions:
 - [2.0.0](#200)
 - [1.0.0](#100)
 
+## Upcoming changes (in development)
+
+- Python bindings for clproto (#204)
+
 ## 4.0.0
 
 Version 4.0.0 introduces some powerful new features for using `state_representation` objects
diff --git a/protocol/clproto_cpp/README.md b/protocol/clproto_cpp/README.md
index af70703d1..467c561c8 100644
--- a/protocol/clproto_cpp/README.md
+++ b/protocol/clproto_cpp/README.md
@@ -80,14 +80,14 @@ clproto::pack_fields(encoded_robot_state, encoded_packet_buffer);
 
 // The message fields can also be packed into a std::string,
 // provided that sufficient space is reserved for all messages
-std::string encoded_packet_str();
+std::string encoded_packet_str;
 encoded_packet_str.reserve(2 * CLPROTO_PACKING_MAX_FIELD_LENGTH);
 clproto::pack_fields(encoded_robot_state, encoded_packet_str.data());
 
 // Unpack a combined message packet back into an ordered vector of encoded state variables
-std::vector<std::string> unpacked_encoded_robot_state = clproto::unpack(encoded_packet_buffer);
+std::vector<std::string> unpacked_encoded_robot_state = clproto::unpack_fields(encoded_packet_buffer);
 // (or, for a string type encoded packet:)
-unpacked_encoded_robot_state = clproto::unpack(encoded_packet_str.c_str());
+unpacked_encoded_robot_state = clproto::unpack_fields(encoded_packet_str.c_str());
 
 // Finally, decode the encoded fields in the same order as the original packing
 auto decoded_cart_state = clproto::decode<CartesianState>(unpacked_encoded_robot_state.at(0));
@@ -137,10 +137,10 @@ If bandwidth is limited, the following formula can be used to calculate the mini
 ```c++
 std::vector<std::string> encoded_fields = ...;
 
-// The packet header starts with N + 1 values of size proto::field_length_t,
+// The packet header starts with N + 1 values of size clproto::field_length_t,
 // where N is the number of fields N. The first value stores the number of fields,
 // while the following N values store the data length of each respective field.
-std::size_t packet_size = sizeof(proto::field_length_t) * (encoded_fields.size() + 1);
+std::size_t packet_size = sizeof(clproto::field_length_t) * (encoded_fields.size() + 1);
 
 // Add the size of each field
 for (const auto& field : encoded_fields) {
diff --git a/python/Dockerfile.python b/python/Dockerfile.python
index 053e936da..31d749ed8 100644
--- a/python/Dockerfile.python
+++ b/python/Dockerfile.python
@@ -4,7 +4,7 @@ ARG BRANCH=develop
 WORKDIR /source
 RUN git clone --single-branch --branch $BRANCH https://github.com/epfl-lasa/control-libraries
 RUN bash control-libraries/source/install.sh --auto --no-controllers --no-dynamical-systems --no-robot-model
-RUN ldconfig
+RUN bash control-libraries/protocol/install.sh --auto
 
 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 db879bd41..e52508591 100644
--- a/python/README.md
+++ b/python/README.md
@@ -51,10 +51,23 @@ from state_representation import JointState
 B = JointState.Random("B", 3)
 ```
 
+If the `clproto` C++ library is installed, the installation steps above will automatically install the `clproto`
+Python module which can be used to encode and decode objects into bytes of serialized data.
+```python
+#!/usr/bin/env python
+from state_representation import JointState
+import clproto
+
+B = JointState.Random("B", 3)
+encoded_msg = clproto.encode(B, clproto.MessageType.JOINT_STATE_MESSAGE)
+
+decoded_object = clproto.decode(encoded_msg)
+```
+
 ## Current status
 
 The Python binding project is currently under development.
-Bindings exist for the following libraries and classes:
+Bindings exist for the following modules, classes and methods:
 
 - `state_representation`:
   - `State`
@@ -68,6 +81,16 @@ Bindings exist for the following libraries and classes:
   - `JointVelocities`
   - `JointTorques`
   - `Jacobian`
+- `clproto`:
+  - `MessageType`
+  - `ParameterMessageType`
+  - `is_valid(msg)`
+  - `check_message_type(msg)`
+  - `check_parameter_message_type(msg)`
+  - `encode(obj, type)`
+  - `decode(msg)`
+  - `pack_fields(encoded_fields)`
+  - `unpack_fields(packet)`
 
 ## About
 
diff --git a/python/run.sh b/python/run.sh
index b9d6061bd..77d036cd9 100755
--- a/python/run.sh
+++ b/python/run.sh
@@ -6,6 +6,7 @@ if [ -n "$1" ]; then
 fi
 echo "Using control libraries branch ${BRANCH}"
 
+docker pull ghcr.io/epfl-lasa/control-libraries/development-dependencies:latest
 docker build . --file ./Dockerfile.python --build-arg BRANCH="${BRANCH}" --tag control-libraries/python/test || exit 1
 
 docker run -it --rm \
diff --git a/python/setup.py b/python/setup.py
index 44926f2b2..2ef32a71b 100644
--- a/python/setup.py
+++ b/python/setup.py
@@ -2,15 +2,19 @@
 from setuptools import setup
 from pybind11.setup_helpers import Pybind11Extension
 from pybind11.setup_helpers import ParallelCompile, naive_recompile
+import warnings
 import os
 
 __version__ = "4.0.0"
-__libraries__ = ['state_representation']
+__libraries__ = ['state_representation', 'clproto']
 __include_dirs__ = []
 
+__install_clproto_module__ = True
+
 # check that eigen and state_representation libraries can be found
 try:
-    eigen_dir = os.popen('cmake --find-package -DNAME=Eigen3 -DCOMPILER_ID=GNU -DLANGUAGE=C -DMODE=COMPILE').read().strip()
+    eigen_dir = os.popen(
+        'cmake --find-package -DNAME=Eigen3 -DCOMPILER_ID=GNU -DLANGUAGE=C -DMODE=COMPILE').read().strip()
     if eigen_dir.startswith('-I'):
         __include_dirs__.append(eigen_dir.lstrip('-I'))
     else:
@@ -19,7 +23,12 @@
     for lib in __libraries__:
         status = os.popen(f'ldconfig -p | grep {lib}').read().strip()
         if len(status) == 0:
-            raise Exception(f'Could not find {lib}!')
+            msg = f'Could not find library {lib}!'
+            if lib == 'clproto':
+                warnings.warn(f'{msg} The clproto module will not be installed.')
+                __install_clproto_module__ = False
+            else:
+                raise Exception(msg)
 except Exception as e:
     msg = f'Error with control library dependencies: {e.args[0]}. Ensure the control libraries are properly installed.'
 
@@ -30,11 +39,22 @@
                       sorted(glob("source/state_representation/*.cpp")),
                       cxx_std=17,
                       include_dirs=__include_dirs__,
-                      libraries=__libraries__,
+                      libraries=['state_representation'],
                       define_macros=[('MODULE_VERSION_INFO', __version__)],
-                      ),
+                      )
 ]
 
+if __install_clproto_module__:
+    ext_modules.append(
+        Pybind11Extension("clproto",
+                          sorted(glob("source/clproto/*.cpp")),
+                          cxx_std=17,
+                          include_dirs=__include_dirs__,
+                          libraries=['clproto'],
+                          define_macros=[('MODULE_VERSION_INFO', __version__)],
+                          )
+    )
+
 setup(
     name="control-libraries",
     version=__version__,
@@ -48,4 +68,4 @@
     python_requires='>=3',
     license='GNU GPL v3',
     zip_safe=False,
-)
\ No newline at end of file
+)
diff --git a/python/source/clproto/bind_clproto.cpp b/python/source/clproto/bind_clproto.cpp
new file mode 100644
index 000000000..9d192d873
--- /dev/null
+++ b/python/source/clproto/bind_clproto.cpp
@@ -0,0 +1,153 @@
+#include "clproto_bindings.h"
+
+#include <string>
+
+#include <state_representation/State.hpp>
+#include <state_representation/space/SpatialState.hpp>
+#include <state_representation/space/cartesian/CartesianState.hpp>
+#include <state_representation/space/cartesian/CartesianPose.hpp>
+#include <state_representation/space/cartesian/CartesianTwist.hpp>
+#include <state_representation/space/cartesian/CartesianWrench.hpp>
+#include <state_representation/robot/Jacobian.hpp>
+#include <state_representation/robot/JointState.hpp>
+#include <state_representation/robot/JointPositions.hpp>
+#include <state_representation/robot/JointVelocities.hpp>
+#include <state_representation/robot/JointAccelerations.hpp>
+#include <state_representation/robot/JointTorques.hpp>
+#include <state_representation/geometry/Shape.hpp>
+#include <state_representation/geometry/Ellipsoid.hpp>
+
+using namespace clproto;
+using namespace state_representation;
+
+template<typename T>
+inline py::bytes encode_bytes(const T& object) {
+  return py::bytes(encode(object));
+}
+
+void message_type(py::module_& m) {
+  py::enum_<MessageType>(m, "MessageType")
+      .value("UNKNOWN_MESSAGE", MessageType::UNKNOWN_MESSAGE)
+      .value("STATE_MESSAGE", MessageType::STATE_MESSAGE)
+      .value("SPATIAL_STATE_MESSAGE", MessageType::SPATIAL_STATE_MESSAGE)
+      .value("CARTESIAN_STATE_MESSAGE", MessageType::CARTESIAN_STATE_MESSAGE)
+      .value("CARTESIAN_POSE_MESSAGE", MessageType::CARTESIAN_POSE_MESSAGE)
+      .value("CARTESIAN_TWIST_MESSAGE", MessageType::CARTESIAN_TWIST_MESSAGE)
+      .value("CARTESIAN_ACCELERATION_MESSAGE", MessageType::CARTESIAN_ACCELERATION_MESSAGE)
+      .value("CARTESIAN_WRENCH_MESSAGE", MessageType::CARTESIAN_WRENCH_MESSAGE)
+      .value("JACOBIAN_MESSAGE", MessageType::JACOBIAN_MESSAGE)
+      .value("JOINT_STATE_MESSAGE", MessageType::JOINT_STATE_MESSAGE)
+      .value("JOINT_POSITIONS_MESSAGE", MessageType::JOINT_POSITIONS_MESSAGE)
+      .value("JOINT_VELOCITIES_MESSAGE", MessageType::JOINT_VELOCITIES_MESSAGE)
+      .value("JOINT_ACCELERATIONS_MESSAGE", MessageType::JOINT_ACCELERATIONS_MESSAGE)
+      .value("JOINT_TORQUES_MESSAGE", MessageType::JOINT_TORQUES_MESSAGE)
+      .value("SHAPE_MESSAGE", MessageType::SHAPE_MESSAGE)
+      .value("ELLIPSOID_MESSAGE", MessageType::ELLIPSOID_MESSAGE)
+      .value("PARAMETER_MESSAGE", MessageType::PARAMETER_MESSAGE)
+      .export_values();
+}
+
+void parameter_message_type(py::module_& m) {
+  py::enum_<ParameterMessageType>(m, "ParameterMessageType")
+      .value("UNKNOWN_PARAMETER", ParameterMessageType::UNKNOWN_PARAMETER)
+      .value("DOUBLE", ParameterMessageType::DOUBLE)
+      .value("DOUBLE_ARRAY", ParameterMessageType::DOUBLE_ARRAY)
+      .value("BOOL", ParameterMessageType::BOOL)
+      .value("BOOL_ARRAY", ParameterMessageType::BOOL_ARRAY)
+      .value("STRING", ParameterMessageType::STRING)
+      .value("STRING_ARRAY", ParameterMessageType::STRING_ARRAY)
+      .value("MATRIX", ParameterMessageType::MATRIX)
+      .value("VECTOR", ParameterMessageType::VECTOR)
+      .export_values();
+}
+
+void methods(py::module_& m) {
+  m.def("is_valid", &is_valid, "Check if a serialized binary string can be decoded into a support control libraries message type.", "msg"_a);
+  m.def("check_message_type", &check_message_type, "Check which control libraries message type a serialized binary string can be decoded as, if at all.", "msg"_a);
+  m.def("check_parameter_message_type", &check_parameter_message_type, "Check which control libraries message type a serialized binary string can be decoded as, if at all.", "msg"_a);
+
+  m.def("pack_fields", [](const std::vector<std::string>& encoded_fields) -> py::bytes {
+    char data[CLPROTO_PACKING_MAX_FIELDS * CLPROTO_PACKING_MAX_FIELD_LENGTH];
+    std::size_t packet_size = sizeof(field_length_t) * (encoded_fields.size() + 1);
+    for (const auto& field : encoded_fields) {
+      packet_size += field.size();
+    }
+    pack_fields(encoded_fields, data);
+    return std::string(data, packet_size);
+  }, "Pack an ordered vector of encoded field messages into a single data array.", "encoded_fields"_a);
+  m.def("unpack_fields", [](const std::string& packet) -> std::vector<py::bytes> {
+    std::vector<py::bytes> encoded_fields;
+    for (std::string field : unpack_fields(packet.c_str())) {
+      encoded_fields.emplace_back(field);
+    }
+    return encoded_fields;
+  }, "Unpack a data array into an ordered vector of encoded field messages.", "data"_a);
+
+  m.def("encode", [](const py::object& object, const MessageType& type) -> py::bytes {
+    switch (type) {
+      case MessageType::STATE_MESSAGE:
+        return encode_bytes<State>(object.cast<State>());
+      case MessageType::SPATIAL_STATE_MESSAGE:
+        return encode_bytes<SpatialState>(object.cast<SpatialState>());
+      case MessageType::CARTESIAN_STATE_MESSAGE:
+        return encode_bytes<CartesianState>(object.cast<CartesianState>());
+      case MessageType::CARTESIAN_POSE_MESSAGE:
+        return encode_bytes<CartesianPose>(object.cast<CartesianPose>());
+      case MessageType::CARTESIAN_TWIST_MESSAGE:
+        return encode_bytes<CartesianTwist>(object.cast<CartesianTwist>());
+      case MessageType::CARTESIAN_WRENCH_MESSAGE:
+        return encode_bytes<CartesianWrench>(object.cast<CartesianWrench>());
+      case MessageType::JACOBIAN_MESSAGE:
+        return encode_bytes<Jacobian>(object.cast<Jacobian>());
+      case MessageType::JOINT_STATE_MESSAGE:
+        return encode_bytes<JointState>(object.cast<JointState>());
+      case MessageType::JOINT_POSITIONS_MESSAGE:
+        return encode_bytes<JointPositions>(object.cast<JointPositions>());
+      case MessageType::JOINT_VELOCITIES_MESSAGE:
+        return encode_bytes<JointVelocities>(object.cast<JointVelocities>());
+      case MessageType::JOINT_ACCELERATIONS_MESSAGE:
+        return encode_bytes<JointAccelerations>(object.cast<JointAccelerations>());
+      case MessageType::JOINT_TORQUES_MESSAGE:
+        return encode_bytes<JointTorques>(object.cast<JointTorques>());
+      default:
+        return py::bytes();
+    }
+  }, "Encode a control libraries object into a serialized binary string representation (wire format).", py::arg("object"), py::arg("type"));
+
+  m.def("decode", [](const std::string& msg) -> py::object {
+    switch (check_message_type(msg)) {
+      case MessageType::STATE_MESSAGE:
+        return py::cast(decode<State>(msg));
+      case MessageType::SPATIAL_STATE_MESSAGE:
+        return py::cast(decode<SpatialState>(msg));
+      case MessageType::CARTESIAN_STATE_MESSAGE:
+        return py::cast<CartesianState>(decode<CartesianState>(msg));
+      case MessageType::CARTESIAN_POSE_MESSAGE:
+        return py::cast(decode<CartesianPose>(msg));
+      case MessageType::CARTESIAN_TWIST_MESSAGE:
+        return py::cast(decode<CartesianTwist>(msg));
+      case MessageType::CARTESIAN_WRENCH_MESSAGE:
+        return py::cast(decode<CartesianWrench>(msg));
+      case MessageType::JACOBIAN_MESSAGE:
+        return py::cast(decode<Jacobian>(msg));
+      case MessageType::JOINT_STATE_MESSAGE:
+        return py::cast(decode<JointState>(msg));
+      case MessageType::JOINT_POSITIONS_MESSAGE:
+        return py::cast(decode<JointPositions>(msg));
+      case MessageType::JOINT_VELOCITIES_MESSAGE:
+        return py::cast(decode<JointVelocities>(msg));
+      case MessageType::JOINT_ACCELERATIONS_MESSAGE:
+        return py::cast(decode<JointAccelerations>(msg));
+      case MessageType::JOINT_TORQUES_MESSAGE:
+        return py::cast(decode<JointTorques>(msg));
+      default:
+        return py::none();
+    }
+  }, "Decode a serialized binary string from wire format into a control libraries object instance.", "message"_a);
+}
+
+void bind_clproto(py::module_& m) {
+  message_type(m);
+  parameter_message_type(m);
+  methods(m);
+}
\ No newline at end of file
diff --git a/python/source/clproto/clproto_bindings.cpp b/python/source/clproto/clproto_bindings.cpp
new file mode 100644
index 000000000..eeb526cd6
--- /dev/null
+++ b/python/source/clproto/clproto_bindings.cpp
@@ -0,0 +1,17 @@
+#include "clproto_bindings.h"
+
+#define STRINGIFY(x) #x
+#define MACRO_STRINGIFY(x) STRINGIFY(x)
+
+PYBIND11_MODULE(clproto, m) {
+m.doc() = "Python bindings for control_libraries clproto";
+
+#ifdef MODULE_VERSION_INFO
+m.attr("__version__") = MACRO_STRINGIFY(MODULE_VERSION_INFO);
+#else
+m.attr("__version__") = "dev";
+#endif
+
+py::module_::import("state_representation");
+bind_clproto(m);
+}
\ No newline at end of file
diff --git a/python/source/clproto/clproto_bindings.h b/python/source/clproto/clproto_bindings.h
new file mode 100644
index 000000000..f1ae0df2d
--- /dev/null
+++ b/python/source/clproto/clproto_bindings.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <iostream>
+#include <string>
+#include <sstream>
+#include <vector>
+
+#include <pybind11/pybind11.h>
+#include <pybind11/chrono.h>
+#include <pybind11/eigen.h>
+#include <pybind11/operators.h>
+#include <pybind11/stl.h>
+
+#include <eigen3/Eigen/Core>
+
+#include <clproto.h>
+
+namespace py = pybind11;
+using namespace pybind11::literals;
+
+void bind_clproto(py::module_& m);
\ No newline at end of file
diff --git a/python/source/state_representation/bind_joint_space.cpp b/python/source/state_representation/bind_joint_space.cpp
index 38c1dc60b..9435cb903 100644
--- a/python/source/state_representation/bind_joint_space.cpp
+++ b/python/source/state_representation/bind_joint_space.cpp
@@ -332,5 +332,6 @@ void bind_joint_space(py::module_& m) {
   joint_state(m);
   joint_positions(m);
   joint_velocities(m);
+  joint_accelerations(m);
   joint_torques(m);
 }
\ No newline at end of file
diff --git a/python/test/test_clproto.py b/python/test/test_clproto.py
new file mode 100755
index 000000000..43917a67c
--- /dev/null
+++ b/python/test/test_clproto.py
@@ -0,0 +1,112 @@
+import unittest
+import state_representation as sr
+import clproto
+
+class TestClprotoPackUnpack(unittest.TestCase):
+    def test_pack_unpack(self):
+        objects = [sr.CartesianState.Random("A", "B"), sr.JointState.Random("robot", ["j1", "j2", "j3"])]
+        encoded_fields = [clproto.encode(objects[0], clproto.MessageType.CARTESIAN_STATE_MESSAGE),
+                          clproto.encode(objects[1], clproto.MessageType.JOINT_STATE_MESSAGE)]
+        packet = clproto.pack_fields(encoded_fields)
+
+        received_fields = clproto.unpack_fields(packet)
+        self.assertEqual(len(received_fields), len(encoded_fields))
+        [self.assertTrue(clproto.is_valid(msg)) for msg in received_fields]
+        decoded_objects = [clproto.decode(msg) for msg in received_fields]
+        [self.assertIsInstance(new, type(ref)) for new, ref in zip(decoded_objects, objects)]
+
+        # confirm cartesian state decoding
+        self.assertEqual(objects[0].get_name(), decoded_objects[0].get_name())
+        self.assertEqual(objects[0].get_reference_frame(), decoded_objects[0].get_reference_frame())
+        self.assertAlmostEqual(sr.dist(objects[0], decoded_objects[0]), 0)
+
+        # confirm joint state decoding
+        self.assertEqual(objects[1].get_name(), decoded_objects[1].get_name())
+        self.assertListEqual(objects[1].get_names(), decoded_objects[1].get_names())
+        self.assertAlmostEqual(sr.dist(objects[1], decoded_objects[1]), 0)
+
+class TestClprotoState(unittest.TestCase):
+    def state_class_assertions(self, reference_object, message_type):
+        object_type = type(reference_object)
+        msg = clproto.encode(reference_object, message_type)
+        self.assertTrue(clproto.is_valid(msg))
+        self.assertEqual(clproto.check_message_type(msg), message_type)
+        decoded_object = clproto.decode(msg)
+        self.assertIsInstance(decoded_object, object_type)
+        self.assertEqual(reference_object.get_name(), decoded_object.get_name())
+        self.assertEqual(reference_object.get_type(), decoded_object.get_type())
+
+    def test_encode_decode_state(self):
+        self.state_class_assertions(sr.State(sr.StateType.STATE, "A"), clproto.MessageType.STATE_MESSAGE)
+
+    def test_encode_decode_spatial_state(self):
+        self.state_class_assertions(sr.SpatialState(sr.StateType.STATE, "A", "B"), clproto.MessageType.SPATIAL_STATE_MESSAGE)
+
+class TestClprotoCartesian(unittest.TestCase):
+    def cartesian_class_assertions(self, reference_object, message_type):
+        object_type = type(reference_object)
+        msg = clproto.encode(reference_object, message_type)
+        self.assertTrue(clproto.is_valid(msg))
+        self.assertEqual(clproto.check_message_type(msg), message_type)
+        decoded_object = clproto.decode(msg)
+        self.assertIsInstance(decoded_object, object_type)
+        self.assertEqual(reference_object.get_name(), decoded_object.get_name())
+        self.assertEqual(reference_object.get_reference_frame(), decoded_object.get_reference_frame())
+        self.assertAlmostEqual(sr.dist(reference_object, decoded_object), 0)
+
+    def test_encode_decode_cartesian_state(self):
+        self.cartesian_class_assertions(sr.CartesianState.Random("A", "B"), clproto.MessageType.CARTESIAN_STATE_MESSAGE)
+
+    def test_encode_decode_cartesian_pose(self):
+        self.cartesian_class_assertions(sr.CartesianPose.Random("A", "B"), clproto.MessageType.CARTESIAN_POSE_MESSAGE)
+
+    def test_encode_decode_cartesian_twist(self):
+        self.cartesian_class_assertions(sr.CartesianTwist.Random("A", "B"), clproto.MessageType.CARTESIAN_TWIST_MESSAGE)
+
+    def test_encode_decode_cartesian_wrench(self):
+        self.cartesian_class_assertions(sr.CartesianWrench.Random("A", "B"), clproto.MessageType.CARTESIAN_WRENCH_MESSAGE)
+
+class TestClprotoJoint(unittest.TestCase):
+    def joint_class_assertions(self, reference_object, message_type):
+        object_type = type(reference_object)
+        msg = clproto.encode(reference_object, message_type)
+        self.assertTrue(clproto.is_valid(msg))
+        self.assertEqual(clproto.check_message_type(msg), message_type)
+        decoded_object = clproto.decode(msg)
+        self.assertIsInstance(decoded_object, object_type)
+        self.assertEqual(reference_object.get_name(), decoded_object.get_name())
+        self.assertListEqual(reference_object.get_names(), decoded_object.get_names())
+        self.assertAlmostEqual(sr.dist(reference_object, decoded_object), 0)
+
+    def test_encode_decode_joint_state(self):
+        self.joint_class_assertions(sr.JointState.Random("robot", ["j1", "j2", "j3"]), clproto.MessageType.JOINT_STATE_MESSAGE)
+
+    def test_encode_decode_joint_positions(self):
+        self.joint_class_assertions(sr.JointPositions.Random("robot", ["j1", "j2", "j3"]), clproto.MessageType.JOINT_POSITIONS_MESSAGE)
+
+    def test_encode_decode_joint_velocities(self):
+        self.joint_class_assertions(sr.JointVelocities.Random("robot", ["j1", "j2", "j3"]), clproto.MessageType.JOINT_VELOCITIES_MESSAGE)
+
+    def test_encode_decode_joint_accelerations(self):
+        self.joint_class_assertions(sr.JointAccelerations.Random("robot", ["j1", "j2", "j3"]), clproto.MessageType.JOINT_ACCELERATIONS_MESSAGE)
+
+    def test_encode_decode_joint_torques(self):
+        self.joint_class_assertions(sr.JointTorques.Random("robot", ["j1", "j2", "j3"]), clproto.MessageType.JOINT_TORQUES_MESSAGE)
+
+class TestClprotoJacobian(unittest.TestCase):
+    def test_encode_decode_jacobian(self):
+        obj1 = sr.Jacobian.Random("robot", ["j1", "j2", "j3"], "A", "B")
+        msg = clproto.encode(obj1, clproto.MessageType.JACOBIAN_MESSAGE)
+        self.assertTrue(clproto.is_valid(msg))
+        self.assertEqual(clproto.check_message_type(msg), clproto.MessageType.JACOBIAN_MESSAGE)
+        obj2 = clproto.decode(msg)
+        self.assertIsInstance(obj2, sr.Jacobian)
+        self.assertEqual(obj1.get_name(), obj2.get_name())
+        self.assertEqual(obj1.get_frame(), obj2.get_frame())
+        self.assertListEqual(obj1.get_joint_names(), obj2.get_joint_names())
+        self.assertEqual(obj1.rows(), obj2.rows())
+        self.assertEqual(obj1.cols(), obj2.cols())
+        [self.assertAlmostEqual(x, y) for x, y in zip(obj1.data().flatten(), obj2.data().flatten())]
+
+if __name__ == '__main__':
+    unittest.main()
\ No newline at end of file

From a92108323ff4018065a4b625001437563712cbe3 Mon Sep 17 00:00:00 2001
From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>
Date: Thu, 9 Sep 2021 11:44:32 +0200
Subject: [PATCH 04/22] JSON conversion methods (#205)

* JSON conversion methods

* Add to_json and from_json methods
to clproto to operate on encoded
wire format or state objects direclty,
allowing for more human-readable
printing and logging

* Add JSON parsing exception for
cases when JSON conversion fails

* Extend clproto tests to cover
JSON utilities

* Minor refactor of test names

* Python bindings for JSON

* Bind to_json and from_json
methods from clproto in Python.
Note that only the translation
between wire format and JSON is
defined, because the lack of
templates makes automatic deduction
of the desired conversion difficult.
The python usage should simply be
json = to_json(encode(obj, type) and
decode(from_json(json))

* Test json methods

* Update CHANGELOG
---
 CHANGELOG.md                                  |  1 +
 protocol/clproto_cpp/include/clproto.h        | 59 +++++++++++++++++-
 protocol/clproto_cpp/src/clproto.cpp          | 42 +++++++++++++
 protocol/clproto_cpp/test/tests/test_json.cpp | 62 +++++++++++++++++++
 .../clproto_cpp/test/tests/test_messages.cpp  |  4 +-
 .../test/tests/test_parameter_proto.cpp       | 16 ++---
 python/README.md                              |  6 +-
 python/source/clproto/bind_clproto.cpp        |  5 +-
 python/test/test_clproto.py                   | 16 +++++
 9 files changed, 195 insertions(+), 16 deletions(-)
 create mode 100644 protocol/clproto_cpp/test/tests/test_json.cpp

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 783a6e1b8..f70864e0f 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -10,6 +10,7 @@ Release Versions:
 ## Upcoming changes (in development)
 
 - Python bindings for clproto (#204)
+- JSON conversion support for clproto (#205)
 
 ## 4.0.0
 
diff --git a/protocol/clproto_cpp/include/clproto.h b/protocol/clproto_cpp/include/clproto.h
index 0731a7022..a8394ef03 100644
--- a/protocol/clproto_cpp/include/clproto.h
+++ b/protocol/clproto_cpp/include/clproto.h
@@ -31,6 +31,16 @@ class DecodingException : public std::runtime_error {
   explicit DecodingException(const std::string& msg);
 };
 
+/**
+ * @class JsonParsingException
+ * @brief A JsonParsingException is raised whenever a
+ * JSON conversion operation fails due to invalid encoding.
+ */
+class JsonParsingException : public std::runtime_error {
+public:
+  explicit JsonParsingException(const std::string& msg);
+};
+
 /**
  * @enum MessageType
  * @brief The MessageType enumeration contains the possible
@@ -118,9 +128,9 @@ std::string encode(const T& obj);
 /**
  * @brief Decode a serialized binary string from
  * wire format into a control libraries object instance.
- * Throws an exception if the message cannot be decoded
+ * @details Throws an exception if the message cannot be decoded
  * into the desired type.
- * @tparam The desired control libraries object type
+ * @tparam T The desired control libraries object type
  * @param msg The serialized binary string to decode
  * @return A new instance of the control libraries object
  */
@@ -130,7 +140,7 @@ T decode(const std::string& msg);
 /**
  * @brief Exception safe decoding of a serialized binary string
  * wire format into a control libraries object instance.
- * It modifies the object by reference if the decoding is
+ * @details It modifies the object by reference if the decoding is
  * successful, and leaves it unmodified otherwise.
  * @tparam T The desired control libraries object type
  * @param msg The serialized binary string to decode
@@ -168,4 +178,47 @@ void pack_fields(const std::vector<std::string>& fields, char* data);
  */
 std::vector<std::string> unpack_fields(const char* data);
 
+/**
+ * @brief Convert a serialized binary string from
+ * wire format into a JSON formatted state message description.
+ * @param msg The serialized binary string to decode
+ * @return The JSON formatted state message description
+ */
+std::string to_json(const std::string& msg);
+
+/**
+ * @brief Convert a control libraries object into
+ * into a JSON formatted state message description.
+ * @tparam T The provided control libraries object type
+ * @param obj The control libraries object to encode
+ * @return The JSON formatted state message description
+ */
+template<typename T>
+std::string to_json(const T& obj) {
+  return to_json(encode<T>(obj));
+}
+
+/**
+ * @brief Convert a JSON formatted state message description
+ * into a serialized binary string representation (wire format).
+ * @details Throws an exception if the message cannot be decoded
+ * into the desired type.
+ * @param json The JSON formatted state message description
+ * @return The serialized binary string encoding
+ */
+std::string from_json(const std::string& json);
+
+/**
+ * @brief Convert a JSON formatted state message description
+ * into a control libraries object instance.
+ * @details Throws an exception if the message cannot be
+ * converted into the desired type.
+ * @tparam T The desired control libraries object type
+ * @param json The JSON formatted state message description
+ * @return A new instance of the control libraries object
+ */
+template<typename T>
+T from_json(const std::string& json) {
+  return decode<T>(from_json(json));
+}
 }
diff --git a/protocol/clproto_cpp/src/clproto.cpp b/protocol/clproto_cpp/src/clproto.cpp
index d1e000ccd..1b9cfa886 100644
--- a/protocol/clproto_cpp/src/clproto.cpp
+++ b/protocol/clproto_cpp/src/clproto.cpp
@@ -1,5 +1,8 @@
 #include "clproto.h"
 
+#include <google/protobuf/util/json_util.h>
+#include <google/protobuf/util/type_resolver_util.h>
+
 #include "clproto/encoders.h"
 #include "clproto/decoders.h"
 
@@ -26,6 +29,8 @@ namespace clproto {
 
 DecodingException::DecodingException(const std::string& msg) : std::runtime_error(msg) {}
 
+JsonParsingException::JsonParsingException(const std::string& msg) : std::runtime_error(msg) {}
+
 bool is_valid(const std::string& msg) {
   return check_message_type(msg) != MessageType::UNKNOWN_MESSAGE;
 }
@@ -112,6 +117,43 @@ std::vector<std::string> unpack_fields(const char* data) {
   return fields;
 }
 
+// --- JSON utilities --- //
+
+std::string to_json(const std::string& msg) {
+  std::string json;
+
+  auto resolver = std::unique_ptr<google::protobuf::util::TypeResolver>{
+      google::protobuf::util::NewTypeResolverForDescriptorPool(
+          "", google::protobuf::DescriptorPool::generated_pool())
+  };
+
+  auto status = google::protobuf::util::BinaryToJsonString(
+      resolver.get(), "/state_representation.proto.StateMessage", msg, std::addressof(json));
+
+  if (!status.ok() || json.size() <= 2) {
+    throw JsonParsingException("Could not parse the binary data into a JSON formatted state message");
+  }
+  return json;
+}
+
+std::string from_json(const std::string& json) {
+  std::string msg;
+
+  auto resolver = std::unique_ptr<google::protobuf::util::TypeResolver>{
+      google::protobuf::util::NewTypeResolverForDescriptorPool(
+          "", google::protobuf::DescriptorPool::generated_pool())
+  };
+
+  auto status = google::protobuf::util::JsonToBinaryString(
+      resolver.get(), "/state_representation.proto.StateMessage", json, std::addressof(msg));
+
+  if (!status.ok()) {
+    throw JsonParsingException("Could not parse a valid state from the JSON message: " + json);
+  }
+
+  return msg;
+}
+
 /* ----------------------
  *         State
  * ---------------------- */
diff --git a/protocol/clproto_cpp/test/tests/test_json.cpp b/protocol/clproto_cpp/test/tests/test_json.cpp
new file mode 100644
index 000000000..e3d0c6313
--- /dev/null
+++ b/protocol/clproto_cpp/test/tests/test_json.cpp
@@ -0,0 +1,62 @@
+#include <gtest/gtest.h>
+
+#include <state_representation/space/cartesian/CartesianState.hpp>
+#include <state_representation/robot/JointState.hpp>
+
+#include "clproto.h"
+
+using namespace state_representation;
+
+TEST(JsonProtoTest, JsonToFromBinary) {
+  auto send_state = CartesianState::Random("A", "B");
+  std::string msg = clproto::encode(send_state);
+  ASSERT_TRUE(clproto::is_valid(msg));
+  ASSERT_TRUE(clproto::check_message_type(msg) == clproto::CARTESIAN_STATE_MESSAGE);
+
+  auto json = clproto::to_json(msg);
+  EXPECT_GT(json.size(), 2);  // empty JSON would be "{}"
+  auto msg2 = clproto::from_json(json);
+
+  CartesianState recv_state;
+  EXPECT_NO_THROW(clproto::decode<CartesianState>(msg));
+  EXPECT_TRUE(clproto::decode(msg, recv_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);
+}
+
+TEST(JsonProtoTest, JsonToFromObject) {
+  auto send_state = CartesianState::Random("A", "B");
+
+  auto json = clproto::to_json(send_state);
+  EXPECT_GT(json.size(), 2);  // empty JSON would be "{}"
+  auto recv_state = clproto::from_json<CartesianState>(json);
+
+  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(JsonProtoTest, JsonToFromInvalid) {
+  auto json = clproto::to_json(CartesianState::Random("A", "B"));
+
+  EXPECT_NO_THROW(clproto::from_json<CartesianState>(json));
+  EXPECT_THROW(clproto::from_json<CartesianState>("definitely not json"), clproto::JsonParsingException);
+  EXPECT_THROW(clproto::from_json("definitely not json"), clproto::JsonParsingException);
+
+  // valid JSON of the wrong state type should instead throw a decoding exception
+  EXPECT_THROW(clproto::from_json<JointState>(json), clproto::DecodingException);
+
+  EXPECT_THROW(clproto::to_json(std::string("definitely not binary encoded data")), clproto::JsonParsingException);
+}
+
+/* If a to_json template is invoked that is not implemented in clproto,
+ * there will be a linker error "undefined reference" at compile time.
+ * Of course, it's not really possible to test this at run-time.
+PSEUDO_TEST(JsonProtoTest, JsonToInvalidObject) {
+  foo::Object invalid_object;
+
+  EXPECT_LINKER_ERROR(clproto::to_json(invalid_object));
+}
+*/
\ No newline at end of file
diff --git a/protocol/clproto_cpp/test/tests/test_messages.cpp b/protocol/clproto_cpp/test/tests/test_messages.cpp
index 22ce98443..5176e87a1 100644
--- a/protocol/clproto_cpp/test/tests/test_messages.cpp
+++ b/protocol/clproto_cpp/test/tests/test_messages.cpp
@@ -6,7 +6,7 @@
 
 using namespace state_representation;
 
-TEST(CartesianProtoTest, EncodeDecodeState) {
+TEST(MessageProtoTest, EncodeDecodeState) {
   auto send_state = State(StateType::STATE, "A", false);
   std::string msg = clproto::encode(send_state);
   EXPECT_TRUE(clproto::is_valid(msg));
@@ -21,7 +21,7 @@ TEST(CartesianProtoTest, EncodeDecodeState) {
   EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str());
 }
 
-TEST(CartesianProtoTest, DecodeInvalidString) {
+TEST(MessageProtoTest, DecodeInvalidString) {
   std::string dummy_msg = "hello world";
 
   EXPECT_FALSE(clproto::is_valid(dummy_msg));
diff --git a/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp b/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp
index c43853200..01da9bc25 100644
--- a/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp
+++ b/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp
@@ -6,7 +6,7 @@
 
 using namespace state_representation;
 
-TEST(CartesianProtoTest, EncodeDecodeParameterDouble) {
+TEST(ParameterProtoTest, EncodeDecodeParameterDouble) {
   double value = 1.0;
 
   auto send_state = Parameter("A", value);
@@ -23,7 +23,7 @@ TEST(CartesianProtoTest, EncodeDecodeParameterDouble) {
   EXPECT_EQ(send_state.get_value(), recv_state.get_value());
 }
 
-TEST(CartesianProtoTest, EncodeDecodeParameterDoubleArray) {
+TEST(ParameterProtoTest, EncodeDecodeParameterDoubleArray) {
   std::vector<double> value = {1.0, 2.0, 3.0};
 
   auto send_state = Parameter("A", value);
@@ -43,7 +43,7 @@ TEST(CartesianProtoTest, EncodeDecodeParameterDoubleArray) {
   }
 }
 
-TEST(CartesianProtoTest, EncodeDecodeParameterBool) {
+TEST(ParameterProtoTest, EncodeDecodeParameterBool) {
   bool value = true;
 
   auto send_state = Parameter("A", value);
@@ -60,7 +60,7 @@ TEST(CartesianProtoTest, EncodeDecodeParameterBool) {
   EXPECT_EQ(send_state.get_value(), recv_state.get_value());
 }
 
-TEST(CartesianProtoTest, EncodeDecodeParameterBoolArray) {
+TEST(ParameterProtoTest, EncodeDecodeParameterBoolArray) {
   std::vector<bool> value = {true, false, true, false};
 
   auto send_state = Parameter("A", value);
@@ -80,7 +80,7 @@ TEST(CartesianProtoTest, EncodeDecodeParameterBoolArray) {
   }
 }
 
-TEST(CartesianProtoTest, EncodeDecodeParameterString) {
+TEST(ParameterProtoTest, EncodeDecodeParameterString) {
   std::string value = "value";
 
   auto send_state = Parameter("A", value);
@@ -97,7 +97,7 @@ TEST(CartesianProtoTest, EncodeDecodeParameterString) {
   EXPECT_STREQ(send_state.get_value().c_str(), recv_state.get_value().c_str());
 }
 
-TEST(CartesianProtoTest, EncodeDecodeParameterStringArray) {
+TEST(ParameterProtoTest, EncodeDecodeParameterStringArray) {
   std::vector<std::string> value = {"value 1", "value 2", "value 3"};
 
   auto send_state = Parameter("A", value);
@@ -117,7 +117,7 @@ TEST(CartesianProtoTest, EncodeDecodeParameterStringArray) {
   }
 }
 
-TEST(CartesianProtoTest, EncodeDecodeParameterVector) {
+TEST(ParameterProtoTest, EncodeDecodeParameterVector) {
   Eigen::VectorXd value(5);
   value << 1.0, 2.0, 3.0, 4.0, 5.0;
 
@@ -138,7 +138,7 @@ TEST(CartesianProtoTest, EncodeDecodeParameterVector) {
   }
 }
 
-TEST(CartesianProtoTest, EncodeDecodeParameterMatrix) {
+TEST(ParameterProtoTest, EncodeDecodeParameterMatrix) {
   Eigen::MatrixXd value(2, 3);
   value << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0;
 
diff --git a/python/README.md b/python/README.md
index e52508591..77218c101 100644
--- a/python/README.md
+++ b/python/README.md
@@ -87,10 +87,12 @@ Bindings exist for the following modules, classes and methods:
   - `is_valid(msg)`
   - `check_message_type(msg)`
   - `check_parameter_message_type(msg)`
-  - `encode(obj, type)`
-  - `decode(msg)`
+  - `msg = encode(obj, type)`
+  - `obj = decode(msg)`
   - `pack_fields(encoded_fields)`
   - `unpack_fields(packet)`
+  - `json = to_json(msg)`
+  - `msg = from_json(json)`
 
 ## About
 
diff --git a/python/source/clproto/bind_clproto.cpp b/python/source/clproto/bind_clproto.cpp
index 9d192d873..81747d16a 100644
--- a/python/source/clproto/bind_clproto.cpp
+++ b/python/source/clproto/bind_clproto.cpp
@@ -143,7 +143,10 @@ void methods(py::module_& m) {
       default:
         return py::none();
     }
-  }, "Decode a serialized binary string from wire format into a control libraries object instance.", "message"_a);
+  }, "Decode a serialized binary string from wire format into a control libraries object instance.", "msg"_a);
+
+  m.def("to_json", [](const std::string& msg) { return to_json(msg); }, "Convert a serialized binary string from wire format into a JSON formatted state message description", "msg"_a);
+  m.def("from_json", [](const std::string& json) -> py::bytes { return from_json(json); }, "Convert a JSON formatted state message description into a serialized binary string representation (wire format).", "msg"_a);
 }
 
 void bind_clproto(py::module_& m) {
diff --git a/python/test/test_clproto.py b/python/test/test_clproto.py
index 43917a67c..08df07eec 100755
--- a/python/test/test_clproto.py
+++ b/python/test/test_clproto.py
@@ -25,6 +25,22 @@ def test_pack_unpack(self):
         self.assertListEqual(objects[1].get_names(), decoded_objects[1].get_names())
         self.assertAlmostEqual(sr.dist(objects[1], decoded_objects[1]), 0)
 
+class TestClprotoJSON(unittest.TestCase):
+    def test_to_from_json(self):
+        reference_object = sr.CartesianState.Random("A", "B")
+        message_type = clproto.MessageType.CARTESIAN_STATE_MESSAGE
+        msg = clproto.encode(reference_object, message_type)
+        json = clproto.to_json(msg)
+        msg2 = clproto.from_json(json)
+        self.assertTrue(clproto.is_valid(msg2))
+        self.assertEqual(clproto.check_message_type(msg2), message_type)
+        decoded_object = clproto.decode(msg)
+        self.assertIsInstance(decoded_object, type(reference_object))
+
+        self.assertEqual(reference_object.get_name(), decoded_object.get_name())
+        self.assertEqual(reference_object.get_reference_frame(), decoded_object.get_reference_frame())
+        self.assertAlmostEqual(sr.dist(reference_object, decoded_object), 0)
+
 class TestClprotoState(unittest.TestCase):
     def state_class_assertions(self, reference_object, message_type):
         object_type = type(reference_object)

From bd48994c15080271c67fe9fcc4059a2a1b94fefa Mon Sep 17 00:00:00 2001
From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>
Date: Fri, 10 Sep 2021 11:56:32 +0200
Subject: [PATCH 05/22] Fix doc generation on release (#206)

* Fix parsing of release tag

* Get the tagged version from the
GITHUB_REF variable correctly by
stripping `refs-tags/` instead of
`refs/heads/`

* Restrict workflow to publish event

* Only trigger docs build once on release
publish, instead of multiple times
(created, released, published)
---
 .github/workflows/generate-docs.yml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/generate-docs.yml b/.github/workflows/generate-docs.yml
index aaee082fc..ae0602e8f 100644
--- a/.github/workflows/generate-docs.yml
+++ b/.github/workflows/generate-docs.yml
@@ -5,6 +5,7 @@ on:
       - main
       - develop
   release:
+    types: [published]
   workflow_dispatch:
 
 jobs:
@@ -24,7 +25,7 @@ jobs:
         if: ${{ github.event_name == 'release' }}
         shell: bash
         run: |
-          TAG="${GITHUB_REF#refs/heads/}"
+          TAG="${GITHUB_REF#refs-tags/}"
           TAG="${TAG/\//-}"
           mkdir -p doxygen/docs/versions
           sudo mv doxygen/docs/html doxygen/docs/versions/${TAG}

From f40cf7d79b2dfb9f3d17c5d9cd0e12f292c12a2b Mon Sep 17 00:00:00 2001
From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>
Date: Mon, 13 Sep 2021 09:31:06 +0200
Subject: [PATCH 06/22] Handle empty states in clproto (#207)

* Handle empty state messages

* Explicitly support and test encoding
and decoding empty state messages
for joint, cartesian, jacobian and param

* Fix bug in Parameter which was setting
filled on empty double constructor

* Add missing empty constructor
for Parameter<Ellipsoid>

* Update CHANGELOG

* Remove empty Parameter<Ellipsoid>

* Parameter<Ellipsoid> cannot be
empty-constructed because Ellipsoid
cannot be default constructed.
Therefore the template specialisation
for an empty Parameter<Ellipsoid>
cannot be implemented.

A general solution to this would
be to make the parameter value_
property a pointer, so construction
can be elided.
---
 CHANGELOG.md                                  |  1 +
 protocol/clproto_cpp/src/clproto.cpp          | 17 ++++-
 protocol/clproto_cpp/src/encoders.cpp         |  9 +++
 .../test/tests/test_cartesian_space_proto.cpp | 48 ++++++++++++
 .../test/tests/test_joint_space_proto.cpp     | 75 ++++++++++++++++++-
 .../test/tests/test_parameter_proto.cpp       | 33 ++++++++
 python/test/test_clproto.py                   |  1 +
 .../src/parameters/Parameter.cpp              |  4 +-
 8 files changed, 181 insertions(+), 7 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index f70864e0f..6d701a8f6 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -11,6 +11,7 @@ Release Versions:
 
 - Python bindings for clproto (#204)
 - JSON conversion support for clproto (#205)
+- Support empty state objects in clproto (#207)
 
 ## 4.0.0
 
diff --git a/protocol/clproto_cpp/src/clproto.cpp b/protocol/clproto_cpp/src/clproto.cpp
index 1b9cfa886..8999b5046 100644
--- a/protocol/clproto_cpp/src/clproto.cpp
+++ b/protocol/clproto_cpp/src/clproto.cpp
@@ -476,10 +476,15 @@ bool decode(const std::string& msg, Jacobian& obj) {
     }
 
     auto jacobian = message.jacobian();
-    auto raw_data = const_cast<double*>(jacobian.data().data());
-    auto data = Eigen::Map<Eigen::MatrixXd>(raw_data, jacobian.rows(), jacobian.cols());
     obj = Jacobian(
-        jacobian.state().name(), decoder(jacobian.joint_names()), jacobian.frame(), data, jacobian.reference_frame());
+        jacobian.state().name(), decoder(jacobian.joint_names()), jacobian.frame(), jacobian.reference_frame());
+    if (!jacobian.state().empty() && !jacobian.data().empty()) {
+      auto raw_data = const_cast<double*>(jacobian.data().data());
+      auto data = Eigen::Map<Eigen::MatrixXd>(raw_data, jacobian.rows(), jacobian.cols());
+      obj.set_data(data);
+    } else {
+      obj.set_empty();
+    }
     return true;
   } catch (...) {
     return false;
@@ -735,6 +740,9 @@ template<typename T>
 static std::string encode_parameter(const Parameter<T>& obj) {
   proto::StateMessage message;
   *message.mutable_parameter() = encoder<T>(obj);
+  if (obj.is_empty()) {
+    message.mutable_parameter()->mutable_state()->set_empty(true);
+  }
   return message.SerializeAsString();
 }
 template<typename T>
@@ -756,6 +764,9 @@ static bool decode_parameter(const std::string& msg, Parameter<T>& obj) {
       }
     }
     obj = decoder<T>(message.parameter());
+    if (message.parameter().state().empty()) {
+      obj.set_empty();
+    }
     return true;
   } catch (...) {
     return false;
diff --git a/protocol/clproto_cpp/src/encoders.cpp b/protocol/clproto_cpp/src/encoders.cpp
index f5eba0d09..6f6014ce6 100644
--- a/protocol/clproto_cpp/src/encoders.cpp
+++ b/protocol/clproto_cpp/src/encoders.cpp
@@ -46,6 +46,9 @@ proto::Quaterniond encoder(const Eigen::Quaterniond& quaternion) {
 proto::CartesianState encoder(const CartesianState& cartesian_state) {
   proto::CartesianState message;
   *message.mutable_spatial_state() = encoder(static_cast<SpatialState>(cartesian_state));
+  if (cartesian_state.is_empty()) {
+    return message;
+  }
   *message.mutable_position() = encoder(cartesian_state.get_position());
   *message.mutable_orientation() = encoder(cartesian_state.get_orientation());
   *message.mutable_linear_velocity() = encoder(cartesian_state.get_linear_velocity());
@@ -60,6 +63,9 @@ proto::CartesianState encoder(const CartesianState& cartesian_state) {
 proto::Jacobian encoder(const Jacobian& jacobian) {
   proto::Jacobian message;
   *message.mutable_state() = encoder(static_cast<State>(jacobian));
+  if (jacobian.is_empty()) {
+    return message;
+  }
   *message.mutable_joint_names() = {jacobian.get_joint_names().begin(), jacobian.get_joint_names().end()};
   message.set_frame(jacobian.get_frame());
   message.set_reference_frame(jacobian.get_reference_frame());
@@ -72,6 +78,9 @@ proto::Jacobian encoder(const Jacobian& jacobian) {
 proto::JointState encoder(const JointState& joint_state) {
   proto::JointState message;
   *message.mutable_state() = encoder(static_cast<State>(joint_state));
+  if (joint_state.is_empty()) {
+    return message;
+  }
   *message.mutable_joint_names() = {joint_state.get_names().begin(), joint_state.get_names().end()};
   *message.mutable_positions() = matrix_encoder(joint_state.get_positions());
   *message.mutable_velocities() = matrix_encoder(joint_state.get_velocities());
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 b5cdfbdb8..71c7d1fec 100644
--- a/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp
+++ b/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp
@@ -35,6 +35,7 @@ TEST(CartesianProtoTest, EncodeDecodeCartesianState) {
   CartesianState recv_state;
   EXPECT_NO_THROW(clproto::decode<CartesianState>(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());
@@ -50,6 +51,7 @@ TEST(CartesianProtoTest, EncodeDecodeCartesianPose) {
   CartesianPose recv_state;
   EXPECT_NO_THROW(clproto::decode<CartesianPose>(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());
@@ -65,6 +67,7 @@ TEST(CartesianProtoTest, EncodeDecodeCartesianTwist) {
   CartesianTwist recv_state;
   EXPECT_NO_THROW(clproto::decode<CartesianTwist>(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());
@@ -80,8 +83,53 @@ TEST(CartesianProtoTest, EncodeDecodeCartesianWrench) {
   CartesianWrench recv_state;
   EXPECT_NO_THROW(clproto::decode<CartesianWrench>(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(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());
+}
+
+TEST(CartesianProtoTest, EncodeDecodeEmptyCartesianPose) {
+  CartesianPose empty_state;
+  EXPECT_TRUE(empty_state.is_empty());
+  std::string msg;
+  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
+
+  CartesianPose recv_state;
+  EXPECT_NO_THROW(recv_state = clproto::decode<CartesianPose>(msg));
+  EXPECT_TRUE(recv_state.is_empty());
+}
+
+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, 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 7bfcb175c..dfddd5ee3 100644
--- a/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp
+++ b/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp
@@ -12,7 +12,7 @@
 using namespace state_representation;
 
 TEST(JointProtoTest, EncodeDecodeJacobian) {
-  auto send_state = Jacobian("robot", 3, "A", "B");
+  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);
@@ -20,8 +20,10 @@ TEST(JointProtoTest, EncodeDecodeJacobian) {
   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());
+  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());
   EXPECT_EQ(send_state.rows(), recv_state.rows());
   EXPECT_EQ(send_state.cols(), recv_state.cols());
@@ -42,6 +44,7 @@ TEST(JointProtoTest, EncodeDecodeJointState) {
   JointState recv_state;
   EXPECT_NO_THROW(clproto::decode<JointState>(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());
@@ -60,6 +63,7 @@ TEST(JointProtoTest, EncodeDecodeJointPositions) {
   JointPositions recv_state;
   EXPECT_NO_THROW(clproto::decode<JointPositions>(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());
@@ -78,6 +82,7 @@ TEST(JointProtoTest, EncodeDecodeJointVelocities) {
   JointVelocities recv_state;
   EXPECT_NO_THROW(clproto::decode<JointVelocities>(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());
@@ -96,6 +101,7 @@ TEST(JointProtoTest, EncodeDecodeJointAccelerations) {
   JointAccelerations recv_state;
   EXPECT_NO_THROW(clproto::decode<JointAccelerations>(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());
@@ -114,6 +120,7 @@ TEST(JointProtoTest, EncodeDecodeJointTorques) {
   JointTorques recv_state;
   EXPECT_NO_THROW(clproto::decode<JointTorques>(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());
@@ -122,3 +129,69 @@ TEST(JointProtoTest, EncodeDecodeJointTorques) {
   }
   EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5);
 }
+
+TEST(JointProtoTest, EncodeDecodeEmptyJacobian) {
+  Jacobian empty_state;
+  EXPECT_TRUE(empty_state.is_empty());
+  std::string msg;
+  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
+
+  Jacobian recv_state;
+  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_parameter_proto.cpp b/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp
index 01da9bc25..54632d025 100644
--- a/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp
+++ b/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp
@@ -159,4 +159,37 @@ TEST(ParameterProtoTest, EncodeDecodeParameterMatrix) {
   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));
   }
+}
+
+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(ParameterProtoTest, EncodeDecodeEmptyParameterBool) {
+  Parameter<bool> empty_state("");
+  EXPECT_TRUE(empty_state.is_empty());
+  std::string msg;
+  EXPECT_NO_THROW(msg = clproto::encode(empty_state));
+
+  Parameter<bool> recv_state("");
+  EXPECT_NO_THROW(recv_state = clproto::decode<Parameter<bool>>(msg));
+  EXPECT_TRUE(recv_state.is_empty());
+}
+
+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/python/test/test_clproto.py b/python/test/test_clproto.py
index 08df07eec..67011ed16 100755
--- a/python/test/test_clproto.py
+++ b/python/test/test_clproto.py
@@ -119,6 +119,7 @@ def test_encode_decode_jacobian(self):
         self.assertIsInstance(obj2, sr.Jacobian)
         self.assertEqual(obj1.get_name(), obj2.get_name())
         self.assertEqual(obj1.get_frame(), obj2.get_frame())
+        self.assertEqual(obj1.get_reference_frame(), obj2.get_reference_frame())
         self.assertListEqual(obj1.get_joint_names(), obj2.get_joint_names())
         self.assertEqual(obj1.rows(), obj2.rows())
         self.assertEqual(obj1.cols(), obj2.cols())
diff --git a/source/state_representation/src/parameters/Parameter.cpp b/source/state_representation/src/parameters/Parameter.cpp
index 211468e56..1940df710 100644
--- a/source/state_representation/src/parameters/Parameter.cpp
+++ b/source/state_representation/src/parameters/Parameter.cpp
@@ -6,9 +6,7 @@
 namespace state_representation {
 template <>
 Parameter<double>::Parameter(const std::string& name) :
-    ParameterInterface(StateType::PARAMETER_DOUBLE, name) {
-  this->set_filled();
-}
+    ParameterInterface(StateType::PARAMETER_DOUBLE, name) {}
 
 template <>
 Parameter<double>::Parameter(const std::string& name, const double& value) :

From 1777969acc54bdff3173e097b3b3e4378798f2ef Mon Sep 17 00:00:00 2001
From: domire8 <71256590+domire8@users.noreply.github.com>
Date: Wed, 15 Sep 2021 15:02:11 +0200
Subject: [PATCH 07/22] Add int and int array parameters (#208)

* Add int and int array parameter

* Update protocol with int and int array parameter

* protocol needs int32

* clproto tests passed

* Add int and int param to python bindings

* Fix enum

* Add test coverage

* Fix tests

* Update CHANGELOG
---
 CHANGELOG.md                                  |  1 +
 protocol/clproto_cpp/include/clproto.h        |  2 +
 protocol/clproto_cpp/src/clproto.cpp          | 44 +++++++++
 protocol/clproto_cpp/src/decoders.cpp         |  9 ++
 protocol/clproto_cpp/src/encoders.cpp         | 16 +++
 .../test/tests/test_parameter_proto.cpp       | 47 +++++++++
 .../parameters/parameter.proto                |  2 +
 .../parameters/parameter_value.proto          |  8 ++
 .../proto/state_representation/state.proto    |  2 +
 python/source/clproto/bind_clproto.cpp        |  2 +
 .../state_representation/bind_state.cpp       |  2 +
 .../include/state_representation/State.hpp    |  9 +-
 .../src/parameters/Parameter.cpp              | 99 +++++++++++++------
 .../test/tests/test_parameter.cpp             | 21 +++-
 14 files changed, 225 insertions(+), 39 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 6d701a8f6..05bb28520 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,7 @@ Release Versions:
 - Python bindings for clproto (#204)
 - JSON conversion support for clproto (#205)
 - Support empty state objects in clproto (#207)
+- Add int and int array parameters (#208)
 
 ## 4.0.0
 
diff --git a/protocol/clproto_cpp/include/clproto.h b/protocol/clproto_cpp/include/clproto.h
index a8394ef03..e6d2371b8 100644
--- a/protocol/clproto_cpp/include/clproto.h
+++ b/protocol/clproto_cpp/include/clproto.h
@@ -81,6 +81,8 @@ enum MessageType {
  */
 enum ParameterMessageType {
   UNKNOWN_PARAMETER = 0,
+  INT = 9,
+  INT_ARRAY = 10,
   DOUBLE = 1,
   DOUBLE_ARRAY = 2,
   BOOL = 3,
diff --git a/protocol/clproto_cpp/src/clproto.cpp b/protocol/clproto_cpp/src/clproto.cpp
index 8999b5046..0bdb10a97 100644
--- a/protocol/clproto_cpp/src/clproto.cpp
+++ b/protocol/clproto_cpp/src/clproto.cpp
@@ -773,6 +773,50 @@ static bool decode_parameter(const std::string& msg, Parameter<T>& obj) {
   }
 }
 
+/* ----------------------
+ *          INT
+ * ---------------------- */
+template<>
+std::string encode<Parameter<int>>(const Parameter<int>& obj);
+template<>
+Parameter<int> decode(const std::string& msg);
+template<>
+bool decode(const std::string& msg, Parameter<int>& obj);
+template<>
+std::string encode<Parameter<int>>(const Parameter<int>& obj) {
+return encode_parameter(obj);
+}
+template<>
+Parameter<int> decode(const std::string& msg) {
+  return decode_parameter<int>(msg);
+}
+template<>
+bool decode(const std::string& msg, Parameter<int>& obj) {
+  return decode_parameter(msg, obj);
+}
+
+/* ----------------------
+ *       INT_ARRAY
+ * ---------------------- */
+template<>
+std::string encode<Parameter<std::vector<int>>>(const Parameter<std::vector<int>>& obj);
+template<>
+Parameter<std::vector<int>> decode(const std::string& msg);
+template<>
+bool decode(const std::string& msg, Parameter<std::vector<int>>& obj);
+template<>
+std::string encode<Parameter<std::vector<int>>>(const Parameter<std::vector<int>>& obj) {
+  return encode_parameter(obj);
+}
+template<>
+Parameter<std::vector<int>> decode(const std::string& msg) {
+  return decode_parameter<std::vector<int>>(msg);
+}
+template<>
+bool decode(const std::string& msg, Parameter<std::vector<int>>& obj) {
+  return decode_parameter(msg, obj);
+}
+
 /* ----------------------
  *        DOUBLE
  * ---------------------- */
diff --git a/protocol/clproto_cpp/src/decoders.cpp b/protocol/clproto_cpp/src/decoders.cpp
index 15d98e1a3..d5e235e60 100644
--- a/protocol/clproto_cpp/src/decoders.cpp
+++ b/protocol/clproto_cpp/src/decoders.cpp
@@ -22,6 +22,15 @@ Eigen::Quaterniond decoder(const proto::Quaterniond& message) {
   return {message.w(), message.vec().x(), message.vec().y(), message.vec().z()};
 }
 
+template<>
+Parameter<int> decoder(const state_representation::proto::Parameter& message) {
+  return Parameter<int>(message.state().name(), message.parameter_value().int_().value());
+}
+template<>
+Parameter<std::vector<int>> decoder(const state_representation::proto::Parameter& message) {
+  return Parameter<std::vector<int>>(
+      message.state().name(), decoder(message.parameter_value().int_array().value()));
+}
 template<>
 Parameter<double> decoder(const state_representation::proto::Parameter& message) {
   return Parameter<double>(message.state().name(), message.parameter_value().double_().value());
diff --git a/protocol/clproto_cpp/src/encoders.cpp b/protocol/clproto_cpp/src/encoders.cpp
index 6f6014ce6..aee63e145 100644
--- a/protocol/clproto_cpp/src/encoders.cpp
+++ b/protocol/clproto_cpp/src/encoders.cpp
@@ -89,6 +89,22 @@ proto::JointState encoder(const JointState& joint_state) {
   return message;
 }
 
+template<>
+state_representation::proto::Parameter encoder(const state_representation::Parameter<int>& parameter) {
+  state_representation::proto::Parameter message;
+  *message.mutable_state() = encoder(static_cast<state_representation::State>(parameter));
+  message.mutable_parameter_value()->mutable_int_()->set_value(parameter.get_value());
+  return message;
+}
+
+template<>
+state_representation::proto::Parameter encoder(const state_representation::Parameter<std::vector<int>>& parameter) {
+  state_representation::proto::Parameter message;
+  *message.mutable_state() = encoder(static_cast<state_representation::State>(parameter));
+  *message.mutable_parameter_value()->mutable_int_array()->mutable_value() =
+      {parameter.get_value().begin(), parameter.get_value().end()};
+  return message;
+}
 template<>
 state_representation::proto::Parameter encoder(const state_representation::Parameter<double>& parameter) {
   state_representation::proto::Parameter message;
diff --git a/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp b/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp
index 54632d025..0d3ce1a14 100644
--- a/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp
+++ b/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp
@@ -6,6 +6,45 @@
 
 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;
 
@@ -21,6 +60,7 @@ TEST(ParameterProtoTest, EncodeDecodeParameterDouble) {
 
   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) {
@@ -41,6 +81,7 @@ TEST(ParameterProtoTest, EncodeDecodeParameterDoubleArray) {
   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) {
@@ -58,6 +99,7 @@ TEST(ParameterProtoTest, EncodeDecodeParameterBool) {
 
   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) {
@@ -78,6 +120,7 @@ TEST(ParameterProtoTest, EncodeDecodeParameterBoolArray) {
   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) {
@@ -95,6 +138,7 @@ TEST(ParameterProtoTest, EncodeDecodeParameterString) {
 
   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());
 }
 
 TEST(ParameterProtoTest, EncodeDecodeParameterStringArray) {
@@ -115,6 +159,7 @@ TEST(ParameterProtoTest, EncodeDecodeParameterStringArray) {
   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());
 }
 
 TEST(ParameterProtoTest, EncodeDecodeParameterVector) {
@@ -136,6 +181,7 @@ TEST(ParameterProtoTest, EncodeDecodeParameterVector) {
   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, EncodeDecodeParameterMatrix) {
@@ -159,6 +205,7 @@ TEST(ParameterProtoTest, EncodeDecodeParameterMatrix) {
   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, EncodeDecodeEmptyParameterDouble) {
diff --git a/protocol/protobuf/proto/state_representation/parameters/parameter.proto b/protocol/protobuf/proto/state_representation/parameters/parameter.proto
index c5b3c1126..afb95c268 100644
--- a/protocol/protobuf/proto/state_representation/parameters/parameter.proto
+++ b/protocol/protobuf/proto/state_representation/parameters/parameter.proto
@@ -12,6 +12,8 @@ message Parameter {
 
 message ParameterValue {
   oneof value_type {
+    parameter.Int int = 9;
+    parameter.IntArray int_array = 10;
     parameter.Double double = 1;
     parameter.DoubleArray double_array = 2;
     parameter.Bool bool = 3;
diff --git a/protocol/protobuf/proto/state_representation/parameters/parameter_value.proto b/protocol/protobuf/proto/state_representation/parameters/parameter_value.proto
index 77dbc37dd..060f26089 100644
--- a/protocol/protobuf/proto/state_representation/parameters/parameter_value.proto
+++ b/protocol/protobuf/proto/state_representation/parameters/parameter_value.proto
@@ -2,6 +2,14 @@ syntax = "proto3";
 
 package state_representation.proto.parameter;
 
+message Int {
+  sint32 value = 1;
+}
+
+message IntArray {
+  repeated sint32 value = 1;
+}
+
 message Double {
   double value = 1;
 }
diff --git a/protocol/protobuf/proto/state_representation/state.proto b/protocol/protobuf/proto/state_representation/state.proto
index f0f74d255..a726adbd6 100644
--- a/protocol/protobuf/proto/state_representation/state.proto
+++ b/protocol/protobuf/proto/state_representation/state.proto
@@ -26,6 +26,8 @@ enum StateType {
   PARAMETER_ELLIPSOID = 18;
   PARAMETER_MATRIX = 19;
   PARAMETER_VECTOR = 20;
+  PARAMETER_INT = 21;
+  PARAMETER_INT_ARRAY = 22;
 };
 
 message State {
diff --git a/python/source/clproto/bind_clproto.cpp b/python/source/clproto/bind_clproto.cpp
index 81747d16a..bc2089afe 100644
--- a/python/source/clproto/bind_clproto.cpp
+++ b/python/source/clproto/bind_clproto.cpp
@@ -50,6 +50,8 @@ void message_type(py::module_& m) {
 void parameter_message_type(py::module_& m) {
   py::enum_<ParameterMessageType>(m, "ParameterMessageType")
       .value("UNKNOWN_PARAMETER", ParameterMessageType::UNKNOWN_PARAMETER)
+      .value("INT", ParameterMessageType::INT)
+      .value("INT_ARRAY", ParameterMessageType::INT_ARRAY)
       .value("DOUBLE", ParameterMessageType::DOUBLE)
       .value("DOUBLE_ARRAY", ParameterMessageType::DOUBLE_ARRAY)
       .value("BOOL", ParameterMessageType::BOOL)
diff --git a/python/source/state_representation/bind_state.cpp b/python/source/state_representation/bind_state.cpp
index 64a6fa438..126f1e4d5 100644
--- a/python/source/state_representation/bind_state.cpp
+++ b/python/source/state_representation/bind_state.cpp
@@ -12,6 +12,8 @@ void state_type(py::module_& m) {
       .value("JACOBIANMATRIX", StateType::JACOBIANMATRIX)
       .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)
diff --git a/source/state_representation/include/state_representation/State.hpp b/source/state_representation/include/state_representation/State.hpp
index be3e40fbf..20a5d3a80 100644
--- a/source/state_representation/include/state_representation/State.hpp
+++ b/source/state_representation/include/state_representation/State.hpp
@@ -34,7 +34,9 @@ enum class StateType {
   PARAMETER_JOINTPOSITIONS,
   PARAMETER_ELLIPSOID,
   PARAMETER_MATRIX,
-  PARAMETER_VECTOR
+  PARAMETER_VECTOR,
+  PARAMETER_INT,
+  PARAMETER_INT_ARRAY
 };
 
 /**
@@ -133,7 +135,7 @@ class State {
    * @brief Check if the state is deprecated given a certain time delay
    * @param time_delay the time after which to consider the state as deprecated
    */
-  template <typename DurationT>
+  template<typename DurationT>
   bool is_deprecated(const std::chrono::duration<int64_t, DurationT>& time_delay);
 
   /**
@@ -165,7 +167,6 @@ class State {
    */
   virtual void set_data(const Eigen::MatrixXd& data);
 
-
   /**
    * @brief Overload the ostream operator for printing
    * @param os the ostream to append the string representing the State to
@@ -213,7 +214,7 @@ inline void State::reset_timestamp() {
   this->timestamp_ = std::chrono::steady_clock::now();
 }
 
-template <typename DurationT>
+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);
 }
diff --git a/source/state_representation/src/parameters/Parameter.cpp b/source/state_representation/src/parameters/Parameter.cpp
index 1940df710..0b2577e99 100644
--- a/source/state_representation/src/parameters/Parameter.cpp
+++ b/source/state_representation/src/parameters/Parameter.cpp
@@ -4,133 +4,153 @@
 #include "state_representation/space/cartesian/CartesianPose.hpp"
 
 namespace state_representation {
-template <>
+template<>
+Parameter<int>::Parameter(const std::string& name) :
+    ParameterInterface(StateType::PARAMETER_INT, name) {}
+
+template<>
+Parameter<int>::Parameter(const std::string& name, const int& value) :
+    ParameterInterface(StateType::PARAMETER_INT, name), value(value) {
+  this->set_filled();
+}
+
+template<>
+Parameter<std::vector<int>>::Parameter(const std::string& name) :
+    ParameterInterface(StateType::PARAMETER_INT_ARRAY, name) {}
+
+template<>
+Parameter<std::vector<int>>::Parameter(const std::string& name, const std::vector<int>& value) :
+    ParameterInterface(StateType::PARAMETER_INT_ARRAY, name), value(value) {
+  this->set_filled();
+}
+
+template<>
 Parameter<double>::Parameter(const std::string& name) :
     ParameterInterface(StateType::PARAMETER_DOUBLE, name) {}
 
-template <>
+template<>
 Parameter<double>::Parameter(const std::string& name, const double& value) :
     ParameterInterface(StateType::PARAMETER_DOUBLE, name), value(value) {
   this->set_filled();
 }
 
-template <>
+template<>
 Parameter<std::vector<double>>::Parameter(const std::string& name) :
     ParameterInterface(StateType::PARAMETER_DOUBLE_ARRAY, name) {}
 
-template <>
+template<>
 Parameter<std::vector<double>>::Parameter(const std::string& name, const std::vector<double>& value) :
     ParameterInterface(StateType::PARAMETER_DOUBLE_ARRAY, name), value(value) {
   this->set_filled();
 }
 
-template <>
+template<>
 Parameter<bool>::Parameter(const std::string& name) : ParameterInterface(StateType::PARAMETER_BOOL, name) {}
 
-template <>
+template<>
 Parameter<bool>::Parameter(const std::string& name, const bool& value) :
     ParameterInterface(StateType::PARAMETER_BOOL, name), value(value) {
   this->set_filled();
 }
 
-template <>
+template<>
 Parameter<std::vector<bool>>::Parameter(const std::string& name) :
     ParameterInterface(StateType::PARAMETER_BOOL_ARRAY, name) {
 }
 
-template <>
+template<>
 Parameter<std::vector<bool>>::Parameter(const std::string& name, const std::vector<bool>& value) :
     ParameterInterface(StateType::PARAMETER_BOOL_ARRAY, name), value(value) {
   this->set_filled();
 }
 
-template <>
+template<>
 Parameter<std::string>::Parameter(const std::string& name) :
     ParameterInterface(StateType::PARAMETER_STRING, name) {}
 
-template <>
+template<>
 Parameter<std::string>::Parameter(const std::string& name, const std::string& value) :
     ParameterInterface(StateType::PARAMETER_STRING, name), value(value) {
   this->set_filled();
 }
 
-template <>
+template<>
 Parameter<std::vector<std::string>>::Parameter(const std::string& name) :
     ParameterInterface(StateType::PARAMETER_STRING_ARRAY, name) {}
 
-template <>
+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) {
   this->set_filled();
 }
 
-template <>
+template<>
 Parameter<CartesianState>::Parameter(const std::string& name) :
     ParameterInterface(StateType::PARAMETER_CARTESIANSTATE, name) {}
 
-template <>
+template<>
 Parameter<CartesianState>::Parameter(const std::string& name, const CartesianState& value) :
     ParameterInterface(StateType::PARAMETER_CARTESIANSTATE, name), value(value) {
   this->set_filled();
 }
 
-template <>
+template<>
 Parameter<CartesianPose>::Parameter(const std::string& name) :
     ParameterInterface(StateType::PARAMETER_CARTESIANPOSE, name) {}
 
-template <>
+template<>
 Parameter<CartesianPose>::Parameter(const std::string& name, const CartesianPose& value) :
     ParameterInterface(StateType::PARAMETER_CARTESIANPOSE, name), value(value) {
   this->set_filled();
 }
 
-template <>
+template<>
 Parameter<JointState>::Parameter(const std::string& name) :
     ParameterInterface(StateType::PARAMETER_JOINTSTATE, name) {}
 
-template <>
+template<>
 Parameter<JointState>::Parameter(const std::string& name, const JointState& value) :
     ParameterInterface(StateType::PARAMETER_JOINTSTATE, name), value(value) {
   this->set_filled();
 }
 
-template <>
+template<>
 Parameter<JointPositions>::Parameter(const std::string& name) :
     ParameterInterface(StateType::PARAMETER_JOINTPOSITIONS, name) {}
 
-template <>
+template<>
 Parameter<JointPositions>::Parameter(const std::string& name, const JointPositions& value) :
     ParameterInterface(StateType::PARAMETER_JOINTPOSITIONS, name), value(value) {
   this->set_filled();
 }
 
-template <>
+template<>
 Parameter<Ellipsoid>::Parameter(const std::string& name, const Ellipsoid& value) :
     ParameterInterface(StateType::PARAMETER_ELLIPSOID, name), value(value) {
   this->set_filled();
 }
 
-template <>
+template<>
 Parameter<Eigen::MatrixXd>::Parameter(const std::string& name) :
     ParameterInterface(StateType::PARAMETER_MATRIX, name) {}
 
-template <>
+template<>
 Parameter<Eigen::MatrixXd>::Parameter(const std::string& name, const Eigen::MatrixXd& value) :
     ParameterInterface(StateType::PARAMETER_MATRIX, name), value(value) {
   this->set_filled();
 }
 
-template <>
+template<>
 Parameter<Eigen::VectorXd>::Parameter(const std::string& name) :
     ParameterInterface(StateType::PARAMETER_VECTOR, name) {}
 
-template <>
+template<>
 Parameter<Eigen::VectorXd>::Parameter(const std::string& name, const Eigen::VectorXd& value) :
     ParameterInterface(StateType::PARAMETER_VECTOR, name), value(value) {
   this->set_filled();
 }
 
-template <typename T>
+template<typename T>
 std::ostream& operator<<(std::ostream& os, const Parameter<T>& parameter) {
   if (parameter.is_empty()) {
     os << " Parameter " << parameter.get_name() << " is empty" << std::endl;
@@ -140,6 +160,7 @@ std::ostream& operator<<(std::ostream& os, const Parameter<T>& parameter) {
   return os;
 }
 
+template std::ostream& operator<<(std::ostream& os, const Parameter<int>& parameter);
 template std::ostream& operator<<(std::ostream& os, const Parameter<double>& parameter);
 template std::ostream& operator<<(std::ostream& os, const Parameter<bool>& parameter);
 template std::ostream& operator<<(std::ostream& os, const Parameter<std::string>& parameter);
@@ -151,13 +172,27 @@ template std::ostream& operator<<(std::ostream& os, const Parameter<Ellipsoid>&
 template std::ostream& operator<<(std::ostream& os, const Parameter<Eigen::MatrixXd>& parameter);
 template std::ostream& operator<<(std::ostream& os, const Parameter<Eigen::VectorXd>& parameter);
 
-template <>
+template<>
+std::ostream& operator<<(std::ostream& os, const Parameter<std::vector<int>>& parameter) {
+  if (parameter.is_empty()) {
+    os << " Parameter " << parameter.get_name() << " is empty" << std::endl;
+  } else {
+    os << " Parameter " << parameter.get_name() << ": ";
+    for (auto& v: parameter.value) {
+      os << v << " | ";
+    }
+    os << std::endl;
+  }
+  return os;
+}
+
+template<>
 std::ostream& operator<<(std::ostream& os, const Parameter<std::vector<double>>& parameter) {
   if (parameter.is_empty()) {
     os << " Parameter " << parameter.get_name() << " is empty" << std::endl;
   } else {
     os << " Parameter " << parameter.get_name() << ": ";
-    for (auto& v : parameter.value) {
+    for (auto& v: parameter.value) {
       os << v << " | ";
     }
     os << std::endl;
@@ -165,13 +200,13 @@ std::ostream& operator<<(std::ostream& os, const Parameter<std::vector<double>>&
   return os;
 }
 
-template <>
+template<>
 std::ostream& operator<<(std::ostream& os, const Parameter<std::vector<bool>>& parameter) {
   if (parameter.is_empty()) {
     os << " Parameter " << parameter.get_name() << " is empty" << std::endl;
   } else {
     os << " Parameter " << parameter.get_name() << ": ";
-    for (auto v : parameter.value) {
+    for (auto v: parameter.value) {
       os << v << " | ";
     }
     os << std::endl;
@@ -179,13 +214,13 @@ std::ostream& operator<<(std::ostream& os, const Parameter<std::vector<bool>>& p
   return os;
 }
 
-template <>
+template<>
 std::ostream& operator<<(std::ostream& os, const Parameter<std::vector<std::string>>& parameter) {
   if (parameter.is_empty()) {
     os << " Parameter " << parameter.get_name() << " is empty" << std::endl;
   } else {
     os << " Parameter " << parameter.get_name() << ": ";
-    for (auto& v : parameter.value) {
+    for (auto& v: parameter.value) {
       os << v << " | ";
     }
     os << std::endl;
diff --git a/source/state_representation/test/tests/test_parameter.cpp b/source/state_representation/test/tests/test_parameter.cpp
index e8c54b406..0a45629ec 100644
--- a/source/state_representation/test/tests/test_parameter.cpp
+++ b/source/state_representation/test/tests/test_parameter.cpp
@@ -4,18 +4,33 @@
 #include "state_representation/space/cartesian/CartesianState.hpp"
 #include <gtest/gtest.h>
 
+using namespace state_representation;
+
 TEST(ParameterTest, Conversion) {
-  using namespace state_representation;
+  Parameter<int> int_param("test");
+  EXPECT_TRUE(int_param.is_empty());
+  EXPECT_EQ(int_param.get_type(), StateType::PARAMETER_INT);
+  int_param.set_value(2);
+  EXPECT_EQ(int_param.get_value(), 2);
+
+  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);
+  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"));
   Parameter<CartesianState> test2(test1);
   EXPECT_EQ(test2.get_type(), StateType::PARAMETER_CARTESIANPOSE);
 
-  std::shared_ptr<Parameter<CartesianState>> test3 = std::make_shared<Parameter<CartesianState>>(Parameter<CartesianPose>("test", CartesianPose::Random("test")));
+  std::shared_ptr<Parameter<CartesianState>> test3 =
+      std::make_shared<Parameter<CartesianState>>(Parameter<CartesianPose>("test", CartesianPose::Random("test")));
   EXPECT_EQ(test3->get_type(), StateType::PARAMETER_CARTESIANPOSE);
 }
 
 TEST(ParameterTest, Event) {
-  using namespace state_representation;
   Event e("test");
   EXPECT_FALSE(e.get_value());
   EXPECT_FALSE(e.get_previous_value());

From 37fa1b1f582761f5305cd093fd88992083c133c4 Mon Sep 17 00:00:00 2001
From: domire8 <71256590+domire8@users.noreply.github.com>
Date: Thu, 16 Sep 2021 16:07:56 +0200
Subject: [PATCH 08/22] Get joint state by name or index of joint (#210)

* Add bare inline functions without doc

* Delete methods from derived classes

* Rename function and move to source file

* Improve test coverage

* Improve docstrings

* Add python bindings

* Reduce code duplication

* Improve documentation
---
 CHANGELOG.md                                  |   2 +
 .../state_representation/bind_joint_space.cpp |  93 +++--
 python/test/test_joint_state.py               |   5 +
 .../exceptions/JointNotFoundException.hpp     |  16 +
 .../robot/JointAccelerations.hpp              |   6 +
 .../robot/JointPositions.hpp                  |   6 +
 .../state_representation/robot/JointState.hpp | 372 +++++++++++-------
 .../robot/JointTorques.hpp                    |   6 +
 .../robot/JointVelocities.hpp                 |   6 +
 .../src/robot/JointState.cpp                  |  54 ++-
 .../test/tests/robot/test_joint_state.cpp     |  30 ++
 11 files changed, 418 insertions(+), 178 deletions(-)
 create mode 100644 source/state_representation/include/state_representation/exceptions/JointNotFoundException.hpp

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 05bb28520..dabaa0e11 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -13,6 +13,8 @@ Release Versions:
 - JSON conversion support for clproto (#205)
 - Support empty state objects in clproto (#207)
 - Add int and int array parameters (#208)
+- Add method to get a joint state by name or index of the joint,
+  and to get the index of the joint by its name (#210)
 
 ## 4.0.0
 
diff --git a/python/source/state_representation/bind_joint_space.cpp b/python/source/state_representation/bind_joint_space.cpp
index 9435cb903..314085353 100644
--- a/python/source/state_representation/bind_joint_space.cpp
+++ b/python/source/state_representation/bind_joint_space.cpp
@@ -17,45 +17,54 @@ void joint_state_variable(py::module_& m) {
 }
 
 void joint_state(py::module_& m) {
-  m.def("dist", py::overload_cast<const JointState&, const JointState&, const JointStateVariable&>(&state_representation::dist), "Compute the distance between two JointStates", "s1"_a, "s2"_a, "state_variable_type"_a=JointStateVariable::ALL);
+  m.def("dist", py::overload_cast<const JointState&, const JointState&, const JointStateVariable&>(&state_representation::dist), "Compute the distance between two JointState.", "s1"_a, "s2"_a, "state_variable_type"_a=JointStateVariable::ALL);
 
   py::class_<JointState, State> c(m, "JointState");
-  c.def(py::init(), "Empty constructor");
-  c.def(py::init<const std::string&, unsigned int>(), "Constructor with name and number of joints provided", "robot_name"_a, "nb_joints"_a=0);
-  c.def(py::init<const std::string&, const std::vector<std::string>&>(), "Constructor with name and list of joint names provided", "robot_name"_a, "joint_names"_a);
-  c.def(py::init<const JointState&>(), "Copy constructor of a JointState", "state"_a);
-
-  c.def_static("Zero", py::overload_cast<const std::string&, unsigned int>(&JointState::Zero), "Constructor for the zero JointState", "robot_name"_a, "nb_joints"_a);
-  c.def_static("Zero", py::overload_cast<const std::string&, const std::vector<std::string>&>(&JointState::Zero), "Constructor for the zero JointState", "robot_name"_a, "joint_names"_a);
-  c.def_static("Random", py::overload_cast<const std::string&, unsigned int>(&JointState::Random), "Constructor for the random JointState", "robot_name"_a, "nb_joints"_a);
-  c.def_static("Random", py::overload_cast<const std::string&, const std::vector<std::string>&>(&JointState::Random), "Constructor for the random JointState", "robot_name"_a, "joint_names"_a);
-
-  c.def("get_size", &JointState::get_size, "Getter of the size from the attributes");
-  c.def("get_names", &JointState::get_names, "Getter of the names attribute");
-  c.def("get_positions", &JointState::get_positions, "Getter of the positions attribute");
+  c.def(py::init(), "Empty constructor for a JointState.");
+  c.def(py::init<const std::string&, unsigned int>(), "Constructor with name and number of joints provided.", "robot_name"_a, "nb_joints"_a=0);
+  c.def(py::init<const std::string&, const std::vector<std::string>&>(), "Constructor with name and list of joint names provided.", "robot_name"_a, "joint_names"_a);
+  c.def(py::init<const JointState&>(), "Copy constructor of a JointState.", "state"_a);
+
+  c.def_static("Zero", py::overload_cast<const std::string&, unsigned int>(&JointState::Zero), "Constructor for a zero JointState.", "robot_name"_a, "nb_joints"_a);
+  c.def_static("Zero", py::overload_cast<const std::string&, const std::vector<std::string>&>(&JointState::Zero), "Constructor for a zero JointState.", "robot_name"_a, "joint_names"_a);
+  c.def_static("Random", py::overload_cast<const std::string&, unsigned int>(&JointState::Random), "Constructor for a random JointState.", "robot_name"_a, "nb_joints"_a);
+  c.def_static("Random", py::overload_cast<const std::string&, const std::vector<std::string>&>(&JointState::Random), "Constructor for a random JointState.", "robot_name"_a, "joint_names"_a);
+
+  c.def("get_size", &JointState::get_size, "Getter of the size from the attributes.");
+  c.def("get_names", &JointState::get_names, "Getter of the names attribute.");
+  c.def("get_joint_index", &JointState::get_joint_index, "Get joint index by the name of the joint, if it exists.", "joint_name"_a);
+  c.def("get_positions", &JointState::get_positions, "Getter of the positions attribute.");
+  c.def("get_position", [](const JointState& joint_state, const std::string& joint_name) { return joint_state.get_position(joint_name); }, "Get the position of a joint by its name, if it exists.", "joint_name"_a);
+  c.def("get_position", [](const JointState& joint_state, unsigned int joint_index) { return joint_state.get_position(joint_index); }, "Get the position of a joint by its name, if it exists.", "joint_index"_a);
   c.def("get_velocities", &JointState::get_velocities, "Getter of the velocities attribute");
+  c.def("get_velocity", [](const JointState& joint_state, const std::string& joint_name) { return joint_state.get_velocity(joint_name); }, "Get the velocity of a joint by its name, if it exists.", "joint_name"_a);
+  c.def("get_velocity", [](const JointState& joint_state, unsigned int joint_index) { return joint_state.get_velocity(joint_index); }, "Get the velocity of a joint by its name, if it exists.", "joint_index"_a);
   c.def("get_accelerations", &JointState::get_accelerations, "Getter of the accelerations attribute");
+  c.def("get_acceleration", [](const JointState& joint_state, const std::string& joint_name) { return joint_state.get_acceleration(joint_name); }, "Get the acceleration of a joint by its name, if it exists.", "joint_name"_a);
+  c.def("get_acceleration", [](const JointState& joint_state, unsigned int joint_index) { return joint_state.get_acceleration(joint_index); }, "Get the acceleration of a joint by its name, if it exists.", "joint_index"_a);
   c.def("get_torques", &JointState::get_torques, "Getter of the torques attribute");
-
-  c.def("set_names", py::overload_cast<unsigned int>(&JointState::set_names), "Setter of the names attribute from the number of joints");
-  c.def("set_names", py::overload_cast<const std::vector<std::string>&>(&JointState::set_names), "Setter of the names attribute");
-  c.def("set_positions", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_positions), "Setter of the positions attribute from a vector");
-  c.def("set_positions", py::overload_cast<const std::vector<double>&>(&JointState::set_positions), "Setter of the positions attribute from a list");
-  c.def("set_velocities", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_velocities), "Setter of the velocities attribute from a vector");
-  c.def("set_velocities", py::overload_cast<const std::vector<double>&>(&JointState::set_velocities), "Setter of the velocities attribute from a list");
-  c.def("set_accelerations", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_accelerations), "Setter of the accelerations attribute from a vector");
-  c.def("set_accelerations", py::overload_cast<const std::vector<double>&>(&JointState::set_accelerations), "Setter of the accelerations attribute from a list");
-  c.def("set_torques", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_torques), "Setter of the torques attribute from a vector");
-  c.def("set_torques", py::overload_cast<const std::vector<double>&>(&JointState::set_torques), "Setter of the torques attribute from a list");
-
-  c.def("set_zero", &JointState::set_zero, "Set the JointState to a zero value");
-  c.def("clamp_state_variable", py::overload_cast<double, const JointStateVariable&, double>(&JointState::clamp_state_variable), "Clamp inplace the magnitude of the a specific state variable (velocities, accelerations or forces)", "value"_a, "state_variable_type"_a, "noise_ratio"_a=double(0));
-  c.def("clamp_state_variable", py::overload_cast<const Eigen::ArrayXd&, const JointStateVariable&, const Eigen::ArrayXd&>(&JointState::clamp_state_variable), "Clamp inplace the magnitude of the a specific state variable (velocities, accelerations or forces)", "max_absolute_value_array"_a, "state_variable_type"_a, "noise_ratio_array"_a);
-  c.def("copy", &JointState::copy, "Return a copy of the JointState");
-  c.def("data", &JointState::data, "Returns the data as the concatenation of all the state variables in a single vector");
-  c.def("array", &JointState::array, "Returns the data vector as an array");
-  c.def("set_data", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_data), "Set the data of the state from all the state variables in a single vector", "data"_a);
-  c.def("set_data", py::overload_cast<const std::vector<double>&>(&JointState::set_data), "Set the data of the state from all the state variables in a single list", "data"_a);
+  c.def("get_torque", [](const JointState& joint_state, const std::string& joint_name) { return joint_state.get_torque(joint_name); }, "Get the torque of a joint by its name, if it exists.", "joint_name"_a);
+  c.def("get_torque", [](const JointState& joint_state, unsigned int joint_index) { return joint_state.get_torque(joint_index); }, "Get the torque of a joint by its name, if it exists.", "joint_index"_a);
+
+  c.def("set_names", py::overload_cast<unsigned int>(&JointState::set_names), "Setter of the names attribute from the number of joints.", "nb_joints"_a);
+  c.def("set_names", py::overload_cast<const std::vector<std::string>&>(&JointState::set_names), "Setter of the names attribute.", "names"_a);
+  c.def("set_positions", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_positions), "Setter of the positions attribute from a vector.", "positions"_a);
+  c.def("set_positions", py::overload_cast<const std::vector<double>&>(&JointState::set_positions), "Setter of the positions attribute from a list.", "positions"_a);
+  c.def("set_velocities", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_velocities), "Setter of the velocities attribute from a vector.", "velocities"_a);
+  c.def("set_velocities", py::overload_cast<const std::vector<double>&>(&JointState::set_velocities), "Setter of the velocities attribute from a list.", "velocities"_a);
+  c.def("set_accelerations", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_accelerations), "Setter of the accelerations attribute from a vector.", "accelerations"_a);
+  c.def("set_accelerations", py::overload_cast<const std::vector<double>&>(&JointState::set_accelerations), "Setter of the accelerations attribute from a list.", "accelerations"_a);
+  c.def("set_torques", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_torques), "Setter of the torques attribute from a vector.", "torques"_a);
+  c.def("set_torques", py::overload_cast<const std::vector<double>&>(&JointState::set_torques), "Setter of the torques attribute from a list.", "torques"_a);
+
+  c.def("set_zero", &JointState::set_zero, "Set the JointState to a zero value.");
+  c.def("clamp_state_variable", py::overload_cast<double, const JointStateVariable&, double>(&JointState::clamp_state_variable), "Clamp inplace the magnitude of the a specific state variable.", "value"_a, "state_variable_type"_a, "noise_ratio"_a=double(0));
+  c.def("clamp_state_variable", py::overload_cast<const Eigen::ArrayXd&, const JointStateVariable&, const Eigen::ArrayXd&>(&JointState::clamp_state_variable), "Clamp inplace the magnitude of the a specific state variable.", "max_absolute_value_array"_a, "state_variable_type"_a, "noise_ratio_array"_a);
+  c.def("copy", &JointState::copy, "Return a copy of the JointState.");
+  c.def("data", &JointState::data, "Returns the data as the concatenation of all the state variables in a single vector.");
+  c.def("array", &JointState::array, "Returns the data vector as an array.");
+  c.def("set_data", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_data), "Set the data of the state from all the state variables in a single vector.", "data"_a);
+  c.def("set_data", py::overload_cast<const std::vector<double>&>(&JointState::set_data), "Set the data of the state from all the state variables in a single list.", "data"_a);
 
   c.def(py::self += py::self);
   c.def(py::self + py::self);
@@ -73,9 +82,9 @@ void joint_state(py::module_& m) {
   c.def(Eigen::ArrayXd() * py::self);
   c.def(Eigen::MatrixXd() * py::self);
 
-  c.def("dist", &JointState::dist, "Compute the distance to another state as the sum of distances between each features", "state"_a, "state_variable_type"_a=JointStateVariable::ALL);
+  c.def("dist", &JointState::dist, "Compute the distance to another state as the sum of distances between each attribute.", "state"_a, "state_variable_type"_a=JointStateVariable::ALL);
 
-  c.def("to_list", &JointState::to_std_vector, "Return the state as a list");
+  c.def("to_list", &JointState::to_std_vector, "Return the state as a list.");
 
   c.def("__repr__", [](const JointState& state) {
     std::stringstream buffer;
@@ -103,8 +112,11 @@ void joint_positions(py::module_& m) {
 
   std::vector<std::string> deleted_attributes = {
       "velocities",
+      "velocity",
       "accelerations",
+      "acceleration",
       "torques",
+      "torque",
   };
 
   for (const std::string& attr : deleted_attributes) {
@@ -161,8 +173,11 @@ void joint_velocities(py::module_& m) {
 
   std::vector<std::string> deleted_attributes = {
       "positions",
+      "position",
       "accelerations",
+      "acceleration",
       "torques",
+      "torque",
   };
 
   for (const std::string& attr : deleted_attributes) {
@@ -224,8 +239,11 @@ void joint_accelerations(py::module_& m) {
 
   std::vector<std::string> deleted_attributes = {
       "positions",
+      "position",
       "velocities",
+      "velocity",
       "torques",
+      "torque",
   };
 
   for (const std::string& attr : deleted_attributes) {
@@ -285,8 +303,11 @@ void joint_torques(py::module_& m) {
 
   std::vector<std::string> deleted_attributes = {
       "positions",
+      "position",
       "velocities",
+      "velocity",
       "accelerations",
+      "acceleration",
   };
 
   for (const std::string& attr : deleted_attributes) {
diff --git a/python/test/test_joint_state.py b/python/test/test_joint_state.py
index 1bd8ec47f..927da933e 100755
--- a/python/test/test_joint_state.py
+++ b/python/test/test_joint_state.py
@@ -11,14 +11,19 @@
     'set_data',
     'dist',
     'get_accelerations',
+    'get_acceleration',
+    'get_joint_index',
     'get_name',
     'get_names',
     'get_positions',
+    'get_position',
     'get_size',
     'get_timestamp',
     'get_torques',
+    'get_torque',
     'get_type',
     'get_velocities',
+    'get_velocity',
     'initialize',
     'is_compatible',
     'is_deprecated',
diff --git a/source/state_representation/include/state_representation/exceptions/JointNotFoundException.hpp b/source/state_representation/include/state_representation/exceptions/JointNotFoundException.hpp
new file mode 100644
index 000000000..dada45b43
--- /dev/null
+++ b/source/state_representation/include/state_representation/exceptions/JointNotFoundException.hpp
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <exception>
+#include <iostream>
+
+namespace state_representation::exceptions {
+
+/**
+ * @class JointNotFoundException
+ * @brief Exception that is thrown when a joint name or index is out of range
+ */
+class JointNotFoundException : public std::logic_error {
+public:
+  explicit JointNotFoundException(const std::string& msg) : logic_error(msg) {};
+};
+}// namespace state_representation::exceptions
diff --git a/source/state_representation/include/state_representation/robot/JointAccelerations.hpp b/source/state_representation/include/state_representation/robot/JointAccelerations.hpp
index c376d6194..4688325cb 100644
--- a/source/state_representation/include/state_representation/robot/JointAccelerations.hpp
+++ b/source/state_representation/include/state_representation/robot/JointAccelerations.hpp
@@ -16,12 +16,18 @@ class JointAccelerations : public JointState {
 
 public:
   const Eigen::VectorXd& get_positions() const = delete;
+  double get_position(unsigned int joint_index) const = delete;
+  double get_position(const std::string& joint_name) const = delete;
   void set_positions(const Eigen::VectorXd& positions) = delete;
   void set_positions(const std::vector<double>& positions) = delete;
   const Eigen::VectorXd& get_velocities() const = delete;
+  double get_velocity(unsigned int joint_index) const = delete;
+  double get_velocity(const std::string& joint_name) const = delete;
   void set_velocities(const Eigen::VectorXd& accelerations) = delete;
   void set_velocities(const std::vector<double>& accelerations) = delete;
   const Eigen::VectorXd& get_torques() const = delete;
+  double get_torque(unsigned int joint_index) const = delete;
+  double get_torque(const std::string& joint_name) const = delete;
   void set_torques(const Eigen::VectorXd& torques) = delete;
   void set_torques(const std::vector<double>& torques) = delete;
 
diff --git a/source/state_representation/include/state_representation/robot/JointPositions.hpp b/source/state_representation/include/state_representation/robot/JointPositions.hpp
index c06a695e2..d8c6830c9 100644
--- a/source/state_representation/include/state_representation/robot/JointPositions.hpp
+++ b/source/state_representation/include/state_representation/robot/JointPositions.hpp
@@ -16,12 +16,18 @@ class JointPositions : public JointState {
 
 public:
   const Eigen::VectorXd& get_velocities() const = delete;
+  double get_velocity(unsigned int joint_index) const = delete;
+  double get_velocity(const std::string& joint_name) const = delete;
   void set_velocities(const Eigen::VectorXd& velocities) = delete;
   void set_velocities(const std::vector<double>& velocities) = delete;
   const Eigen::VectorXd& get_accelerations() const = delete;
+  double get_acceleration(unsigned int joint_index) const = delete;
+  double get_acceleration(const std::string& joint_name) const = delete;
   void set_accelerations(const Eigen::VectorXd& accelerations) = delete;
   void set_accelerations(const std::vector<double>& accelerations) = delete;
   const Eigen::VectorXd& get_torques() const = delete;
+  double get_torque(unsigned int joint_index) const = delete;
+  double get_torque(const std::string& joint_name) const = delete;
   void set_torques(const Eigen::VectorXd& torques) = delete;
   void set_torques(const std::vector<double>& torques) = delete;
 
diff --git a/source/state_representation/include/state_representation/robot/JointState.hpp b/source/state_representation/include/state_representation/robot/JointState.hpp
index d18fd7421..5681ced5c 100644
--- a/source/state_representation/include/state_representation/robot/JointState.hpp
+++ b/source/state_representation/include/state_representation/robot/JointState.hpp
@@ -18,12 +18,12 @@ enum class JointStateVariable {
 };
 
 /**
- * @brief Compute the distance between two JointState
- * @param s1 the first JointState
- * @param s2 the second JointState
- * @param state_variable_type name of the field from the JointStateVariable structure to apply
+ * @brief Compute the distance between two JointState.
+ * @param s1 The first JointState
+ * @param s2 The second JointState
+ * @param state_variable_type Name of the field from the JointStateVariable structure to apply
  * the distance on (default ALL for full distance across all dimensions)
- * @return the distance between the two states
+ * @return The distance between the two states
  */
 double dist(
     const JointState& s1, const JointState& s2, const JointStateVariable& state_variable_type = JointStateVariable::ALL
@@ -31,7 +31,7 @@ double dist(
 
 /**
  * @class JointState
- * @brief Class to define a state in joint space
+ * @brief Class to define a state in joint space.
  */
 class JointState : public State {
 private:
@@ -42,232 +42,322 @@ class JointState : public State {
   Eigen::VectorXd torques_;       ///< joints torques
 
   /**
-   * @brief Getter of all the state variables (positions, velocities, accelerations and torques)
-   * @return the concatenated vector of all the state variables
+   * @brief Getter of all the state variables (positions, velocities, accelerations and torques).
+   * @return The concatenated vector of all the state variables
    */
   Eigen::VectorXd get_all_state_variables() const;
 
   /**
-   * @brief Set new_value in the provided state_variable (positions, velocities, accelerations or torques)
-   * @param state_variable the state variable to fill
-   * @param new_value the new value of the state variable
+   * @brief Set new_value in the provided state_variable (positions, velocities, accelerations or torques).
+   * @param state_variable The state variable to fill
+   * @param new_value The new value of the state variable
    */
   void set_state_variable(Eigen::VectorXd& state_variable, const Eigen::VectorXd& new_value);
 
   /**
-   * @brief Set new_value in the provided state_variable (positions, velocities, accelerations or torques)
-   * @param state_variable the state variable to fill
-   * @param new_value the new value of the state variable
+   * @brief Set new_value in the provided state_variable (positions, velocities, accelerations or torques).
+   * @param state_variable The state variable to fill
+   * @param new_value The new value of the state variable
    */
   void set_state_variable(Eigen::VectorXd& state_variable, const std::vector<double>& new_value);
 
   /**
-   * @brief Set new_value in all the state variables (positions, velocities, accelerations and torques)
-   * @param new_values the new values of the state variables
+   * @brief Set new_value in all the state variables (positions, velocities, accelerations and torques).
+   * @param new_values The new values of the state variables
    */
   void set_all_state_variables(const Eigen::VectorXd& new_values);
 
 protected:
   /**
-   * @brief Proxy function that multiply the specified state variable by an array of gain
-   * @param lambda the gain array to multiply with
-   * @param state_variable_type the state variable on which to apply the multiplication
+   * @brief Proxy function that multiply the specified state variable by an array of gain.
+   * @param lambda The gain array to multiply with
+   * @param state_variable_type The state variable on which to apply the multiplication
    */
   void multiply_state_variable(const Eigen::ArrayXd& lambda, const JointStateVariable& state_variable_type);
 
   /**
-   * @brief Proxy function that multiply the specified state variable by an array of gain
-   * @param lambda the gain array to multiply with
-   * @param state_variable_type the state variable on which to apply the multiplication
+   * @brief Proxy function that multiply the specified state variable by an array of gain.
+   * @param lambda The gain array to multiply with
+   * @param state_variable_type The state variable on which to apply the multiplication
    */
   void multiply_state_variable(const Eigen::MatrixXd& lambda, const JointStateVariable& state_variable_type);
 
   /**
-   * @brief Getter of the variable value corresponding to the input
-   * @param state_variable_type the type of variable to get
-   * @return the value of the variable as a vector
+   * @brief Getter of the variable value corresponding to the input.
+   * @param state_variable_type The type of variable to get
+   * @return The value of the variable as a vector
    */
   Eigen::VectorXd get_state_variable(const JointStateVariable& state_variable_type) const;
 
   /**
-   * @brief Setter of the variable value corresponding to the input
-   * @param new_value the new value of the variable
-   * @param state_variable_type the type of variable to get
+   * @brief Setter of the variable value corresponding to the input.
+   * @param new_value The new value of the variable
+   * @param state_variable_type The type of variable to get
    */
   void set_state_variable(const Eigen::VectorXd& new_value, const JointStateVariable& state_variable_type);
 
 public:
   /**
-   * @brief Empty constructor for a JointState
+   * @brief Empty constructor for a JointState.
    */
   explicit JointState();
 
   /**
-   * @brief Constructor with name and number of joints provided
-   * @param robot_name the name of the associated robot
-   * @param nb_joints the number of joints for initialization
+   * @brief Constructor with name and number of joints provided.
+   * @param robot_name The name of the associated robot
+   * @param nb_joints The number of joints for initialization
    */
   explicit JointState(const std::string& robot_name, unsigned int nb_joints = 0);
 
   /**
-   * @brief Constructor with name and list of joint names provided
-   * @param robot_name the name of the associated robot
-   * @param joint_names list of joint names
+   * @brief Constructor with name and list of joint names provided.
+   * @param robot_name The name of the associated robot
+   * @param joint_names List of joint names
    */
   explicit JointState(const std::string& robot_name, const std::vector<std::string>& joint_names);
 
   /**
-   * @brief Copy constructor of a JointState
+   * @brief Copy constructor of a JointState.
+   * @param state The JointState to copy from
    */
   JointState(const JointState& state) = default;
 
   /**
-   * @brief Constructor for the zero JointState
-   * @param robot_name the name of the associated robot
-   * @param nb_joints the number of joints for initialization
+   * @brief Constructor for a zero JointState.
+   * @param robot_name The name of the associated robot
+   * @param nb_joints The number of joints for initialization
    * @return JointState with zero values in all attributes
    */
   static JointState Zero(const std::string& robot_name, unsigned int nb_joints);
 
   /**
-   * @brief Constructor for the zero JointState
-   * @param robot_name the name of the associated robot
-   * @param joint_names list of joint names
+   * @brief Constructor for a zero JointState.
+   * @param robot_name The name of the associated robot
+   * @param joint_names List of joint names
    * @return JointState with zero values in all attributes
    */
   static JointState Zero(const std::string& robot_name, const std::vector<std::string>& joint_names);
 
   /**
-   * @brief Constructor for the random JointState
-   * @param robot_name the name of the associated robot
-   * @param nb_joints the number of joints for initialization
+   * @brief Constructor for a random JointState.
+   * @param robot_name The name of the associated robot
+   * @param nb_joints The number of joints for initialization
    * @return JointState with random values in all attributes
    */
   static JointState Random(const std::string& robot_name, unsigned int nb_joints);
 
   /**
-   * @brief Constructor for the random JointState
-   * @param robot_name the name of the associated robot
-   * @param joint_names list of joint names
+   * @brief Constructor for a random JointState.
+   * @param robot_name The name of the associated robot
+   * @param joint_names List of joint names
    * @return JointState with random values in all attributes
    */
   static JointState Random(const std::string& robot_name, const std::vector<std::string>& joint_names);
 
   /**
-   * @brief Swap the values of the two JointState
+   * @brief Swap the values of the two JointState.
    * @param state1 JointState to be swapped with 2
    * @param state2 JointState to be swapped with 1
    */
   friend void swap(JointState& state1, JointState& state2);
 
   /**
-   * @brief Copy assignment operator that have to be defined to the custom assignment operator
-   * @param state the state with value to assign
-   * @return reference to the current state with new values
+   * @brief Copy assignment operator that have to be defined to the custom assignment operator.
+   * @param state The state with value to assign
+   * @return Reference to the current state with new values
    */
   JointState& operator=(const JointState& state);
 
   /**
-   * @brief Getter of the size from the attributes
+   * @brief Getter of the size from the attributes.
+   * @return The size of the joint state
    */
   unsigned int get_size() const;
 
   /**
-   * @brief Getter of the names attribute
+   * @brief Getter of the names attribute.
+   * @return The vector of strings containing the joint names
    */
   const std::vector<std::string>& get_names() const;
 
   /**
-   * @brief Setter of the names attribute from the number of joints
+   * @brief Setter of the names attribute from the number of joints.
+   * @param nb_joints The number of joints of the joint state
    */
   void set_names(unsigned int nb_joints);
 
   /**
-   * @brief Setter of the names attribute
+   * @brief Setter of the names attribute.
+   * @param names The vector of strings containing the joint names
    */
   void set_names(const std::vector<std::string>& names);
 
   /**
-   * @brief Getter of the positions attribute
+   * @brief Get joint index by the name of the joint, if it exists.
+   * @details A JointNotFoundException exception is thrown if the
+   * desired joint doesn't exist.
+   * @param joint_name The name of the desired joint
+   * @return The index of the joint, if it exists
+   */
+  unsigned int get_joint_index(const std::string& joint_name) const;
+
+  /**
+   * @brief Getter of the positions attribute.
+   * @return The joint positions
    */
   const Eigen::VectorXd& get_positions() const;
 
   /**
-   * @brief Setter of the positions attribute
+   * @brief Get the position of a joint by its name, if it exists.
+   * @copydetails JointState::get_joint_index()
+   * @param joint_name The name of the joint
+   * @return The position of the joint, if it exists
+   */
+  double get_position(const std::string& joint_name) const;
+
+  /**
+   * @brief Get the position of a joint by its index, if it exists.
+   * @copydetails JointState::get_joint_index()
+   * @param joint_index The index of the joint
+   * @return The position of the joint, if it exists
+   */
+  double get_position(unsigned int joint_index) const;
+
+  /**
+   * @brief Setter of the positions attribute.
+   * @param positions The joint positions as Eigen vector
    */
   void set_positions(const Eigen::VectorXd& positions);
 
   /**
-   * @brief Setter of the positions from std vector
+   * @brief Setter of the positions from std vector.
+   * @param positions The joint positions as std vector
    */
   void set_positions(const std::vector<double>& positions);
 
   /**
-   * @brief Getter of the velocities attribute
+   * @brief Getter of the velocities attribute.
+   * @return The joint velocities
    */
   const Eigen::VectorXd& get_velocities() const;
 
   /**
-   * @brief Setter of the velocities attribute
+   * @brief Get the velocity of a joint by its name, if it exists.
+   * @copydetails JointState::get_joint_index()
+   * @param joint_name The name of the joint
+   * @return The velocity of the joint, if it exists
+   */
+  double get_velocity(const std::string& joint_name) const;
+
+  /**
+   * @brief Get the velocity of a joint by its index, if it exists.
+   * @copydetails JointState::get_joint_index()
+   * @param joint_index The index of the joint
+   * @return The velocity of the joint, if it exists
+   */
+  double get_velocity(unsigned int joint_index) const;
+
+  /**
+   * @brief Setter of the velocities attribute.
+   * @param velocities The joint velocities as Eigen vector
    */
   void set_velocities(const Eigen::VectorXd& velocities);
 
   /**
-   * @brief Setter of the velocities from std vector
+   * @brief Setter of the velocities from std vector.
+   * @param velocities The joint velocities as std vector
    */
   void set_velocities(const std::vector<double>& velocities);
 
   /**
-   * @brief Getter of the accelerations attribute
+   * @brief Getter of the accelerations attribute.
+   * @return The joint accelerations
    */
   const Eigen::VectorXd& get_accelerations() const;
 
   /**
-   * @brief Setter of the accelerations attribute
+   * @brief Get the acceleration of a joint by its name, if it exists.
+   * @copydetails JointState::get_joint_index()
+   * @param joint_name The name of the joint
+   * @return The acceleration of the joint, if it exists
+   */
+  double get_acceleration(const std::string& joint_name) const;
+
+  /**
+   * @brief Get the acceleration of a joint by its index, if it exists.
+   * @copydetails JointState::get_joint_index()
+   * @param joint_index The index of the joint
+   * @return The acceleration of the joint, if it exists
+   */
+  double get_acceleration(unsigned int joint_index) const;
+
+  /**
+   * @brief Setter of the accelerations attribute.
+   * @param accelerations The joint accelerations as Eigen vector
    */
   void set_accelerations(const Eigen::VectorXd& accelerations);
 
   /**
-   * @brief Setter of the accelerations from std vector
+   * @brief Setter of the accelerations from std vector.
+   * @param accelerations The joint accelerations as std vector
    */
   void set_accelerations(const std::vector<double>& accelerations);
 
   /**
-   * @brief Getter of the torques attribute
+   * @brief Getter of the torques attribute.
+   * @return The joint torques
    */
   const Eigen::VectorXd& get_torques() const;
 
   /**
-   * @brief Setter of the torques attribute
+   * @brief Get the torque of a joint by its name, if it exists.
+   * @copydetails JointState::get_joint_index()
+   * @param joint_name The name of the joint
+   * @return The torque of the joint, if it exists
+   */
+  double get_torque(const std::string& joint_name) const;
+
+  /**
+   * @brief Get the torque of a joint by its index, if it exists.
+   * @copydetails JointState::get_joint_index()
+   * @param joint_index The index of the joint
+   * @return The torque of the joint, if it exists
+   */
+  double get_torque(unsigned int joint_index) const;
+
+  /**
+   * @brief Setter of the torques attribute.
+   * @param torques The joint torques as Eigen vector
    */
   void set_torques(const Eigen::VectorXd& torques);
 
   /**
-   * @brief Setter of the torques from std vector
+   * @brief Setter of the torques attributes.
+   * @param torques The joint torques as std vector
    */
   void set_torques(const std::vector<double>& torques);
 
   /**
-   * @brief Check if the state is compatible for operations with the state given as argument
-   * @param state the state to check compatibility with
+   * @brief Check if the state is compatible for operations with the state given as argument.
+   * @param state The state to check compatibility with
    */
   bool is_compatible(const State& state) const;
 
   /**
-   * @brief Initialize the State to a zero value
+   * @brief Initialize the State to a zero value.
    */
   void initialize();
 
   /**
-   * @brief Set the State to a zero value
+   * @brief Set the JointState to a zero value.
    */
   void set_zero();
 
   /**
-   * @brief Clamp inplace the magnitude of the a specific joint state variable
-   * @param max_absolute_value the maximum absolute value of the state variable
-   * @param state_variable_type name of the variable from the JointStateVariable structure to clamp
-   * @param noise_ratio if provided, this value will be used to apply a dead zone relative to the maximum absolute value
+   * @brief Clamp inplace the magnitude of the a specific joint state variable.
+   * @param max_absolute_value The maximum absolute value of the state variable
+   * @param state_variable_type Name of the variable from the JointStateVariable structure to clamp
+   * @param noise_ratio If provided, this value will be used to apply a dead zone relative to the maximum absolute value
    * under which the state variable will be set to 0
    */
   void clamp_state_variable(
@@ -276,10 +366,10 @@ class JointState : public State {
 
   /**
    * @brief Clamp inplace the magnitude of the a specific joint state variable
-   * for each individual joint
-   * @param max_absolute_value_array the maximum absolute value of the state variable for each joints individually
-   * @param state_variable_type name of the variable from the JointStateVariable structure to clamp
-   * @param noise_ratio_array those values will be used to apply a dead zone relative to the maximum absolute value
+   * for each individual joint.
+   * @param max_absolute_value_array The maximum absolute value of the state variable for each joints individually
+   * @param state_variable_type Name of the variable from the JointStateVariable structure to clamp
+   * @param noise_ratio_array Those values will be used to apply a dead zone relative to the maximum absolute value
    * under which the state variable will be set to 0 for each individual joint
    */
   void clamp_state_variable(
@@ -288,163 +378,163 @@ class JointState : public State {
   );
 
   /**
-   * @brief Return a copy of the JointState
-   * @return the copy
+   * @brief Return a copy of the JointState.
+   * @return The copy
    */
   JointState copy() const;
 
   /**
    * @brief Returns the data as the concatenation of
-   * all the state variables in a single vector
-   * @return the concatenated data vector
+   * all the state variables in a single vector.
+   * @return The concatenated data vector
    */
   virtual Eigen::VectorXd data() const;
 
   /**
    * @brief Set the data of the state from
-   * all the state variables in a single Eigen vector
-   * @param the concatenated data vector
+   * all the state variables in a single Eigen vector.
+   * @param The concatenated data vector
    */
   virtual void set_data(const Eigen::VectorXd& data) override;
 
   /**
    * @brief Set the data of the state from
-   * all the state variables in a single std vector
-   * @param the concatenated data vector
+   * all the state variables in a single std vector.
+   * @param The concatenated data vector
    */
   virtual void set_data(const std::vector<double>& data) override;
 
   /**
-   * @brief Returns the data vector as an Eigen Array
-   * @return the concatenated data array
+   * @brief Returns the data vector as an Eigen Array.
+   * @return The concatenated data array
    */
   Eigen::ArrayXd array() const;
 
   /**
-   * @brief Overload the += operator
+   * @brief Overload the += operator.
    * @param state JointState to add
-   * @return the current JointState added the JointState given in argument
+   * @return The current JointState added the JointState given in argument
    */
   JointState& operator+=(const JointState& state);
 
   /**
-   * @brief Overload the + operator
+   * @brief Overload the + operator.
    * @param state JointState to add
-   * @return the current JointState added the JointState given in argument
+   * @return The current JointState added the JointState given in argument
    */
   JointState operator+(const JointState& state) const;
 
   /**
-   * @brief Overload the -= operator
+   * @brief Overload the -= operator.
    * @param state JointState to subtract
-   * @return the current JointState subtracted the JointState given in argument
+   * @return The current JointState subtracted the JointState given in argument
    */
   JointState& operator-=(const JointState& state);
 
   /**
-   * @brief Overload the - operator
+   * @brief Overload the - operator.
    * @param state JointState to subtract
-   * @return the current JointState subtracted the JointState given in argument
+   * @return The current JointState subtracted the JointState given in argument
    */
   JointState operator-(const JointState& state) const;
 
   /**
-   * @brief Overload the *= operator with a double gain
-   * @param lambda the gain to multiply with
-   * @return the JointState multiplied by lambda
+   * @brief Overload the *= operator with a double gain.
+   * @param lambda The gain to multiply with
+   * @return The JointState multiplied by lambda
    */
   JointState& operator*=(double lambda);
 
   /**
-   * @brief Overload the * operator with a double gain
-   * @param lambda the gain to multiply with
-   * @return the JointState multiplied by lambda
+   * @brief Overload the * operator with a double gain.
+   * @param lambda The gain to multiply with
+   * @return The JointState multiplied by lambda
    */
   JointState operator*(double lambda) const;
 
   /**
-   * @brief Overload the *= operator with an array of gains
-   * @param lambda the gain array to multiply with
-   * @return the JointState multiplied by lambda
+   * @brief Overload the *= operator with an array of gains.
+   * @param lambda The gain array to multiply with
+   * @return The JointState multiplied by lambda
    */
   JointState& operator*=(const Eigen::ArrayXd& lambda);
 
   /**
-   * @brief Overload the * operator with an array of gains
-   * @param lambda the gain array to multiply with
-   * @return the JointState multiplied by lambda
+   * @brief Overload the * operator with an array of gains.
+   * @param lambda The gain array to multiply with
+   * @return The JointState multiplied by lambda
    */
   JointState operator*(const Eigen::ArrayXd& lambda) const;
 
   /**
-   * @brief Overload the *= operator with a matrix of gains
-   * @param lambda the matrix to multiply with
-   * @return the JointState multiplied by lambda
+   * @brief Overload the *= operator with a matrix of gains.
+   * @param lambda The matrix to multiply with
+   * @return The JointState multiplied by lambda
    */
   JointState& operator*=(const Eigen::MatrixXd& lambda);
 
   /**
-   * @brief Overload the * operator with a matrix of gains
-   * @param lambda the matrix to multiply with
-   * @return the JointState multiplied by lambda
+   * @brief Overload the * operator with a matrix of gains.
+   * @param lambda The matrix to multiply with
+   * @return The JointState multiplied by lambda
    */
   JointState operator*(const Eigen::MatrixXd& lambda) const;
 
   /**
-   * @brief Overload the /= operator with a scalar
-   * @param lambda the scalar to divide with
-   * @return the JointState divided by lambda
+   * @brief Overload the /= operator with a scalar.
+   * @param lambda The scalar to divide with
+   * @return The JointState divided by lambda
    */
   JointState& operator/=(double lambda);
 
   /**
-   * @brief Overload the / operator with a scalar
-   * @param lambda the scalar to divide with
-   * @return the JointState divided by lambda
+   * @brief Overload the / operator with a scalar.
+   * @param lambda The scalar to divide with
+   * @return The JointState divided by lambda
    */
   JointState operator/(double lambda) const;
 
   /**
-   * @brief Compute the distance to another state as the sum of distances between each features
-   * @param state the second state
-   * @param state_variable_type name of the variable from the JointStateVariable structure to apply
+   * @brief Compute the distance to another state as the sum of distances between each attribute.
+   * @param state The second state
+   * @param state_variable_type Name of the variable from the JointStateVariable structure to apply
    * the distance on (default ALL for full distance across all dimensions)
-   * @return dist the distance value as a double
+   * @return dist The distance value as a double
    */
   double dist(const JointState& state, const JointStateVariable& state_variable_type = JointStateVariable::ALL) const;
 
   /**
-   * @brief Overload the ostream operator for printing
-   * @param os the ostream to append the string representing the state
-   * @param state the state to print
-   * @return the appended ostream
+   * @brief Overload the ostream operator for printing.
+   * @param os The ostream to append the string representing the state
+   * @param state The state to print
+   * @return The appended ostream
    */
   friend std::ostream& operator<<(std::ostream& os, const JointState& state);
 
   /**
-   * @brief Overload the * operator with a scalar
-   * @param lambda the scalar to multiply with
-   * @return the JointState provided multiply by lambda
+   * @brief Overload the * operator with a scalar.
+   * @param lambda The scalar to multiply with
+   * @return The JointState provided multiply by lambda
    */
   friend JointState operator*(double lambda, const JointState& state);
 
   /**
-   * @brief Overload the * operator with an array of gains
-   * @param lambda the gain array to multiply with
-   * @return the JointState provided multiply by lambda
+   * @brief Overload the * operator with an array of gains.
+   * @param lambda The gain array to multiply with
+   * @return The JointState provided multiply by lambda
    */
   friend JointState operator*(const Eigen::ArrayXd& lambda, const JointState& state);
 
   /**
-   * @brief Overload the * operator with a matrix of gains
-   * @param lambda the matrix to multiply with
-   * @return the JointState provided multiply by lambda
+   * @brief Overload the * operator with a matrix of gains.
+   * @param lambda The matrix to multiply with
+   * @return The JointState provided multiply by lambda
    */
   friend JointState operator*(const Eigen::MatrixXd& lambda, const JointState& state);
 
   /**
-   * @brief Return the joint state as a std vector of floats
-   * @return std::vector<float> the joint vector as a std vector
+   * @brief Return the joint state as a std vector of floats.
+   * @return The joint vector as a std vector
    */
   std::vector<double> to_std_vector() const;
 };
diff --git a/source/state_representation/include/state_representation/robot/JointTorques.hpp b/source/state_representation/include/state_representation/robot/JointTorques.hpp
index 558b4f601..905022284 100644
--- a/source/state_representation/include/state_representation/robot/JointTorques.hpp
+++ b/source/state_representation/include/state_representation/robot/JointTorques.hpp
@@ -13,12 +13,18 @@ class JointTorques : public JointState {
 
 public:
   const Eigen::VectorXd& get_positions() const = delete;
+  double get_position(unsigned int joint_index) const = delete;
+  double get_position(const std::string& joint_name) const = delete;
   void set_positions(const Eigen::VectorXd& positions) = delete;
   void set_positions(const std::vector<double>& positions) = delete;
   const Eigen::VectorXd& get_velocities() const = delete;
+  double get_velocity(unsigned int joint_index) const = delete;
+  double get_velocity(const std::string& joint_name) const = delete;
   void set_velocities(const Eigen::VectorXd& velocities) = delete;
   void set_velocities(const std::vector<double>& velocities) = delete;
   const Eigen::VectorXd& get_accelerations() const = delete;
+  double get_acceleration(unsigned int joint_index) const = delete;
+  double get_acceleration(const std::string& joint_name) const = delete;
   void set_accelerations(const Eigen::VectorXd& accelerations) = delete;
   void set_accelerations(const std::vector<double>& accelerations) = delete;
 
diff --git a/source/state_representation/include/state_representation/robot/JointVelocities.hpp b/source/state_representation/include/state_representation/robot/JointVelocities.hpp
index d9cdf1f1f..e91a9d5c2 100644
--- a/source/state_representation/include/state_representation/robot/JointVelocities.hpp
+++ b/source/state_representation/include/state_representation/robot/JointVelocities.hpp
@@ -18,12 +18,18 @@ class JointVelocities : public JointState {
 
 public:
   const Eigen::VectorXd& get_positions() const = delete;
+  double get_position(unsigned int joint_index) const = delete;
+  double get_position(const std::string& joint_name) const = delete;
   void set_positions(const Eigen::VectorXd& positions) = delete;
   void set_positions(const std::vector<double>& positions) = delete;
   const Eigen::VectorXd& get_accelerations() const = delete;
+  double get_acceleration(unsigned int joint_index) const = delete;
+  double get_acceleration(const std::string& joint_name) const = delete;
   void set_accelerations(const Eigen::VectorXd& accelerations) = delete;
   void set_accelerations(const std::vector<double>& accelerations) = delete;
   const Eigen::VectorXd& get_torques() const = delete;
+  double get_torque(unsigned int joint_index) const = delete;
+  double get_torque(const std::string& joint_name) const = delete;
   void set_torques(const Eigen::VectorXd& torques) = delete;
   void set_torques(const std::vector<double>& torques) = delete;
 
diff --git a/source/state_representation/src/robot/JointState.cpp b/source/state_representation/src/robot/JointState.cpp
index 4bf2646c0..a7321cc63 100644
--- a/source/state_representation/src/robot/JointState.cpp
+++ b/source/state_representation/src/robot/JointState.cpp
@@ -1,10 +1,18 @@
 #include "state_representation/robot/JointState.hpp"
 
 #include "state_representation/exceptions/EmptyStateException.hpp"
+#include "state_representation/exceptions/JointNotFoundException.hpp"
 #include "state_representation/exceptions/IncompatibleStatesException.hpp"
-#include "state_representation/exceptions/NotImplementedException.hpp"
 
 namespace state_representation {
+
+static void assert_index_in_range(unsigned int joint_index, unsigned int size) {
+  if (joint_index > size) {
+    throw JointNotFoundException(
+        "Index '" + std::to_string(joint_index) + "' is out of range for joint state with size" + std::to_string(size));
+  }
+}
+
 JointState::JointState() : State(StateType::JOINTSTATE) {
   this->initialize();
 }
@@ -67,6 +75,50 @@ JointState JointState::Random(const std::string& robot_name, const std::vector<s
   return random;
 }
 
+unsigned int JointState::get_joint_index(const std::string& joint_name) const {
+  auto finder = std::find(this->names_.begin(), this->names_.end(), joint_name);
+  if (finder == this->names_.end()) {
+    throw JointNotFoundException("The joint with name '" + joint_name + "' could not be found in the joint state.");
+  }
+  return std::distance(this->names_.begin(), finder);
+}
+
+double JointState::get_position(const std::string& joint_name) const {
+  return this->positions_(this->get_joint_index(joint_name));
+}
+
+double JointState::get_position(unsigned int joint_index) const {
+  assert_index_in_range(joint_index, this->get_size());
+  return this->positions_(joint_index);
+}
+
+double JointState::get_velocity(const std::string& joint_name) const {
+  return this->velocities_(this->get_joint_index(joint_name));
+}
+
+double JointState::get_velocity(unsigned int joint_index) const {
+  assert_index_in_range(joint_index, this->get_size());
+  return this->velocities_(joint_index);
+}
+
+double JointState::get_acceleration(const std::string& joint_name) const {
+  return this->accelerations_(this->get_joint_index(joint_name));
+}
+
+double JointState::get_acceleration(unsigned int joint_index) const {
+  assert_index_in_range(joint_index, this->get_size());
+  return this->accelerations_(joint_index);
+}
+
+double JointState::get_torque(const std::string& joint_name) const {
+  return this->torques_(this->get_joint_index(joint_name));
+}
+
+double JointState::get_torque(unsigned int joint_index) const {
+  assert_index_in_range(joint_index, this->get_size());
+  return this->torques_(joint_index);
+}
+
 JointState& JointState::operator+=(const JointState& state) {
   if (!this->is_compatible(state)) {
     throw IncompatibleStatesException(
diff --git a/source/state_representation/test/tests/robot/test_joint_state.cpp b/source/state_representation/test/tests/robot/test_joint_state.cpp
index 9c7c47b09..1c05ca2e1 100644
--- a/source/state_representation/test/tests/robot/test_joint_state.cpp
+++ b/source/state_representation/test/tests/robot/test_joint_state.cpp
@@ -3,6 +3,7 @@
 #include "state_representation/robot/JointState.hpp"
 #include "state_representation/exceptions/IncompatibleSizeException.hpp"
 #include "state_representation/exceptions/IncompatibleStatesException.hpp"
+#include "state_representation/exceptions/JointNotFoundException.hpp"
 #include "state_representation/exceptions/EmptyStateException.hpp"
 
 using namespace state_representation;
@@ -109,22 +110,42 @@ TEST(JointStateTest, GetSetFields) {
   js.set_positions(positions);
   for (std::size_t i = 0; i < positions.size(); ++i) {
     EXPECT_EQ(js.get_positions()(i), positions.at(i));
+    EXPECT_EQ(js.get_position(i), positions.at(i));
+    EXPECT_EQ(js.get_position(js.get_names().at(i)), positions.at(i));
   }
+  EXPECT_THROW(js.get_position(js.get_size() + 1), exceptions::JointNotFoundException);
+  EXPECT_THROW(js.get_position("test"), exceptions::JointNotFoundException);
+
   std::vector<double> velocities{4, 5, 6};
   js.set_velocities(velocities);
   for (std::size_t i = 0; i < velocities.size(); ++i) {
     EXPECT_EQ(js.get_velocities()(i), velocities.at(i));
+    EXPECT_EQ(js.get_velocity(i), velocities.at(i));
+    EXPECT_EQ(js.get_velocity(js.get_names().at(i)), velocities.at(i));
   }
+  EXPECT_THROW(js.get_velocity(js.get_size() + 1), exceptions::JointNotFoundException);
+  EXPECT_THROW(js.get_velocity("test"), exceptions::JointNotFoundException);
+
   std::vector<double> accelerations{7, 8, 9};
   js.set_accelerations(accelerations);
   for (std::size_t i = 0; i < accelerations.size(); ++i) {
     EXPECT_EQ(js.get_accelerations()(i), accelerations.at(i));
+    EXPECT_EQ(js.get_acceleration(i), accelerations.at(i));
+    EXPECT_EQ(js.get_acceleration(js.get_names().at(i)), accelerations.at(i));
   }
+  EXPECT_THROW(js.get_acceleration(js.get_size() + 1), exceptions::JointNotFoundException);
+  EXPECT_THROW(js.get_acceleration("test"), exceptions::JointNotFoundException);
+
   std::vector<double> torques{10, 11, 12};
   js.set_torques(torques);
   for (std::size_t i = 0; i < torques.size(); ++i) {
     EXPECT_EQ(js.get_torques()(i), torques.at(i));
+    EXPECT_EQ(js.get_torque(i), torques.at(i));
+    EXPECT_EQ(js.get_torque(js.get_names().at(i)), torques.at(i));
   }
+  EXPECT_THROW(js.get_torque(js.get_size() + 1), exceptions::JointNotFoundException);
+  EXPECT_THROW(js.get_torque("test"), exceptions::JointNotFoundException);
+
   EXPECT_THROW(js.set_positions(Eigen::VectorXd::Zero(4)), exceptions::IncompatibleSizeException);
   EXPECT_THROW(js.set_velocities(Eigen::VectorXd::Zero(5)), exceptions::IncompatibleSizeException);
   EXPECT_THROW(js.set_accelerations(Eigen::VectorXd::Zero(6)), exceptions::IncompatibleSizeException);
@@ -228,6 +249,15 @@ TEST(JointStateTest, GetSetData) {
   EXPECT_THROW(js1.set_data(Eigen::Vector2d::Zero()), exceptions::IncompatibleSizeException);
 }
 
+TEST(JointStateTest, GetIndexByName) {
+  JointState js = JointState::Random("test", 3);
+  for (std::size_t i = 0; i < js.get_size(); ++i) {
+    auto index = js.get_joint_index("joint" + std::to_string(i));
+    EXPECT_EQ(index, i);
+  }
+  EXPECT_THROW(js.get_joint_index("joint5"), exceptions::JointNotFoundException);
+}
+
 TEST(JointStateTest, Distance) {
   JointState js;
   JointState js1 = JointState::Random("test", 3);

From 1ca25f95c4558137d0ceb27ae131e5d546f33e6a Mon Sep 17 00:00:00 2001
From: domire8 <71256590+domire8@users.noreply.github.com>
Date: Thu, 16 Sep 2021 16:24:30 +0200
Subject: [PATCH 09/22] Add tests for State and SpatialState (#212)

* Add cpp tests for State and SpatialState

* Correctly define ostream operator for SpatialState

* Remove swap binding and add __repr__ for SpatialState

* Move tests to dedicated subfolder

* Add python tests for State and SpatialState

* Fix tests

* Change default name of State to empty string

* Fix tests
---
 .../bind_cartesian_space.cpp                  | 18 +++--
 .../state_representation/bind_state.cpp       |  2 -
 python/test/robot/__init__.py                 |  0
 python/test/{ => robot}/test_jacobian.py      |  7 +-
 python/test/{ => robot}/test_joint_state.py   |  0
 python/test/space/__init__.py                 |  0
 .../test/{ => space}/test_cartesian_pose.py   |  0
 .../test/{ => space}/test_cartesian_state.py  |  0
 python/test/space/test_spatial_state.py       | 45 +++++++++++
 python/test/test_state.py                     | 75 +++++++++++++++++++
 .../space/SpatialState.hpp                    | 48 +++++++-----
 source/state_representation/src/State.cpp     |  4 +-
 .../test/tests/robot/test_joint_state.cpp     |  2 +-
 .../test/tests/space/test_spatial_state.cpp   | 50 +++++++++++++
 .../test/tests/test_state.cpp                 | 67 +++++++++++++++++
 15 files changed, 285 insertions(+), 33 deletions(-)
 create mode 100644 python/test/robot/__init__.py
 rename python/test/{ => robot}/test_jacobian.py (96%)
 rename python/test/{ => robot}/test_joint_state.py (100%)
 create mode 100644 python/test/space/__init__.py
 rename python/test/{ => space}/test_cartesian_pose.py (100%)
 rename python/test/{ => space}/test_cartesian_state.py (100%)
 create mode 100644 python/test/space/test_spatial_state.py
 create mode 100644 python/test/test_state.py
 create mode 100644 source/state_representation/test/tests/space/test_spatial_state.cpp
 create mode 100644 source/state_representation/test/tests/test_state.cpp

diff --git a/python/source/state_representation/bind_cartesian_space.cpp b/python/source/state_representation/bind_cartesian_space.cpp
index 805073848..3a9dc99db 100644
--- a/python/source/state_representation/bind_cartesian_space.cpp
+++ b/python/source/state_representation/bind_cartesian_space.cpp
@@ -11,15 +11,19 @@
 void spatial_state(py::module_& m) {
   py::class_<SpatialState, State> c(m, "SpatialState");
 
-  m.def("swap", py::overload_cast<SpatialState&, SpatialState&>(&state_representation::swap), "Swap the values of the two SpatialStates", "state1"_a, "state2"_a);
+  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);
 
-  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, "empty"_a=true);
-  c.def(py::init<const SpatialState&>(), "Copy constructor from another SpatialState", "state"_a);
+  c.def("get_reference_frame", &SpatialState::get_reference_frame, "Getter of the reference frame.");
+  c.def("set_reference_frame", &SpatialState::set_reference_frame, "Setter of the reference frame.", "reference_frame"_a);
+  c.def("is_compatible", &SpatialState::is_compatible, "Check if the state is compatible for operations with the state given as argument.", "state"_a);
 
-  c.def("get_reference_frame", &SpatialState::get_reference_frame, "Getter of the reference frame");
-  c.def("set_reference_frame", &SpatialState::set_reference_frame, "Setter of the reference frame");
-  c.def("is_compatible", &SpatialState::is_compatible, "Check if the state is compatible for operations with the state given as argument", "state"_a);
+  c.def("__repr__", [](const SpatialState& state) {
+    std::stringstream buffer;
+    buffer << state;
+    return buffer.str();
+  });
 }
 
 void cartesian_state_variable(py::module_& m) {
diff --git a/python/source/state_representation/bind_state.cpp b/python/source/state_representation/bind_state.cpp
index 126f1e4d5..71982acc4 100644
--- a/python/source/state_representation/bind_state.cpp
+++ b/python/source/state_representation/bind_state.cpp
@@ -33,8 +33,6 @@ void state_type(py::module_& m) {
 void state(py::module_& m) {
   py::class_<State> c(m, "State");
 
-  m.def("swap", py::overload_cast<State&, State&>(&state_representation::swap), "Swap the values of the two States", "state1"_a, "state2"_a);
-
   c.def(py::init(), "Empty constructor");
   c.def(py::init<const StateType&>(), "Constructor only specifying the type of the state from the StateType enumeration", "type"_a);
   c.def(py::init<const StateType&, const std::string&, const bool&>(), "Constructor with name specification", "type"_a, "name"_a, "empty"_a=true);
diff --git a/python/test/robot/__init__.py b/python/test/robot/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/python/test/test_jacobian.py b/python/test/robot/test_jacobian.py
similarity index 96%
rename from python/test/test_jacobian.py
rename to python/test/robot/test_jacobian.py
index 247b78aee..3f6c6f81b 100644
--- a/python/test/test_jacobian.py
+++ b/python/test/robot/test_jacobian.py
@@ -1,6 +1,7 @@
 import unittest
-from state_representation import Jacobian, CartesianPose, CartesianTwist, CartesianWrench, JointVelocities
+
 import numpy as np
+from state_representation import Jacobian, CartesianPose, CartesianTwist, CartesianWrench, JointVelocities
 
 JACOBIAN_METHOD_EXPECTS = [
     'Random',
@@ -24,6 +25,7 @@
     'copy',
 ]
 
+
 class TestJacobian(unittest.TestCase):
     def assert_np_array_equal(self, a, b):
         self.assertListEqual(list(a), list(b))
@@ -82,7 +84,8 @@ def test_operators(self):
 
         joint_velocities = JointVelocities.Random("test", 3)
         twist = jac * joint_velocities
-        self.assert_np_array_almost_equal(joint_velocities.get_velocities(), (jac.pseudoinverse() * twist).get_velocities())
+        self.assert_np_array_almost_equal(joint_velocities.get_velocities(),
+                                          (jac.pseudoinverse() * twist).get_velocities())
 
         wrench = CartesianWrench.Random("ee", "robot")
         torques = jac.transpose() * wrench
diff --git a/python/test/test_joint_state.py b/python/test/robot/test_joint_state.py
similarity index 100%
rename from python/test/test_joint_state.py
rename to python/test/robot/test_joint_state.py
diff --git a/python/test/space/__init__.py b/python/test/space/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/python/test/test_cartesian_pose.py b/python/test/space/test_cartesian_pose.py
similarity index 100%
rename from python/test/test_cartesian_pose.py
rename to python/test/space/test_cartesian_pose.py
diff --git a/python/test/test_cartesian_state.py b/python/test/space/test_cartesian_state.py
similarity index 100%
rename from python/test/test_cartesian_state.py
rename to python/test/space/test_cartesian_state.py
diff --git a/python/test/space/test_spatial_state.py b/python/test/space/test_spatial_state.py
new file mode 100644
index 000000000..a8fd18f50
--- /dev/null
+++ b/python/test/space/test_spatial_state.py
@@ -0,0 +1,45 @@
+import unittest
+
+from state_representation import SpatialState, StateType
+
+from test.test_state import STATE_METHOD_EXPECTS
+
+SPATIAL_STATE_METHOD_EXPECTS = [
+    'get_reference_frame',
+    'set_reference_frame',
+    'is_compatible'
+]
+
+
+class TestState(unittest.TestCase):
+
+    def test_callable_methods(self):
+        methods = [m for m in dir(SpatialState) if callable(getattr(SpatialState, m))]
+        for expected in STATE_METHOD_EXPECTS:
+            self.assertIn(expected, methods)
+        for expected in SPATIAL_STATE_METHOD_EXPECTS:
+            self.assertIn(expected, methods)
+
+    def test_constructors(self):
+        state1 = SpatialState(StateType.JOINTSTATE)
+        self.assertEqual(state1.get_type(), StateType.JOINTSTATE)
+        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)
+        self.assertEqual(state2.get_name(), "test")
+        self.assertEqual(state2.get_reference_frame(), "robot")
+        self.assertFalse(state2.is_empty())
+
+    def test_compatibility(self):
+        state1 = SpatialState(StateType.CARTESIANSTATE, "test", "robot", True)
+        state2 = SpatialState(StateType.STATE, "test")
+        self.assertFalse(state1.is_compatible(state2))
+        state2.set_reference_frame("robot")
+        self.assertTrue(state1.is_compatible(state2))
+
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/python/test/test_state.py b/python/test/test_state.py
new file mode 100644
index 000000000..457a6c291
--- /dev/null
+++ b/python/test/test_state.py
@@ -0,0 +1,75 @@
+import time
+import unittest
+
+from state_representation import State, StateType
+
+STATE_METHOD_EXPECTS = [
+    'get_type',
+    'is_empty',
+    'set_empty',
+    'set_filled',
+    'get_timestamp',
+    'reset_timestamp',
+    'get_name',
+    'set_name',
+    'is_deprecated',
+    'is_compatible',
+    'initialize'
+]
+
+
+class TestState(unittest.TestCase):
+
+    def test_callable_methods(self):
+        methods = [m for m in dir(State) if callable(getattr(State, m))]
+        for expected in STATE_METHOD_EXPECTS:
+            self.assertIn(expected, methods)
+
+    def test_constructors(self):
+        empty1 = State()
+        self.assertEqual(empty1.get_type(), StateType.STATE)
+        self.assertEqual(empty1.get_name(), "")
+        self.assertTrue(empty1.is_empty())
+
+        empty2 = State(StateType.JOINTSTATE)
+        self.assertEqual(empty2.get_type(), StateType.JOINTSTATE)
+        self.assertEqual(empty2.get_name(), "")
+        self.assertTrue(empty2.is_empty())
+
+        empty3 = State(StateType.CARTESIANSTATE, "test", True)
+        self.assertEqual(empty3.get_type(), StateType.CARTESIANSTATE)
+        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_name(), "test")
+        self.assertFalse(state.is_empty())
+        state.set_empty()
+        self.assertTrue(state.is_empty())
+
+    def test_compatibility(self):
+        state1 = State()
+        state1.set_name("test")
+        self.assertEqual(state1.get_name(), "test")
+
+        state2 = State(StateType.STATE, "test", False)
+        self.assertTrue(state1.is_compatible(state2))
+        state2.set_name("world")
+        self.assertFalse(state1.is_compatible(state2))
+
+        state2.initialize()
+        self.assertTrue(state2.is_empty())
+
+    def test_timestamp(self):
+        state = State(StateType.STATE, "test", False)
+        time.sleep(0.2)
+        self.assertTrue(state.is_deprecated(0.1))
+        state.reset_timestamp()
+        self.assertFalse(state.is_deprecated(0.1))
+
+
+if __name__ == '__main__':
+    unittest.main()
diff --git a/source/state_representation/include/state_representation/space/SpatialState.hpp b/source/state_representation/include/state_representation/space/SpatialState.hpp
index 124f06545..e1f06a572 100644
--- a/source/state_representation/include/state_representation/space/SpatialState.hpp
+++ b/source/state_representation/include/state_representation/space/SpatialState.hpp
@@ -9,56 +9,66 @@ class SpatialState : public State {
 
 public:
   /**
-   * @brief Empty constructor only specifying the type
+   * @brief Empty constructor only specifying the type.
    */
   explicit SpatialState(const StateType& type);
 
   /**
-   * @brief Constructor with name and reference frame specification
-   * @param type the type of SpatialState (Cartesian or DualQuaternion)
-   * @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
+   * @brief Constructor with name and reference frame specification.
+   * @param type The type of SpatialState (Cartesian or DualQuaternion)
+   * @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
    */
-  explicit SpatialState(const StateType& type,
-                        const std::string& name,
-                        const std::string& reference_frame = "world",
-                        const bool& empty = true);
+  explicit SpatialState(
+      const StateType& type, const std::string& name, const std::string& reference_frame = "world",
+      const bool& empty = true
+  );
 
   /**
-   * @brief Copy constructor from another SpatialState
+   * @brief Copy constructor from another SpatialState.
    */
   SpatialState(const SpatialState& state) = default;
 
   /**
-   * @brief Swap the values of the two SpatialState
+   * @brief Swap the values of the two SpatialState.
    * @param state1 SpatialState to be swapped with 2
    * @param state2 SpatialState to be swapped with 1
    */
   friend void swap(SpatialState& state1, SpatialState& state2);
 
   /**
-   * @brief Copy assignment operator that have to be defined to the custom assignment operator
-   * @param state the state with value to assign
-   * @return reference to the current state with new values
+   * @brief Copy assignment operator that have to be defined to the custom assignment operator.
+   * @param state The state with value to assign
+   * @return Reference to the current state with new values
    */
   SpatialState& operator=(const SpatialState& state);
 
   /**
-   * @brief Getter of the reference frame as const reference
+   * @brief Getter of the reference frame as const reference.
+   * @return The name of the reference frame
    */
   const std::string& get_reference_frame() const;
 
   /**
-   * @brief Setter of the reference frame
+   * @brief Setter of the reference frame.
+   * @param reference_frame The reference frame
    */
   virtual void set_reference_frame(const std::string& reference_frame);
 
   /**
-   * @brief Check if the state is compatible for operations with the state given as argument
-   * @param state the state to check compatibility with
+   * @brief Check if the state is compatible for operations with the state given as argument.
+   * @param state The state to check compatibility with
    */
   virtual bool is_compatible(const State& state) const override;
+
+  /**
+   * @brief Overload the ostream operator for printing.
+   * @param os The ostream to append the string representing the SpatialState to
+   * @param state The SpatialState to print
+   * @return The appended ostream
+   */
+  friend std::ostream& operator<<(std::ostream& os, const SpatialState& state);
 };
 
 inline void swap(SpatialState& state1, SpatialState& state2) {
diff --git a/source/state_representation/src/State.cpp b/source/state_representation/src/State.cpp
index ce3ca3e0e..cc20999f1 100644
--- a/source/state_representation/src/State.cpp
+++ b/source/state_representation/src/State.cpp
@@ -2,9 +2,9 @@
 #include "state_representation/exceptions/NotImplementedException.hpp"
 
 namespace state_representation {
-State::State() : type_(StateType::STATE), name_("none"), empty_(true) {}
+State::State() : type_(StateType::STATE), name_(""), empty_(true) {}
 
-State::State(const StateType& type) : type_(type), name_("none"), empty_(true) {}
+State::State(const StateType& type) : type_(type), name_(""), empty_(true) {}
 
 State::State(const StateType& type, const std::string& name, const bool& empty)
     : type_(type), name_(name), empty_(empty), timestamp_(std::chrono::steady_clock::now()) {}
diff --git a/source/state_representation/test/tests/robot/test_joint_state.cpp b/source/state_representation/test/tests/robot/test_joint_state.cpp
index 1c05ca2e1..411db262d 100644
--- a/source/state_representation/test/tests/robot/test_joint_state.cpp
+++ b/source/state_representation/test/tests/robot/test_joint_state.cpp
@@ -10,7 +10,7 @@ using namespace state_representation;
 
 TEST(JointStateTest, Constructors) {
   JointState empty;
-  EXPECT_EQ(empty.get_name(), "none");
+  EXPECT_EQ(empty.get_name(), "");
   EXPECT_EQ(empty.get_type(), StateType::JOINTSTATE);
   EXPECT_TRUE(empty.is_empty());
   EXPECT_EQ(empty.get_size(), 0);
diff --git a/source/state_representation/test/tests/space/test_spatial_state.cpp b/source/state_representation/test/tests/space/test_spatial_state.cpp
new file mode 100644
index 000000000..73d2ef87c
--- /dev/null
+++ b/source/state_representation/test/tests/space/test_spatial_state.cpp
@@ -0,0 +1,50 @@
+#include <gtest/gtest.h>
+
+#include "state_representation/space/SpatialState.hpp"
+
+using namespace state_representation;
+
+TEST(SpatialStateTest, Constructors) {
+  SpatialState state1(StateType::JOINTSTATE);
+  EXPECT_EQ(state1.get_type(), StateType::JOINTSTATE);
+  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);
+  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_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");
+  EXPECT_FALSE(state1.is_compatible(state2));
+  state2.set_reference_frame("robot");
+  EXPECT_FALSE(state1.is_compatible(state2));
+  state2.set_name("test");
+  EXPECT_TRUE(state1.is_compatible(state2));
+}
+
+TEST(SpatialStateTest, Swap) {
+  SpatialState state1(StateType::CARTESIANSTATE, "cartesian");
+  SpatialState state2(StateType::JOINTSTATE, "joint", "robot", false);
+  swap(state1, state2);
+  EXPECT_EQ(state1.get_type(), StateType::JOINTSTATE);
+  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_name(), "cartesian");
+  EXPECT_EQ(state2.get_reference_frame(), "world");
+  EXPECT_TRUE(state2.is_empty());
+}
\ No newline at end of file
diff --git a/source/state_representation/test/tests/test_state.cpp b/source/state_representation/test/tests/test_state.cpp
new file mode 100644
index 000000000..e9a42a781
--- /dev/null
+++ b/source/state_representation/test/tests/test_state.cpp
@@ -0,0 +1,67 @@
+#include <chrono>
+#include <gtest/gtest.h>
+#include <thread>
+
+#include "state_representation/State.hpp"
+
+using namespace state_representation;
+
+TEST(StateTest, Constructors) {
+  State empty1;
+  EXPECT_EQ(empty1.get_type(), StateType::STATE);
+  EXPECT_EQ(empty1.get_name(), "");
+  EXPECT_TRUE(empty1.is_empty());
+
+  State empty2(StateType::JOINTSTATE);
+  EXPECT_EQ(empty2.get_type(), StateType::JOINTSTATE);
+  EXPECT_EQ(empty2.get_name(), "");
+  EXPECT_TRUE(empty2.is_empty());
+
+  State empty3(StateType::CARTESIANSTATE, "test", true);
+  EXPECT_EQ(empty3.get_type(), StateType::CARTESIANSTATE);
+  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_name(), "test");
+  EXPECT_FALSE(state.is_empty());
+  state.set_empty();
+  EXPECT_TRUE(state.is_empty());
+}
+
+TEST(StateTest, Compatibility) {
+  State state1;
+  state1.set_name("test");
+  EXPECT_EQ(state1.get_name(), "test");
+
+  State state2(StateType::STATE, "test", false);
+  EXPECT_TRUE(state1.is_compatible(state2));
+  state2.set_name("world");
+  EXPECT_FALSE(state1.is_compatible(state2));
+
+  state2.initialize();
+  EXPECT_TRUE(state2.is_empty());
+}
+
+TEST(StateTest, Timestamp) {
+  State state(StateType::STATE, "test", false);
+  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  EXPECT_TRUE(state.is_deprecated(std::chrono::milliseconds(100)));
+  state.reset_timestamp();
+  EXPECT_FALSE(state.is_deprecated(std::chrono::milliseconds(100)));
+}
+
+TEST(StateTest, Swap) {
+  State state1(StateType::CARTESIANSTATE, "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_name(), "cartesian");
+  EXPECT_TRUE(state2.is_empty());
+}
\ No newline at end of file

From ef9e4449529c397c8748a55442c6c51403b9b073 Mon Sep 17 00:00:00 2001
From: domire8 <71256590+domire8@users.noreply.github.com>
Date: Thu, 16 Sep 2021 16:33:06 +0200
Subject: [PATCH 10/22] Improve Docker resources for python (#211)

* Improve Docker resources for python

* Check for base image existence

* Update CHANGELOG
---
 CHANGELOG.md             |  1 +
 python/Dockerfile.python | 16 +++++----
 python/dev-server.sh     | 75 ++++++++++++++++++++++++++++++++++++++++
 python/run.sh            |  5 ++-
 4 files changed, 90 insertions(+), 7 deletions(-)
 create mode 100644 python/dev-server.sh

diff --git a/CHANGELOG.md b/CHANGELOG.md
index dabaa0e11..5d520a090 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -15,6 +15,7 @@ Release Versions:
 - Add int and int array parameters (#208)
 - Add method to get a joint state by name or index of the joint,
   and to get the index of the joint by its name (#210)
+- Add possibility to have a ssh server for Python testing (#211)
 
 ## 4.0.0
 
diff --git a/python/Dockerfile.python b/python/Dockerfile.python
index 31d749ed8..cd1860216 100644
--- a/python/Dockerfile.python
+++ b/python/Dockerfile.python
@@ -1,4 +1,5 @@
-FROM ghcr.io/epfl-lasa/control-libraries/development-dependencies:latest
+ARG BASE_IMAGE=ghcr.io/epfl-lasa/control-libraries/development-dependencies:latest
+FROM ${BASE_IMAGE} AS python-install
 ARG BRANCH=develop
 
 WORKDIR /source
@@ -10,16 +11,19 @@ COPY source control-libraries/python/source
 COPY pyproject.toml setup.py control-libraries/python/
 RUN pip3 install control-libraries/python
 
+
+FROM python-install AS dev-user
+
 RUN useradd --create-home --shell /bin/bash dev
 USER dev
 WORKDIR /home/dev
 
-RUN (\
-    echo "alias pip=pip3"; \
-    echo 'alias python=python3'; \
-    ) >> ./.bashrc
-
 COPY test ./test
 RUN python3 -m unittest
 
 CMD ["/bin/bash"]
+
+
+FROM python-install AS remote-user
+
+ENTRYPOINT ["/.ssh_entrypoint.sh"]
diff --git a/python/dev-server.sh b/python/dev-server.sh
new file mode 100644
index 000000000..65a0d0999
--- /dev/null
+++ b/python/dev-server.sh
@@ -0,0 +1,75 @@
+#!/bin/bash
+
+BASE_IMAGE=controle-libraries/remote-development:latest
+IMAGE_NAME=control-libraries/python/remote
+CONTAINER_NAME=control-libraries-python-ssh
+CONTAINER_HOSTNAME=control-libraries-python
+BRANCH=$(git branch --show-current)
+
+SSH_PORT=2233
+SSH_KEY_FILE="$HOME/.ssh/id_rsa.pub"
+
+HELP_MESSAGE="Usage: ./dev-server.sh [--port <port>] [--key-file </path/to/id_rsa.pub>]
+
+Build and run a docker container as an SSH toolchain server for remote development.
+
+The server is bound to the specified port on localhost (127.0.0.1)
+and uses passwordless RSA key-pair authentication. The host public key
+is read from the specified key file and copied to the server on startup.
+
+The server will run in the background as ${CONTAINER_NAME}.
+
+You can connect with 'ssh remote@localhost -p <port>'.
+
+Close the server with 'docker container stop ${CONTAINER_NAME}'.
+
+Options:
+  -b, --branch [branch]    Specify the branch of control libraries
+                           that should be used to build the image.
+  -p, --port [XXXX]        Specify the port to bind for SSH
+                           connection.
+                           (default: ${SSH_PORT})
+
+  -k, --key-file [path]    Specify the path of the RSA
+                           public key file.
+                           (default: ${SSH_KEY_FILE})
+
+  -h, --help               Show this help message."
+
+function image_not_found() {
+  MESSAGE="Could not find the required Docker image '${BASE_IMAGE}'
+from ../source/Dockerfile.source. Make sure to build it with the 'dev-server.sh' script
+in the 'source' directory."
+  echo "${MESSAGE}" && exit 1
+}
+
+docker image inspect "${BASE_IMAGE}" >/dev/null 2>&1 || image_not_found
+
+while [ "$#" -gt 0 ]; do
+  case "$1" in
+    -b|--branch) BRANCH=$2; shift 2;;
+    -p|--port) SSH_PORT=$2; shift 2;;
+    -k|--key-file) SSH_KEY_FILE=$2; shift 2;;
+    -h|--help) echo "${HELP_MESSAGE}"; exit 0;;
+    *) echo "Unknown option: $1" >&2; echo "${HELP_MESSAGE}"; exit 1;;
+  esac
+done
+
+echo "Using control libraries branch ${BRANCH}"
+
+DOCKER_BUILDKIT=1 docker build . --file ./Dockerfile.python \
+  --build-arg BASE_IMAGE=control-libraries/remote-development:latest \
+  --build-arg BRANCH="${BRANCH}" \
+  --target remote-user \
+  --tag "${IMAGE_NAME}" || exit 1
+
+docker container stop "${CONTAINER_NAME}" >/dev/null 2>&1
+docker rm --force "${CONTAINER_NAME}" >/dev/null 2>&1
+
+docker run -d --cap-add sys_ptrace \
+  --publish 127.0.0.1:"${SSH_PORT}":22 \
+  --name "${CONTAINER_NAME}" \
+  --hostname "${CONTAINER_HOSTNAME}" \
+  "${IMAGE_NAME}" "$(cat "${SSH_KEY_FILE}")"
+
+echo "${CONTAINER_NAME}"
diff --git a/python/run.sh b/python/run.sh
index 77d036cd9..24a63c53d 100755
--- a/python/run.sh
+++ b/python/run.sh
@@ -7,7 +7,10 @@ fi
 echo "Using control libraries branch ${BRANCH}"
 
 docker pull ghcr.io/epfl-lasa/control-libraries/development-dependencies:latest
-docker build . --file ./Dockerfile.python --build-arg BRANCH="${BRANCH}" --tag control-libraries/python/test || exit 1
+docker build . --file ./Dockerfile.python \
+  --build-arg BRANCH="${BRANCH}" \
+  --target dev-user \
+  --tag control-libraries/python/test || exit 1
 
 docker run -it --rm \
   --volume "$(pwd)":/source/control-libraries/python \

From f3634ea1cb5d7c9fcc9a069c2c0d068937e5b84c Mon Sep 17 00:00:00 2001
From: domire8 <71256590+domire8@users.noreply.github.com>
Date: Fri, 17 Sep 2021 13:00:33 +0200
Subject: [PATCH 11/22] Add python bindings for parameters (#209)

* Add python bindings for parameters

* Add bindings for int and int array

* Make get_value and set_value class methods

* Add test for invalid StateType

* Fix script
---
 CHANGELOG.md                                  |   1 +
 python/dev-server.sh                          |   2 +-
 .../state_representation/bind_parameters.cpp  | 226 ++++++++++++++++++
 .../state_representation_bindings.cpp         |   1 +
 .../state_representation_bindings.h           |   3 +-
 python/test/test_parameters.py                | 176 ++++++++++++++
 6 files changed, 407 insertions(+), 2 deletions(-)
 create mode 100644 python/source/state_representation/bind_parameters.cpp
 create mode 100755 python/test/test_parameters.py

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 5d520a090..6f0f1fd6d 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -16,6 +16,7 @@ Release Versions:
 - Add method to get a joint state by name or index of the joint,
   and to get the index of the joint by its name (#210)
 - Add possibility to have a ssh server for Python testing (#211)
+- Add Python bindings for Parameter class
 
 ## 4.0.0
 
diff --git a/python/dev-server.sh b/python/dev-server.sh
index 65a0d0999..47b2b6ad7 100644
--- a/python/dev-server.sh
+++ b/python/dev-server.sh
@@ -1,6 +1,6 @@
 #!/bin/bash
 
-BASE_IMAGE=controle-libraries/remote-development:latest
+BASE_IMAGE=control-libraries/remote-development:latest
 IMAGE_NAME=control-libraries/python/remote
 CONTAINER_NAME=control-libraries-python-ssh
 CONTAINER_HOSTNAME=control-libraries-python
diff --git a/python/source/state_representation/bind_parameters.cpp b/python/source/state_representation/bind_parameters.cpp
new file mode 100644
index 000000000..4a8d6ab18
--- /dev/null
+++ b/python/source/state_representation/bind_parameters.cpp
@@ -0,0 +1,226 @@
+#include "state_representation_bindings.h"
+
+#include <state_representation/State.hpp>
+#include <state_representation/parameters/ParameterInterface.hpp>
+#include <state_representation/parameters/Parameter.hpp>
+#include <state_representation/space/cartesian/CartesianPose.hpp>
+#include <state_representation/robot/JointPositions.hpp>
+
+struct ParameterValues {
+  int int_value;
+  std::vector<int> int_array_value;
+  double double_value;
+  std::vector<double> double_array_value;
+  bool bool_value;
+  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;
+  Eigen::MatrixXd matrix_value;
+  Eigen::VectorXd vector_value;
+};
+
+class ParameterContainer : public ParameterInterface {
+public:
+  ParameterContainer(const std::string& name, const StateType& type) : ParameterInterface(type, name) {}
+
+  ParameterContainer(const std::string& name, const py::object& value, const StateType& type) :
+      ParameterInterface(type, name) {
+    set_value(value);
+  }
+
+  explicit ParameterContainer(const ParameterContainer& parameter) :
+      ParameterInterface(parameter.get_type(), parameter.get_name()) {}
+
+  void set_value(const py::object& value) {
+    switch(this->get_type()) {
+      case StateType::PARAMETER_INT:
+        values.int_value = value.cast<int>();
+        break;
+      case StateType::PARAMETER_INT_ARRAY:
+        values.int_array_value = value.cast<std::vector<int>>();
+        break;
+      case StateType::PARAMETER_DOUBLE:
+        values.double_value = value.cast<double>();
+        break;
+      case StateType::PARAMETER_DOUBLE_ARRAY:
+        values.double_array_value = value.cast<std::vector<double>>();
+        break;
+      case StateType::PARAMETER_BOOL:
+        values.bool_value = value.cast<bool>();
+        break;
+      case StateType::PARAMETER_BOOL_ARRAY:
+        values.bool_array_value = value.cast<std::vector<bool>>();
+        break;
+      case StateType::PARAMETER_STRING:
+        values.string_value = value.cast<std::string>();
+        break;
+      case StateType::PARAMETER_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>();
+        break;
+      case StateType::PARAMETER_MATRIX:
+        values.matrix_value = value.cast<Eigen::MatrixXd>();
+        break;
+      case StateType::PARAMETER_VECTOR:
+        values.vector_value = value.cast<Eigen::VectorXd>();
+        break;
+      default:
+        throw std::invalid_argument("This StateType is not a valid Parameter.");
+        break;
+    }
+  }
+
+  py::object get_value() {
+    switch (this->get_type()) {
+      case StateType::PARAMETER_INT:
+        return py::cast(values.int_value);
+      case StateType::PARAMETER_INT_ARRAY:
+        return py::cast(values.int_array_value);
+      case StateType::PARAMETER_DOUBLE:
+        return py::cast(values.double_value);
+      case StateType::PARAMETER_DOUBLE_ARRAY:
+        return py::cast(values.double_array_value);
+      case StateType::PARAMETER_BOOL:
+        return py::cast(values.bool_value);
+      case StateType::PARAMETER_BOOL_ARRAY:
+        return py::cast(values.bool_array_value);
+      case StateType::PARAMETER_STRING:
+        return py::cast(values.string_value);
+      case StateType::PARAMETER_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_MATRIX:
+        return py::cast(values.matrix_value);
+      case StateType::PARAMETER_VECTOR:
+        return py::cast(values.vector_value);
+      default:
+        return py::none();
+    }
+  }
+
+  ParameterValues values;
+};
+
+void parameter_interface(py::module_& m) {
+  py::class_<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 ParameterInterface&>(), "Copy constructor from another ParameterInterface", "parameter"_a);
+}
+
+void parameter(py::module_& m) {
+  py::class_<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 ParameterContainer&>(), "Copy constructor from another Parameter", "parameter"_a);
+
+  c.def("get_value", &ParameterContainer::get_value, "Getter of the value attribute.");
+  c.def("set_value", &ParameterContainer::set_value, "Setter of the value attribute.", py::arg("value"));
+
+  c.def("__repr__", [](const ParameterContainer& parameter) {
+    std::stringstream buffer;
+    switch (parameter.get_type()) {
+      case StateType::PARAMETER_INT: {
+        Parameter<int> param(parameter.get_name(), parameter.values.int_value);
+        buffer << param;
+        break;
+      }
+      case StateType::PARAMETER_INT_ARRAY: {
+        Parameter<std::vector<int>> param(parameter.get_name(), parameter.values.int_array_value);
+        buffer << param;
+        break;
+      }
+      case StateType::PARAMETER_DOUBLE: {
+        Parameter<double> param(parameter.get_name(), parameter.values.double_value);
+        buffer << param;
+        break;
+      }
+      case StateType::PARAMETER_DOUBLE_ARRAY: {
+        Parameter<std::vector<double>> param(parameter.get_name(), parameter.values.double_array_value);
+        buffer << param;
+        break;
+      }
+      case StateType::PARAMETER_BOOL: {
+        Parameter<bool> param(parameter.get_name(), parameter.values.bool_value);
+        buffer << param;
+        break;
+      }
+      case StateType::PARAMETER_BOOL_ARRAY: {
+        Parameter<std::vector<bool>> param(parameter.get_name(), parameter.values.bool_array_value);
+        buffer << param;
+        break;
+      }
+      case StateType::PARAMETER_STRING: {
+        Parameter<std::string> param(parameter.get_name(), parameter.values.string_value);
+        buffer << param;
+        break;
+      }
+      case StateType::PARAMETER_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;
+        break;
+      }
+      case StateType::PARAMETER_MATRIX: {
+        Parameter<Eigen::MatrixXd> param(parameter.get_name(), parameter.values.matrix_value);
+        buffer << param;
+        break;
+      }
+      case StateType::PARAMETER_VECTOR: {
+        Parameter<Eigen::VectorXd> param(parameter.get_name(), parameter.values.vector_value);
+        buffer << param;
+        break;
+      }
+      default:
+        break;
+    }
+    return buffer.str();
+  });
+}
+
+void bind_parameters(py::module_& m) {
+  parameter_interface(m);
+  parameter(m);
+}
\ No newline at end of file
diff --git a/python/source/state_representation/state_representation_bindings.cpp b/python/source/state_representation/state_representation_bindings.cpp
index 328357d3e..e67abe219 100644
--- a/python/source/state_representation/state_representation_bindings.cpp
+++ b/python/source/state_representation/state_representation_bindings.cpp
@@ -16,4 +16,5 @@ PYBIND11_MODULE(state_representation, m) {
   bind_cartesian_space(m);
   bind_joint_space(m);
   bind_jacobian(m);
+  bind_parameters(m);
 }
\ No newline at end of file
diff --git a/python/source/state_representation/state_representation_bindings.h b/python/source/state_representation/state_representation_bindings.h
index 68293d330..a2b33699d 100644
--- a/python/source/state_representation/state_representation_bindings.h
+++ b/python/source/state_representation/state_representation_bindings.h
@@ -22,4 +22,5 @@ using namespace state_representation;
 void bind_state(py::module_& m);
 void bind_cartesian_space(py::module_& m);
 void bind_joint_space(py::module_& m);
-void bind_jacobian(py::module_& m);
\ No newline at end of file
+void bind_jacobian(py::module_& m);
+void bind_parameters(py::module_& m);
\ No newline at end of file
diff --git a/python/test/test_parameters.py b/python/test/test_parameters.py
new file mode 100755
index 000000000..416ebcb4c
--- /dev/null
+++ b/python/test/test_parameters.py
@@ -0,0 +1,176 @@
+import unittest
+import state_representation as sr
+
+import numpy as np
+from numpy.testing import assert_array_equal
+
+
+class TestParameters(unittest.TestCase):
+
+    def cartesian_equal(self, cartesian1, cartesian2):
+        self.assertTrue(cartesian1.get_name(), cartesian2.get_name())
+        self.assertTrue(cartesian1.get_reference_frame(), cartesian2.get_reference_frame())
+        assert_array_equal(cartesian1.data(), cartesian2.data())
+
+    def joint_equal(self, joint1, joint2):
+        self.assertTrue(joint1.get_name(), joint2.get_name())
+        self.assertTrue(joint1.get_size(), joint2.get_size())
+        assert_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)
+
+    def test_param_int(self):
+        param = sr.Parameter("int", sr.StateType.PARAMETER_INT)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "int")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_INT)
+        param.set_value(1)
+        self.assertEqual(param.get_value(), 1)
+        param1 = sr.Parameter("int", 1, sr.StateType.PARAMETER_INT)
+        self.assertEqual(param1.get_value(), 1)
+
+    def test_param_int_array(self):
+        param = sr.Parameter("int_array", sr.StateType.PARAMETER_INT_ARRAY)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "int_array")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_INT_ARRAY)
+        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)
+        [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)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "double")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_DOUBLE)
+        param.set_value(1.5)
+        self.assertEqual(param.get_value(), 1.5)
+        param1 = sr.Parameter("double", 1.5, sr.StateType.PARAMETER_DOUBLE)
+        self.assertEqual(param1.get_value(), 1.5)
+
+    def test_param_double_array(self):
+        param = sr.Parameter("double_array", sr.StateType.PARAMETER_DOUBLE_ARRAY)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "double_array")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_DOUBLE_ARRAY)
+        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)
+        [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)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "bool")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_BOOL)
+        param.set_value(False)
+        self.assertEqual(param.get_value(), False)
+        param1 = sr.Parameter("bool", False, sr.StateType.PARAMETER_BOOL)
+        self.assertEqual(param1.get_value(), False)
+
+    def test_param_bool_array(self):
+        param = sr.Parameter("bool_array", sr.StateType.PARAMETER_BOOL_ARRAY)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "bool_array")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_BOOL_ARRAY)
+        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)
+        [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)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "string")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_STRING)
+        param.set_value("parameter")
+        self.assertEqual(param.get_value(), "parameter")
+        param1 = sr.Parameter("string", "parameter", sr.StateType.PARAMETER_STRING)
+        self.assertEqual(param1.get_value(), "parameter")
+
+    def test_param_string_array(self):
+        param = sr.Parameter("string_array", sr.StateType.PARAMETER_STRING_ARRAY)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "string_array")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_STRING_ARRAY)
+        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)
+        [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)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "cartesian_state")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_CARTESIANSTATE)
+        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)
+        self.cartesian_equal(param1.get_value(), values)
+
+    def test_param_cartesian_pose(self):
+        param = sr.Parameter("cartesian_pose", sr.StateType.PARAMETER_CARTESIANPOSE)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "cartesian_pose")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_CARTESIANPOSE)
+        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)
+        self.cartesian_equal(param1.get_value(), values)
+
+    def test_param_joint_state(self):
+        param = sr.Parameter("joint_state", sr.StateType.PARAMETER_JOINTSTATE)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "joint_state")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_JOINTSTATE)
+        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)
+        self.joint_equal(param1.get_value(), values)
+
+    def test_param_cartesian_pose(self):
+        param = sr.Parameter("joint_positions", sr.StateType.PARAMETER_JOINTPOSITIONS)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "joint_positions")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_JOINTPOSITIONS)
+        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)
+        self.joint_equal(param1.get_value(), values)
+
+    def test_param_matrix(self):
+        param = sr.Parameter("matrix", sr.StateType.PARAMETER_MATRIX)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "matrix")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_MATRIX)
+        values = np.random.rand(3, 2)
+        param.set_value(values)
+        assert_array_equal(param.get_value(), values)
+        param1 = sr.Parameter("matrix", values, sr.StateType.PARAMETER_MATRIX)
+        assert_array_equal(param1.get_value(), values)
+
+    def test_param_vector(self):
+        param = sr.Parameter("vector", sr.StateType.PARAMETER_VECTOR)
+        self.assertTrue(param.is_empty())
+        self.assertEqual(param.get_name(), "vector")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_VECTOR)
+        values = np.random.rand(3)
+        param.set_value(values)
+        assert_array_equal(param.get_value(), values)
+        param1 = sr.Parameter("vector", values, sr.StateType.PARAMETER_VECTOR)
+        assert_array_equal(param1.get_value(), values)
+
+
+if __name__ == '__main__':
+    unittest.main()

From b99eaa51da80bab9e778822ab2d3e03ee3d4fe76 Mon Sep 17 00:00:00 2001
From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>
Date: Mon, 20 Sep 2021 15:58:19 +0200
Subject: [PATCH 12/22] Python clproto encode / decode for parameters (#214)

* Bindings for clproto parameters

* Add binding code specifically for encoding
and decoding Parameter message types using the intermediate
ParameterContainer object

* Split parameter container declarations into
a header file for re-usability

* Move headers to a dedicated include folder so that
the resources can be shared between modules

* Rework structure to fix decoding

* Construct intermediate Parameter before returning a py::object
ParameterContainer to avoid issues of object scope and
ownership

* Move ParameterContainer sources to a common directory
so the source can be compiled in both modules

* Add tests

* Update changelog and fix prior changelog entry 209

Co-authored-by: domire8 <71256590+domire8@users.noreply.github.com>
---
 CHANGELOG.md                                  |   3 +-
 .../clproto => include}/clproto_bindings.h    |   3 +-
 python/include/parameter_container.h          |  39 ++++++
 .../state_representation_bindings.h           |   0
 python/setup.py                               |   9 +-
 python/source/clproto/bind_clproto.cpp        | 117 ++++++++++++++---
 python/source/clproto/clproto_bindings.cpp    |   1 +
 python/source/common/parameter_container.cpp  |  97 ++++++++++++++
 .../state_representation/bind_parameters.cpp  | 124 +-----------------
 python/test/test_clproto.py                   |  67 ++++++++++
 python/test/test_parameters.py                |  10 ++
 11 files changed, 324 insertions(+), 146 deletions(-)
 rename python/{source/clproto => include}/clproto_bindings.h (81%)
 create mode 100644 python/include/parameter_container.h
 rename python/{source/state_representation => include}/state_representation_bindings.h (100%)
 create mode 100644 python/source/common/parameter_container.cpp

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 6f0f1fd6d..7c195f04f 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -16,7 +16,8 @@ Release Versions:
 - Add method to get a joint state by name or index of the joint,
   and to get the index of the joint by its name (#210)
 - Add possibility to have a ssh server for Python testing (#211)
-- Add Python bindings for Parameter class
+- Add Python bindings for Parameter class (#209)
+- Add Python bindings for clproto encode / decode of Parameter class (#214)
 
 ## 4.0.0
 
diff --git a/python/source/clproto/clproto_bindings.h b/python/include/clproto_bindings.h
similarity index 81%
rename from python/source/clproto/clproto_bindings.h
rename to python/include/clproto_bindings.h
index f1ae0df2d..99cebe31f 100644
--- a/python/source/clproto/clproto_bindings.h
+++ b/python/include/clproto_bindings.h
@@ -18,4 +18,5 @@
 namespace py = pybind11;
 using namespace pybind11::literals;
 
-void bind_clproto(py::module_& m);
\ No newline at end of file
+void bind_clproto(py::module_& m);
+void bind_clproto_parameters(py::module_& m);
\ No newline at end of file
diff --git a/python/include/parameter_container.h b/python/include/parameter_container.h
new file mode 100644
index 000000000..55ce44be2
--- /dev/null
+++ b/python/include/parameter_container.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include "state_representation_bindings.h"
+
+#include <state_representation/State.hpp>
+#include <state_representation/parameters/ParameterInterface.hpp>
+#include <state_representation/parameters/Parameter.hpp>
+#include <state_representation/space/cartesian/CartesianPose.hpp>
+#include <state_representation/robot/JointPositions.hpp>
+
+struct ParameterValues {
+  int int_value;
+  std::vector<int> int_array_value;
+  double double_value;
+  std::vector<double> double_array_value;
+  bool bool_value;
+  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;
+  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 ParameterContainer& parameter);
+
+  void set_value(const py::object& value);
+
+  py::object get_value();
+
+  ParameterValues values;
+};
diff --git a/python/source/state_representation/state_representation_bindings.h b/python/include/state_representation_bindings.h
similarity index 100%
rename from python/source/state_representation/state_representation_bindings.h
rename to python/include/state_representation_bindings.h
diff --git a/python/setup.py b/python/setup.py
index 2ef32a71b..333f54ba3 100644
--- a/python/setup.py
+++ b/python/setup.py
@@ -7,7 +7,7 @@
 
 __version__ = "4.0.0"
 __libraries__ = ['state_representation', 'clproto']
-__include_dirs__ = []
+__include_dirs__ = ['include']
 
 __install_clproto_module__ = True
 
@@ -31,12 +31,13 @@
                 raise Exception(msg)
 except Exception as e:
     msg = f'Error with control library dependencies: {e.args[0]}. Ensure the control libraries are properly installed.'
+    warnings.warn(msg)
 
 ParallelCompile("NPY_NUM_BUILD_JOBS", needs_recompile=naive_recompile).install()
 
 ext_modules = [
     Pybind11Extension("state_representation",
-                      sorted(glob("source/state_representation/*.cpp")),
+                      sorted(glob("source/state_representation/*.cpp") + glob("source/common/*.cpp")),
                       cxx_std=17,
                       include_dirs=__include_dirs__,
                       libraries=['state_representation'],
@@ -47,10 +48,10 @@
 if __install_clproto_module__:
     ext_modules.append(
         Pybind11Extension("clproto",
-                          sorted(glob("source/clproto/*.cpp")),
+                          sorted(glob("source/clproto/*.cpp") + glob("source/common/*.cpp")),
                           cxx_std=17,
                           include_dirs=__include_dirs__,
-                          libraries=['clproto'],
+                          libraries=['state_representation', 'clproto'],
                           define_macros=[('MODULE_VERSION_INFO', __version__)],
                           )
     )
diff --git a/python/source/clproto/bind_clproto.cpp b/python/source/clproto/bind_clproto.cpp
index bc2089afe..2c74361d3 100644
--- a/python/source/clproto/bind_clproto.cpp
+++ b/python/source/clproto/bind_clproto.cpp
@@ -1,4 +1,5 @@
 #include "clproto_bindings.h"
+#include "parameter_container.h"
 
 #include <string>
 
@@ -25,6 +26,99 @@ inline py::bytes encode_bytes(const T& object) {
   return py::bytes(encode(object));
 }
 
+py::bytes encode_parameter_container(const ParameterContainer& container) {
+  switch (container.get_type()) {
+    case StateType::PARAMETER_INT:
+      return py::bytes(encode(Parameter(container.get_name(), container.values.int_value)));
+    case StateType::PARAMETER_INT_ARRAY:
+      return py::bytes(encode(Parameter(container.get_name(), container.values.int_array_value)));
+    case StateType::PARAMETER_DOUBLE:
+      return py::bytes(encode(Parameter(container.get_name(), container.values.double_value)));
+    case StateType::PARAMETER_DOUBLE_ARRAY:
+      return py::bytes(encode(Parameter(container.get_name(), container.values.double_array_value)));
+    case StateType::PARAMETER_BOOL:
+      return py::bytes(encode(Parameter(container.get_name(), container.values.bool_value)));
+    case StateType::PARAMETER_BOOL_ARRAY:
+      return py::bytes(encode(Parameter(container.get_name(), container.values.bool_array_value)));
+    case StateType::PARAMETER_STRING:
+      return py::bytes(encode(Parameter(container.get_name(), container.values.string_value)));
+    case StateType::PARAMETER_STRING_ARRAY:
+      return py::bytes(encode(Parameter(container.get_name(), container.values.string_array_value)));
+    case StateType::PARAMETER_MATRIX:
+      return py::bytes(encode(Parameter(container.get_name(), container.values.matrix_value)));
+    case StateType::PARAMETER_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.");
+      break;
+  }
+}
+
+py::object decode_parameter(const std::string& msg) {
+  py::object PyParameter = py::module_::import("state_representation").attr("Parameter");
+  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());
+    }
+    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());
+    }
+    case ParameterMessageType::DOUBLE: {
+      auto param = decode<Parameter<double>>(msg);
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_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());
+    }
+    case ParameterMessageType::BOOL: {
+      auto param = decode<Parameter<bool>>(msg);
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_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());
+    }
+    case ParameterMessageType::STRING: {
+      auto param = decode<Parameter<std::string>>(msg);
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_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());
+    }
+    case ParameterMessageType::VECTOR: {
+      auto param = decode<Parameter<Eigen::VectorXd>>(msg);
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type());
+    }
+    case ParameterMessageType::MATRIX: {
+      auto param = decode<Parameter<Eigen::MatrixXd>>(msg);
+      return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type());
+    }
+    default:
+      throw std::invalid_argument("The message is not a valid encoded Parameter.");
+      break;
+  }
+}
+
+void parameter_message_type(py::module_& m) {
+  py::enum_<ParameterMessageType>(m, "ParameterMessageType")
+  .value("UNKNOWN_PARAMETER", ParameterMessageType::UNKNOWN_PARAMETER)
+  .value("INT", ParameterMessageType::INT)
+  .value("INT_ARRAY", ParameterMessageType::INT_ARRAY)
+  .value("DOUBLE", ParameterMessageType::DOUBLE)
+  .value("DOUBLE_ARRAY", ParameterMessageType::DOUBLE_ARRAY)
+  .value("BOOL", ParameterMessageType::BOOL)
+  .value("BOOL_ARRAY", ParameterMessageType::BOOL_ARRAY)
+  .value("STRING", ParameterMessageType::STRING)
+  .value("STRING_ARRAY", ParameterMessageType::STRING_ARRAY)
+  .value("MATRIX", ParameterMessageType::MATRIX)
+  .value("VECTOR", ParameterMessageType::VECTOR)
+  .export_values();
+}
+
 void message_type(py::module_& m) {
   py::enum_<MessageType>(m, "MessageType")
       .value("UNKNOWN_MESSAGE", MessageType::UNKNOWN_MESSAGE)
@@ -47,22 +141,6 @@ void message_type(py::module_& m) {
       .export_values();
 }
 
-void parameter_message_type(py::module_& m) {
-  py::enum_<ParameterMessageType>(m, "ParameterMessageType")
-      .value("UNKNOWN_PARAMETER", ParameterMessageType::UNKNOWN_PARAMETER)
-      .value("INT", ParameterMessageType::INT)
-      .value("INT_ARRAY", ParameterMessageType::INT_ARRAY)
-      .value("DOUBLE", ParameterMessageType::DOUBLE)
-      .value("DOUBLE_ARRAY", ParameterMessageType::DOUBLE_ARRAY)
-      .value("BOOL", ParameterMessageType::BOOL)
-      .value("BOOL_ARRAY", ParameterMessageType::BOOL_ARRAY)
-      .value("STRING", ParameterMessageType::STRING)
-      .value("STRING_ARRAY", ParameterMessageType::STRING_ARRAY)
-      .value("MATRIX", ParameterMessageType::MATRIX)
-      .value("VECTOR", ParameterMessageType::VECTOR)
-      .export_values();
-}
-
 void methods(py::module_& m) {
   m.def("is_valid", &is_valid, "Check if a serialized binary string can be decoded into a support control libraries message type.", "msg"_a);
   m.def("check_message_type", &check_message_type, "Check which control libraries message type a serialized binary string can be decoded as, if at all.", "msg"_a);
@@ -111,8 +189,11 @@ void methods(py::module_& m) {
         return encode_bytes<JointAccelerations>(object.cast<JointAccelerations>());
       case MessageType::JOINT_TORQUES_MESSAGE:
         return encode_bytes<JointTorques>(object.cast<JointTorques>());
+      case MessageType::PARAMETER_MESSAGE:
+        return encode_parameter_container(object.cast<ParameterContainer>());
       default:
-        return py::bytes();
+        throw std::invalid_argument("The message is not a valid encoded StateMessage.");
+        break;
     }
   }, "Encode a control libraries object into a serialized binary string representation (wire format).", py::arg("object"), py::arg("type"));
 
@@ -142,6 +223,8 @@ void methods(py::module_& m) {
         return py::cast(decode<JointAccelerations>(msg));
       case MessageType::JOINT_TORQUES_MESSAGE:
         return py::cast(decode<JointTorques>(msg));
+      case MessageType::PARAMETER_MESSAGE:
+        return decode_parameter(msg);
       default:
         return py::none();
     }
diff --git a/python/source/clproto/clproto_bindings.cpp b/python/source/clproto/clproto_bindings.cpp
index eeb526cd6..495c73326 100644
--- a/python/source/clproto/clproto_bindings.cpp
+++ b/python/source/clproto/clproto_bindings.cpp
@@ -13,5 +13,6 @@ m.attr("__version__") = "dev";
 #endif
 
 py::module_::import("state_representation");
+
 bind_clproto(m);
 }
\ No newline at end of file
diff --git a/python/source/common/parameter_container.cpp b/python/source/common/parameter_container.cpp
new file mode 100644
index 000000000..3506debc8
--- /dev/null
+++ b/python/source/common/parameter_container.cpp
@@ -0,0 +1,97 @@
+#include "parameter_container.h"
+
+ParameterContainer::ParameterContainer(const std::string& name, const StateType& type) :
+    ParameterInterface(type, name) {}
+
+ParameterContainer::ParameterContainer(const std::string& name, const py::object& value, const StateType& type) :
+    ParameterInterface(type, name) {
+  set_value(value);
+}
+
+ParameterContainer::ParameterContainer(const ParameterContainer& parameter) :
+    ParameterInterface(parameter.get_type(), parameter.get_name()), values(parameter.values) {}
+
+void ParameterContainer::set_value(const py::object& value) {
+  switch (this->get_type()) {
+    case StateType::PARAMETER_INT:
+      values.int_value = value.cast<int>();
+      break;
+    case StateType::PARAMETER_INT_ARRAY:
+      values.int_array_value = value.cast < std::vector < int >> ();
+      break;
+    case StateType::PARAMETER_DOUBLE:
+      values.double_value = value.cast<double>();
+      break;
+    case StateType::PARAMETER_DOUBLE_ARRAY:
+      values.double_array_value = value.cast < std::vector < double >> ();
+      break;
+    case StateType::PARAMETER_BOOL:
+      values.bool_value = value.cast<bool>();
+      break;
+    case StateType::PARAMETER_BOOL_ARRAY:
+      values.bool_array_value = value.cast < std::vector < bool >> ();
+      break;
+    case StateType::PARAMETER_STRING:
+      values.string_value = value.cast<std::string>();
+      break;
+    case StateType::PARAMETER_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>();
+      break;
+    case StateType::PARAMETER_MATRIX:
+      values.matrix_value = value.cast<Eigen::MatrixXd>();
+      break;
+    case StateType::PARAMETER_VECTOR:
+      values.vector_value = value.cast<Eigen::VectorXd>();
+      break;
+    default:
+      throw std::invalid_argument("This StateType is not a valid Parameter.");
+      break;
+  }
+}
+
+py::object ParameterContainer::get_value() {
+  switch (this->get_type()) {
+    case StateType::PARAMETER_INT:
+      return py::cast(values.int_value);
+    case StateType::PARAMETER_INT_ARRAY:
+      return py::cast(values.int_array_value);
+    case StateType::PARAMETER_DOUBLE:
+      return py::cast(values.double_value);
+    case StateType::PARAMETER_DOUBLE_ARRAY:
+      return py::cast(values.double_array_value);
+    case StateType::PARAMETER_BOOL:
+      return py::cast(values.bool_value);
+    case StateType::PARAMETER_BOOL_ARRAY:
+      return py::cast(values.bool_array_value);
+    case StateType::PARAMETER_STRING:
+      return py::cast(values.string_value);
+    case StateType::PARAMETER_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_MATRIX:
+      return py::cast(values.matrix_value);
+    case StateType::PARAMETER_VECTOR:
+      return py::cast(values.vector_value);
+    default:
+      return py::none();
+  }
+}
\ No newline at end of file
diff --git a/python/source/state_representation/bind_parameters.cpp b/python/source/state_representation/bind_parameters.cpp
index 4a8d6ab18..0c2a58faa 100644
--- a/python/source/state_representation/bind_parameters.cpp
+++ b/python/source/state_representation/bind_parameters.cpp
@@ -1,127 +1,5 @@
 #include "state_representation_bindings.h"
-
-#include <state_representation/State.hpp>
-#include <state_representation/parameters/ParameterInterface.hpp>
-#include <state_representation/parameters/Parameter.hpp>
-#include <state_representation/space/cartesian/CartesianPose.hpp>
-#include <state_representation/robot/JointPositions.hpp>
-
-struct ParameterValues {
-  int int_value;
-  std::vector<int> int_array_value;
-  double double_value;
-  std::vector<double> double_array_value;
-  bool bool_value;
-  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;
-  Eigen::MatrixXd matrix_value;
-  Eigen::VectorXd vector_value;
-};
-
-class ParameterContainer : public ParameterInterface {
-public:
-  ParameterContainer(const std::string& name, const StateType& type) : ParameterInterface(type, name) {}
-
-  ParameterContainer(const std::string& name, const py::object& value, const StateType& type) :
-      ParameterInterface(type, name) {
-    set_value(value);
-  }
-
-  explicit ParameterContainer(const ParameterContainer& parameter) :
-      ParameterInterface(parameter.get_type(), parameter.get_name()) {}
-
-  void set_value(const py::object& value) {
-    switch(this->get_type()) {
-      case StateType::PARAMETER_INT:
-        values.int_value = value.cast<int>();
-        break;
-      case StateType::PARAMETER_INT_ARRAY:
-        values.int_array_value = value.cast<std::vector<int>>();
-        break;
-      case StateType::PARAMETER_DOUBLE:
-        values.double_value = value.cast<double>();
-        break;
-      case StateType::PARAMETER_DOUBLE_ARRAY:
-        values.double_array_value = value.cast<std::vector<double>>();
-        break;
-      case StateType::PARAMETER_BOOL:
-        values.bool_value = value.cast<bool>();
-        break;
-      case StateType::PARAMETER_BOOL_ARRAY:
-        values.bool_array_value = value.cast<std::vector<bool>>();
-        break;
-      case StateType::PARAMETER_STRING:
-        values.string_value = value.cast<std::string>();
-        break;
-      case StateType::PARAMETER_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>();
-        break;
-      case StateType::PARAMETER_MATRIX:
-        values.matrix_value = value.cast<Eigen::MatrixXd>();
-        break;
-      case StateType::PARAMETER_VECTOR:
-        values.vector_value = value.cast<Eigen::VectorXd>();
-        break;
-      default:
-        throw std::invalid_argument("This StateType is not a valid Parameter.");
-        break;
-    }
-  }
-
-  py::object get_value() {
-    switch (this->get_type()) {
-      case StateType::PARAMETER_INT:
-        return py::cast(values.int_value);
-      case StateType::PARAMETER_INT_ARRAY:
-        return py::cast(values.int_array_value);
-      case StateType::PARAMETER_DOUBLE:
-        return py::cast(values.double_value);
-      case StateType::PARAMETER_DOUBLE_ARRAY:
-        return py::cast(values.double_array_value);
-      case StateType::PARAMETER_BOOL:
-        return py::cast(values.bool_value);
-      case StateType::PARAMETER_BOOL_ARRAY:
-        return py::cast(values.bool_array_value);
-      case StateType::PARAMETER_STRING:
-        return py::cast(values.string_value);
-      case StateType::PARAMETER_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_MATRIX:
-        return py::cast(values.matrix_value);
-      case StateType::PARAMETER_VECTOR:
-        return py::cast(values.vector_value);
-      default:
-        return py::none();
-    }
-  }
-
-  ParameterValues values;
-};
+#include "parameter_container.h"
 
 void parameter_interface(py::module_& m) {
   py::class_<ParameterInterface, State> c(m, "ParameterInterface");
diff --git a/python/test/test_clproto.py b/python/test/test_clproto.py
index 67011ed16..b1dafa672 100755
--- a/python/test/test_clproto.py
+++ b/python/test/test_clproto.py
@@ -2,6 +2,9 @@
 import state_representation as sr
 import clproto
 
+import numpy as np
+from numpy.testing import assert_array_equal
+
 class TestClprotoPackUnpack(unittest.TestCase):
     def test_pack_unpack(self):
         objects = [sr.CartesianState.Random("A", "B"), sr.JointState.Random("robot", ["j1", "j2", "j3"])]
@@ -125,5 +128,69 @@ def test_encode_decode_jacobian(self):
         self.assertEqual(obj1.cols(), obj2.cols())
         [self.assertAlmostEqual(x, y) for x, y in zip(obj1.data().flatten(), obj2.data().flatten())]
 
+class TestClprotoParameters(unittest.TestCase):
+    def param_encode_decode_tester(self, obj1, message_type):
+        msg = clproto.encode(obj1, clproto.MessageType.PARAMETER_MESSAGE)
+        self.assertTrue(clproto.is_valid(msg))
+        self.assertEqual(clproto.check_message_type(msg), clproto.MessageType.PARAMETER_MESSAGE)
+        self.assertEqual(clproto.check_parameter_message_type(msg), message_type)
+        obj2 = clproto.decode(msg)
+        self.assertEqual(obj1.get_name(), obj2.get_name())
+        self.assertEqual(obj1.get_type(), obj2.get_type())
+        if isinstance(obj1, list):
+            self.assertCountEqual(obj1.get_value(), obj2.get_value())
+            for index in range(len(obj1.get_value())):
+                self.assertEqual(type(obj1.get_value()[index]), type(obj2.get_value()[index]))
+                self.assertEqual(obj1.get_value()[index], obj2.get_value()[index])
+        elif isinstance(obj1.get_value(), np.ndarray):
+            assert_array_equal(obj1.get_value(), obj2.get_value())
+        else:
+            self.assertEqual(type(obj1.get_value()), type(obj2.get_value()))
+            self.assertEqual(obj1.get_value(), obj2.get_value())
+
+    def test_encode_decode_param_int(self):
+        obj1 = sr.Parameter("int", 1, sr.StateType.PARAMETER_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)
+        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)
+        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)
+        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)
+        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)
+        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)
+        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)
+        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)
+        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)
+        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)
+        self.assertRaises(ValueError, clproto.encode, obj, clproto.MessageType.PARAMETER_MESSAGE)
+
 if __name__ == '__main__':
     unittest.main()
\ No newline at end of file
diff --git a/python/test/test_parameters.py b/python/test/test_parameters.py
index 416ebcb4c..1978c38ac 100755
--- a/python/test/test_parameters.py
+++ b/python/test/test_parameters.py
@@ -21,6 +21,16 @@ def test_param_invalid(self):
         param = sr.Parameter("test", sr.StateType.CARTESIANSTATE)
         self.assertRaises(ValueError, param.set_value, 1)
 
+    def test_param_copy(self):
+        param = sr.Parameter("test", 1, sr.StateType.PARAMETER_INT)
+        self.assertEqual(param.get_name(), "test")
+        self.assertEqual(param.get_type(), sr.StateType.PARAMETER_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_value(), 1)
+
     def test_param_int(self):
         param = sr.Parameter("int", sr.StateType.PARAMETER_INT)
         self.assertTrue(param.is_empty())

From d210ed82b4d99efae5fae5ce44769e2575e5f37d Mon Sep 17 00:00:00 2001
From: domire8 <71256590+domire8@users.noreply.github.com>
Date: Mon, 20 Sep 2021 16:50:55 +0200
Subject: [PATCH 13/22] Refactor CartesianState tests (#213)

* Everything tested except operators

* Everything tested except inverse

* Add clamping tests

* Improve CartesianState tests

* Improve tests

* Add bindings for division operator

* Add CartesianState tests

* Use numpy testing

* Test copy constructors

* Update CHANGELOG

* Add rebuild option to scripts

* Update CHANGELOG
---
 CHANGELOG.md                                  |   6 +
 python/dev-server.sh                          |   6 +-
 python/run.sh                                 |  16 +-
 .../bind_cartesian_space.cpp                  |  10 +-
 python/test/space/test_cartesian_state.py     | 437 +++++++++++++++--
 python/test/space/test_spatial_state.py       |   6 +
 .../space/cartesian/test_cartesian_state.cpp  | 462 ++++++++++++++----
 7 files changed, 793 insertions(+), 150 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 7c195f04f..8b3a0122c 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -18,6 +18,12 @@ Release Versions:
 - Add possibility to have a ssh server for Python testing (#211)
 - Add Python bindings for Parameter class (#209)
 - Add Python bindings for clproto encode / decode of Parameter class (#214)
+- Improve CartesianState tests in C++ and Python (#213)
+
+### Pending TODOs for the next release
+
+- Including a quaternion extension (numpy-quaternion, pyquaternion, ...)
+  for binding the Eigen::Quaternion to a specific object in Python.
 
 ## 4.0.0
 
diff --git a/python/dev-server.sh b/python/dev-server.sh
index 47b2b6ad7..7038f2deb 100644
--- a/python/dev-server.sh
+++ b/python/dev-server.sh
@@ -45,9 +45,11 @@ in the 'source' directory."
 
 docker image inspect "${BASE_IMAGE}" >/dev/null 2>&1 || image_not_found
 
+BUILD_ARGS=(--build-arg BASE_IMAGE=control-libraries/remote-development:latest)
 while [ "$#" -gt 0 ]; do
   case "$1" in
     -b|--branch) BRANCH=$2; shift 2;;
+    -r|--rebuild) BUILD_ARGS+=(--no-cache); shift 1;;
     -p|--port) SSH_PORT=$2; shift 2;;
     -k|--key-file) SSH_KEY_FILE=$2; shift 2;;
     -h|--help) echo "${HELP_MESSAGE}"; exit 0;;
@@ -56,10 +58,10 @@ while [ "$#" -gt 0 ]; do
 done
 
 echo "Using control libraries branch ${BRANCH}"
+BUILD_ARGS+=(--build-arg BRANCH="${BRANCH}")
 
 DOCKER_BUILDKIT=1 docker build . --file ./Dockerfile.python \
-  --build-arg BASE_IMAGE=control-libraries/remote-development:latest \
-  --build-arg BRANCH="${BRANCH}" \
+  "${BUILD_ARGS[@]}" \
   --target remote-user \
   --tag "${IMAGE_NAME}" || exit 1
 
diff --git a/python/run.sh b/python/run.sh
index 24a63c53d..06bc7ced2 100755
--- a/python/run.sh
+++ b/python/run.sh
@@ -1,14 +1,22 @@
 #!/bin/sh
 
 BRANCH=$(git branch --show-current)
-if [ -n "$1" ]; then
-  BRANCH=$1
-fi
+
+BUILD_ARGS=()
+while [ "$#" -gt 0 ]; do
+  case "$1" in
+    -b|--branch) BRANCH=$2; shift 2;;
+    -r|--rebuild) BUILD_ARGS+=(--no-cache); shift 1;;
+    *) echo "Unknown option: $1" >&2; exit 1;;
+  esac
+done
+
 echo "Using control libraries branch ${BRANCH}"
+BUILD_ARGS+=(--build-arg BRANCH="${BRANCH}")
 
 docker pull ghcr.io/epfl-lasa/control-libraries/development-dependencies:latest
 docker build . --file ./Dockerfile.python \
-  --build-arg BRANCH="${BRANCH}" \
+  "${BUILD_ARGS[@]}" \
   --target dev-user \
   --tag control-libraries/python/test || exit 1
 
diff --git a/python/source/state_representation/bind_cartesian_space.cpp b/python/source/state_representation/bind_cartesian_space.cpp
index 3a9dc99db..301395763 100644
--- a/python/source/state_representation/bind_cartesian_space.cpp
+++ b/python/source/state_representation/bind_cartesian_space.cpp
@@ -105,6 +105,8 @@ void cartesian_state(py::module_& m) {
   c.def(py::self *= double());
   c.def(py::self * double());
   c.def(double() * py::self);
+  c.def(py::self /= double());
+  c.def(py::self / double());
   c.def(py::self += py::self);
   c.def(py::self + py::self);
   c.def(py::self -= py::self);
@@ -174,6 +176,8 @@ void cartesian_pose(py::module_& m) {
   c.def(py::self *= double());
   c.def(py::self * double());
   c.def(double() * py::self);
+  c.def(py::self /= double());
+  c.def(py::self / double());
 
   c.def(py::self += py::self);
   c.def(py::self + py::self);
@@ -237,6 +241,8 @@ void cartesian_twist(py::module_& m) {
 
   c.def(py::self *= double());
   c.def(py::self * double());
+  c.def(py::self /= double());
+  c.def(py::self / double());
   c.def(py::self *= Eigen::Matrix<double, 6, 6>());
   c.def(py::self * std::chrono::nanoseconds());
 
@@ -302,6 +308,8 @@ void cartesian_wrench(py::module_& m) {
   c.def(py::self *= double());
   c.def(py::self * double());
   c.def(double() * py::self);
+  c.def(py::self /= double());
+  c.def(py::self / double());
 
   c.def("clamp", &CartesianWrench::clamp, "Clamp inplace the magnitude of the wrench to the values in argument", "max_force"_a, "max_torque"_a, "force_noise_ratio"_a=0, "torque_noise_ratio"_a=0);
   c.def("clamped", &CartesianWrench::clamped, "Return the clamped wrench", "max_force"_a, "max_torque"_a, "force_noise_ratio"_a=0, "torque_noise_ratio"_a=0);
@@ -327,4 +335,4 @@ void bind_cartesian_space(py::module_& m) {
   cartesian_pose(m);
   cartesian_twist(m);
   cartesian_wrench(m);
-}
\ No newline at end of file
+}
diff --git a/python/test/space/test_cartesian_state.py b/python/test/space/test_cartesian_state.py
index 15dca2d0d..6df30d34a 100755
--- a/python/test/space/test_cartesian_state.py
+++ b/python/test/space/test_cartesian_state.py
@@ -1,6 +1,11 @@
 import unittest
-from state_representation import CartesianState, dist
+
 import numpy as np
+from numpy.testing import assert_array_equal, assert_array_almost_equal
+from state_representation import State, CartesianState, StateType, CartesianStateVariable
+
+from .test_spatial_state import SPATIAL_STATE_METHOD_EXPECTS
+from ..test_state import STATE_METHOD_EXPECTS
 
 CARTESIAN_STATE_METHOD_EXPECTS = [
     'Identity',
@@ -57,66 +62,392 @@
     'to_list'
 ]
 
+
 class TestCartesianState(unittest.TestCase):
-    def assert_np_array_equal(self, a, b):
-        self.assertListEqual(list(a), list(b))
+    def assert_name_empty_frame_equal(self, state, name, empty, reference_frame):
+        self.assertEqual(state.get_name(), name)
+        self.assertEqual(state.is_empty(), empty)
+        self.assertEqual(state.get_reference_frame(), reference_frame)
+
+    def assert_name_frame_data_equal(self, state1, state2):
+        self.assertEqual(state1.get_name(), state2.get_name())
+        self.assertEqual(state1.get_reference_frame(), state2.get_reference_frame())
+        assert_array_almost_equal(state1.data(), state2.data())
 
-    def assert_np_array_almost_equal(self, a, b):
-        [self.assertAlmostEqual(x, y) for x, y in zip(list(a),list(b))]
+    def clamping_helper(self, state, getter, setter, variable, dim):
+        if dim == 3:
+            data = [-2.0, 1.0, 5.0]
+        elif dim == 6:
+            data = [-2.0, 1.0, 5.0, 1.0, -3.0, 2.4]
+        else:
+            raise ValueError("Provide 3 or 6 as dimension")
+
+        setter(data)
+        state.clamp_state_variable(10.0, variable)
+        self.assertEqual(type(state), CartesianState)
+        [self.assertAlmostEqual(getter()[i], data[i]) for i in range(dim)]
+        state.clamp_state_variable(3.0, variable)
+        self.assertAlmostEqual(np.linalg.norm(getter()), 3.0)
+        state.clamp_state_variable(10.0, variable, 0.5)
+        self.assertAlmostEqual(np.linalg.norm(getter()), 0.0)
 
     def test_callable_methods(self):
         methods = [m for m in dir(CartesianState) if callable(getattr(CartesianState, m))]
+        for expected in STATE_METHOD_EXPECTS:
+            self.assertIn(expected, methods)
+        for expected in SPATIAL_STATE_METHOD_EXPECTS:
+            self.assertIn(expected, methods)
+        for expected in CARTESIAN_STATE_METHOD_EXPECTS:
+            self.assertIn(expected, methods)
         for expected in CARTESIAN_STATE_METHOD_EXPECTS:
             self.assertIn(expected, methods)
 
     def test_constructors(self):
-        CartesianState()
-        A = CartesianState("A")
-        CartesianState(A)
-        CartesianState("B", "C")
-        A = CartesianState.Identity("A")
-        B = CartesianState.Random("B")
-
-    def test_setters(self):
-        A = CartesianState.Identity("A")
-
-        A.set_position(1, 2, 3)
-        self.assert_np_array_equal(A.get_position(), [1, 2, 3])
-        A.set_position([1, 2, 3])
-        self.assert_np_array_equal(A.get_position(), [1, 2, 3])
-        A.set_position(np.array([1, 2, 3]))
-        self.assert_np_array_equal(A.get_position(), [1, 2, 3])
-
-        A.set_orientation([1, 2, 3, 4])
-        self.assert_np_array_equal(A.get_orientation(), [1, 2, 3, 4] / np.linalg.norm([1, 2, 3, 4]))
-
-        A.set_twist([1, 2, 3, 4, 5, 6])
-        self.assert_np_array_equal(A.get_twist(), [1, 2, 3, 4, 5, 6])
-        A.set_accelerations([1, 2, 3, 4, 5, 6])
-        self.assert_np_array_equal(A.get_accelerations(), [1, 2, 3, 4, 5, 6])
-        A.set_wrench([1, 2, 3, 4, 5, 6])
-        self.assert_np_array_equal(A.get_wrench(), [1, 2, 3, 4, 5, 6])
-
-        data = [i for i in range(25)]
-        data[3:7] = A.get_orientation().tolist()
-        A.set_data(data)
-        self.assert_np_array_almost_equal(A.data(), data)
-
-    def test_operators(self):
-        A = CartesianState.Random("A")
-        B = CartesianState.Random("B")
-        CinA = CartesianState.Random("C", "A")
-
-        C = A * CinA
-        A = A * 2
-        A = 2 * A
-        A *= 2
-
-        A = A + B
-        A += B
-
-        A = A - B
-        A -= B
+        empty1 = CartesianState()
+        self.assertTrue(isinstance(empty1, State))
+        self.assertEqual(type(empty1), CartesianState)
+        self.assertEqual(empty1.get_type(), StateType.CARTESIANSTATE)
+        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.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.assert_name_empty_frame_equal(empty3, "test", True, "reference")
+        self.assertAlmostEqual(np.linalg.norm(empty3.data()), 1)
+
+    def test_identity_initialization(self):
+        identity = CartesianState().Identity("test")
+        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()[0], 1)
+        self.assertAlmostEqual(np.linalg.norm(identity.get_twist()), 0)
+        self.assertAlmostEqual(np.linalg.norm(identity.get_accelerations()), 0)
+        self.assertAlmostEqual(np.linalg.norm(identity.get_wrench()), 0)
+
+    def test_random_initialization(self):
+        random = CartesianState().Random("test")
+        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.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_accelerations()) > 0)
+        self.assertTrue(np.linalg.norm(random.get_wrench()) > 0)
+
+    def test_copy_constructor(self):
+        random = CartesianState().Random("test")
+        copy1 = random
+        self.assert_name_frame_data_equal(random, copy1)
+
+        copy2 = random.copy()
+        self.assert_name_frame_data_equal(random, copy2)
+
+        copy3 = CartesianState(random)
+        self.assert_name_frame_data_equal(random, copy3)
+
+        empty = CartesianState()
+        copy4 = empty
+        self.assertTrue(copy4.is_empty())
+        copy5 = CartesianState(empty)
+        self.assertTrue(copy5.is_empty())
+        copy6 = empty.copy()
+        self.assertTrue(copy6.is_empty())
+
+    def test_get_set_fields(self):
+        cs = CartesianState("test")
+
+        # name
+        cs.set_name("robot")
+        self.assertEqual(cs.get_name(), "robot")
+        self.assertEqual(cs.get_reference_frame(), "world")
+        cs.set_reference_frame("base")
+        self.assertEqual(cs.get_reference_frame(), "base")
+
+        # position
+        position = [1., 2., 3.]
+        cs.set_position(position)
+        [self.assertAlmostEqual(cs.get_position()[i], position[i]) for i in range(3)]
+        cs.set_position(1.1, 2.2, 3.3)
+        assert_array_equal(np.array([1.1, 2.2, 3.3]), cs.get_position())
+
+        # orientation
+        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)
+
+        matrix = cs.get_transformation_matrix()
+        trans = matrix[:3, 3]
+        # rot = matrix[:3, :3]
+        bottom = matrix[3, :]
+        assert_array_almost_equal(trans, cs.get_position())
+        # TODO rotation matrix from quaternion
+        assert_array_equal(bottom, np.array([0, 0, 0, 1]))
+
+        # pose
+        position = [4.4, 5.5, 6.6]
+        orientation_vec = np.random.rand(4)
+        orientation_vec = orientation_vec / np.linalg.norm(orientation_vec)
+        cs.set_pose(np.hstack((position, orientation_vec)))
+        assert_array_almost_equal(np.hstack((position, orientation_vec)), cs.get_pose())
+        with self.assertRaises(RuntimeError):
+            cs.set_pose(position)
+
+        # twist
+        linear_velocity = np.random.rand(3)
+        cs.set_linear_velocity(linear_velocity)
+        assert_array_almost_equal(cs.get_linear_velocity(), linear_velocity)
+        angular_velocity = np.random.rand(3)
+        cs.set_angular_velocity(angular_velocity)
+        assert_array_almost_equal(cs.get_angular_velocity(), angular_velocity)
+        twist = np.random.rand(6)
+        cs.set_twist(twist)
+        assert_array_almost_equal(cs.get_twist(), twist)
+
+        # acceleration
+        linear_acceleration = np.random.rand(3)
+        cs.set_linear_acceleration(linear_acceleration)
+        assert_array_almost_equal(cs.get_linear_acceleration(), linear_acceleration)
+        angular_acceleration = np.random.rand(3)
+        cs.set_angular_acceleration(angular_acceleration)
+        assert_array_almost_equal(cs.get_angular_acceleration(), angular_acceleration)
+        accelerations = np.random.rand(6)
+        cs.set_accelerations(accelerations)
+        assert_array_almost_equal(cs.get_accelerations(), accelerations)
+
+        # wrench
+        force = np.random.rand(3)
+        cs.set_force(force)
+        assert_array_almost_equal(cs.get_force(), force)
+        torque = np.random.rand(3)
+        cs.set_torque(torque)
+        assert_array_almost_equal(cs.get_torque(), torque)
+        wrench = np.random.rand(6)
+        cs.set_wrench(wrench)
+        assert_array_almost_equal(cs.get_wrench(), wrench)
+
+        cs.set_zero()
+        self.assertAlmostEqual(np.linalg.norm(cs.data()), 1)
+        self.assertFalse(cs.is_empty())
+        cs.set_empty()
+        self.assertTrue(cs.is_empty())
+
+    def test_compatibility(self):
+        cs1 = CartesianState("test")
+        cs2 = CartesianState("robot")
+        cs3 = CartesianState("robot", "test")
+        cs4 = CartesianState("test", "robot")
+
+        self.assertFalse(cs1.is_compatible(cs2))
+        self.assertFalse(cs1.is_compatible(cs3))
+        self.assertFalse(cs1.is_compatible(cs4))
+
+    def test_set_zero(self):
+        random1 = CartesianState().Random("test")
+        random1.initialize()
+        self.assertAlmostEqual(np.linalg.norm(random1.data()), 1)
+
+        random2 = CartesianState().Random("test")
+        random2.set_zero()
+        self.assertAlmostEqual(np.linalg.norm(random1.data()), 1)
+
+    def test_get_set_data(self):
+        cs1 = CartesianState().Identity("test")
+        cs2 = CartesianState().Random("test")
+        concatenated_state = np.hstack((cs1.get_pose(), cs1.get_twist(), cs1.get_accelerations(), cs1.get_wrench()))
+        assert_array_almost_equal(cs1.data(), concatenated_state)
+        assert_array_almost_equal(cs1.array(), concatenated_state)
+
+        cs1.set_data(cs2.data())
+        assert_array_almost_equal(cs1.data(), cs2.data())
+
+        cs2 = CartesianState.Random("test")
+        state_vec = cs2.to_list()
+        cs1.set_data(state_vec)
+        [self.assertAlmostEqual(cs1.data()[i], state_vec[i]) for i in range(len(state_vec))]
+
+        with self.assertRaises(RuntimeError):
+            cs1.set_data(np.array([0, 0]))
+
+    def test_clamping(self):
+        state = CartesianState().Identity("test")
+        with self.assertRaises(RuntimeError):
+            state.clamp_state_variable(1, CartesianStateVariable.ORIENTATION)
+        with self.assertRaises(RuntimeError):
+            state.clamp_state_variable(1, CartesianStateVariable.POSE)
+        with self.assertRaises(RuntimeError):
+            state.clamp_state_variable(1, CartesianStateVariable.ALL)
+
+        self.clamping_helper(state, state.get_position, state.set_position, CartesianStateVariable.POSITION, 3)
+        self.clamping_helper(state, state.get_linear_velocity, state.set_linear_velocity,
+                             CartesianStateVariable.LINEAR_VELOCITY, 3)
+        self.clamping_helper(state, state.get_angular_velocity, state.set_angular_velocity,
+                             CartesianStateVariable.ANGULAR_VELOCITY, 3)
+        self.clamping_helper(state, state.get_twist, state.set_twist, CartesianStateVariable.TWIST, 6)
+        self.clamping_helper(state, state.get_linear_acceleration, state.set_linear_acceleration,
+                             CartesianStateVariable.LINEAR_ACCELERATION, 3)
+        self.clamping_helper(state, state.get_angular_acceleration, state.set_angular_acceleration,
+                             CartesianStateVariable.ANGULAR_ACCELERATION, 3)
+        self.clamping_helper(state, state.get_accelerations, state.set_accelerations,
+                             CartesianStateVariable.ACCELERATIONS, 6)
+        self.clamping_helper(state, state.get_force, state.set_force, CartesianStateVariable.FORCE, 3)
+        self.clamping_helper(state, state.get_torque, state.set_torque, CartesianStateVariable.TORQUE, 3)
+        self.clamping_helper(state, state.get_wrench, state.set_wrench, CartesianStateVariable.WRENCH, 6)
+
+    def test_norms(self):
+        cs = CartesianState().Random("test")
+        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[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()))
+        self.assertAlmostEqual(norms[5], np.linalg.norm(cs.get_angular_acceleration()))
+        self.assertAlmostEqual(norms[6], np.linalg.norm(cs.get_force()))
+        self.assertAlmostEqual(norms[7], np.linalg.norm(cs.get_torque()))
+
+    def test_normalize(self):
+        cs = CartesianState().Random("test")
+        normalized = cs.normalized()
+        self.assertEqual(type(normalized), CartesianState)
+        norms1 = normalized.norms()
+        [self.assertAlmostEqual(n, 1) for n in norms1]
+
+        cs.normalize()
+        norms2 = cs.norms()
+        [self.assertAlmostEqual(n, 1) for n in norms2]
+
+    def test_distance(self):
+        empty = CartesianState()
+        cs1 = CartesianState().Random("test")
+        cs2 = CartesianState().Random("test", "robot")
+
+        with self.assertRaises(RuntimeError):
+            empty.dist(cs1)
+        with self.assertRaises(RuntimeError):
+            cs1.dist(empty)
+        with self.assertRaises(RuntimeError):
+            cs1.dist(cs2)
+
+        data1 = np.random.rand(25)
+        cs1.set_data(data1)
+        cs3 = CartesianState("test")
+        data3 = np.random.rand(25)
+        cs3.set_data(data3)
+
+        pos_dist = np.linalg.norm(data1[:3] - data3[:3])
+        # TODO orientation distance
+        orient_dist = cs1.dist(cs3, CartesianStateVariable.ORIENTATION)
+        lin_vel_dist = np.linalg.norm(data1[7:10] - data3[7:10])
+        ang_vel_dist = np.linalg.norm(data1[10:13] - data3[10:13])
+        lin_acc_dist = np.linalg.norm(data1[13:16] - data3[13:16])
+        ang_acc_dist = np.linalg.norm(data1[16:19] - data3[16:19])
+        force_dist = np.linalg.norm(data1[19:22] - data3[19:22])
+        torque_dist = np.linalg.norm(data1[22:] - data3[22:])
+
+        self.assertAlmostEqual(cs1.dist(cs3, CartesianStateVariable.POSITION), pos_dist)
+        self.assertAlmostEqual(cs1.dist(cs3, CartesianStateVariable.ORIENTATION), orient_dist)
+        self.assertAlmostEqual(cs1.dist(cs3, CartesianStateVariable.POSE), pos_dist + orient_dist)
+        self.assertAlmostEqual(cs1.dist(cs3, CartesianStateVariable.TWIST), lin_vel_dist + ang_vel_dist)
+        self.assertAlmostEqual(cs1.dist(cs3, CartesianStateVariable.ACCELERATIONS), lin_acc_dist + ang_acc_dist)
+        self.assertAlmostEqual(cs1.dist(cs3, CartesianStateVariable.WRENCH), force_dist + torque_dist)
+        self.assertAlmostEqual(cs1.dist(cs3), cs3.dist(cs1, CartesianStateVariable.ALL))
+
+    def test_addition(self):
+        cs1 = CartesianState().Random("test")
+        cs2 = CartesianState().Random("test")
+        cs3 = CartesianState().Random("test", "reference")
+
+        with self.assertRaises(RuntimeError):
+            cs1 + cs3
+
+        csum = cs1 + cs2
+        self.assertEqual(type(csum), CartesianState)
+        assert_array_almost_equal(csum.get_position(), cs1.get_position() + cs2.get_position())
+        # TODO orientation sum
+        assert_array_almost_equal(csum.get_twist(), cs1.get_twist() + cs2.get_twist())
+        assert_array_almost_equal(csum.get_accelerations(), cs1.get_accelerations() + cs2.get_accelerations())
+        assert_array_almost_equal(csum.get_wrench(), cs1.get_wrench() + cs2.get_wrench())
+
+        cs1 += cs2
+        self.assertEqual(type(cs1), CartesianState)
+        assert_array_almost_equal(cs1.data(), csum.data())
+
+    def test_subtraction(self):
+        cs1 = CartesianState().Random("test")
+        cs2 = CartesianState().Random("test")
+        cs3 = CartesianState().Random("test", "reference")
+
+        with self.assertRaises(RuntimeError):
+            cs1 - cs3
+
+        cdiff = cs1 - cs2
+        self.assertEqual(type(cdiff), CartesianState)
+        assert_array_almost_equal(cdiff.get_position(), cs1.get_position() - cs2.get_position())
+        # TODO orientation diff
+        assert_array_almost_equal(cdiff.get_twist(), cs1.get_twist() - cs2.get_twist())
+        assert_array_almost_equal(cdiff.get_accelerations(), cs1.get_accelerations() - cs2.get_accelerations())
+        assert_array_almost_equal(cdiff.get_wrench(), cs1.get_wrench() - cs2.get_wrench())
+
+        cs1 -= cs2
+        self.assertEqual(type(cs1), CartesianState)
+        assert_array_almost_equal(cs1.data(), cdiff.data())
+
+    def test_scalar_multiplication(self):
+        scalar = 2.0
+        cs = CartesianState().Random("test")
+        cscaled = scalar * cs
+        self.assertEqual(type(cscaled), CartesianState)
+        assert_array_almost_equal(cscaled.get_position(), scalar * cs.get_position())
+        # TODO orientation mult
+        assert_array_almost_equal(cscaled.get_twist(), scalar * cs.get_twist())
+        assert_array_almost_equal(cscaled.get_accelerations(), scalar * cs.get_accelerations())
+        assert_array_almost_equal(cscaled.get_wrench(), scalar * cs.get_wrench())
+
+        cs *= scalar
+        self.assertEqual(type(cs), CartesianState)
+        assert_array_almost_equal(cs.data(), cscaled.data())
+
+        empty = CartesianState()
+        with self.assertRaises(RuntimeError):
+            scalar * empty
+
+    def test_scalar_division(self):
+        scalar = 2.0
+        cs = CartesianState().Random("test")
+        cscaled = cs / scalar
+        self.assertEqual(type(cscaled), CartesianState)
+        assert_array_almost_equal(cscaled.get_position(), cs.get_position() / scalar)
+        # TODO orientation diff
+        assert_array_almost_equal(cscaled.get_twist(), cs.get_twist() / scalar)
+        assert_array_almost_equal(cscaled.get_accelerations(), cs.get_accelerations() / scalar)
+        assert_array_almost_equal(cscaled.get_wrench(), cs.get_wrench() / scalar)
+
+        cs /= scalar
+        self.assertEqual(type(cs), CartesianState)
+        assert_array_almost_equal(cs.data(), cscaled.data())
+
+        with self.assertRaises(RuntimeError):
+            cs / 0.0
+
+        empty = CartesianState()
+        with self.assertRaises(RuntimeError):
+            empty / scalar
+
 
 if __name__ == '__main__':
-    unittest.main()
\ No newline at end of file
+    unittest.main()
diff --git a/python/test/space/test_spatial_state.py b/python/test/space/test_spatial_state.py
index a8fd18f50..78f534207 100644
--- a/python/test/space/test_spatial_state.py
+++ b/python/test/space/test_spatial_state.py
@@ -33,6 +33,12 @@ def test_constructors(self):
         self.assertEqual(state2.get_reference_frame(), "robot")
         self.assertFalse(state2.is_empty())
 
+        state3 = SpatialState(state2)
+        self.assertEqual(state2.get_type(), state3.get_type())
+        self.assertEqual(state2.get_name(), state3.get_name())
+        self.assertEqual(state2.get_reference_frame(), state3.get_reference_frame())
+        self.assertEqual(state2.is_empty(), state3.is_empty())
+
     def test_compatibility(self):
         state1 = SpatialState(StateType.CARTESIANSTATE, "test", "robot", True)
         state2 = SpatialState(StateType.STATE, "test")
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 9718567b7..87f11d729 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
@@ -1,145 +1,427 @@
+#include <functional>
 #include <gtest/gtest.h>
 
 #include "state_representation/space/cartesian/CartesianState.hpp"
 #include "state_representation/exceptions/EmptyStateException.hpp"
+#include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp"
 #include "state_representation/exceptions/NotImplementedException.hpp"
 
 using namespace state_representation;
 
+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.is_empty(), empty);
+  EXPECT_EQ(state1.get_reference_frame(), reference_frame);
+}
+
+static void assert_name_frame_data_equal(const CartesianState& state1, const CartesianState& state2) {
+  EXPECT_EQ(state1.get_name(), state2.get_name());
+  EXPECT_EQ(state1.get_reference_frame(), state2.get_reference_frame());
+  EXPECT_TRUE(state1.data().isApprox(state2.data()));
+}
+
+template<int dim>
+void test_clamping(
+    CartesianState& state, std::function<Eigen::Matrix<double, dim, 1>(const CartesianState&)> getter,
+    std::function<void(CartesianState&, Eigen::Matrix<double, dim, 1>)> setter, const CartesianStateVariable& type
+) {
+  Eigen::Matrix<double, dim, 1> data;
+  if (dim == 3) {
+    data << -2.0, 1, 5;
+  } else if (dim == 6) {
+    data << -2.0, 1, 5, 1, -3.0, 2.4;
+  }
+  setter(state, data);
+  state.clamp_state_variable(10.0, type);
+  EXPECT_TRUE(getter(state).isApprox(data));
+  state.clamp_state_variable(3.0, type);
+  EXPECT_FLOAT_EQ(getter(state).norm(), 3.0);
+  state.clamp_state_variable(10.0, type, 0.5);
+  EXPECT_FLOAT_EQ(getter(state).norm(), 0.0);
+}
+
+TEST(CartesianStateTest, Constructors) {
+  CartesianState empty1;
+  assert_name_empty_frame_equal(empty1, "", true, "world");
+  EXPECT_FLOAT_EQ(empty1.data().norm(), 1);
+
+  CartesianState empty2("test");
+  assert_name_empty_frame_equal(empty2, "test", true, "world");
+  EXPECT_FLOAT_EQ(empty2.data().norm(), 1);
+
+  CartesianState empty3("test", "reference");
+  assert_name_empty_frame_equal(empty3, "test", true, "reference");
+  EXPECT_FLOAT_EQ(empty3.data().norm(), 1);
+}
+
 TEST(CartesianStateTest, IdentityInitialization) {
   CartesianState identity = CartesianState::Identity("test");
-  // the joint state should not be considered empty (as it is properly initialized)
   EXPECT_FALSE(identity.is_empty());
-  // all data should be zero except for orientation that should be identity
-  EXPECT_EQ(identity.get_position().norm(), 0);
-  EXPECT_EQ(identity.get_orientation().norm(), 1);
-  EXPECT_EQ(identity.get_orientation().w(), 1);
-  EXPECT_EQ(identity.get_twist().norm(), 0);
-  EXPECT_EQ(identity.get_accelerations().norm(), 0);
-  EXPECT_EQ(identity.get_wrench().norm(), 0);
+  EXPECT_FLOAT_EQ(identity.get_position().norm(), 0);
+  EXPECT_FLOAT_EQ(identity.get_orientation().norm(), 1);
+  EXPECT_FLOAT_EQ(identity.get_orientation().w(), 1);
+  EXPECT_FLOAT_EQ(identity.get_twist().norm(), 0);
+  EXPECT_FLOAT_EQ(identity.get_accelerations().norm(), 0);
+  EXPECT_FLOAT_EQ(identity.get_wrench().norm(), 0);
 }
 
 TEST(CartesianStateTest, RandomStateInitialization) {
   CartesianState random = CartesianState::Random("test");
-  // all data should be random (non 0)
   EXPECT_NE(random.get_position().norm(), 0);
-  EXPECT_NE(abs(random.get_orientation().w()), 0);
-  EXPECT_NE(abs(random.get_orientation().x()), 0);
-  EXPECT_NE(abs(random.get_orientation().y()), 0);
-  EXPECT_NE(abs(random.get_orientation().z()), 0);
+  EXPECT_FLOAT_EQ(random.get_orientation().norm(), 1);
+  EXPECT_NE(random.get_orientation().w(), 0);
+  EXPECT_NE(random.get_orientation().x(), 0);
+  EXPECT_NE(random.get_orientation().y(), 0);
+  EXPECT_NE(random.get_orientation().z(), 0);
   EXPECT_NE(random.get_twist().norm(), 0);
   EXPECT_NE(random.get_accelerations().norm(), 0);
   EXPECT_NE(random.get_wrench().norm(), 0);
 }
 
-TEST(CartesianStateTest, CopyState) {
-  CartesianState state1 = CartesianState::Random("test");
-  CartesianState state2(state1);
-  EXPECT_EQ(state1.get_name(), state2.get_name());
-  EXPECT_EQ(state1.get_reference_frame(), state2.get_reference_frame());
-  EXPECT_TRUE(state1.data().isApprox(state2.data()));
-  CartesianState state3 = state1;
-  EXPECT_EQ(state1.get_name(), state3.get_name());
-  EXPECT_EQ(state1.get_reference_frame(), state3.get_reference_frame());
-  EXPECT_TRUE(state1.data().isApprox(state3.data()));
+TEST(CartesianStateTest, CopyConstructor) {
+  CartesianState random = CartesianState::Random("test");
+  CartesianState copy1(random);
+  assert_name_frame_data_equal(random, copy1);
+
+  CartesianState copy2 = random;
+  assert_name_frame_data_equal(random, copy2);
+
+  CartesianState copy3 = random.copy();
+  assert_name_frame_data_equal(random, copy3);
 
-  CartesianState state4;
-  EXPECT_TRUE(state4.is_empty());
-  CartesianState state5 = state4;
-  EXPECT_TRUE(state5.is_empty());
+  CartesianState empty;
+  CartesianState copy4(empty);
+  EXPECT_TRUE(copy4.is_empty());
+  CartesianState copy5 = empty;
+  EXPECT_TRUE(copy5.is_empty());
+  CartesianState copy6 = empty.copy();
+  EXPECT_TRUE(copy6.is_empty());
 }
 
-TEST(CartesianStateTest, GetData) {
-  CartesianState cs = CartesianState::Random("test");
-  Eigen::VectorXd concatenated_state(25);
-  concatenated_state << cs.get_pose(), cs.get_twist(), cs.get_accelerations(), cs.get_wrench();
-  EXPECT_TRUE(concatenated_state.isApprox(cs.data()));
+TEST(CartesianStateTest, GetSetFields) {
+  CartesianState cs("test");
+
+  // name
+  cs.set_name("robot");
+  EXPECT_EQ(cs.get_name(), "robot");
+  EXPECT_EQ(cs.get_reference_frame(), "world");
+  cs.set_reference_frame("base");
+  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));
+  }
+  cs.set_position(1.1, 2.2, 3.3);
+  EXPECT_TRUE(Eigen::Vector3d(1.1, 2.2, 3.3).isApprox(cs.get_position()));
+
+  // orientation
+  Eigen::Vector4d orientation_vec = Eigen::Vector4d::Random().normalized();
+  cs.set_orientation(orientation_vec);
+  for (auto i = 0; i < orientation_vec.size(); ++i) {
+    EXPECT_FLOAT_EQ(cs.get_orientation_coefficients()(i), orientation_vec(i));
+  }
+  Eigen::Quaterniond random_orientation = Eigen::Quaterniond::UnitRandom();
+  std::vector<double>
+      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);
+
+  auto matrix = cs.get_transformation_matrix();
+  Eigen::Vector3d trans = matrix.topRightCorner<3, 1>();
+  Eigen::Matrix3d rot = matrix.topLeftCorner<3, 3>();
+  Eigen::Vector4d bottom = matrix.bottomLeftCorner<1, 4>();
+  EXPECT_TRUE(trans.isApprox(cs.get_position()));
+  EXPECT_TRUE(rot.isApprox(random_orientation.toRotationMatrix()));
+  EXPECT_TRUE(bottom.isApprox(Eigen::Vector4d(0, 0, 0, 1)));
+
+  // pose
+  EXPECT_THROW(cs.set_pose(std::vector<double>(8)), 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);
+  cs.set_twist(twist);
+  EXPECT_TRUE(cs.get_twist().isApprox(twist));
+
+  // 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_accelerations(accerlerations);
+  EXPECT_TRUE(cs.get_accelerations().isApprox(accerlerations));
+
+  // 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);
+  cs.set_wrench(wrench);
+  EXPECT_TRUE(cs.get_wrench().isApprox(wrench));
+
+  cs.set_zero();
+  EXPECT_FLOAT_EQ(cs.data().norm(), 1);
+  EXPECT_EQ(cs.is_empty(), false);
+  cs.set_empty();
+  EXPECT_EQ(cs.is_empty(), true);
+}
+
+TEST(CartesianStateTest, Compatibility) {
+  CartesianState cs1("test");
+  CartesianState cs2("robot");
+  CartesianState cs3("robot", "test");
+  CartesianState cs4("test", "robot");
+
+  EXPECT_FALSE(cs1.is_compatible(cs2));
+  EXPECT_FALSE(cs1.is_compatible(cs3));
+  EXPECT_FALSE(cs1.is_compatible(cs4));
+}
+
+TEST(CartesianStateTest, SetZero) {
+  CartesianState random1 = CartesianState::Random("test");
+  random1.initialize();
+  EXPECT_FLOAT_EQ(random1.data().norm(), 1);
+
+  CartesianState random2 = CartesianState::Random("test");
+  random2.set_zero();
+  EXPECT_FLOAT_EQ(random2.data().norm(), 1);
 }
 
-TEST(CartesianStateTest, SetData) {
+TEST(CartesianStateTest, GetSetData) {
   CartesianState cs1 = CartesianState::Identity("test");
   CartesianState cs2 = CartesianState::Random("test");
+  Eigen::VectorXd concatenated_state(25);
+  concatenated_state << cs1.get_pose(), cs1.get_twist(), cs1.get_accelerations(), cs1.get_wrench();
+  EXPECT_TRUE(concatenated_state.isApprox(cs1.data()));
+  for (std::size_t i = 0; i < 25; ++i) {
+    EXPECT_FLOAT_EQ(concatenated_state.array()(i), cs1.array()(i));
+  }
+
   cs1.set_data(cs2.data());
-  EXPECT_TRUE(cs2.data().isApprox(cs1.data()));
+  EXPECT_TRUE(cs1.data().isApprox(cs2.data()));
 
+  cs2 = CartesianState::Random("test");
   auto state_vec = cs2.to_std_vector();
   cs1.set_data(state_vec);
-  for (std::size_t j = 0; j < state_vec.size(); ++j) {
-    EXPECT_FLOAT_EQ(state_vec.at(j), cs1.data()(j));
-  }
-  EXPECT_THROW(cs1.set_data(Eigen::Vector3d::Zero()), exceptions::IncompatibleSizeException);
-}
-
-TEST(CartesianStateTest, CartesianStateToStdVector) {
-  CartesianState cs = CartesianState::Random("test");
-  std::vector<double> vec_data = cs.to_std_vector();
-  for (size_t i = 0; i < vec_data.size(); ++i) {
-    EXPECT_EQ(cs.data()(i), vec_data[i]);
+  for (std::size_t i = 0; i < state_vec.size(); ++i) {
+    EXPECT_FLOAT_EQ(state_vec.at(i), cs1.data()(i));
   }
+  EXPECT_THROW(cs1.set_data(Eigen::Vector2d::Zero()), exceptions::IncompatibleSizeException);
 }
 
-TEST(CartesianStateTest, TestStateClamping) {
+TEST(CartesianStateTest, ClampVariable) {
   CartesianState state = CartesianState::Identity("test");
   EXPECT_THROW(state.clamp_state_variable(1, CartesianStateVariable::ORIENTATION), exceptions::NotImplementedException);
   EXPECT_THROW(state.clamp_state_variable(1, CartesianStateVariable::POSE), exceptions::NotImplementedException);
-  Eigen::Vector3d position(-2.0, 1, 5);
-  state.set_position(position);
-  state.clamp_state_variable(10.0, CartesianStateVariable::POSITION);
-  EXPECT_EQ(state.get_position(), position);
-  state.clamp_state_variable(3.0, CartesianStateVariable::POSITION);
-  EXPECT_EQ(state.get_position().norm(), 3.0);
-  state.clamp_state_variable(10.0, CartesianStateVariable::POSITION, 0.5);
-  EXPECT_EQ(state.get_position().norm(), 0.0);
-}
-
-TEST(CartesianStateTest, TestAllNorms) {
-  double tolerance = 1e-4;
-  CartesianState cs = CartesianState::Random("cs");
-  // first test all norms
+  EXPECT_THROW(state.clamp_state_variable(1, CartesianStateVariable::ALL), exceptions::NotImplementedException);
+
+  test_clamping<3>(
+      state, [](const CartesianState& state) -> const Eigen::Vector3d& { return state.get_position(); },
+      [](CartesianState& state, const Eigen::Vector3d& data) { state.set_position(data); },
+      CartesianStateVariable::POSITION
+  );
+  test_clamping<3>(
+      state, [](const CartesianState& state) -> const Eigen::Vector3d& { return state.get_linear_velocity(); },
+      [](CartesianState& state, const Eigen::Vector3d& data) { state.set_linear_velocity(data); },
+      CartesianStateVariable::LINEAR_VELOCITY
+  );
+  test_clamping<3>(
+      state, [](const CartesianState& state) -> const Eigen::Vector3d& { return state.get_angular_velocity(); },
+      [](CartesianState& state, const Eigen::Vector3d& data) { state.set_angular_velocity(data); },
+      CartesianStateVariable::ANGULAR_VELOCITY
+  );
+  test_clamping<6>(
+      state, [](const CartesianState& state) -> Eigen::Matrix<double, 6, 1> { return state.get_twist(); },
+      [](CartesianState& state, const Eigen::Matrix<double, 6, 1>& data) { state.set_twist(data); },
+      CartesianStateVariable::TWIST
+  );
+  test_clamping<3>(
+      state, [](const CartesianState& state) -> const Eigen::Vector3d& { return state.get_linear_acceleration(); },
+      [](CartesianState& state, const Eigen::Vector3d& data) { state.set_linear_acceleration(data); },
+      CartesianStateVariable::LINEAR_ACCELERATION
+  );
+  test_clamping<3>(
+      state, [](const CartesianState& state) -> const Eigen::Vector3d& { return state.get_angular_acceleration(); },
+      [](CartesianState& state, const Eigen::Vector3d& data) { state.set_angular_acceleration(data); },
+      CartesianStateVariable::ANGULAR_ACCELERATION
+  );
+  test_clamping<6>(
+      state, [](const CartesianState& state) -> Eigen::MatrixXd { return state.get_accelerations(); },
+      [](CartesianState& state, const Eigen::MatrixXd& data) { state.set_accelerations(data); },
+      CartesianStateVariable::ACCELERATIONS
+  );
+  test_clamping<3>(
+      state, [](const CartesianState& state) -> const Eigen::Vector3d& { return state.get_force(); },
+      [](CartesianState& state, const Eigen::Vector3d& data) { state.set_force(data); }, CartesianStateVariable::FORCE
+  );
+  test_clamping<3>(
+      state, [](const CartesianState& state) -> const Eigen::Vector3d& { return state.get_torque(); },
+      [](CartesianState& state, const Eigen::Vector3d& data) { state.set_torque(data); }, CartesianStateVariable::TORQUE
+  );
+  test_clamping<6>(
+      state, [](const CartesianState& state) -> Eigen::MatrixXd { return state.get_wrench(); },
+      [](CartesianState& state, const Eigen::MatrixXd& data) { state.set_wrench(data); }, CartesianStateVariable::WRENCH
+  );
+}
+
+TEST(CartesianStateTest, Norms) {
+  CartesianState cs = CartesianState::Random("test");
   std::vector<double> norms = cs.norms();
   ASSERT_TRUE(norms.size() == 8);
-  EXPECT_NEAR(norms[0], cs.get_position().norm(), tolerance);
-  EXPECT_NEAR(norms[1], cs.get_orientation().norm(), tolerance);
-  EXPECT_NEAR(norms[2], cs.get_linear_velocity().norm(), tolerance);
-  EXPECT_NEAR(norms[3], cs.get_angular_velocity().norm(), tolerance);
-  EXPECT_NEAR(norms[4], cs.get_linear_acceleration().norm(), tolerance);
-  EXPECT_NEAR(norms[5], cs.get_angular_acceleration().norm(), tolerance);
-  EXPECT_NEAR(norms[6], cs.get_force().norm(), tolerance);
-  EXPECT_NEAR(norms[7], cs.get_torque().norm(), tolerance);
-}
-
-TEST(CartesianStateTest, TestNormalize) {
-  double tolerance = 1e-4;
-  CartesianState cs = CartesianState::Random("cs");
+  EXPECT_FLOAT_EQ(norms[0], cs.get_position().norm());
+  EXPECT_FLOAT_EQ(norms[1], cs.get_orientation().norm());
+  EXPECT_FLOAT_EQ(norms[2], cs.get_linear_velocity().norm());
+  EXPECT_FLOAT_EQ(norms[3], cs.get_angular_velocity().norm());
+  EXPECT_FLOAT_EQ(norms[4], cs.get_linear_acceleration().norm());
+  EXPECT_FLOAT_EQ(norms[5], cs.get_angular_acceleration().norm());
+  EXPECT_FLOAT_EQ(norms[6], cs.get_force().norm());
+  EXPECT_FLOAT_EQ(norms[7], cs.get_torque().norm());
+}
+
+TEST(CartesianStateTest, Normalize) {
+  CartesianState cs = CartesianState::Random("test");
+  auto normalized = cs.normalized();
+  std::vector<double> norms1 = normalized.norms();
+  for (double n: norms1) {
+    EXPECT_FLOAT_EQ(n, 1.0);
+  }
   cs.normalize();
-  std::vector<double> norms = cs.norms();
-  for (double n: norms) {
-    EXPECT_NEAR(n, 1.0, tolerance);
+  std::vector<double> norms2 = cs.norms();
+  for (double n: norms2) {
+    EXPECT_FLOAT_EQ(n, 1.0);
   }
 }
 
-TEST(CartesianStateTest, TestNormalized) {
-  double tolerance = 1e-4;
-  CartesianState cs = CartesianState::Random("cs");
-  CartesianState csn = cs.normalized();
-  std::vector<double> norms = csn.norms();
-  for (double n: norms) {
-    EXPECT_NEAR(n, 1.0, tolerance);
-  }
+TEST(CartesianStateTest, Distance) {
+  CartesianState empty;
+  CartesianState cs1 = CartesianState::Random("test");
+  CartesianState cs2 = CartesianState::Random("test", "robot");
+  EXPECT_THROW(empty.dist(cs1), exceptions::EmptyStateException);
+  EXPECT_THROW(cs1.dist(empty), exceptions::EmptyStateException);
+  EXPECT_THROW(cs1.dist(cs2), exceptions::IncompatibleReferenceFramesException);
+
+  Eigen::VectorXd data1 = Eigen::VectorXd::Random(25);
+  cs1.set_data(data1);
+  CartesianState cs3 = CartesianState("test");
+  Eigen::VectorXd data3 = Eigen::VectorXd::Random(25);
+  cs3.set_data(data3);
+
+  double pos_dist = (data1.head(3) - data3.head(3)).norm();
+  double inner_product = cs1.get_orientation().dot(cs3.get_orientation());
+  double orient_dist = acos(std::min(1.0, std::max(-1.0, 2 * inner_product * inner_product - 1)));
+  double lin_vel_dist = (data1.segment(7, 3) - data3.segment(7, 3)).norm();
+  double ang_vel_dist = (data1.segment(10, 3) - data3.segment(10, 3)).norm();
+  double lin_acc_dist = (data1.segment(13, 3) - data3.segment(13, 3)).norm();
+  double ang_acc_dist = (data1.segment(16, 3) - data3.segment(16, 3)).norm();
+  double force_dist = (data1.segment(19, 3) - data3.segment(19, 3)).norm();
+  double torque_dist = (data1.segment(22, 3) - data3.segment(22, 3)).norm();
+  double total_dist =
+      pos_dist + orient_dist + lin_vel_dist + ang_vel_dist + lin_acc_dist + ang_acc_dist + force_dist + torque_dist;
+
+  EXPECT_FLOAT_EQ(cs1.dist(cs3, CartesianStateVariable::POSITION), pos_dist);
+  EXPECT_FLOAT_EQ(cs1.dist(cs3, CartesianStateVariable::ORIENTATION), orient_dist);
+  EXPECT_FLOAT_EQ(cs1.dist(cs3, CartesianStateVariable::POSE), pos_dist + orient_dist);
+  EXPECT_FLOAT_EQ(cs1.dist(cs3, CartesianStateVariable::TWIST), lin_vel_dist + ang_vel_dist);
+  EXPECT_FLOAT_EQ(cs1.dist(cs3, CartesianStateVariable::ACCELERATIONS), lin_acc_dist + ang_acc_dist);
+  EXPECT_FLOAT_EQ(cs1.dist(cs3, CartesianStateVariable::WRENCH), force_dist + torque_dist);
+  EXPECT_FLOAT_EQ(cs1.dist(cs3, CartesianStateVariable::ALL), total_dist);
+  EXPECT_FLOAT_EQ(cs1.dist(cs3), cs3.dist(cs1));
+}
+
+TEST(CartesianStateTest, Inverse) {
+  // TODO
+}
+
+TEST(CartesianStateTest, Addition) {
+  CartesianState cs1 = CartesianState::Random("test");
+  CartesianState cs2 = CartesianState::Random("test");
+  CartesianState cs3 = CartesianState::Random("test", "reference");
+  EXPECT_THROW(cs1 + cs3, exceptions::IncompatibleReferenceFramesException);
+
+  CartesianState csum = cs1 + cs2;
+  EXPECT_TRUE(csum.get_position().isApprox(cs1.get_position() + cs2.get_position()));
+  Eigen::Quaterniond
+      orientation = (cs1.get_orientation().dot(cs2.get_orientation()) > 0) ? cs2.get_orientation() : Eigen::Quaterniond(
+      -cs2.get_orientation().coeffs());
+  orientation = cs1.get_orientation() * orientation;
+  EXPECT_TRUE(csum.get_orientation().coeffs().isApprox(orientation.coeffs()));
+  EXPECT_TRUE(csum.get_twist().isApprox(cs1.get_twist() + cs2.get_twist()));
+  EXPECT_TRUE(csum.get_accelerations().isApprox(cs1.get_accelerations() + cs2.get_accelerations()));
+  EXPECT_TRUE(csum.get_wrench().isApprox(cs1.get_wrench() + cs2.get_wrench()));
+
+  cs1 += cs2;
+  EXPECT_TRUE(cs1.data().isApprox(csum.data()));
+}
+
+TEST(CartesianStateTest, Subtraction) {
+  CartesianState cs1 = CartesianState::Random("test");
+  CartesianState cs2 = CartesianState::Random("test");
+  CartesianState cs3 = CartesianState::Random("test", "reference");
+  EXPECT_THROW(cs1 - cs3, exceptions::IncompatibleReferenceFramesException);
+
+  CartesianState cdiff = cs1 - cs2;
+  EXPECT_TRUE(cdiff.get_position().isApprox(cs1.get_position() - cs2.get_position()));
+  Eigen::Quaterniond
+      orientation = (cs1.get_orientation().dot(cs2.get_orientation()) > 0) ? cs2.get_orientation() : Eigen::Quaterniond(
+      -cs2.get_orientation().coeffs());
+  orientation = cs1.get_orientation() * orientation.conjugate();
+  EXPECT_TRUE(cdiff.get_orientation().coeffs().isApprox(orientation.coeffs()));
+  EXPECT_TRUE(cdiff.get_twist().isApprox(cs1.get_twist() - cs2.get_twist()));
+  EXPECT_TRUE(cdiff.get_accelerations().isApprox(cs1.get_accelerations() - cs2.get_accelerations()));
+  EXPECT_TRUE(cdiff.get_wrench().isApprox(cs1.get_wrench() - cs2.get_wrench()));
+
+  cs1 -= cs2;
+  EXPECT_TRUE(cs1.data().isApprox(cdiff.data()));
+}
+
+TEST(CartesianStateTest, ScalarMultiplication) {
+  double scalar = 2;
+  CartesianState cs = CartesianState::Random("test");
+  CartesianState cscaled = scalar * cs;
+  EXPECT_TRUE(cscaled.get_position().isApprox(scalar * cs.get_position()));
+  Eigen::Quaterniond qscaled = math_tools::exp(math_tools::log(cs.get_orientation()), scalar / 2.);
+  EXPECT_TRUE(cscaled.get_orientation().coeffs().isApprox(qscaled.coeffs()));
+  EXPECT_TRUE(cscaled.get_twist().isApprox(scalar * cs.get_twist()));
+  EXPECT_TRUE(cscaled.get_accelerations().isApprox(scalar * cs.get_accelerations()));
+  EXPECT_TRUE(cscaled.get_wrench().isApprox(scalar * cs.get_wrench()));
+  EXPECT_TRUE((cs * scalar).data().isApprox(cscaled.data()));
+  cs *= scalar;
+  EXPECT_TRUE(cscaled.data().isApprox(cs.data()));
+
+  CartesianState empty;
+  EXPECT_THROW(scalar * empty, exceptions::EmptyStateException);
 }
 
-TEST(CartesianStateTest, TestScalarDivison) {
+TEST(CartesianStateTest, ScalarDivision) {
   double scalar = 2;
   CartesianState cs = CartesianState::Random("test");
   CartesianState cscaled = cs / scalar;
-  EXPECT_EQ(cscaled.get_position(), cs.get_position() / scalar);
+  EXPECT_TRUE(cscaled.get_position().isApprox(cs.get_position() / scalar));
   Eigen::Quaterniond qscaled = math_tools::exp(math_tools::log(cs.get_orientation()), 1.0 / (2. * scalar));
   EXPECT_TRUE(cscaled.get_orientation().coeffs().isApprox(qscaled.coeffs()));
-  EXPECT_EQ(cscaled.get_twist(), cs.get_twist() / scalar);
-  EXPECT_EQ(cscaled.get_accelerations(), cs.get_accelerations() / scalar);
-  EXPECT_EQ(cscaled.get_wrench(), cs.get_wrench() / scalar);
+  EXPECT_TRUE(cscaled.get_twist().isApprox(cs.get_twist() / scalar));
+  EXPECT_TRUE(cscaled.get_accelerations().isApprox(cs.get_accelerations() / scalar));
+  EXPECT_TRUE(cscaled.get_wrench().isApprox(cs.get_wrench() / scalar));
   cs /= scalar;
-  EXPECT_EQ(cscaled.data(), cs.data());
+  EXPECT_TRUE(cscaled.data().isApprox(cs.data()));
 
   EXPECT_THROW(cs / 0.0, std::runtime_error);
 

From 50921634515113efc900c863b4817cccc190bc0e Mon Sep 17 00:00:00 2001
From: domire8 <71256590+domire8@users.noreply.github.com>
Date: Thu, 23 Sep 2021 11:45:44 +0200
Subject: [PATCH 14/22] Remove newline from ostream operators (#215)

---
 source/state_representation/src/State.cpp                | 2 +-
 source/state_representation/src/parameters/Event.cpp     | 2 +-
 source/state_representation/src/parameters/Parameter.cpp | 4 ++--
 source/state_representation/src/parameters/Predicate.cpp | 2 +-
 source/state_representation/src/robot/Jacobian.cpp       | 5 ++++-
 source/state_representation/src/space/SpatialState.cpp   | 2 +-
 6 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/source/state_representation/src/State.cpp b/source/state_representation/src/State.cpp
index cc20999f1..7ffedaa8e 100644
--- a/source/state_representation/src/State.cpp
+++ b/source/state_representation/src/State.cpp
@@ -28,7 +28,7 @@ std::ostream& operator<<(std::ostream& os, const State& state) {
   if (state.is_empty()) {
     os << "Empty ";
   }
-  os << " State: " << state.get_name() << std::endl;
+  os << " State: " << state.get_name();
   return os;
 }
 }// namespace state_representation
diff --git a/source/state_representation/src/parameters/Event.cpp b/source/state_representation/src/parameters/Event.cpp
index 885a92b77..922c28a75 100644
--- a/source/state_representation/src/parameters/Event.cpp
+++ b/source/state_representation/src/parameters/Event.cpp
@@ -4,7 +4,7 @@ namespace state_representation {
 Event::Event(const std::string& name) : Predicate(name), previous_predicate_value_(false) {}
 
 std::ostream& operator<<(std::ostream& os, const Event& event) {
-  os << " Event " << event.get_name() << ": " << event.get_value() << std::endl;
+  os << " Event " << event.get_name() << ": " << event.get_value();
   return os;
 }
 }// namespace state_representation
\ No newline at end of file
diff --git a/source/state_representation/src/parameters/Parameter.cpp b/source/state_representation/src/parameters/Parameter.cpp
index 0b2577e99..7662a6a06 100644
--- a/source/state_representation/src/parameters/Parameter.cpp
+++ b/source/state_representation/src/parameters/Parameter.cpp
@@ -153,9 +153,9 @@ Parameter<Eigen::VectorXd>::Parameter(const std::string& name, const Eigen::Vect
 template<typename T>
 std::ostream& operator<<(std::ostream& os, const Parameter<T>& parameter) {
   if (parameter.is_empty()) {
-    os << " Parameter " << parameter.get_name() << " is empty" << std::endl;
+    os << " Parameter " << parameter.get_name() << " is empty";
   } else {
-    os << " Parameter " << parameter.get_name() << ": " << parameter.value << std::endl;
+    os << " Parameter " << parameter.get_name() << ": " << parameter.value;
   }
   return os;
 }
diff --git a/source/state_representation/src/parameters/Predicate.cpp b/source/state_representation/src/parameters/Predicate.cpp
index aed5be686..0daab6a2d 100644
--- a/source/state_representation/src/parameters/Predicate.cpp
+++ b/source/state_representation/src/parameters/Predicate.cpp
@@ -6,7 +6,7 @@ Predicate::Predicate(const std::string& name) : Parameter<bool>(name, false) {}
 Predicate::Predicate(const std::string& name, bool value) : Parameter<bool>(name, value) {}
 
 std::ostream& operator<<(std::ostream& os, const Predicate& predicate) {
-  os << " Predicate " << predicate.get_name() << ": " << predicate.get_value() << std::endl;
+  os << " Predicate " << predicate.get_name() << ": " << predicate.get_value();
   return os;
 }
 }// namespace state_representation
\ No newline at end of file
diff --git a/source/state_representation/src/robot/Jacobian.cpp b/source/state_representation/src/robot/Jacobian.cpp
index 2cdeea7ff..7d62eb98e 100644
--- a/source/state_representation/src/robot/Jacobian.cpp
+++ b/source/state_representation/src/robot/Jacobian.cpp
@@ -268,7 +268,10 @@ std::ostream& operator<<(std::ostream& os, const Jacobian& jacobian) {
       for (unsigned int j = 1; j < jacobian.cols_; ++j) {
         os << ", " << jacobian(i, j);
       }
-      os << " |" << std::endl;
+      os << " |";
+      if (i != jacobian.rows_ - 1) {
+        os << std::endl;
+      }
     }
   }
   return os;
diff --git a/source/state_representation/src/space/SpatialState.cpp b/source/state_representation/src/space/SpatialState.cpp
index 94a7a9cd9..f75ef1da0 100644
--- a/source/state_representation/src/space/SpatialState.cpp
+++ b/source/state_representation/src/space/SpatialState.cpp
@@ -14,7 +14,7 @@ std::ostream& operator<<(std::ostream& os, const SpatialState& state) {
   if (state.is_empty()) {
     os << "Empty ";
   }
-  os << " State: " << state.get_name() << " expressed in " << state.get_reference_frame() << " frame" << std::endl;
+  os << " State: " << state.get_name() << " expressed in " << state.get_reference_frame() << " frame";
   return os;
 }
 }
\ No newline at end of file

From 80bea1fe63477965cb689392e1ae454192e2fc32 Mon Sep 17 00:00:00 2001
From: domire8 <71256590+domire8@users.noreply.github.com>
Date: Fri, 24 Sep 2021 17:02:28 +0200
Subject: [PATCH 15/22] Add build testing option to clproto install (#216)

* Add build testing option to clproto install

* Update CHANGELOG
---
 CHANGELOG.md                        | 1 +
 protocol/clproto_cpp/CMakeLists.txt | 3 ++-
 protocol/install.sh                 | 9 ++++++++-
 3 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 8b3a0122c..d8c5c8c4b 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -19,6 +19,7 @@ Release Versions:
 - Add Python bindings for Parameter class (#209)
 - Add Python bindings for clproto encode / decode of Parameter class (#214)
 - Improve CartesianState tests in C++ and Python (#213)
+- Add build testing option to clproto install (#216)
 
 ### Pending TODOs for the next release
 
diff --git a/protocol/clproto_cpp/CMakeLists.txt b/protocol/clproto_cpp/CMakeLists.txt
index 230df7120..b5e963a8c 100644
--- a/protocol/clproto_cpp/CMakeLists.txt
+++ b/protocol/clproto_cpp/CMakeLists.txt
@@ -3,6 +3,7 @@ project(clproto VERSION 4.0.0)
 
 set(CMAKE_CXX_STANDARD 17)
 
+include(FindProtobuf)
 find_package(Protobuf REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_library(state_representation REQUIRED)
@@ -38,7 +39,7 @@ add_library(${PROJECT_NAME} SHARED
 )
 target_include_directories(${PROJECT_NAME} PUBLIC ${PROJECT_SOURCE_DIR}/include)
 set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER ${PROJECT_SOURCE_DIR}/include/clproto.h)
-target_link_libraries(${PROJECT_NAME} PUBLIC protobuf state_representation PRIVATE ${PROJECT_NAME}_bindings)
+target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} state_representation PRIVATE ${PROJECT_NAME}_bindings)
 
 install(TARGETS ${PROJECT_NAME}
     PUBLIC_HEADER DESTINATION include
diff --git a/protocol/install.sh b/protocol/install.sh
index ce8f343c3..e0cb91d33 100755
--- a/protocol/install.sh
+++ b/protocol/install.sh
@@ -4,6 +4,7 @@ PROTOBUF_DIR="${SCRIPT_DIR}"/protobuf
 CLPROTO_DIR="${SCRIPT_DIR}"/clproto_cpp
 
 INSTALL_DESTINATION="/usr/local"
+BUILD_TESTING="OFF"
 AUTO_INSTALL=""
 BINDINGS_ONLY=false
 PROTOBUF_VERSION="3.17.0"
@@ -22,6 +23,8 @@ Options:
   -d, --dir [path]         Configure the installation directory
                            (default: ${INSTALL_DESTINATION}).
 
+  --build-tests            Build the unittest targets.
+
   --clean-bindings         Clean any previously generated protobuf
                            bindings.
 
@@ -75,7 +78,7 @@ function install_state_representation() {
 function install_clproto() {
   cd "${CLPROTO_DIR}" && mkdir -p build && cd build || exit 1
 
-  cmake -DCMAKE_BUILD_TYPE=Release \
+  cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING="${BUILD_TESTING}" \
     -DCMAKE_INSTALL_PREFIX="${INSTALL_DESTINATION}" .. || exit 1
 
   make -j && make install || exit 1
@@ -108,6 +111,10 @@ while [ "$#" -gt 0 ]; do
     BINDINGS_ONLY=true
     shift 1
     ;;
+  --build-tests)
+    BUILD_TESTING="ON"
+    shift 1
+    ;;
   --clean-bindings)
     clean_bindings
     exit 0

From 7a38cd14abd11bfde35a24e596b7a56dd9902a6d Mon Sep 17 00:00:00 2001
From: domire8 <71256590+domire8@users.noreply.github.com>
Date: Thu, 14 Oct 2021 22:04:19 +0200
Subject: [PATCH 16/22] Set joint field by name or index (#217)

* Add set_field by name/index and move inlined methods to src

* Improve unittests

* Delete setters from derived classes

* Add python bindings

* Apply suggestions from code review

Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>

* Improve tests

Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>
---
 CHANGELOG.md                                  |   1 +
 .../state_representation/bind_joint_space.cpp |   8 ++
 python/test/robot/test_joint_state.py         |   7 +-
 .../robot/JointAccelerations.hpp              |   6 +
 .../robot/JointPositions.hpp                  |   6 +
 .../state_representation/robot/JointState.hpp | 104 ++++++++++--------
 .../robot/JointTorques.hpp                    |   6 +
 .../robot/JointVelocities.hpp                 |   6 +
 .../src/robot/JointState.cpp                  |  84 ++++++++++++++
 .../test/tests/robot/test_joint_state.cpp     |  37 +++++++
 10 files changed, 216 insertions(+), 49 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index d8c5c8c4b..b2bcd85c4 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -20,6 +20,7 @@ Release Versions:
 - Add Python bindings for clproto encode / decode of Parameter class (#214)
 - Improve CartesianState tests in C++ and Python (#213)
 - Add build testing option to clproto install (#216)
+- Add method to set a joint state by name or index of the joint (#217)
 
 ### Pending TODOs for the next release
 
diff --git a/python/source/state_representation/bind_joint_space.cpp b/python/source/state_representation/bind_joint_space.cpp
index 314085353..26d679781 100644
--- a/python/source/state_representation/bind_joint_space.cpp
+++ b/python/source/state_representation/bind_joint_space.cpp
@@ -50,12 +50,20 @@ void joint_state(py::module_& m) {
   c.def("set_names", py::overload_cast<const std::vector<std::string>&>(&JointState::set_names), "Setter of the names attribute.", "names"_a);
   c.def("set_positions", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_positions), "Setter of the positions attribute from a vector.", "positions"_a);
   c.def("set_positions", py::overload_cast<const std::vector<double>&>(&JointState::set_positions), "Setter of the positions attribute from a list.", "positions"_a);
+  c.def("set_position", py::overload_cast<double, const std::string&>(&JointState::set_position), "Set the position of a joint by its name.", "position"_a, "joint_name"_a);
+  c.def("set_position", py::overload_cast<double, unsigned int>(&JointState::set_position), "Set the position of a joint by its index.", "position"_a, "joint_index"_a);
   c.def("set_velocities", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_velocities), "Setter of the velocities attribute from a vector.", "velocities"_a);
   c.def("set_velocities", py::overload_cast<const std::vector<double>&>(&JointState::set_velocities), "Setter of the velocities attribute from a list.", "velocities"_a);
+  c.def("set_velocity", py::overload_cast<double, const std::string&>(&JointState::set_velocity), "Set the velocity of a joint by its name.", "velocity"_a, "joint_name"_a);
+  c.def("set_velocity", py::overload_cast<double, unsigned int>(&JointState::set_velocity), "Set the velocity of a joint by its index.", "velocity"_a, "joint_index"_a);
   c.def("set_accelerations", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_accelerations), "Setter of the accelerations attribute from a vector.", "accelerations"_a);
   c.def("set_accelerations", py::overload_cast<const std::vector<double>&>(&JointState::set_accelerations), "Setter of the accelerations attribute from a list.", "accelerations"_a);
+  c.def("set_acceleration", py::overload_cast<double, const std::string&>(&JointState::set_acceleration), "Set the acceleration of a joint by its name.", "acceleration"_a, "joint_name"_a);
+  c.def("set_acceleration", py::overload_cast<double, unsigned int>(&JointState::set_acceleration), "Set the acceleration of a joint by its index.", "acceleration"_a, "joint_index"_a);
   c.def("set_torques", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_torques), "Setter of the torques attribute from a vector.", "torques"_a);
   c.def("set_torques", py::overload_cast<const std::vector<double>&>(&JointState::set_torques), "Setter of the torques attribute from a list.", "torques"_a);
+  c.def("set_torque", py::overload_cast<double, const std::string&>(&JointState::set_torque), "Set the torque of a joint by its name.", "torque"_a, "joint_name"_a);
+  c.def("set_torque", py::overload_cast<double, unsigned int>(&JointState::set_torque), "Set the torque of a joint by its index.", "torque"_a, "joint_index"_a);
 
   c.def("set_zero", &JointState::set_zero, "Set the JointState to a zero value.");
   c.def("clamp_state_variable", py::overload_cast<double, const JointStateVariable&, double>(&JointState::clamp_state_variable), "Clamp inplace the magnitude of the a specific state variable.", "value"_a, "state_variable_type"_a, "noise_ratio"_a=double(0));
diff --git a/python/test/robot/test_joint_state.py b/python/test/robot/test_joint_state.py
index 927da933e..5332e36f3 100755
--- a/python/test/robot/test_joint_state.py
+++ b/python/test/robot/test_joint_state.py
@@ -35,8 +35,13 @@
     'set_name',
     'set_names',
     'set_positions',
-    'set_torques',
+    'set_position',
     'set_velocities',
+    'set_velocity',
+    'set_accelerations',
+    'set_acceleration',
+    'set_torques',
+    'set_torque',
     'set_zero',
     'to_list'
 ]
diff --git a/source/state_representation/include/state_representation/robot/JointAccelerations.hpp b/source/state_representation/include/state_representation/robot/JointAccelerations.hpp
index 4688325cb..87fdc7e96 100644
--- a/source/state_representation/include/state_representation/robot/JointAccelerations.hpp
+++ b/source/state_representation/include/state_representation/robot/JointAccelerations.hpp
@@ -20,16 +20,22 @@ class JointAccelerations : public JointState {
   double get_position(const std::string& joint_name) const = delete;
   void set_positions(const Eigen::VectorXd& positions) = delete;
   void set_positions(const std::vector<double>& positions) = delete;
+  void set_position(double position, unsigned int joint_index) const = delete;
+  void set_position(double position, const std::string& joint_name) const = delete;
   const Eigen::VectorXd& get_velocities() const = delete;
   double get_velocity(unsigned int joint_index) const = delete;
   double get_velocity(const std::string& joint_name) const = delete;
   void set_velocities(const Eigen::VectorXd& accelerations) = delete;
   void set_velocities(const std::vector<double>& accelerations) = delete;
+  void set_velocity(double velocity, unsigned int joint_index) const = delete;
+  void set_velocity(double velocity, const std::string& joint_name) const = delete;
   const Eigen::VectorXd& get_torques() const = delete;
   double get_torque(unsigned int joint_index) const = delete;
   double get_torque(const std::string& joint_name) const = delete;
   void set_torques(const Eigen::VectorXd& torques) = delete;
   void set_torques(const std::vector<double>& torques) = delete;
+  void set_torque(double torque, unsigned int joint_index) const = delete;
+  void set_torque(double torque, const std::string& joint_name) const = delete;
 
   /**
    * @brief Empty constructor
diff --git a/source/state_representation/include/state_representation/robot/JointPositions.hpp b/source/state_representation/include/state_representation/robot/JointPositions.hpp
index d8c6830c9..431d2d5b0 100644
--- a/source/state_representation/include/state_representation/robot/JointPositions.hpp
+++ b/source/state_representation/include/state_representation/robot/JointPositions.hpp
@@ -20,16 +20,22 @@ class JointPositions : public JointState {
   double get_velocity(const std::string& joint_name) const = delete;
   void set_velocities(const Eigen::VectorXd& velocities) = delete;
   void set_velocities(const std::vector<double>& velocities) = delete;
+  void set_velocity(double velocity, unsigned int joint_index) const = delete;
+  void set_velocity(double velocity, const std::string& joint_name) const = delete;
   const Eigen::VectorXd& get_accelerations() const = delete;
   double get_acceleration(unsigned int joint_index) const = delete;
   double get_acceleration(const std::string& joint_name) const = delete;
   void set_accelerations(const Eigen::VectorXd& accelerations) = delete;
   void set_accelerations(const std::vector<double>& accelerations) = delete;
+  void set_acceleration(double acceleration, unsigned int joint_index) const = delete;
+  void set_acceleration(double acceleration, const std::string& joint_name) const = delete;
   const Eigen::VectorXd& get_torques() const = delete;
   double get_torque(unsigned int joint_index) const = delete;
   double get_torque(const std::string& joint_name) const = delete;
   void set_torques(const Eigen::VectorXd& torques) = delete;
   void set_torques(const std::vector<double>& torques) = delete;
+  void set_torque(double torque, unsigned int joint_index) const = delete;
+  void set_torque(double torque, const std::string& joint_name) const = delete;
 
   /**
    * @brief Empty constructor
diff --git a/source/state_representation/include/state_representation/robot/JointState.hpp b/source/state_representation/include/state_representation/robot/JointState.hpp
index 5681ced5c..f7ef238ea 100644
--- a/source/state_representation/include/state_representation/robot/JointState.hpp
+++ b/source/state_representation/include/state_representation/robot/JointState.hpp
@@ -235,6 +235,20 @@ class JointState : public State {
    */
   void set_positions(const std::vector<double>& positions);
 
+  /**
+   * @brief Set the position of a joint by its name.
+   * @param position The position of the joint
+   * @param joint_name The name of the joint
+   */
+  void set_position(double position, const std::string& joint_name);
+
+  /**
+   * @brief Set the position of a joint by its index.
+   * @param position The position of the joint
+   * @param joint_index The index of the joint
+   */
+  void set_position(double position, unsigned int joint_index);
+
   /**
    * @brief Getter of the velocities attribute.
    * @return The joint velocities
@@ -269,6 +283,20 @@ class JointState : public State {
    */
   void set_velocities(const std::vector<double>& velocities);
 
+  /**
+   * @brief Set the velocity of a joint by its name.
+   * @param velocity The velocity of the joint
+   * @param joint_name The name of the joint
+   */
+  void set_velocity(double velocity, const std::string& joint_name);
+
+  /**
+   * @brief Set the velocity of a joint by its index.
+   * @param velocity The velocity of the joint
+   * @param joint_index The index of the joint
+   */
+  void set_velocity(double velocity, unsigned int joint_index);
+
   /**
    * @brief Getter of the accelerations attribute.
    * @return The joint accelerations
@@ -303,6 +331,20 @@ class JointState : public State {
    */
   void set_accelerations(const std::vector<double>& accelerations);
 
+  /**
+   * @brief Set the acceleration of a joint by its name.
+   * @param acceleration The acceleration of the joint
+   * @param joint_name The name of the joint
+   */
+  void set_acceleration(double acceleration, const std::string& joint_name);
+
+  /**
+   * @brief Set the acceleration of a joint by its index.
+   * @param acceleration The acceleration of the joint
+   * @param joint_index The index of the joint
+   */
+  void set_acceleration(double acceleration, unsigned int joint_index);
+
   /**
    * @brief Getter of the torques attribute.
    * @return The joint torques
@@ -337,6 +379,20 @@ class JointState : public State {
    */
   void set_torques(const std::vector<double>& torques);
 
+  /**
+   * @brief Set the torque of a joint by its name.
+   * @param torque The torque of the joint
+   * @param joint_name The name of the joint
+   */
+  void set_torque(double torque, const std::string& joint_name);
+
+  /**
+   * @brief Set the torque of a joint by its index.
+   * @param torque The torque of the joint
+   * @param joint_index The index of the joint
+   */
+  void set_torque(double torque, unsigned int joint_index);
+
   /**
    * @brief Check if the state is compatible for operations with the state given as argument.
    * @param state The state to check compatibility with
@@ -626,54 +682,6 @@ inline void JointState::set_names(const std::vector<std::string>& names) {
   this->names_ = names;
 }
 
-inline const Eigen::VectorXd& JointState::get_positions() const {
-  return this->positions_;
-}
-
-inline void JointState::set_positions(const Eigen::VectorXd& positions) {
-  this->set_state_variable(this->positions_, positions);
-}
-
-inline void JointState::set_positions(const std::vector<double>& positions) {
-  this->set_state_variable(this->positions_, positions);
-}
-
-inline const Eigen::VectorXd& JointState::get_velocities() const {
-  return this->velocities_;
-}
-
-inline void JointState::set_velocities(const Eigen::VectorXd& velocities) {
-  this->set_state_variable(this->velocities_, velocities);
-}
-
-inline void JointState::set_velocities(const std::vector<double>& velocities) {
-  this->set_state_variable(this->velocities_, velocities);
-}
-
-inline const Eigen::VectorXd& JointState::get_accelerations() const {
-  return this->accelerations_;
-}
-
-inline void JointState::set_accelerations(const Eigen::VectorXd& accelerations) {
-  this->set_state_variable(this->accelerations_, accelerations);
-}
-
-inline void JointState::set_accelerations(const std::vector<double>& accelerations) {
-  this->set_state_variable(this->accelerations_, accelerations);
-}
-
-inline const Eigen::VectorXd& JointState::get_torques() const {
-  return this->torques_;
-}
-
-inline void JointState::set_torques(const Eigen::VectorXd& torques) {
-  this->set_state_variable(this->torques_, torques);
-}
-
-inline void JointState::set_torques(const std::vector<double>& torques) {
-  this->set_state_variable(this->torques_, torques);
-}
-
 inline Eigen::VectorXd JointState::get_state_variable(const JointStateVariable& state_variable_type) const {
   switch (state_variable_type) {
     case JointStateVariable::POSITIONS:
diff --git a/source/state_representation/include/state_representation/robot/JointTorques.hpp b/source/state_representation/include/state_representation/robot/JointTorques.hpp
index 905022284..35aac58f8 100644
--- a/source/state_representation/include/state_representation/robot/JointTorques.hpp
+++ b/source/state_representation/include/state_representation/robot/JointTorques.hpp
@@ -17,16 +17,22 @@ class JointTorques : public JointState {
   double get_position(const std::string& joint_name) const = delete;
   void set_positions(const Eigen::VectorXd& positions) = delete;
   void set_positions(const std::vector<double>& positions) = delete;
+  void set_position(double position, unsigned int joint_index) const = delete;
+  void set_position(double position, const std::string& joint_name) const = delete;
   const Eigen::VectorXd& get_velocities() const = delete;
   double get_velocity(unsigned int joint_index) const = delete;
   double get_velocity(const std::string& joint_name) const = delete;
   void set_velocities(const Eigen::VectorXd& velocities) = delete;
   void set_velocities(const std::vector<double>& velocities) = delete;
+  void set_velocity(double velocity, unsigned int joint_index) const = delete;
+  void set_velocity(double velocity, const std::string& joint_name) const = delete;
   const Eigen::VectorXd& get_accelerations() const = delete;
   double get_acceleration(unsigned int joint_index) const = delete;
   double get_acceleration(const std::string& joint_name) const = delete;
   void set_accelerations(const Eigen::VectorXd& accelerations) = delete;
   void set_accelerations(const std::vector<double>& accelerations) = delete;
+  void set_acceleration(double acceleration, unsigned int joint_index) const = delete;
+  void set_acceleration(double acceleration, const std::string& joint_name) const = delete;
 
   /**
    * @brief Empty constructor
diff --git a/source/state_representation/include/state_representation/robot/JointVelocities.hpp b/source/state_representation/include/state_representation/robot/JointVelocities.hpp
index e91a9d5c2..d2205ba9a 100644
--- a/source/state_representation/include/state_representation/robot/JointVelocities.hpp
+++ b/source/state_representation/include/state_representation/robot/JointVelocities.hpp
@@ -22,16 +22,22 @@ class JointVelocities : public JointState {
   double get_position(const std::string& joint_name) const = delete;
   void set_positions(const Eigen::VectorXd& positions) = delete;
   void set_positions(const std::vector<double>& positions) = delete;
+  void set_position(double position, unsigned int joint_index) const = delete;
+  void set_position(double position, const std::string& joint_name) const = delete;
   const Eigen::VectorXd& get_accelerations() const = delete;
   double get_acceleration(unsigned int joint_index) const = delete;
   double get_acceleration(const std::string& joint_name) const = delete;
   void set_accelerations(const Eigen::VectorXd& accelerations) = delete;
   void set_accelerations(const std::vector<double>& accelerations) = delete;
+  void set_acceleration(double acceleration, unsigned int joint_index) const = delete;
+  void set_acceleration(double acceleration, const std::string& joint_name) const = delete;
   const Eigen::VectorXd& get_torques() const = delete;
   double get_torque(unsigned int joint_index) const = delete;
   double get_torque(const std::string& joint_name) const = delete;
   void set_torques(const Eigen::VectorXd& torques) = delete;
   void set_torques(const std::vector<double>& torques) = delete;
+  void set_torque(double torque, unsigned int joint_index) const = delete;
+  void set_torque(double torque, const std::string& joint_name) const = delete;
 
   /**
    * @brief Empty constructor
diff --git a/source/state_representation/src/robot/JointState.cpp b/source/state_representation/src/robot/JointState.cpp
index a7321cc63..ddea1a0b8 100644
--- a/source/state_representation/src/robot/JointState.cpp
+++ b/source/state_representation/src/robot/JointState.cpp
@@ -83,6 +83,10 @@ unsigned int JointState::get_joint_index(const std::string& joint_name) const {
   return std::distance(this->names_.begin(), finder);
 }
 
+const Eigen::VectorXd& JointState::get_positions() const {
+  return this->positions_;
+}
+
 double JointState::get_position(const std::string& joint_name) const {
   return this->positions_(this->get_joint_index(joint_name));
 }
@@ -92,6 +96,27 @@ double JointState::get_position(unsigned int joint_index) const {
   return this->positions_(joint_index);
 }
 
+void JointState::set_positions(const Eigen::VectorXd& positions) {
+  this->set_state_variable(this->positions_, positions);
+}
+
+void JointState::set_positions(const std::vector<double>& positions) {
+  this->set_state_variable(this->positions_, positions);
+}
+
+void JointState::set_position(double position, const std::string& joint_name) {
+  this->positions_(this->get_joint_index(joint_name)) = position;
+}
+
+void JointState::set_position(double position, unsigned int joint_index) {
+  assert_index_in_range(joint_index, this->get_size());
+  this->positions_(joint_index) = position;
+}
+
+const Eigen::VectorXd& JointState::get_velocities() const {
+  return this->velocities_;
+}
+
 double JointState::get_velocity(const std::string& joint_name) const {
   return this->velocities_(this->get_joint_index(joint_name));
 }
@@ -101,6 +126,27 @@ double JointState::get_velocity(unsigned int joint_index) const {
   return this->velocities_(joint_index);
 }
 
+void JointState::set_velocities(const Eigen::VectorXd& velocities) {
+  this->set_state_variable(this->velocities_, velocities);
+}
+
+void JointState::set_velocities(const std::vector<double>& velocities) {
+  this->set_state_variable(this->velocities_, velocities);
+}
+
+void JointState::set_velocity(double velocity, const std::string& joint_name) {
+  this->velocities_(this->get_joint_index(joint_name)) = velocity;
+}
+
+void JointState::set_velocity(double velocity, unsigned int joint_index) {
+  assert_index_in_range(joint_index, this->get_size());
+  this->velocities_(joint_index) = velocity;
+}
+
+const Eigen::VectorXd& JointState::get_accelerations() const {
+  return this->accelerations_;
+}
+
 double JointState::get_acceleration(const std::string& joint_name) const {
   return this->accelerations_(this->get_joint_index(joint_name));
 }
@@ -110,6 +156,27 @@ double JointState::get_acceleration(unsigned int joint_index) const {
   return this->accelerations_(joint_index);
 }
 
+void JointState::set_accelerations(const Eigen::VectorXd& accelerations) {
+  this->set_state_variable(this->accelerations_, accelerations);
+}
+
+void JointState::set_accelerations(const std::vector<double>& accelerations) {
+  this->set_state_variable(this->accelerations_, accelerations);
+}
+
+void JointState::set_acceleration(double acceleration, const std::string& joint_name) {
+  this->accelerations_(this->get_joint_index(joint_name)) = acceleration;
+}
+
+void JointState::set_acceleration(double acceleration, unsigned int joint_index) {
+  assert_index_in_range(joint_index, this->get_size());
+  this->accelerations_(joint_index) = acceleration;
+}
+
+const Eigen::VectorXd& JointState::get_torques() const {
+  return this->torques_;
+}
+
 double JointState::get_torque(const std::string& joint_name) const {
   return this->torques_(this->get_joint_index(joint_name));
 }
@@ -119,6 +186,23 @@ double JointState::get_torque(unsigned int joint_index) const {
   return this->torques_(joint_index);
 }
 
+void JointState::set_torques(const Eigen::VectorXd& torques) {
+  this->set_state_variable(this->torques_, torques);
+}
+
+void JointState::set_torques(const std::vector<double>& torques) {
+  this->set_state_variable(this->torques_, torques);
+}
+
+void JointState::set_torque(double torque, const std::string& joint_name) {
+  this->torques_(this->get_joint_index(joint_name)) = torque;
+}
+
+void JointState::set_torque(double torque, unsigned int joint_index) {
+  assert_index_in_range(joint_index, this->get_size());
+  this->torques_(joint_index) = torque;
+}
+
 JointState& JointState::operator+=(const JointState& state) {
   if (!this->is_compatible(state)) {
     throw IncompatibleStatesException(
diff --git a/source/state_representation/test/tests/robot/test_joint_state.cpp b/source/state_representation/test/tests/robot/test_joint_state.cpp
index 411db262d..155f64d2d 100644
--- a/source/state_representation/test/tests/robot/test_joint_state.cpp
+++ b/source/state_representation/test/tests/robot/test_joint_state.cpp
@@ -158,6 +158,43 @@ TEST(JointStateTest, GetSetFields) {
   EXPECT_EQ(js.is_empty(), true);
 }
 
+TEST(JointStateTest, GetSetField) {
+  JointState js("test", 3);
+
+  // fields
+  js.set_position(1.0, "joint0");
+  js.set_position(1.1, 1);
+  EXPECT_EQ(js.get_position(0), 1.0);
+  EXPECT_EQ(js.get_position(1), 1.1);
+  EXPECT_EQ(js.get_position(2), 0);
+  EXPECT_THROW(js.set_position(1, js.get_size() + 1), exceptions::JointNotFoundException);
+  EXPECT_THROW(js.set_position(1, "test"), exceptions::JointNotFoundException);
+
+  js.set_velocity(2.0, "joint0");
+  js.set_velocity(2.2, 1);
+  EXPECT_EQ(js.get_velocity(0), 2.0);
+  EXPECT_EQ(js.get_velocity(1), 2.2);
+  EXPECT_EQ(js.get_velocity(2), 0);
+  EXPECT_THROW(js.set_velocity(1, js.get_size() + 1), exceptions::JointNotFoundException);
+  EXPECT_THROW(js.set_velocity(1, "test"), exceptions::JointNotFoundException);
+
+  js.set_acceleration(3.0, "joint0");
+  js.set_acceleration(3.3, 1);
+  EXPECT_EQ(js.get_acceleration(0), 3);
+  EXPECT_EQ(js.get_acceleration(1), 3.3);
+  EXPECT_EQ(js.get_acceleration(2), 0);
+  EXPECT_THROW(js.set_acceleration(1, js.get_size() + 1), exceptions::JointNotFoundException);
+  EXPECT_THROW(js.set_acceleration(1, "test"), exceptions::JointNotFoundException);
+
+  js.set_torque(4.0, "joint0");
+  js.set_torque(4.4, 1);
+  EXPECT_EQ(js.get_torque(0), 4);
+  EXPECT_EQ(js.get_torque(1), 4.4);
+  EXPECT_EQ(js.get_torque(2), 0);
+  EXPECT_THROW(js.set_torque(1, js.get_size() + 1), exceptions::JointNotFoundException);
+  EXPECT_THROW(js.set_torque(1, "test"), exceptions::JointNotFoundException);
+}
+
 TEST(JointStateTest, Compatibility) {
   JointState js1("test", 3);
   JointState js2("test", std::vector<std::string>{"j1", "j2", "j3"});

From c27b63f17d5a65d0c7ec50908f972ad24f94b61a Mon Sep 17 00:00:00 2001
From: Baptiste Busch <baptiste.busch@gmail.com>
Date: Mon, 18 Oct 2021 10:40:31 +0200
Subject: [PATCH 17/22] Add the State set_timestamp function (#218)

* feat: add the setter for the state timestamp attribute

* feat: add the python bindings for the set_timestamp function

* fix: update changelog

* fix: correct the binding to the time module

Co-authored-by: domire8 <71256590+domire8@users.noreply.github.com>

* fix: add the `set_timestamp` function in the list of expected functions

* fix: add missing import

* Set timestamp when decoding message (#219)

* Specify nanosecond count since epoch as the integer timestamp
representation in state.proto

* Add an explicit duration type of nanoseconds to clproto header

* Set the timestamp when decoding a message into a State

Co-authored-by: domire8 <71256590+domire8@users.noreply.github.com>
Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>
---
 CHANGELOG.md                                           |  1 +
 protocol/clproto_cpp/include/clproto.h                 |  8 ++++++++
 protocol/clproto_cpp/include/clproto/encoders.h        |  1 +
 protocol/clproto_cpp/src/clproto.cpp                   |  5 ++---
 protocol/clproto_cpp/src/encoders.cpp                  |  3 ++-
 protocol/clproto_cpp/test/tests/test_messages.cpp      |  4 +++-
 .../protobuf/proto/state_representation/state.proto    |  2 +-
 python/source/state_representation/bind_state.cpp      |  1 +
 python/test/test_state.py                              |  6 ++++++
 .../include/state_representation/State.hpp             | 10 ++++++++++
 source/state_representation/test/tests/test_state.cpp  |  4 ++++
 11 files changed, 39 insertions(+), 6 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index b2bcd85c4..4a17e7bd9 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -21,6 +21,7 @@ Release Versions:
 - Improve CartesianState tests in C++ and Python (#213)
 - Add build testing option to clproto install (#216)
 - Add method to set a joint state by name or index of the joint (#217)
+- Add the `set_timestamp` method to the `State` base class (#218)
 
 ### Pending TODOs for the next release
 
diff --git a/protocol/clproto_cpp/include/clproto.h b/protocol/clproto_cpp/include/clproto.h
index e6d2371b8..d05d7f385 100644
--- a/protocol/clproto_cpp/include/clproto.h
+++ b/protocol/clproto_cpp/include/clproto.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <chrono>
 #include <stdexcept>
 #include <string>
 #include <vector>
@@ -21,6 +22,13 @@ namespace clproto {
  */
 typedef std::size_t field_length_t;
 
+/**
+ * @typedef timestamp_duration_t
+ * @brief Duration type to use when representing
+ * chrono timestamps as integer count since epoch
+ */
+typedef std::chrono::nanoseconds timestamp_duration_t;
+
 /**
  * @class DecodingException
  * @brief A DecodingException is raised whenever a
diff --git a/protocol/clproto_cpp/include/clproto/encoders.h b/protocol/clproto_cpp/include/clproto/encoders.h
index 6cea03e4e..d79eccd81 100644
--- a/protocol/clproto_cpp/include/clproto/encoders.h
+++ b/protocol/clproto_cpp/include/clproto/encoders.h
@@ -9,6 +9,7 @@
 #include <state_representation/robot/JointState.hpp>
 #include <state_representation/parameters/Parameter.hpp>
 
+#include "clproto.h"
 #include "state_representation/state_message.pb.h"
 
 namespace clproto {
diff --git a/protocol/clproto_cpp/src/clproto.cpp b/protocol/clproto_cpp/src/clproto.cpp
index 0bdb10a97..b65fe701d 100644
--- a/protocol/clproto_cpp/src/clproto.cpp
+++ b/protocol/clproto_cpp/src/clproto.cpp
@@ -190,9 +190,8 @@ bool decode(const std::string& msg, State& obj) {
 
     auto state = message.state();
     obj = State(decoder(state.type()), state.name(), state.empty());
-
-    //TODO: (maybe) add set_timestamp method to State and add decoder for int to chrono
-    //obj.set_timestamp(state.timestamp());
+    std::chrono::time_point<std::chrono::steady_clock> timepoint(timestamp_duration_t(state.timestamp()));
+    obj.set_timestamp(timepoint);
 
     return true;
   } catch (...) {
diff --git a/protocol/clproto_cpp/src/encoders.cpp b/protocol/clproto_cpp/src/encoders.cpp
index aee63e145..03b614a2f 100644
--- a/protocol/clproto_cpp/src/encoders.cpp
+++ b/protocol/clproto_cpp/src/encoders.cpp
@@ -17,7 +17,8 @@ proto::State encoder(const State& state) {
   message.set_name(state.get_name());
   message.set_type(encoder(state.get_type()));
   message.set_empty(state.is_empty());
-  message.set_timestamp(state.get_timestamp().time_since_epoch().count());
+  auto timestamp = std::chrono::duration_cast<timestamp_duration_t>(state.get_timestamp().time_since_epoch());
+  message.set_timestamp(timestamp.count());
   return message;
 }
 
diff --git a/protocol/clproto_cpp/test/tests/test_messages.cpp b/protocol/clproto_cpp/test/tests/test_messages.cpp
index 5176e87a1..e8490a384 100644
--- a/protocol/clproto_cpp/test/tests/test_messages.cpp
+++ b/protocol/clproto_cpp/test/tests/test_messages.cpp
@@ -17,8 +17,10 @@ TEST(MessageProtoTest, EncodeDecodeState) {
   EXPECT_TRUE(clproto::decode(msg, recv_state));
 
   EXPECT_EQ(send_state.is_empty(), recv_state.is_empty());
-  EXPECT_EQ(recv_state.get_type(), StateType::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_EQ(send_state.get_timestamp().time_since_epoch().count(),
+            recv_state.get_timestamp().time_since_epoch().count());
 }
 
 TEST(MessageProtoTest, DecodeInvalidString) {
diff --git a/protocol/protobuf/proto/state_representation/state.proto b/protocol/protobuf/proto/state_representation/state.proto
index a726adbd6..81d88c892 100644
--- a/protocol/protobuf/proto/state_representation/state.proto
+++ b/protocol/protobuf/proto/state_representation/state.proto
@@ -34,5 +34,5 @@ message State {
   string name = 1;
   StateType type = 2;
   bool empty = 3;
-  int64 timestamp = 4;
+  int64 timestamp = 4;  // timestamp is encoded as nanoseconds since the system steady_clock epoch
 }
\ No newline at end of file
diff --git a/python/source/state_representation/bind_state.cpp b/python/source/state_representation/bind_state.cpp
index 71982acc4..60e787319 100644
--- a/python/source/state_representation/bind_state.cpp
+++ b/python/source/state_representation/bind_state.cpp
@@ -43,6 +43,7 @@ void state(py::module_& m) {
   c.def("set_empty", &State::set_empty, "Setter of the empty attribute", "empty"_a=true);
   c.def("set_filled", &State::set_filled, "Setter of the empty attribute to false and also reset the timestamp");
   c.def("get_timestamp", &State::get_timestamp, "Getter of the timestamp attribute");
+  c.def("set_timestamp", &State::set_timestamp, "Setter of the timestamp attribute");
   c.def("reset_timestamp", &State::reset_timestamp, "Reset the timestamp attribute to now");
   c.def("get_name", &State::get_name, "Getter of the name");
   c.def("set_name", &State::set_name, "Setter of the name");
diff --git a/python/test/test_state.py b/python/test/test_state.py
index 457a6c291..b63b2a321 100644
--- a/python/test/test_state.py
+++ b/python/test/test_state.py
@@ -1,5 +1,6 @@
 import time
 import unittest
+import datetime
 
 from state_representation import State, StateType
 
@@ -9,6 +10,7 @@
     'set_empty',
     'set_filled',
     'get_timestamp',
+    'set_timestamp',
     'reset_timestamp',
     'get_name',
     'set_name',
@@ -69,6 +71,10 @@ def test_timestamp(self):
         self.assertTrue(state.is_deprecated(0.1))
         state.reset_timestamp()
         self.assertFalse(state.is_deprecated(0.1))
+        time.sleep(0.2)
+        self.assertTrue(state.is_deprecated(0.1))
+        state.set_timestamp(datetime.datetime.now().timestamp())
+        self.assertFalse(state.is_deprecated(0.1))
 
 
 if __name__ == '__main__':
diff --git a/source/state_representation/include/state_representation/State.hpp b/source/state_representation/include/state_representation/State.hpp
index 20a5d3a80..99c03f795 100644
--- a/source/state_representation/include/state_representation/State.hpp
+++ b/source/state_representation/include/state_representation/State.hpp
@@ -116,6 +116,12 @@ class State {
    */
   const std::chrono::time_point<std::chrono::steady_clock>& get_timestamp() const;
 
+  /**
+   * @brief Setter of the timestamp attribute
+   * @param timepoint the new value for the timestamp
+   */
+  void set_timestamp(const std::chrono::time_point<std::chrono::steady_clock>& timepoint);
+
   /**
    * @brief Reset the timestamp attribute to now
    */
@@ -210,6 +216,10 @@ inline const std::chrono::time_point<std::chrono::steady_clock>& State::get_time
   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();
 }
diff --git a/source/state_representation/test/tests/test_state.cpp b/source/state_representation/test/tests/test_state.cpp
index e9a42a781..4da2a32ea 100644
--- a/source/state_representation/test/tests/test_state.cpp
+++ b/source/state_representation/test/tests/test_state.cpp
@@ -52,6 +52,10 @@ TEST(StateTest, Timestamp) {
   EXPECT_TRUE(state.is_deprecated(std::chrono::milliseconds(100)));
   state.reset_timestamp();
   EXPECT_FALSE(state.is_deprecated(std::chrono::milliseconds(100)));
+  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  EXPECT_TRUE(state.is_deprecated(std::chrono::milliseconds(100)));
+  state.set_timestamp(std::chrono::steady_clock::now());
+  EXPECT_FALSE(state.is_deprecated(std::chrono::milliseconds(100)));
 }
 
 TEST(StateTest, Swap) {

From 62d6e67dc0cf71a0e88bd50e7f2b31c21be8ae65 Mon Sep 17 00:00:00 2001
From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>
Date: Tue, 19 Oct 2021 11:23:45 +0200
Subject: [PATCH 18/22] Throw when decoding wrong message type (#220)

* Remove fallback if condition in clproto decode functions that
allowed messages to be parsed into the respective mutable fields
even if the message type was not matching.

* Add test case to check expectation of error when decoding
the "parallel" (wrong but valid) type
---
 protocol/clproto_cpp/src/clproto.cpp          | 58 +++++--------------
 .../clproto_cpp/test/tests/test_messages.cpp  | 12 ++++
 2 files changed, 27 insertions(+), 43 deletions(-)

diff --git a/protocol/clproto_cpp/src/clproto.cpp b/protocol/clproto_cpp/src/clproto.cpp
index b65fe701d..7a616f41c 100644
--- a/protocol/clproto_cpp/src/clproto.cpp
+++ b/protocol/clproto_cpp/src/clproto.cpp
@@ -183,9 +183,7 @@ bool decode(const std::string& msg, State& obj) {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg)
         && message.message_type_case() == proto::StateMessage::MessageTypeCase::kState)) {
-      if (!message.mutable_state()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
 
     auto state = message.state();
@@ -228,9 +226,7 @@ bool decode(const std::string& msg, SpatialState& obj) {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg)
         && message.message_type_case() == proto::StateMessage::MessageTypeCase::kSpatialState)) {
-      if (!message.mutable_spatial_state()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
 
     auto spatial_state = message.spatial_state();
@@ -273,9 +269,7 @@ bool decode(const std::string& msg, CartesianState& obj) {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg)
         && message.message_type_case() == proto::StateMessage::MessageTypeCase::kCartesianState)) {
-      if (!message.mutable_cartesian_state()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
 
     auto state = message.cartesian_state();
@@ -328,9 +322,7 @@ bool decode(const std::string& msg, CartesianPose& obj) {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg)
         && message.message_type_case() == proto::StateMessage::MessageTypeCase::kCartesianPose)) {
-      if (!message.mutable_cartesian_pose()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
     auto pose = message.cartesian_pose();
     obj.set_name(pose.spatial_state().state().name());
@@ -376,9 +368,7 @@ bool decode(const std::string& msg, CartesianTwist& obj) {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg)
         && message.message_type_case() == proto::StateMessage::MessageTypeCase::kCartesianTwist)) {
-      if (!message.mutable_cartesian_twist()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
     auto twist = message.cartesian_twist();
     obj.set_name(twist.spatial_state().state().name());
@@ -424,9 +414,7 @@ bool decode(const std::string& msg, CartesianWrench& obj) {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg)
         && message.message_type_case() == proto::StateMessage::MessageTypeCase::kCartesianWrench)) {
-      if (!message.mutable_cartesian_wrench()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
     auto wrench = message.cartesian_wrench();
     obj.set_name(wrench.spatial_state().state().name());
@@ -469,9 +457,7 @@ bool decode(const std::string& msg, Jacobian& obj) {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg)
         && message.message_type_case() == proto::StateMessage::MessageTypeCase::kJacobian)) {
-      if (!message.mutable_jacobian()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
 
     auto jacobian = message.jacobian();
@@ -519,9 +505,7 @@ bool decode(const std::string& msg, JointState& obj) {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg)
         && message.message_type_case() == proto::StateMessage::MessageTypeCase::kJointState)) {
-      if (!message.mutable_joint_state()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
 
     auto state = message.joint_state();
@@ -569,9 +553,7 @@ bool decode(const std::string& msg, JointPositions& obj) {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg)
         && message.message_type_case() == proto::StateMessage::MessageTypeCase::kJointPositions)) {
-      if (!message.mutable_joint_positions()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
 
     auto state = message.joint_positions();
@@ -616,9 +598,7 @@ bool decode(const std::string& msg, JointVelocities& obj) {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg)
         && message.message_type_case() == proto::StateMessage::MessageTypeCase::kJointVelocities)) {
-      if (!message.mutable_joint_velocities()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
 
     auto state = message.joint_velocities();
@@ -663,9 +643,7 @@ bool decode(const std::string& msg, JointAccelerations& obj) {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg)
         && message.message_type_case() == proto::StateMessage::MessageTypeCase::kJointAccelerations)) {
-      if (!message.mutable_joint_accelerations()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
 
     auto state = message.joint_accelerations();
@@ -710,9 +688,7 @@ bool decode(const std::string& msg, JointTorques& obj) {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg)
         && message.message_type_case() == proto::StateMessage::MessageTypeCase::kJointTorques)) {
-      if (!message.mutable_joint_torques()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
 
     auto state = message.joint_torques();
@@ -758,9 +734,7 @@ static bool decode_parameter(const std::string& msg, Parameter<T>& obj) {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg)
         && message.message_type_case() == proto::StateMessage::MessageTypeCase::kParameter)) {
-      if (!message.mutable_parameter()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
     obj = decoder<T>(message.parameter());
     if (message.parameter().state().empty()) {
@@ -783,7 +757,7 @@ template<>
 bool decode(const std::string& msg, Parameter<int>& obj);
 template<>
 std::string encode<Parameter<int>>(const Parameter<int>& obj) {
-return encode_parameter(obj);
+  return encode_parameter(obj);
 }
 template<>
 Parameter<int> decode(const std::string& msg) {
@@ -1015,9 +989,7 @@ template<> bool decode(const std::string& msg, __TYPE__& obj) {
   try {
     proto::StateMessage message;
     if (!(message.ParseFromString(msg) && message.message_type_case() == proto::StateMessage::MessageTypeCase::k__TYPE__)) {
-      if (!message.mutable___TYPE__()->ParseFromString(msg)) {
-        return false;
-      }
+      return false;
     }
     // decode
     return true;
diff --git a/protocol/clproto_cpp/test/tests/test_messages.cpp b/protocol/clproto_cpp/test/tests/test_messages.cpp
index e8490a384..2c6a05d5a 100644
--- a/protocol/clproto_cpp/test/tests/test_messages.cpp
+++ b/protocol/clproto_cpp/test/tests/test_messages.cpp
@@ -1,6 +1,8 @@
 #include <gtest/gtest.h>
 
 #include <state_representation/State.hpp>
+#include <state_representation/space/cartesian/CartesianState.hpp>
+#include <state_representation/space/cartesian/CartesianPose.hpp>
 
 #include "clproto.h"
 
@@ -36,6 +38,16 @@ TEST(MessageProtoTest, DecodeInvalidString) {
   EXPECT_THROW(clproto::decode<State>(dummy_msg), clproto::DecodingException);
 }
 
+TEST(MessageProtoTest, DecodeParallelTypes) {
+  auto state = CartesianState::Random("A", "B");
+  auto pose = CartesianPose::Random("C", "D");
+  auto encoded_state = clproto::encode(state);
+  auto encoded_pose = clproto::encode(pose);
+
+  EXPECT_THROW(clproto::decode<CartesianState>(encoded_pose), clproto::DecodingException);
+  EXPECT_THROW(clproto::decode<CartesianPose>(encoded_state), clproto::DecodingException);
+}
+
 /* If an encode / decode template is invoked that is not implemented in clproto,
  * there will be a linker error "undefined reference" at compile time.
  * Of course, it's not really possible to test this at run-time.

From 49e5d0a6b1b036feaae9615550f0e402173f4530 Mon Sep 17 00:00:00 2001
From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com>
Date: Mon, 8 Nov 2021 15:51:38 +0100
Subject: [PATCH 19/22] Specify fixed width type for field size (#222)

* Use uint32_t instead of size_t for the field length type when
encoding packets, so that it is more reliable across systems.

* Remove unintentional typedef duplication in source file

* Update CHANGELOG
---
 CHANGELOG.md                           | 1 +
 protocol/clproto_cpp/include/clproto.h | 3 ++-
 protocol/clproto_cpp/src/clproto.cpp   | 2 --
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 4a17e7bd9..451d3955d 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -22,6 +22,7 @@ Release Versions:
 - Add build testing option to clproto install (#216)
 - Add method to set a joint state by name or index of the joint (#217)
 - Add the `set_timestamp` method to the `State` base class (#218)
+- Fix the field size type for clproto pack_fields (#222)
 
 ### Pending TODOs for the next release
 
diff --git a/protocol/clproto_cpp/include/clproto.h b/protocol/clproto_cpp/include/clproto.h
index d05d7f385..77392dbfd 100644
--- a/protocol/clproto_cpp/include/clproto.h
+++ b/protocol/clproto_cpp/include/clproto.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <cstdint>
 #include <chrono>
 #include <stdexcept>
 #include <string>
@@ -20,7 +21,7 @@ namespace clproto {
  * and field data length in ::pack_fields() and
  * ::unpack_fields() methods
  */
-typedef std::size_t field_length_t;
+typedef uint32_t field_length_t;
 
 /**
  * @typedef timestamp_duration_t
diff --git a/protocol/clproto_cpp/src/clproto.cpp b/protocol/clproto_cpp/src/clproto.cpp
index 7a616f41c..2e9d4d92e 100644
--- a/protocol/clproto_cpp/src/clproto.cpp
+++ b/protocol/clproto_cpp/src/clproto.cpp
@@ -65,8 +65,6 @@ ParameterMessageType check_parameter_message_type(const std::string& msg) {
   return ParameterMessageType::UNKNOWN_PARAMETER;
 }
 
-typedef std::size_t field_length_t;
-
 // --- Serialization methods --- //
 
 void pack_fields(const std::vector<std::string>& fields, char* data) {

From e60954ca2618eb30cb809dcaf0f6925ec236d187 Mon Sep 17 00:00:00 2001
From: domire8 <71256590+domire8@users.noreply.github.com>
Date: Tue, 9 Nov 2021 09:57:05 +0100
Subject: [PATCH 20/22] Add proto Dockerfile to copy protobuf files from (#221)

---
 .github/workflows/build-push.yml | 22 ++++++++++++++++++++++
 Dockerfile.base                  | 20 +++-----------------
 Dockerfile.proto                 | 32 ++++++++++++++++++++++++++++++++
 3 files changed, 57 insertions(+), 17 deletions(-)
 create mode 100644 Dockerfile.proto

diff --git a/.github/workflows/build-push.yml b/.github/workflows/build-push.yml
index 77fa76ea9..19eb42335 100644
--- a/.github/workflows/build-push.yml
+++ b/.github/workflows/build-push.yml
@@ -9,7 +9,29 @@ on:
 
 jobs:
 
+  build-publish-proto-dependencies:
+    runs-on: ubuntu-latest
+    name: Build and publish proto dependencies image
+    steps:
+      - name: Checkout Repository
+        uses: actions/checkout@v2
+
+      - name: Build image
+        run: |
+          docker build . --file ./Dockerfile.proto  --tag proto-dependencies
+
+      - name: Login to GitHub Container Registry
+        run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.actor }} --password-stdin
+
+      - name: Push image
+        run: |
+          IMAGE_NAME=${{ github.repository }}/proto-dependencies:latest
+          IMAGE_NAME=${IMAGE_NAME/_/-}
+          docker tag proto-dependencies ghcr.io/${IMAGE_NAME}
+          docker push ghcr.io/${IMAGE_NAME}
+
   build-publish-development-dependencies:
+    needs: build-publish-proto-dependencies
     runs-on: ubuntu-latest
     name: Build and publish development dependencies image
     steps:
diff --git a/Dockerfile.base b/Dockerfile.base
index 970380dd5..fc19201a2 100644
--- a/Dockerfile.base
+++ b/Dockerfile.base
@@ -50,7 +50,6 @@ RUN rm -rf /tmp/*
 
 
 FROM core-build-dependencies as google-dependencies
-ENV PROTOBUF_VERSION 3.17.0
 
 RUN apt-get update && apt-get install -y \
     libgtest-dev \
@@ -63,19 +62,6 @@ RUN cmake /usr/src/gtest \
     && make \
     && cp lib/* /usr/local/lib || cp *.a /usr/local/lib
 
-# install protobuf
-WORKDIR /tmp
-RUN wget -O protobuf-cpp-"${PROTOBUF_VERSION}".tar.gz \
-    https://github.com/protocolbuffers/protobuf/releases/download/v"${PROTOBUF_VERSION}"/protobuf-cpp-"${PROTOBUF_VERSION}".tar.gz \
-    && tar -xzf protobuf-cpp-"${PROTOBUF_VERSION}".tar.gz \
-    && rm protobuf-cpp-"${PROTOBUF_VERSION}".tar.gz
-
-WORKDIR /tmp/protobuf-"${PROTOBUF_VERSION}"
-RUN ./autogen.sh \
-    &&./configure \
-    && make \
-    && make install
-
 WORKDIR /home
 RUN rm -rf /tmp/*
 
@@ -103,8 +89,8 @@ RUN pip3 install numpy setuptools pybind11
 
 # install google dependencies
 COPY --from=google-dependencies /usr/include/gtest /usr/include/gtest
-COPY --from=google-dependencies /usr/local/include/google /usr/local/include/google
+COPY --from=ghcr.io/epfl-lasa/control-libraries/proto-dependencies /usr/local/include/google /usr/local/include/google
 COPY --from=google-dependencies /usr/local/lib/libgtest* /usr/local/lib/
-COPY --from=google-dependencies /usr/local/lib/libproto* /usr/local/lib/
-COPY --from=google-dependencies /usr/local/bin/protoc /usr/local/bin
+COPY --from=ghcr.io/epfl-lasa/control-libraries/proto-dependencies /usr/local/lib/libproto* /usr/local/lib/
+COPY --from=ghcr.io/epfl-lasa/control-libraries/proto-dependencies /usr/local/bin/protoc /usr/local/bin
 RUN ldconfig
diff --git a/Dockerfile.proto b/Dockerfile.proto
new file mode 100644
index 000000000..3ddcc1997
--- /dev/null
+++ b/Dockerfile.proto
@@ -0,0 +1,32 @@
+FROM ubuntu:20.04 AS build-stage
+ARG PROTOBUF_VERSION=3.17.0
+ENV DEBIAN_FRONTEND=noninteractive
+
+RUN apt-get update && apt-get install -y \
+    autoconf \
+    cmake \
+    g++ \
+    gcc \
+    libtool \
+    make \
+    wget \
+    && apt-get clean \
+    && rm -rf /var/lib/apt/lists/*
+
+WORKDIR /tmp
+RUN wget -O protobuf-cpp-"${PROTOBUF_VERSION}".tar.gz \
+    https://github.com/protocolbuffers/protobuf/releases/download/v"${PROTOBUF_VERSION}"/protobuf-cpp-"${PROTOBUF_VERSION}".tar.gz \
+    && tar -xzf protobuf-cpp-"${PROTOBUF_VERSION}".tar.gz \
+    && rm protobuf-cpp-"${PROTOBUF_VERSION}".tar.gz
+
+WORKDIR /tmp/protobuf-"${PROTOBUF_VERSION}"
+RUN ./autogen.sh \
+    && ./configure \
+    && make \
+    && make install
+
+FROM ubuntu:20.04 AS google-dependencies
+COPY --from=build-stage /usr/local/include/google /usr/local/include/google
+COPY --from=build-stage /usr/local/lib/libproto* /usr/local/lib/
+COPY --from=build-stage /usr/local/bin/protoc /usr/local/bin
+RUN ldconfig

From d8dc7e80b5c5e27aea77a775fb72d23066e7fcac Mon Sep 17 00:00:00 2001
From: Dominic Reber <dominic.reber@epfl.ch>
Date: Fri, 12 Nov 2021 10:59:24 +0100
Subject: [PATCH 21/22] Update version number

---
 doxygen/doxygen.conf                | 2 +-
 protocol/clproto_cpp/CMakeLists.txt | 2 +-
 python/setup.py                     | 2 +-
 source/CMakeLists.txt               | 2 +-
 4 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf
index c3223e0e1..04f4f45ce 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         = 4.0.0
+PROJECT_NUMBER         = 4.1.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/clproto_cpp/CMakeLists.txt b/protocol/clproto_cpp/CMakeLists.txt
index b5e963a8c..9f437695a 100644
--- a/protocol/clproto_cpp/CMakeLists.txt
+++ b/protocol/clproto_cpp/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.9)
-project(clproto VERSION 4.0.0)
+project(clproto VERSION 4.1.0)
 
 set(CMAKE_CXX_STANDARD 17)
 
diff --git a/python/setup.py b/python/setup.py
index 333f54ba3..033850008 100644
--- a/python/setup.py
+++ b/python/setup.py
@@ -5,7 +5,7 @@
 import warnings
 import os
 
-__version__ = "4.0.0"
+__version__ = "4.1.0"
 __libraries__ = ['state_representation', 'clproto']
 __include_dirs__ = ['include']
 
diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt
index 9f764cbfe..078ae52a8 100644
--- a/source/CMakeLists.txt
+++ b/source/CMakeLists.txt
@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.9)
 
-project(control-libraries VERSION 4.0.0)
+project(control-libraries VERSION 4.1.0)
 
 # Build options
 option(BUILD_TESTING "Build all tests." OFF)

From 0b4a198471238f8f96cff06b99f7ae683641fd66 Mon Sep 17 00:00:00 2001
From: Dominic Reber <dominic.reber@epfl.ch>
Date: Fri, 12 Nov 2021 11:08:31 +0100
Subject: [PATCH 22/22] Update CHANGELOG

---
 CHANGELOG.md | 31 ++++++++++++++++++++-----------
 1 file changed, 20 insertions(+), 11 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 451d3955d..559652986 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,33 +1,42 @@
 # CHANGELOG
 
 Release Versions:
+- [4.1.0](#410)
 - [4.0.0](#400)
 - [3.1.0](#310)
 - [3.0.0](#300)
 - [2.0.0](#200)
 - [1.0.0](#100)
 
-## Upcoming changes (in development)
+## 4.1.0
 
-- Python bindings for clproto (#204)
+Version 4.1.0 contains a few improvements to the behaviour and usage of the libraries,
+namely to the Python bindings and the `clproto` serialization library.
+
+### Fixes and improvements
+
+**clproto**
 - JSON conversion support for clproto (#205)
 - Support empty state objects in clproto (#207)
 - Add int and int array parameters (#208)
-- Add method to get a joint state by name or index of the joint,
-  and to get the index of the joint by its name (#210)
-- Add possibility to have a ssh server for Python testing (#211)
+- Add build testing option to clproto install (#216)
+- Fix the field size type for clproto pack_fields (#222)
+
+**python**
+- Python bindings for clproto (#204)
 - Add Python bindings for Parameter class (#209)
+- Add possibility to have a ssh server for Python testing (#211)
 - Add Python bindings for clproto encode / decode of Parameter class (#214)
+
+**state_representation**
+- Add method to get a joint state by name or index of the joint,
+  and to get the index of the joint by its name (#210)
 - Improve CartesianState tests in C++ and Python (#213)
-- Add build testing option to clproto install (#216)
 - Add method to set a joint state by name or index of the joint (#217)
 - Add the `set_timestamp` method to the `State` base class (#218)
-- Fix the field size type for clproto pack_fields (#222)
 
-### Pending TODOs for the next release
-
-- Including a quaternion extension (numpy-quaternion, pyquaternion, ...)
-  for binding the Eigen::Quaternion to a specific object in Python.
+**general**
+- Add proto Dockerfile to copy protobuf files from (#221)
 
 ## 4.0.0