diff --git a/data/RobotAPI/sensors/OptoForceCalibration.xml b/data/RobotAPI/sensors/OptoForceCalibration.xml
new file mode 100644
index 0000000000000000000000000000000000000000..b863b0de893b1dc467e5ff2b0418c163539badc5
--- /dev/null
+++ b/data/RobotAPI/sensors/OptoForceCalibration.xml
@@ -0,0 +1,11 @@
+<Calibration>
+    <Daq serialNumber="KIT0A003">
+        <Sensor nominalCapacity="10" counts_at_nc="3870" name="DSE0A102"/>
+        <Sensor nominalCapacity="10" counts_at_nc="4001" name="DSE0A103"/>
+        <Sensor nominalCapacity="10" counts_at_nc="4021" name="DSE0A104"/>
+        <Sensor nominalCapacity="10" counts_at_nc="4048" name="DSE0A108"/>
+    </Daq>
+    <Daq serialNumber="DSE0A114">
+        <Sensor nominalCapacity="10" counts_at_nc="4048" name="DSE0A114" />
+    </Daq>
+</Calibration>
diff --git a/etc/cmake/FindOptoForceOMD.cmake b/etc/cmake/FindOptoForceOMD.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..44b699650be8468416e08521b40a0b41a4cf8e4f
--- /dev/null
+++ b/etc/cmake/FindOptoForceOMD.cmake
@@ -0,0 +1,49 @@
+# - Try to find OptoForceOMD
+# Once done this will define
+#
+#  OptoForceOMD_FOUND - OptoForceOMD found
+#  OptoForceOMD_INCLUDE_DIR - the OptoForceOMD include directory
+#  OptoForceOMD_LIBRARIES - OptoForceOMD library
+#
+
+FIND_PATH(OptoForceOMD_INCLUDE_DIR NAMES opto.h
+  PATHS
+  $ENV{OptoForceOMD_DIR}/include/
+  $ENV{OptoForceOMD_DIR}/include/OptoForceOMD/
+  ${OptoForceOMD_DIR}/include/
+  ${OptoForceOMD_DIR}/include/OptoForceOMD/
+  ENV CPATH
+  /usr/include/OptoForceOMD/
+  /usr/local/include/OptoForceOMD/
+  /opt/local/include/OptoForceOMD/
+  NO_DEFAULT_PATH
+)
+
+
+FIND_LIBRARY(OptoForceOMD_LIBRARIES NAMES libOMD.so
+  PATHS
+  $ENV{OptoForceOMD_DIR}/lib
+  ${OptoForceOMD_DIR}/lib
+  ENV LD_LIBRARY_PATH
+  ENV LIBRARY_PATH
+  /usr/lib
+  /usr/local/lib
+  /opt/local/lib
+  NO_DEFAULT_PATH
+)
+
+include(FindPackageHandleStandardArgs)
+# handle the QUIETLY and REQUIRED arguments and set OODL_YOUBOT_FOUND to TRUE
+# if all listed variables are TRUE
+find_package_handle_standard_args(OptoForceOMD DEFAULT_MSG
+                                  OptoForceOMD_LIBRARIES OptoForceOMD_INCLUDE_DIR)
+
+set(OptoForceOMD_FOUND ${OPTOFORCEOMD_FOUND}) # CMake UPPERCASE-FUNTIME!
+
+#message( "OptoForceOMD_FOUND:" ${OptoForceOMD_FOUND})
+#message( "OPTOFORCEOMD_FOUND:" ${OPTOFORCEOMD_FOUND})
+#message( "OptoForceOMD_LIBRARIES:" ${OptoForceOMD_LIBRARIES})
+#message( "OptoForceOMD_INCLUDE_DIR:" ${OptoForceOMD_INCLUDE_DIR})
+
+# show the OptoForceOMD_INCLUDE_DIR and OptoForceOMD_LIBRARY_DIR variables only in the advanced view
+MARK_AS_ADVANCED(OptoForceOMD_INCLUDE_DIR OptoForceOMD_LIBRARIES)
diff --git a/ControllerDataFlow.svg b/etc/doxygen/images/ControllerDataFlow.svg
similarity index 100%
rename from ControllerDataFlow.svg
rename to etc/doxygen/images/ControllerDataFlow.svg
diff --git a/ControllerDataFlow.xml b/etc/doxygen/images/ControllerDataFlow.xml
similarity index 100%
rename from ControllerDataFlow.xml
rename to etc/doxygen/images/ControllerDataFlow.xml
diff --git a/etc/doxygen/images/LVL1ControllerAtomicDataFlow.svg b/etc/doxygen/images/LVL1ControllerAtomicDataFlow.svg
new file mode 100644
index 0000000000000000000000000000000000000000..ce7ead509613aad829ae9e967dddc45097ab94c9
--- /dev/null
+++ b/etc/doxygen/images/LVL1ControllerAtomicDataFlow.svg
@@ -0,0 +1,2 @@
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg xmlns="http://www.w3.org/2000/svg" style="background-color: rgb(255, 255, 255);" xmlns:xlink="http://www.w3.org/1999/xlink" width="602px" height="302px" version="1.1" content="&lt;mxfile userAgent=&quot;Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:51.0) Gecko/20100101 Firefox/51.0&quot; version=&quot;6.1.0.3&quot; editor=&quot;www.draw.io&quot; type=&quot;device&quot;&gt;&lt;diagram name=&quot;Page-1&quot;&gt;7Vttk9o2EP41zLSdOUay/MbHg1zTzFw7mdw1bT8KW4AbYVEhDi6/PpItYQuZCxzG0PbywbFXQi/77LNarXQ9NJpv3nO8mP3KUkJ7Hkg3PfSu53kRiOVTCZ5LQRAHpWDKs7QUwUrwkH0lWgi0dJWlZGlVFIxRkS1sYcLynCTCkmHO2dquNmHU7nWBp8QRPCSYutI/slTMSqkHAKgKfiHZdCZ2S8Y4+TLlbJXrDnsemhT/yuI5No3p+ssZTtm6JkJ3PTTijInybb4ZEap0a/RW/u7nPaXbgXOSi4N+oAqLgYhnM3uSSmXoT8bFjE1ZjuldJR0WEySqCSC/ZmJO5SuUr2STiT+VuO8F+vMvXfQ3EeJZQ41XgklR1fg9Ywtdz52BnpQaVk2g5/OesDkR/FlW4IRikT3ZGGJtCtNtve1PP7JMduGBjTG+yOt7cRBBXz/LFrQNh35otygwnxKhG6mr+Lh2JTf6MRgEHiqfkd3Lkq14Qpxe5Ett7pWogHcf1OANau2gBkE/hAPgR+Uz2EE6eh3SLzcbxGE3QIfawJ4wXWkN3X++hyOWC84oJdwxg+U6m1OcK7zXs0yQhwVOVNFaencb9b2QPREuyOZF0DZGvcDSix/r73XlakPjT2c1L4uM8AWgP8nlAOdTObGj+oOwoT8P2d1hKgjPsSBDxYnlqXSE6Dx0rLNRVZJI8eeyKArM9+lUNVZW2m0pG2iLLwmjZVHbnLaU/qKGBw4Rel5I5cCGE1YQt9J9+M+KmYKbZaGQW1khXmyqMvk2Vf//vkilFZim5DDK1spCB1RJC2FjxYnsAI+LCgqhhSJ9MdFg2AveSQmm2TSXgkSqW/IVDRW9Mhmg3OqCeZamhUFQPCZ0uI06RowyXvRr4o5Gzhrbc1i7jan08KywpInN0t5iT6+Mr/XKpgqbTJbk5GUORP89XoUuryBAlyOW/0asZmIZ42uDWJEX2BEEvCKauTHGp8cT4opJRmlNxSkm8SSR8qUMWb6QWkmYxGTcrPxt3PP9UEQr1AucSAA1RR6eCeUOjDxM826gEXQeZ5j1t4bUg6ixbMwrgrVA4R/sVkohW4hMOb49zP7xUMYfHZwebhGmFMUW5UIXwqgxND3dqw5iB6jHwt9LCwA/XUIXdtwMzY6lA11Aw8KaMj6rV62LN9P9vul6oDvbRQ5av7FcCtSa4GLV1ioxmZAwaVwl0mgwBuDUVWKzsyScedXY191hMJ55FQkchD9ijmULhC9P5yMEjYR8yrAsvBVsniXLtqjGmVz+JKflp1Jkk43s2x3tNZEb6EW2s/QGDmqNi7/fird0+dfhGv/KVi7mWY9GV+53+rX0bez7l4oRoOfy0FGI2rAsDp/20TuTGxjaDspz5u/DBgfVyvxdSz/dICqtHrxzcEMh1AA5bGXKZ9psl271rJvtlWDlIIofmL03JRPRsPMWKikyXErcsnz6WGRIbvyT8bItvW6iTWtoG3gh2IDXhSka+t1RFLmpgRYoig4/Z+ieoqjJK/2/KXoEXhegqJsTuThFUdghRd1UQxsU3Zf7vAqKnuks6N9M0cPx6p6ipuFromjgBvpno6h/QBRxPEW3Wr1GipqLMW8UfQ1eF6Do6RsT1Jj36ff7140WBJVIdzNjPPsqR4qpFhzul/b7H8/2P7E6aY5DD4TxIAwC6O67m7jZTj7exXo0w/lU5S3ricAdeM598H6Bc3eTCe3+2N2MpobBh0QB8EGdZU/UMnDkCuGkQncS7ASmAYmaEuyDMEI4bCnB7oc759yhm2FvPjltxbTdYHiEabKiRQIV5GQtn9uTuE4NHNbNu6zXwYWt+FIGDk0jZ72hCk7S4nXcUA2RdZV0YDffzn3VCLzYyavvq8rP6n57Wb36IwJ09w0=&lt;/diagram&gt;&lt;/mxfile&gt;"><defs/><g transform="translate(0.5,0.5)"><path d="M 432.29 105.81 L 432.29 159.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 432.29 164.88 L 428.79 157.88 L 432.29 159.63 L 435.79 157.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 155.62 106.81 L 155.62 160.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 155.62 165.88 L 152.12 158.88 L 155.62 160.63 L 159.12 158.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 0 23 L 0 0 L 600 0 L 600 23 Z" fill="#ffffff" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 0 23 L 0 300 L 600 300 L 600 23" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(257.5,5.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="85" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 86px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">LVL1Controller</div></div></foreignObject><text x="43" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">LVL1Controller</text></switch></g><path d="M 278.21 174.31 L 190.62 174.31 L 190.62 121.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 190.62 116.12 L 194.12 123.12 L 190.62 121.37 L 187.12 123.12 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(178.5,139.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;"><font style="font-size: 8px">Update</font></div></div></foreignObject><text x="12" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 580.28 175 L 467.17 175 L 467.17 121.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 467.17 116.12 L 470.67 123.12 L 467.17 121.37 L 463.67 123.12 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(454.5,132.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;"><font style="font-size: 8px">Update</font></div></div></foreignObject><text x="12" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 0 48 L 0 25 L 300 25 L 300 48 Z" fill="#dae8fc" stroke="#6c8ebf" stroke-miterlimit="10" pointer-events="none"/><path d="M 0 48 L 0 300 L 300 300 L 300 48" fill="none" stroke="#6c8ebf" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(141.5,30.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="16" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 17px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">RT</div></div></foreignObject><text x="8" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">RT</text></switch></g><rect x="138" y="85" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(156.5,86.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="32" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 33px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">State<br /><font style="font-size: 8px">(<font>optional</font>)</font></div></div></foreignObject><text x="16" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="10" y="162" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(20.5,170.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="48" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 49px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Targets *</div></div></foreignObject><text x="24" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Targets *</text></switch></g><rect x="138" y="225" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(150.5,226.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="45" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 46px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Values *<br /><font style="font-size: 8px">(<font>optional</font>)</font></div></div></foreignObject><text x="23" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 300 48 L 300 25 L 600 25 L 600 48 Z" fill="#ffe6cc" stroke="#d79b00" stroke-miterlimit="10" pointer-events="none"/><path d="M 300 48 L 300 300 L 600 300 L 600 48" fill="none" stroke="#d79b00" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(428.5,30.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="42" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 43px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Non RT<br /></div></div></foreignObject><text x="21" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">Non RT&lt;br&gt;</text></switch></g><rect x="173" y="154" width="250" height="40" fill="#ffffff" stroke="#000000" transform="rotate(270,298,174)" pointer-events="none"/><g transform="translate(266.5,160.5)rotate(270,31.5,13.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="63" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 64px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Parameters<br /><font style="font-size: 10px">(via Atomics)</font></div></div></foreignObject><text x="32" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="414.57" y="85" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(433.5,86.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="32" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 33px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">State<br /><font style="font-size: 8px">(<font style="font-size: 8px">optional</font>)</font></div></div></foreignObject><text x="16" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="284" y="54" width="30" height="10" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(286.5,49.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><font style="font-size: 8px">Atomic</font></div></div></foreignObject><text x="12" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="284" y="96" width="30" height="10" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(286.5,91.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><font style="font-size: 8px">Atomic</font></div></div></foreignObject><text x="12" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="284" y="68" width="30" height="10" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(286.5,63.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><font style="font-size: 8px">Atomic</font></div></div></foreignObject><text x="12" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="284" y="82" width="30" height="10" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(286.5,77.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><font style="font-size: 8px">Atomic</font></div></div></foreignObject><text x="12" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><g transform="translate(268.5,135.5)rotate(-90,0,0)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="34" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><font style="font-size: 30px">...</font></div></div></foreignObject><text x="12" y="23" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 580.28 175 L 449.24 175 L 449.24 174.31 L 324.57 174.31" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 319.32 174.31 L 326.32 170.81 L 324.57 174.31 L 326.32 177.81 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(399.5,168.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="98" height="11" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">Change Parameters</div></div></foreignObject><text x="49" y="11" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica">Change Parameters</text></switch></g><rect x="465" y="165" width="250" height="20" fill="#e1d5e7" stroke="#9673a6" transform="rotate(270,590,175)" pointer-events="none"/><g transform="translate(556.5,168.5)rotate(270,33,6)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="66" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 67px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Ice Interface</div></div></foreignObject><text x="33" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Ice Interface</text></switch></g><path d="M 278.21 174.31 L 178.9 174.31 L 178.9 177.07 L 86.64 177.07" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 81.39 177.07 L 88.39 173.57 L 86.64 177.07 L 88.39 180.57 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(124.5,169.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="108" height="11" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">Calculate new Targets</div></div></foreignObject><text x="54" y="11" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica">Calculate new Targets</text></switch></g><path d="M 155.62 223.81 L 155.62 190.18" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 155.62 184.93 L 159.12 191.93 L 155.62 190.18 L 152.12 191.93 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/></g></svg>
\ No newline at end of file
diff --git a/etc/doxygen/images/LVL1ControllerAtomicDataFlow.xml b/etc/doxygen/images/LVL1ControllerAtomicDataFlow.xml
new file mode 100644
index 0000000000000000000000000000000000000000..e7ff81b81e03cce0634745ea0282c9045c496166
--- /dev/null
+++ b/etc/doxygen/images/LVL1ControllerAtomicDataFlow.xml
@@ -0,0 +1 @@
+<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:51.0) Gecko/20100101 Firefox/51.0" version="6.1.0.3" editor="www.draw.io" type="device"><diagram name="Page-1">7Vttk9o2EP41zLSdOUay/MbHg1zTzFw7mdw1bT8KW4AbYVEhDi6/PpItYQuZCxzG0PbywbFXQi/77LNarXQ9NJpv3nO8mP3KUkJ7Hkg3PfSu53kRiOVTCZ5LQRAHpWDKs7QUwUrwkH0lWgi0dJWlZGlVFIxRkS1sYcLynCTCkmHO2dquNmHU7nWBp8QRPCSYutI/slTMSqkHAKgKfiHZdCZ2S8Y4+TLlbJXrDnsemhT/yuI5No3p+ssZTtm6JkJ3PTTijInybb4ZEap0a/RW/u7nPaXbgXOSi4N+oAqLgYhnM3uSSmXoT8bFjE1ZjuldJR0WEySqCSC/ZmJO5SuUr2STiT+VuO8F+vMvXfQ3EeJZQ41XgklR1fg9Ywtdz52BnpQaVk2g5/OesDkR/FlW4IRikT3ZGGJtCtNtve1PP7JMduGBjTG+yOt7cRBBXz/LFrQNh35otygwnxKhG6mr+Lh2JTf6MRgEHiqfkd3Lkq14Qpxe5Ett7pWogHcf1OANau2gBkE/hAPgR+Uz2EE6eh3SLzcbxGE3QIfawJ4wXWkN3X++hyOWC84oJdwxg+U6m1OcK7zXs0yQhwVOVNFaencb9b2QPREuyOZF0DZGvcDSix/r73XlakPjT2c1L4uM8AWgP8nlAOdTObGj+oOwoT8P2d1hKgjPsSBDxYnlqXSE6Dx0rLNRVZJI8eeyKArM9+lUNVZW2m0pG2iLLwmjZVHbnLaU/qKGBw4Rel5I5cCGE1YQt9J9+M+KmYKbZaGQW1khXmyqMvk2Vf//vkilFZim5DDK1spCB1RJC2FjxYnsAI+LCgqhhSJ9MdFg2AveSQmm2TSXgkSqW/IVDRW9Mhmg3OqCeZamhUFQPCZ0uI06RowyXvRr4o5Gzhrbc1i7jan08KywpInN0t5iT6+Mr/XKpgqbTJbk5GUORP89XoUuryBAlyOW/0asZmIZ42uDWJEX2BEEvCKauTHGp8cT4opJRmlNxSkm8SSR8qUMWb6QWkmYxGTcrPxt3PP9UEQr1AucSAA1RR6eCeUOjDxM826gEXQeZ5j1t4bUg6ixbMwrgrVA4R/sVkohW4hMOb49zP7xUMYfHZwebhGmFMUW5UIXwqgxND3dqw5iB6jHwt9LCwA/XUIXdtwMzY6lA11Aw8KaMj6rV62LN9P9vul6oDvbRQ5av7FcCtSa4GLV1ioxmZAwaVwl0mgwBuDUVWKzsyScedXY191hMJ55FQkchD9ijmULhC9P5yMEjYR8yrAsvBVsniXLtqjGmVz+JKflp1Jkk43s2x3tNZEb6EW2s/QGDmqNi7/fird0+dfhGv/KVi7mWY9GV+53+rX0bez7l4oRoOfy0FGI2rAsDp/20TuTGxjaDspz5u/DBgfVyvxdSz/dICqtHrxzcEMh1AA5bGXKZ9psl271rJvtlWDlIIofmL03JRPRsPMWKikyXErcsnz6WGRIbvyT8bItvW6iTWtoG3gh2IDXhSka+t1RFLmpgRYoig4/Z+ieoqjJK/2/KXoEXhegqJsTuThFUdghRd1UQxsU3Zf7vAqKnuks6N9M0cPx6p6ipuFromjgBvpno6h/QBRxPEW3Wr1GipqLMW8UfQ1eF6Do6RsT1Jj36ff7140WBJVIdzNjPPsqR4qpFhzul/b7H8/2P7E6aY5DD4TxIAwC6O67m7jZTj7exXo0w/lU5S3ricAdeM598H6Bc3eTCe3+2N2MpobBh0QB8EGdZU/UMnDkCuGkQncS7ASmAYmaEuyDMEI4bCnB7oc759yhm2FvPjltxbTdYHiEabKiRQIV5GQtn9uTuE4NHNbNu6zXwYWt+FIGDk0jZ72hCk7S4nXcUA2RdZV0YDffzn3VCLzYyavvq8rP6n57Wb36IwJ09w0=</diagram></mxfile>
\ No newline at end of file
diff --git a/etc/doxygen/images/LVL1ControllerGeneralDataFlow.svg b/etc/doxygen/images/LVL1ControllerGeneralDataFlow.svg
new file mode 100644
index 0000000000000000000000000000000000000000..a51e800c315420ed9c98ada9ba5a0281b5001380
--- /dev/null
+++ b/etc/doxygen/images/LVL1ControllerGeneralDataFlow.svg
@@ -0,0 +1,2 @@
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg xmlns="http://www.w3.org/2000/svg" style="background-color: rgb(255, 255, 255);" xmlns:xlink="http://www.w3.org/1999/xlink" width="602px" height="302px" version="1.1" content="&lt;mxfile userAgent=&quot;Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:51.0) Gecko/20100101 Firefox/51.0&quot; version=&quot;6.1.0.3&quot; editor=&quot;www.draw.io&quot; type=&quot;device&quot;&gt;&lt;diagram name=&quot;Page-1&quot;&gt;7Vltc+I2EP41zLSdCSO/m48Hl15vJu3cXK7X9qOwZeNGWFSIQPrru7IkbCHDkUDgppN8cKxdeWXtPs/uCg+CyXzzgePF7FeWEzrwUb4ZBO8Hvp9GMVyl4EkJ4tRTgpJXuRJ1BPfVv0QLkZauqpwsrYmCMSqqhS3MWF2TTFgyzDlb29MKRu1VF7gkjuA+w9SV/lHlYqakPkKoVfxCqnImdjVTnD2UnK1qveDAD4rmT6nn2BjT85cznLN1RxTcDoIJZ0you/lmQqj0rfGbeu7nPdrti3NSi6MekMrmRcST2T3JwRl6yLiYsZLVmN620nGzQSJNIBjNxJzCrQe3ZFOJP6V46Ed6+JdW/U2EeNKhxivBQNQav2Nsoee5O9Cbkq/VEej9fCBsTgR/ggmcUCyqRzuGWEOh3M7bPvqJVbCEjzYGfIk/BOwmXqivyoLBcBjbFgXmJRHaSNfFz7MbpdEwRaPID9Q1sVdZshXPiLMK3HT23oqa8O4LNXoLtZqRjKJh7I1QmKhrtBPp5GWRPmw2SuPLBDrWAHvEdKU9dPf1zpuwWnBGKeEODJbrak5xLeO9nlWC3C9wJlVryO521PeG7JFwQTYHg7Yx7kWWX8JUj9dtqo1NPp11smxghAcC/RnKAa5L2Niz1vO8nvX8wF4OU0F4jQUZS04sT6WjF7wOHbtslJMgUvxJqZLIjE+nqkGZwq2SjTTiFWG0LDk3py2nH/TwyCHCwI8pvNi4YA1xW9/H/6yYUdwsG4e8gwnpYtPq4K6U/39f5IACYwpeQ1lTSieoQAthx4oTWABPmwkyQgtJ+maj0XgQvQcJplVZgyADdwNfg7GkVwUNyjutmFd53gCC4imh423XMWGU8WZd03f0ctZgz2HttqfSr2e1JX1sBrylvq6ML83KZgoriiU5ucyh5P/Hq9jllYeC6xErfCNWP7EM+M5BrMSP7A7C+45o5vYYn7+c0FcUFaUdF+eYpEUG8iW0LA+ko4mzlEz7nb/te77dimiH+pHTCQR9nYdvWrkjOw9j3m00oov3Gab+diJ1Lzosm/KWYGeg8A+2FSVkC1HJxLeH2T8ey/hnN6fHI8Jog9SiXOyGMOltTU/PqqPUCdSXJt8DAtBP1/CF3Td75sRyAV94hoUdZ3yVt9oXb9D9NnR9dDnsBk60fmM1CGRNcGN1ripRFCTOeqtEnoymCJ1aJTY7JeGVq8a+5Y4L4ytXkciJ8CfMMVggfHk6Hz3US8jHCoMSoHQzpSx7qOoShhM2n69q6NkkMc/FP85EYw+G0rt9wNl3ZNqLmxvPT3YyqBvK3o4gPEsKdUl5wcL/QitXS7fPji4cgoad33TTMLxW4+Ah9zg2mUFWkUmly9ILn4qvcCg2aeryZ2LzNp0YfMxkAD7Kg2YhgXhqStqpfsTLI5L0Vb9RnAQ4PlP1C2Mb2F7slr/+Y81ZoO02xRNMsxVtEhmqyRqu2zb5ogD3uvBW8y7wa2p6LYB7xsirfj5CJ3nx+/h8FIfWd56Rbf48H5MSdHCRF39MgmH78VlNb7/wB7f/AQ==&lt;/diagram&gt;&lt;/mxfile&gt;"><defs/><g transform="translate(0.5,0.5)"><path d="M 432.29 105.81 L 432.29 159.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 432.29 164.88 L 428.79 157.88 L 432.29 159.63 L 435.79 157.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 155.62 106.81 L 155.62 160.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 155.62 165.88 L 152.12 158.88 L 155.62 160.63 L 159.12 158.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 0 23 L 0 0 L 600 0 L 600 23 Z" fill="#ffffff" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 0 23 L 0 300 L 600 300 L 600 23" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(257.5,5.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="85" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 86px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">LVL1Controller</div></div></foreignObject><text x="43" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">LVL1Controller</text></switch></g><path d="M 278.17 174.83 L 190.67 174.83 L 190.67 121.2" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 190.67 115.95 L 194.17 122.95 L 190.67 121.2 L 187.17 122.95 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(178.5,139.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;"><font style="font-size: 8px">Update</font></div></div></foreignObject><text x="12" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 579 174.83 L 467.33 174.83 L 467.33 121.2" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 467.33 115.95 L 470.83 122.95 L 467.33 121.2 L 463.83 122.95 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(454.5,132.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;"><font style="font-size: 8px">Update</font></div></div></foreignObject><text x="12" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 0 48 L 0 25 L 300 25 L 300 48 Z" fill="#dae8fc" stroke="#6c8ebf" stroke-miterlimit="10" pointer-events="none"/><path d="M 0 48 L 0 300 L 300 300 L 300 48" fill="none" stroke="#6c8ebf" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(141.5,30.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="16" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 17px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">RT</div></div></foreignObject><text x="8" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">RT</text></switch></g><rect x="138" y="85" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(156.5,86.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="32" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 33px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">State<br /><font style="font-size: 8px">(<font>optional</font>)</font></div></div></foreignObject><text x="16" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="10" y="162" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(20.5,170.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="48" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 49px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Targets *</div></div></foreignObject><text x="24" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Targets *</text></switch></g><rect x="138" y="225" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(150.5,226.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="45" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 46px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Values *<br /><font style="font-size: 8px">(<font>optional</font>)</font></div></div></foreignObject><text x="23" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 300 48 L 300 25 L 600 25 L 600 48 Z" fill="#ffe6cc" stroke="#d79b00" stroke-miterlimit="10" pointer-events="none"/><path d="M 300 48 L 300 300 L 600 300 L 600 48" fill="none" stroke="#d79b00" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(428.5,30.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="42" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 43px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Non RT<br /></div></div></foreignObject><text x="21" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">Non RT&lt;br&gt;</text></switch></g><rect x="173" y="155" width="250" height="40" fill="#ffffff" stroke="#000000" transform="rotate(270,298,175)" pointer-events="none"/><g transform="translate(219.5,161.5)rotate(270,78,13.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="156" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 157px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Parameters<br /><font style="font-size: 10px">(via Non-blocking Communication)</font></div></div></foreignObject><text x="78" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="414.57" y="85" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(433.5,86.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="32" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 33px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">State<br /><font style="font-size: 8px">(<font style="font-size: 8px">optional</font>)</font></div></div></foreignObject><text x="16" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 579 174.83 L 324.53 174.83" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 319.28 174.83 L 326.28 171.33 L 324.53 174.83 L 326.28 178.33 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(398.5,168.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="98" height="11" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">Change Parameters</div></div></foreignObject><text x="49" y="11" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica">Change Parameters</text></switch></g><rect x="464" y="165" width="250" height="20" fill="#e1d5e7" stroke="#9673a6" transform="rotate(270,589,175)" pointer-events="none"/><g transform="translate(555.5,168.5)rotate(270,33,6)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="66" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 67px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Ice Interface</div></div></foreignObject><text x="33" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Ice Interface</text></switch></g><path d="M 278.17 174.83 L 179 174.83 L 179 177.33 L 86.2 177.33" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 80.95 177.33 L 87.95 173.83 L 86.2 177.33 L 87.95 180.83 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(124.5,170.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="108" height="11" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">Calculate new Targets</div></div></foreignObject><text x="54" y="11" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica">Calculate new Targets</text></switch></g><path d="M 155.62 224.81 L 155.62 191.18" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 155.62 185.93 L 159.12 192.93 L 155.62 191.18 L 152.12 192.93 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/></g></svg>
\ No newline at end of file
diff --git a/etc/doxygen/images/LVL1ControllerGeneralDataFlow.xml b/etc/doxygen/images/LVL1ControllerGeneralDataFlow.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9dd261ed0a85fa954d8134ef30218ac92ab1ecfb
--- /dev/null
+++ b/etc/doxygen/images/LVL1ControllerGeneralDataFlow.xml
@@ -0,0 +1 @@
+<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:51.0) Gecko/20100101 Firefox/51.0" version="6.1.0.3" editor="www.draw.io" type="device"><diagram name="Page-1">7Vltc+I2EP41zLSdCSO/m48Hl15vJu3cXK7X9qOwZeNGWFSIQPrru7IkbCHDkUDgppN8cKxdeWXtPs/uCg+CyXzzgePF7FeWEzrwUb4ZBO8Hvp9GMVyl4EkJ4tRTgpJXuRJ1BPfVv0QLkZauqpwsrYmCMSqqhS3MWF2TTFgyzDlb29MKRu1VF7gkjuA+w9SV/lHlYqakPkKoVfxCqnImdjVTnD2UnK1qveDAD4rmT6nn2BjT85cznLN1RxTcDoIJZ0you/lmQqj0rfGbeu7nPdrti3NSi6MekMrmRcST2T3JwRl6yLiYsZLVmN620nGzQSJNIBjNxJzCrQe3ZFOJP6V46Ed6+JdW/U2EeNKhxivBQNQav2Nsoee5O9Cbkq/VEej9fCBsTgR/ggmcUCyqRzuGWEOh3M7bPvqJVbCEjzYGfIk/BOwmXqivyoLBcBjbFgXmJRHaSNfFz7MbpdEwRaPID9Q1sVdZshXPiLMK3HT23oqa8O4LNXoLtZqRjKJh7I1QmKhrtBPp5GWRPmw2SuPLBDrWAHvEdKU9dPf1zpuwWnBGKeEODJbrak5xLeO9nlWC3C9wJlVryO521PeG7JFwQTYHg7Yx7kWWX8JUj9dtqo1NPp11smxghAcC/RnKAa5L2Niz1vO8nvX8wF4OU0F4jQUZS04sT6WjF7wOHbtslJMgUvxJqZLIjE+nqkGZwq2SjTTiFWG0LDk3py2nH/TwyCHCwI8pvNi4YA1xW9/H/6yYUdwsG4e8gwnpYtPq4K6U/39f5IACYwpeQ1lTSieoQAthx4oTWABPmwkyQgtJ+maj0XgQvQcJplVZgyADdwNfg7GkVwUNyjutmFd53gCC4imh423XMWGU8WZd03f0ctZgz2HttqfSr2e1JX1sBrylvq6ML83KZgoriiU5ucyh5P/Hq9jllYeC6xErfCNWP7EM+M5BrMSP7A7C+45o5vYYn7+c0FcUFaUdF+eYpEUG8iW0LA+ko4mzlEz7nb/te77dimiH+pHTCQR9nYdvWrkjOw9j3m00oov3Gab+diJ1Lzosm/KWYGeg8A+2FSVkC1HJxLeH2T8ey/hnN6fHI8Jog9SiXOyGMOltTU/PqqPUCdSXJt8DAtBP1/CF3Td75sRyAV94hoUdZ3yVt9oXb9D9NnR9dDnsBk60fmM1CGRNcGN1ripRFCTOeqtEnoymCJ1aJTY7JeGVq8a+5Y4L4ytXkciJ8CfMMVggfHk6Hz3US8jHCoMSoHQzpSx7qOoShhM2n69q6NkkMc/FP85EYw+G0rt9wNl3ZNqLmxvPT3YyqBvK3o4gPEsKdUl5wcL/QitXS7fPji4cgoad33TTMLxW4+Ah9zg2mUFWkUmly9ILn4qvcCg2aeryZ2LzNp0YfMxkAD7Kg2YhgXhqStqpfsTLI5L0Vb9RnAQ4PlP1C2Mb2F7slr/+Y81ZoO02xRNMsxVtEhmqyRqu2zb5ogD3uvBW8y7wa2p6LYB7xsirfj5CJ3nx+/h8FIfWd56Rbf48H5MSdHCRF39MgmH78VlNb7/wB7f/AQ==</diagram></mxfile>
\ No newline at end of file
diff --git a/etc/doxygen/images/LVL1ControllerTripleBufferDataFlow.svg b/etc/doxygen/images/LVL1ControllerTripleBufferDataFlow.svg
new file mode 100644
index 0000000000000000000000000000000000000000..9b114c27380e9e94ba591c89f1fcc8e712ebb790
--- /dev/null
+++ b/etc/doxygen/images/LVL1ControllerTripleBufferDataFlow.svg
@@ -0,0 +1,2 @@
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg xmlns="http://www.w3.org/2000/svg" style="background-color: rgb(255, 255, 255);" xmlns:xlink="http://www.w3.org/1999/xlink" width="602px" height="302px" version="1.1" content="&lt;mxfile userAgent=&quot;Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:51.0) Gecko/20100101 Firefox/51.0&quot; version=&quot;6.1.0.3&quot; editor=&quot;www.draw.io&quot; type=&quot;device&quot;&gt;&lt;diagram name=&quot;Page-1&quot;&gt;7Vpdj9o4FP01SLuVFtlJnI/Hgc62lWZX1cxsu300iQPZmpg1ZoD++jqJncQ4MDAEplLbhzS+dmzn3nPuPQ4zcMfzzTuOF7O/WELowAHJZuC+HThOAEJ5LQzbyoBCVBmmPEsqE2wMD9k3ooxAWVdZQpbGQMEYFdnCNMYsz0ksDBvmnK3NYSmj5qoLPCWW4SHG1LZ+zhIxq6wOAKDpeE+y6Uzs9kxw/HXK2SpXCw4cNy3/Vd1zrCdT45cznLB1y+TeDtwxZ0xUd/PNmNDCt9pv1XN/7umtN85JLo56oOgsNyK2+u1JIp2hmoyLGZuyHNPbxjoqX5AUUwDZmok5lbdQ3pJNJv4tzEMHqeYX1fUfEWKrQo1XgklTM/kdYws1zn4D9VLFtloG9T7vCJsTwbdyACcUi+zJjCFWUJjW4+pHP7JMLuEAhVoIQTB0QhRAT12rGRSGfc83ZxSYT4lQk7RdfNq8khvDEETIcatrYK6yZCseE2uVG87xtjVsUQxYHn4583UgeHbXhx6QN9UedKsVisZUom0f8jQFfnrkBREa+jACXlBd0Q7wdiBxLPAOT4tC/wW4e0GgfYX3J0xXykN3n+7gmOWCM0oJt2CwXGdzivMi3utZJsjDAsdF11oWGzPqe0P2RLggm4NB22j3AsMvXqja6ybz+zq9z1pJ39XGA4G+l9UJ51P5YietB2HHeo5rLoepIDzHgowKTizPjBKEbq90lMHdVnwMkG6fT0iNpYoClS0KrsNSB5iBg+FO/PtK1RHsXKe3xAsji5ADx6fSnaOUlTtpMOD/v2K6449lGbIbOSBcbJo+eTct/v9nkUg06qnkNqrZqk4LXJKewsQMJ3IBPCkHFHBSjpKj0WiA3koLptk0l4ZYAkLmDXdU0DyTuu1GdcyzJCmBSfGE0FEtxsaMMl6uq+VYZ+7QHLCyRy011fYGbbXWlVXAEIWOEgwvxZ0ewtJ0Sc7NwnU577vctqstuDzzK5Ip5vt2NoDA7TsdGF4/SCzvF7G6iaXB1wexAgeZSgb+QDSztc794xn6Js0obbk4wSRMY2lfSun0lbR6/Dgkk27n1/rreUmkHOogS5FItdOhSLSkPFIB6eltwYOurne0amhF6kG0WDbhDcF6oPBv5iyVkS1EViS+Pcz+/VjGnyySj0eE7nVDg3K+HcKgUyKfn1Wj0ArUY5nvJQLAm9fwxY4M1CenK/gCagnacsan4lb54hd0n4duLeOvEC/XitbfLJeGoibYseqrSqQp8ePOKpEE0QSAc6uE6nUDkwiXqhp6uZ3jV0cV6QrjhU/NwA7xFcvIC2d5NfLuO17txVgQDVvfTUPPe60qBIGt7cczCdECoR8xx3JGwpeWiy5yxLrMKerS30928wUMPHOKPd81XyLufCtYH+IiUh+K401aAPZELHMmWS1JI5sFxqycS2CCSNCVcyM/cLHfU871/J2jj9+RdDvFdC8csKXYGNN4RcuEB3KyltdanPXIBP1JoW58qXnR87fE8DpcqHNSzYWLfX6Hnv2575FnC0pGK6kU+Bm6Yw96u9y8F9G7nuhQEdA7VUUcjWhk6+l7gpNTs8OM8eybrJSYKvAe5RkVmVOO4R7q8ATqg9sIWp74zDNxcqLs2RXqkchMe1f1jN7vRX+nPK+s/xi/U/qu8YNidFR1P3GRABxcpMe0iXRxbgjxPksSkl+JEYf5AM1DLXJtKdwXI2Sz+XuUypPNH/24t98B&lt;/diagram&gt;&lt;/mxfile&gt;"><defs/><g transform="translate(0.5,0.5)"><path d="M 467.17 106.03 L 467.17 130.17 L 467.17 159.67" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 467.17 164.92 L 463.67 157.92 L 467.17 159.67 L 470.67 157.92 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 155.62 106.81 L 155.62 160.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 155.62 165.88 L 152.12 158.88 L 155.62 160.63 L 159.12 158.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 0 23 L 0 0 L 600 0 L 600 23 Z" fill="#ffffff" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 0 23 L 0 300 L 600 300 L 600 23" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(257.5,5.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="85" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 86px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">LVL1Controller</div></div></foreignObject><text x="43" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">LVL1Controller</text></switch></g><path d="M 200.28 179.83 L 190.62 179.83 L 190.62 121.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 190.62 116.12 L 194.12 123.12 L 190.62 121.37 L 187.12 123.12 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(178.5,124.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;"><font style="font-size: 8px">Update</font></div></div></foreignObject><text x="12" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 580.28 175 L 502.34 175 L 502.34 121.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 502.34 116.12 L 505.84 123.12 L 502.34 121.37 L 498.84 123.12 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(489.5,127.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;"><font style="font-size: 8px">Update</font></div></div></foreignObject><text x="12" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 0 48 L 0 25 L 230 25 L 230 48 Z" fill="#dae8fc" stroke="#6c8ebf" stroke-miterlimit="10" pointer-events="none"/><path d="M 0 48 L 0 300 L 230 300 L 230 48" fill="none" stroke="#6c8ebf" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(106.5,30.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="16" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 17px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">RT</div></div></foreignObject><text x="8" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">RT</text></switch></g><rect x="138" y="85" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(156.5,86.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="32" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 33px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">State<br /><font style="font-size: 8px">(<font>optional</font>)</font></div></div></foreignObject><text x="16" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="10" y="162" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(20.5,170.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="48" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 49px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Targets *</div></div></foreignObject><text x="24" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Targets *</text></switch></g><rect x="138" y="225" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(150.5,226.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="45" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 46px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Values *<br /><font style="font-size: 8px">(<font>optional</font>)</font></div></div></foreignObject><text x="23" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 370 48 L 370 25 L 600 25 L 600 48 Z" fill="#ffe6cc" stroke="#d79b00" stroke-miterlimit="10" pointer-events="none"/><path d="M 370 48 L 370 300 L 600 300 L 600 48" fill="none" stroke="#d79b00" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(463.5,30.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="42" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 43px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Non RT<br /></div></div></foreignObject><text x="21" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">Non RT&lt;br&gt;</text></switch></g><rect x="449.57" y="85" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(468.5,86.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="32" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 33px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">State<br /><font style="font-size: 8px">(<font style="font-size: 8px">optional</font>)</font></div></div></foreignObject><text x="16" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 580.28 175 L 474.76 175 L 474.76 174.31 L 376.37 174.02" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 371.12 174 L 378.13 170.52 L 376.37 174.02 L 378.11 177.52 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(425.5,169.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="98" height="11" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">Change Parameters</div></div></foreignObject><text x="49" y="11" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica">Change Parameters</text></switch></g><rect x="465" y="165" width="250" height="20" fill="#e1d5e7" stroke="#9673a6" transform="rotate(270,590,175)" pointer-events="none"/><g transform="translate(556.5,168.5)rotate(270,33,6)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="66" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 67px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Ice Interface</div></div></foreignObject><text x="33" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Ice Interface</text></switch></g><path d="M 230 177 L 154.76 177.07 L 86.64 177.07" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 81.39 177.07 L 88.39 173.57 L 86.64 177.07 L 88.39 180.57 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(100.5,171.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="108" height="11" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">Calculate new Targets</div></div></foreignObject><text x="54" y="11" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica">Calculate new Targets</text></switch></g><path d="M 230 48 L 230 25 L 370 25 L 370 48 Z" fill="#ffffff" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 230 48 L 230 300 L 370 300 L 370 48" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(266.5,30.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="67" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 68px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">TripleBuffer</div></div></foreignObject><text x="34" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">TripleBuffer</text></switch></g><rect x="230" y="50" width="45" height="250" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(237.5,168.5)rotate(-90,15,6)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="30" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 31px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Read</div></div></foreignObject><text x="15" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Read</text></switch></g><rect x="325" y="50" width="45" height="250" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(333.5,168.5)rotate(-90,14,6)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="28" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 29px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Write</div></div></foreignObject><text x="14" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Write</text></switch></g><path d="M 155.62 223.81 L 155.62 190.18" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 155.62 184.93 L 159.12 191.93 L 155.62 190.18 L 152.12 191.93 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><rect x="278" y="50" width="45" height="250" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(280.5,168.5)rotate(-90,20,6)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="40" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 41px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Hidden</div></div></foreignObject><text x="20" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Hidden</text></switch></g></g></svg>
\ No newline at end of file
diff --git a/etc/doxygen/images/LVL1ControllerTripleBufferDataFlow.xml b/etc/doxygen/images/LVL1ControllerTripleBufferDataFlow.xml
new file mode 100644
index 0000000000000000000000000000000000000000..af98949fb8dc5edf1f212ea8a932330b316e9300
--- /dev/null
+++ b/etc/doxygen/images/LVL1ControllerTripleBufferDataFlow.xml
@@ -0,0 +1 @@
+<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:51.0) Gecko/20100101 Firefox/51.0" version="6.1.0.3" editor="www.draw.io" type="device"><diagram name="Page-1">7Vpdj9o4FP01SLuVFtlJnI/Hgc62lWZX1cxsu300iQPZmpg1ZoD++jqJncQ4MDAEplLbhzS+dmzn3nPuPQ4zcMfzzTuOF7O/WELowAHJZuC+HThOAEJ5LQzbyoBCVBmmPEsqE2wMD9k3ooxAWVdZQpbGQMEYFdnCNMYsz0ksDBvmnK3NYSmj5qoLPCWW4SHG1LZ+zhIxq6wOAKDpeE+y6Uzs9kxw/HXK2SpXCw4cNy3/Vd1zrCdT45cznLB1y+TeDtwxZ0xUd/PNmNDCt9pv1XN/7umtN85JLo56oOgsNyK2+u1JIp2hmoyLGZuyHNPbxjoqX5AUUwDZmok5lbdQ3pJNJv4tzEMHqeYX1fUfEWKrQo1XgklTM/kdYws1zn4D9VLFtloG9T7vCJsTwbdyACcUi+zJjCFWUJjW4+pHP7JMLuEAhVoIQTB0QhRAT12rGRSGfc83ZxSYT4lQk7RdfNq8khvDEETIcatrYK6yZCseE2uVG87xtjVsUQxYHn4583UgeHbXhx6QN9UedKsVisZUom0f8jQFfnrkBREa+jACXlBd0Q7wdiBxLPAOT4tC/wW4e0GgfYX3J0xXykN3n+7gmOWCM0oJt2CwXGdzivMi3utZJsjDAsdF11oWGzPqe0P2RLggm4NB22j3AsMvXqja6ybz+zq9z1pJ39XGA4G+l9UJ51P5YietB2HHeo5rLoepIDzHgowKTizPjBKEbq90lMHdVnwMkG6fT0iNpYoClS0KrsNSB5iBg+FO/PtK1RHsXKe3xAsji5ADx6fSnaOUlTtpMOD/v2K6449lGbIbOSBcbJo+eTct/v9nkUg06qnkNqrZqk4LXJKewsQMJ3IBPCkHFHBSjpKj0WiA3koLptk0l4ZYAkLmDXdU0DyTuu1GdcyzJCmBSfGE0FEtxsaMMl6uq+VYZ+7QHLCyRy011fYGbbXWlVXAEIWOEgwvxZ0ewtJ0Sc7NwnU577vctqstuDzzK5Ip5vt2NoDA7TsdGF4/SCzvF7G6iaXB1wexAgeZSgb+QDSztc794xn6Js0obbk4wSRMY2lfSun0lbR6/Dgkk27n1/rreUmkHOogS5FItdOhSLSkPFIB6eltwYOurne0amhF6kG0WDbhDcF6oPBv5iyVkS1EViS+Pcz+/VjGnyySj0eE7nVDg3K+HcKgUyKfn1Wj0ArUY5nvJQLAm9fwxY4M1CenK/gCagnacsan4lb54hd0n4duLeOvEC/XitbfLJeGoibYseqrSqQp8ePOKpEE0QSAc6uE6nUDkwiXqhp6uZ3jV0cV6QrjhU/NwA7xFcvIC2d5NfLuO17txVgQDVvfTUPPe60qBIGt7cczCdECoR8xx3JGwpeWiy5yxLrMKerS30928wUMPHOKPd81XyLufCtYH+IiUh+K401aAPZELHMmWS1JI5sFxqycS2CCSNCVcyM/cLHfU871/J2jj9+RdDvFdC8csKXYGNN4RcuEB3KyltdanPXIBP1JoW58qXnR87fE8DpcqHNSzYWLfX6Hnv2575FnC0pGK6kU+Bm6Yw96u9y8F9G7nuhQEdA7VUUcjWhk6+l7gpNTs8OM8eybrJSYKvAe5RkVmVOO4R7q8ATqg9sIWp74zDNxcqLs2RXqkchMe1f1jN7vRX+nPK+s/xi/U/qu8YNidFR1P3GRABxcpMe0iXRxbgjxPksSkl+JEYf5AM1DLXJtKdwXI2Sz+XuUypPNH/24t98B</diagram></mxfile>
\ No newline at end of file
diff --git a/etc/doxygen/images/LVL1ControllerWorkerThreadDataFlow.svg b/etc/doxygen/images/LVL1ControllerWorkerThreadDataFlow.svg
new file mode 100644
index 0000000000000000000000000000000000000000..1efe44f90711f6da12f73d1c10c64f8c41da47ca
--- /dev/null
+++ b/etc/doxygen/images/LVL1ControllerWorkerThreadDataFlow.svg
@@ -0,0 +1,2 @@
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg xmlns="http://www.w3.org/2000/svg" style="background-color: rgb(255, 255, 255);" xmlns:xlink="http://www.w3.org/1999/xlink" width="702px" height="302px" version="1.1" content="&lt;mxfile userAgent=&quot;Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:51.0) Gecko/20100101 Firefox/51.0&quot; version=&quot;6.1.0.3&quot; editor=&quot;www.draw.io&quot; type=&quot;device&quot;&gt;&lt;diagram name=&quot;Page-1&quot;&gt;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&lt;/diagram&gt;&lt;/mxfile&gt;"><defs/><g transform="translate(0.5,0.5)"><path d="M 151 158 L 150.7 95.03" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 150.67 89.78 L 154.21 96.77 L 150.7 95.03 L 147.21 96.8 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(137.5,109.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;"><font style="font-size: 8px">Update</font></div></div></foreignObject><text x="12" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 115.62 75.81 L 115.62 129.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 115.62 134.88 L 112.12 127.88 L 115.62 129.63 L 119.12 127.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 0 23 L 0 0 L 700 0 L 700 23 Z" fill="#ffffff" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 0 23 L 0 300 L 700 300 L 700 23" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(307.5,5.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="85" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 86px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">LVL1Controller</div></div></foreignObject><text x="43" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">LVL1Controller</text></switch></g><path d="M 0 48 L 0 25 L 200 25 L 200 48 Z" fill="#dae8fc" stroke="#6c8ebf" stroke-miterlimit="10" pointer-events="none"/><path d="M 0 48 L 0 300 L 200 300 L 200 48" fill="none" stroke="#6c8ebf" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(91.5,30.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="16" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 17px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">RT</div></div></foreignObject><text x="8" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">RT</text></switch></g><rect x="98" y="59" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(116.5,60.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="32" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 33px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">State<br /><font style="font-size: 8px">(<font>optional</font>)</font></div></div></foreignObject><text x="16" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="10" y="136" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(20.5,144.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="48" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 49px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Targets *</div></div></foreignObject><text x="24" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Targets *</text></switch></g><rect x="63" y="223" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(75.5,224.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="45" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 46px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Values *<br /><font style="font-size: 8px">(<font>optional</font>)</font></div></div></foreignObject><text x="23" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 200 151 L 139.83 151.17 L 86.2 151.17" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 80.95 151.17 L 87.95 147.67 L 86.2 151.17 L 87.95 154.67 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(105.5,138.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="68" height="24" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">Calculate new<br />Targets</div></div></foreignObject><text x="34" y="18" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica">Calculate new&lt;br&gt;Targets</text></switch></g><path d="M 200 48 L 200 25 L 500 25 L 500 48 Z" fill="#d5e8d4" stroke="#82b366" stroke-miterlimit="10" pointer-events="none"/><path d="M 200 48 L 200 300 L 500 300 L 500 48" fill="none" stroke="#82b366" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(315.5,30.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="69" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 70px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">WorkerTask</div></div></foreignObject><text x="35" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">WorkerTask</text></switch></g><path d="M 372 180 L 372.3 130.03" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 372.33 124.78 L 375.79 131.81 L 372.3 130.03 L 368.79 131.76 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(361.5,140.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;"><font style="font-size: 8px">Update</font></div></div></foreignObject><text x="12" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="152" y="208" width="125" height="60" fill="#ffffff" stroke="#000000" transform="rotate(270,214.5,238)" pointer-events="none"/><g transform="translate(153.5,217.5)rotate(270,60.5,20.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="121" height="41" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 121px; white-space: normal; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Sensor Values<br /><font style="font-size: 10px">(via Non-blocking Communication)</font></div></div></foreignObject><text x="61" y="27" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="152" y="83" width="125" height="60" fill="#ffffff" stroke="#000000" transform="rotate(270,214.5,113)" pointer-events="none"/><g transform="translate(153.5,92.5)rotate(270,60.5,20.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="121" height="41" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 121px; white-space: normal; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Calculation results<br /><font style="font-size: 10px">(via Non-blocking Communication)</font></div></div></foreignObject><text x="61" y="27" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 679 175.33 L 614 175.33 L 614 121.7" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 614 116.45 L 617.5 123.45 L 614 121.7 L 610.5 123.45 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(600.5,136.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="24" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;"><font style="font-size: 8px">Update</font></div></div></foreignObject><text x="12" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 500 48 L 500 25 L 700 25 L 700 48 Z" fill="#ffe6cc" stroke="#d79b00" stroke-miterlimit="10" pointer-events="none"/><path d="M 500 48 L 500 300 L 700 300 L 700 48" fill="none" stroke="#d79b00" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(578.5,30.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="42" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 43px; white-space: nowrap; overflow-wrap: normal; font-weight: bold; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Non RT<br /></div></div></foreignObject><text x="21" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">Non RT&lt;br&gt;</text></switch></g><path d="M 580.29 101.81 L 580.29 155.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 580.29 160.88 L 576.79 153.88 L 580.29 155.63 L 583.79 153.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><rect x="373" y="155" width="250" height="40" fill="#ffffff" stroke="#000000" transform="rotate(270,498,175)" pointer-events="none"/><g transform="translate(419.5,161.5)rotate(270,78,13.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="156" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 157px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Parameters<br /><font style="font-size: 10px">(via Non-blocking Communication)</font></div></div></foreignObject><text x="78" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="561.57" y="85" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(580.5,86.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="32" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 33px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">State<br /><font style="font-size: 8px">(<font style="font-size: 8px">optional</font>)</font></div></div></foreignObject><text x="16" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 679 175.33 L 524.53 175.33" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 519.28 175.33 L 526.28 171.83 L 524.53 175.33 L 526.28 178.83 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(569.5,162.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="57" height="24" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">Change <br />Parameters</div></div></foreignObject><text x="29" y="18" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica">Change &lt;br&gt;Parameters</text></switch></g><rect x="564" y="165" width="250" height="20" fill="#e1d5e7" stroke="#9673a6" transform="rotate(270,689,175)" pointer-events="none"/><g transform="translate(655.5,168.5)rotate(270,33,6)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="66" height="12" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 67px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Ice Interface</div></div></foreignObject><text x="33" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Ice Interface</text></switch></g><path d="M 478.17 175.33 L 259.83 175.33 L 259.83 112.83 L 251.2 112.83" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 245.95 112.83 L 252.95 109.33 L 251.2 112.83 L 252.95 116.33 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(306.5,169.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="105" height="11" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; white-space: nowrap; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;background-color:#ffffff;">Expensive calculation</div></div></foreignObject><text x="53" y="11" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica">Expensive calculation</text></switch></g><path d="M 133.17 237.83 L 159 237.83 L 178.13 237.89" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 183.38 237.91 L 176.37 241.39 L 178.13 237.89 L 176.39 234.39 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 337.33 121.17 L 337.33 146.17 L 337.33 164.8" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 337.33 170.05 L 333.83 163.05 L 337.33 164.8 L 340.83 163.05 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><rect x="320" y="94" width="70" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(338.5,95.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="32" height="27" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 33px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">State<br /><font style="font-size: 8px">(<font style="font-size: 8px">optional</font>)</font></div></div></foreignObject><text x="16" y="20" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 133.17 237.83 L 139.83 237.83 L 139.83 180.03" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 139.83 174.78 L 143.33 181.78 L 139.83 180.03 L 136.33 181.78 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 244.83 237.83 L 359.83 237.83 L 359.83 190.03" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 359.83 184.78 L 363.33 191.78 L 359.83 190.03 L 356.33 191.78 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/></g></svg>
\ No newline at end of file
diff --git a/etc/doxygen/images/LVL1ControllerWorkerThreadDataFlow.xml b/etc/doxygen/images/LVL1ControllerWorkerThreadDataFlow.xml
new file mode 100644
index 0000000000000000000000000000000000000000..5304d71bddc63d23c85b42a8ebce0ed6c6e20566
--- /dev/null
+++ b/etc/doxygen/images/LVL1ControllerWorkerThreadDataFlow.xml
@@ -0,0 +1 @@
+<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:51.0) Gecko/20100101 Firefox/51.0" version="6.1.0.3" editor="www.draw.io" type="device"><diagram name="Page-1">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</diagram></mxfile>
\ No newline at end of file
diff --git a/scenarios/OptoForceUnit/OptoForceUnit.scx b/scenarios/OptoForceUnit/OptoForceUnit.scx
new file mode 100644
index 0000000000000000000000000000000000000000..217d563cb8db1edbd5df43ab05e530c71b970069
--- /dev/null
+++ b/scenarios/OptoForceUnit/OptoForceUnit.scx
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="OptoForceUnit" lastChange="2017-03-06.18:50:02" creation="2017-03-03.15:11:40" globalConfigName="./config/global.cfg" package="RobotAPI">
+	<application name="OptoForceUnitApp" instance="" package="RobotAPI"/>
+	<application name="OptoForceUnitObserverApp" instance="" package="RobotAPI"/>
+</scenario>
+
diff --git a/scenarios/OptoForceUnit/config/OptoForceUnitApp.cfg b/scenarios/OptoForceUnit/config/OptoForceUnitApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..d36d393fe461df989545cbfd931333583a68c0aa
--- /dev/null
+++ b/scenarios/OptoForceUnit/config/OptoForceUnitApp.cfg
@@ -0,0 +1,151 @@
+# ==================================================================
+# OptoForceUnitApp properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages:  List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+#  Attributes:
+#  - Default:            Default value not mapped.
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName:  Application name
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath:  Path for cache files
+#  Attributes:
+#  - Default:            ${HOME}/.armarx/mongo/.cache
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.CachePath = ${HOME}/.armarx/mongo/.cache
+
+
+# ArmarX.Config:  Comma-separated list of configuration files 
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath:  Semicolon-separated search list for data files
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages:  List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+#  Attributes:
+#  - Default:            Default value not mapped.
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig:  Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+#  Attributes:
+#  - Default:            ./config/dependencies.cfg
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging:  Turn logging off in whole application
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DisableLogging = 0
+
+
+# ArmarX.EnableProfiling:  Enable profiling of CPU load produced by this application
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.EnableProfiling = 0
+
+
+# ArmarX.OptoForceUnit.EnableProfiling:  enable profiler which is used for logging performance events
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OptoForceUnit.EnableProfiling = 0
+
+
+# ArmarX.OptoForceUnit.MinimumLoggingLevel:  Local logging level only for this component
+#  Attributes:
+#  - Default:            Undefined
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OptoForceUnit.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.OptoForceUnit.ObjectName:  Name of IceGrid well-known object
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OptoForceUnit.ObjectName = ""
+
+
+# ArmarX.RedirectStdout:  Redirect std::cout and std::cerr to ArmarXLog
+#  Attributes:
+#  - Default:            1
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.RedirectStdout = 1
+
+
+# ArmarX.RemoteHandlesDeletionTimeout:  The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+#  Attributes:
+#  - Default:            3000
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.StartDebuggerOnCrash:  If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.StartDebuggerOnCrash = 0
+
+
+# ArmarX.TopicSuffix:  Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer:  Enable using a global Timeserver (e.g. from ArmarXSimulator)
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.UseTimeServer = 0
+
+
+# ArmarX.Verbosity:  Global logging level for whole application
+#  Attributes:
+#  - Default:            Info
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.Verbosity = Info
+
+
+
+
+
diff --git a/scenarios/OptoForceUnit/config/OptoForceUnitObserverApp.cfg b/scenarios/OptoForceUnit/config/OptoForceUnitObserverApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..f834041ce30668ad17b90bc1f64f5efdbf7954d1
--- /dev/null
+++ b/scenarios/OptoForceUnit/config/OptoForceUnitObserverApp.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# OptoForceUnitObserverApp properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages:  List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+#  Attributes:
+#  - Default:            Default value not mapped.
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName:  Application name
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath:  Path for cache files
+#  Attributes:
+#  - Default:            ${HOME}/.armarx/mongo/.cache
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.CachePath = ${HOME}/.armarx/mongo/.cache
+
+
+# ArmarX.Config:  Comma-separated list of configuration files 
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath:  Semicolon-separated search list for data files
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages:  List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+#  Attributes:
+#  - Default:            Default value not mapped.
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig:  Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+#  Attributes:
+#  - Default:            ./config/dependencies.cfg
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging:  Turn logging off in whole application
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.DisableLogging = 0
+
+
+# ArmarX.EnableProfiling:  Enable profiling of CPU load produced by this application
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.EnableProfiling = 0
+
+
+# ArmarX.OptoForceUnitObserver.CreateUpdateFrequenciesChannel:  If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OptoForceUnitObserver.CreateUpdateFrequenciesChannel = 0
+
+
+# ArmarX.OptoForceUnitObserver.DebugDrawerTopic:  Name of the DebugDrawerTopic
+#  Attributes:
+#  - Default:            DebugDrawerUpdates
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OptoForceUnitObserver.DebugDrawerTopic = DebugDrawerUpdates
+
+
+# ArmarX.OptoForceUnitObserver.EnableProfiling:  enable profiler which is used for logging performance events
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OptoForceUnitObserver.EnableProfiling = 0
+
+
+# ArmarX.OptoForceUnitObserver.MaxHistoryRecordFrequency:  The Observer history is written with this maximum frequency. Everything faster is being skipped.
+#  Attributes:
+#  - Default:            50
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OptoForceUnitObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.OptoForceUnitObserver.MaxHistorySize:  Maximum number of entries in the Observer history
+#  Attributes:
+#  - Default:            5000
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OptoForceUnitObserver.MaxHistorySize = 5000
+
+
+# ArmarX.OptoForceUnitObserver.MinimumLoggingLevel:  Local logging level only for this component
+#  Attributes:
+#  - Default:            Undefined
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OptoForceUnitObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.OptoForceUnitObserver.ObjectName:  Name of IceGrid well-known object
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OptoForceUnitObserver.ObjectName = ""
+
+
+# ArmarX.OptoForceUnitObserver.OptoForceTopicName:  Name of the OptoForce Topic
+#  Attributes:
+#  - Default:            OptoForceValues
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OptoForceUnitObserver.OptoForceTopicName = OptoForceValues
+
+
+# ArmarX.RedirectStdout:  Redirect std::cout and std::cerr to ArmarXLog
+#  Attributes:
+#  - Default:            1
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.RedirectStdout = 1
+
+
+# ArmarX.RemoteHandlesDeletionTimeout:  The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+#  Attributes:
+#  - Default:            3000
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.StartDebuggerOnCrash:  If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.StartDebuggerOnCrash = 0
+
+
+# ArmarX.TopicSuffix:  Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer:  Enable using a global Timeserver (e.g. from ArmarXSimulator)
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.UseTimeServer = 0
+
+
+# ArmarX.Verbosity:  Global logging level for whole application
+#  Attributes:
+#  - Default:            Info
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.Verbosity = Info
+
+
+# Ice.Config:  Custom Property
+#  Attributes:
+#  - Default:            ::NOT_DEFINED::
+#  - Case sensitivity:   no
+#  - Required:           no
+# Ice.Config = ::NOT_DEFINED::
+
+
diff --git a/scenarios/OptoForceUnit/config/global.cfg b/scenarios/OptoForceUnit/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4602696c2afa4afbeb2dc4495d91705b6bfb5e46
--- /dev/null
+++ b/scenarios/OptoForceUnit/config/global.cfg
@@ -0,0 +1,7 @@
+# ==================================================================
+# Global Config from Scenario OptoForceUnit
+# ==================================================================
+
+
+
+
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 1f4d55e281f65142c52fa2434a62aad73ccabb3c..b2d194d9365882b73a00a2d5405a31739381ded4 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -23,5 +23,7 @@ add_subdirectory(InertialMeasurementUnitObserver)
 add_subdirectory(ViewSelection)
 
 add_subdirectory(OrientedTactileSensorUnit)
+add_subdirectory(OrientedTactileSensorUnitObserver)
 
-add_subdirectory(OrientedTactileSensorUnitObserver)
\ No newline at end of file
+add_subdirectory(OptoForceUnit)
+add_subdirectory(OptoForceUnitObserver)
\ No newline at end of file
diff --git a/source/RobotAPI/applications/CMakeLists.txt.orig b/source/RobotAPI/applications/CMakeLists.txt.orig
new file mode 100644
index 0000000000000000000000000000000000000000..ede34006a9c58f2e6538cb7b3f6fd9dee49b318f
--- /dev/null
+++ b/source/RobotAPI/applications/CMakeLists.txt.orig
@@ -0,0 +1,33 @@
+add_subdirectory(ForceTorqueObserver)
+add_subdirectory(HeadIKUnit)
+add_subdirectory(TCPControlUnit)
+
+add_subdirectory(WeissHapticUnit)
+add_subdirectory(HapticObserver)
+
+
+add_subdirectory(RobotControl)
+add_subdirectory(RobotControlUI)
+add_subdirectory(KinematicUnitObserver)
+add_subdirectory(KinematicUnitSimulation)
+add_subdirectory(PlatformUnitSimulation)
+add_subdirectory(PlatformUnitObserver)
+add_subdirectory(RobotStateComponent)
+add_subdirectory(HandUnitSimulation)
+add_subdirectory(ForceTorqueUnitSimulation)
+
+
+add_subdirectory(XsensIMU)
+add_subdirectory(InertialMeasurementUnitObserver)
+
+add_subdirectory(ViewSelection)
+
+<<<<<<< HEAD
+add_subdirectory(OrientedTactileSensorUnit)
+
+add_subdirectory(OrientedTactileSensorUnitObserver)
+=======
+add_subdirectory(OptoForceUnit)
+
+add_subdirectory(OptoForceUnitObserver)
+>>>>>>> master
diff --git a/source/RobotAPI/applications/OptoForceUnit/CMakeLists.txt b/source/RobotAPI/applications/OptoForceUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..be1955e8199bd33e62b90ef6514c12cf1f28d60f
--- /dev/null
+++ b/source/RobotAPI/applications/OptoForceUnit/CMakeLists.txt
@@ -0,0 +1,23 @@
+armarx_component_set_name("OptoForceUnitApp")
+
+find_package(OptoForceOMD QUIET)
+armarx_build_if(OptoForceOMD_FOUND "OptoForceOMD not available")
+
+if(OptoForceOMD_FOUND)
+    include_directories(${OptoForceOMD_INCLUDE_DIR})
+endif()
+
+#message("OptoForceOMD_INCLUDE_DIR: " ${OptoForceOMD_INCLUDE_DIR})
+#message("OptoForceOMD_LIBRARIES: " ${OptoForceOMD_LIBRARIES})
+
+set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ${OptoForceOMD_LIBRARIES})
+
+set(COMPONENT_LIBS
+    ArmarXCoreInterfaces
+    ArmarXCore
+    OptoForceUnit
+)
+
+set(EXE_SOURCE main.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/OptoForceUnit/main.cpp b/source/RobotAPI/applications/OptoForceUnit/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4c41d5d633660d88e8087cbf7b67320b23c7c642
--- /dev/null
+++ b/source/RobotAPI/applications/OptoForceUnit/main.cpp
@@ -0,0 +1,32 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::application::OptoForceUnit
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h>
+
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+    return armarx::runSimpleComponentApp < armarx::OptoForceUnit > (argc, argv, "OptoForceUnit");
+}
diff --git a/source/RobotAPI/applications/OptoForceUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/OptoForceUnitObserver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e0f6eb95c87fc537c7fe05f4679c8fb220d01d74
--- /dev/null
+++ b/source/RobotAPI/applications/OptoForceUnitObserver/CMakeLists.txt
@@ -0,0 +1,20 @@
+armarx_component_set_name("OptoForceUnitObserverApp")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+#
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+#    include_directories(${MyLib_INCLUDE_DIRS})
+#endif()
+
+set(COMPONENT_LIBS
+    ArmarXCoreInterfaces
+    ArmarXCore
+    RobotAPIUnits
+)
+
+set(EXE_SOURCE main.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/OptoForceUnitObserver/main.cpp b/source/RobotAPI/applications/OptoForceUnitObserver/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5701513b4255edbcbd3e1646b72f9bceaa7078ea
--- /dev/null
+++ b/source/RobotAPI/applications/OptoForceUnitObserver/main.cpp
@@ -0,0 +1,32 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::application::OptoForceUnitObserver
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <RobotAPI/components/units/OptoForceUnitObserver.h>
+
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+    return armarx::runSimpleComponentApp < armarx::OptoForceUnitObserver > (argc, argv, "OptoForceUnitObserver");
+}
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index bf391c4c6bea7637e8137f4319e1e1a4e032d731..09e360143f92067237016c1c5a6469a759d79ded 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -3,3 +3,4 @@ add_subdirectory(DebugDrawer)
 add_subdirectory(RobotState)
 add_subdirectory(EarlyVisionGraph)
 add_subdirectory(ViewSelection)
+
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 66291132a77baf4ce75e5041f07f53698edfaea5..e3453b178590a5e056112fdd171ca0b41d93cfb5 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -32,6 +32,7 @@ set(LIB_HEADERS
     HapticObserver.h
     InertialMeasurementUnit.h
     InertialMeasurementUnitObserver.h
+    OptoForceUnitObserver.h
     OrientedTactileSensorUnitObserver.h
     TCPControlUnit.h
     TCPControlUnitObserver.h
@@ -58,6 +59,7 @@ set(LIB_FILES
     HapticObserver.cpp
     InertialMeasurementUnit.cpp
     InertialMeasurementUnitObserver.cpp
+    OptoForceUnitObserver.cpp
     OrientedTactileSensorUnitObserver.cpp
     TCPControlUnit.cpp
     TCPControlUnitObserver.cpp
diff --git a/source/RobotAPI/components/units/CMakeLists.txt.orig b/source/RobotAPI/components/units/CMakeLists.txt.orig
new file mode 100644
index 0000000000000000000000000000000000000000..34f5c9f14a5eacb9ed7401971b5229ecc7efd18f
--- /dev/null
+++ b/source/RobotAPI/components/units/CMakeLists.txt.orig
@@ -0,0 +1,88 @@
+armarx_set_target("RobotAPI Units Library: RobotAPIUnits")
+
+
+find_package(Eigen3 QUIET)
+find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+if (Eigen3_FOUND AND Simox_FOUND)
+    include_directories(
+        ${Eigen3_INCLUDE_DIR}
+        ${Simox_INCLUDE_DIRS})
+endif()
+
+set(LIB_NAME       RobotAPIUnits)
+
+
+
+set(LIBS
+    RobotAPICore
+    ArmarXCoreObservers
+    ArmarXCoreEigen3Variants
+    ${Simox_LIBRARIES})
+
+set(LIB_HEADERS
+    ForceTorqueObserver.h
+    ForceTorqueUnit.h
+    ForceTorqueUnitSimulation.h
+    HeadIKUnit.h
+    HapticUnit.h
+    HapticObserver.h
+    InertialMeasurementUnit.h
+    InertialMeasurementUnitObserver.h
+<<<<<<< HEAD
+    OrientedTactileSensorUnitObserver.h
+=======
+    OptoForceUnitObserver.h
+>>>>>>> master
+    TCPControlUnit.h
+    TCPControlUnitObserver.h
+
+    HandUnit.h
+    HandUnitSimulation.h
+    KinematicUnit.h
+    KinematicUnitSimulation.h
+    PlatformUnit.h
+    RobotPoseUnit.h
+    PlatformUnitSimulation.h
+    KinematicUnitObserver.h
+    PlatformUnitObserver.h
+    SensorActorUnit.h
+    ATINetFTUnit.h
+)
+
+set(LIB_FILES
+    ForceTorqueObserver.cpp
+    ForceTorqueUnit.cpp
+    ForceTorqueUnitSimulation.cpp
+    HeadIKUnit.cpp
+    HapticUnit.cpp
+    HapticObserver.cpp
+    InertialMeasurementUnit.cpp
+    InertialMeasurementUnitObserver.cpp
+<<<<<<< HEAD
+    OrientedTactileSensorUnitObserver.cpp
+=======
+    OptoForceUnitObserver.cpp
+>>>>>>> master
+    TCPControlUnit.cpp
+    TCPControlUnitObserver.cpp
+
+    HandUnit.cpp
+    HandUnitSimulation.cpp
+    KinematicUnit.cpp
+    KinematicUnitSimulation.cpp
+    PlatformUnit.cpp
+    PlatformUnitSimulation.cpp
+    RobotPoseUnit.cpp
+    KinematicUnitObserver.cpp
+    PlatformUnitObserver.cpp
+    SensorActorUnit.cpp
+    ATINetFTUnit.cpp
+)
+
+armarx_add_library("${LIB_NAME}"  "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+add_subdirectory(relays)
diff --git a/source/RobotAPI/components/units/KinematicUnit.h b/source/RobotAPI/components/units/KinematicUnit.h
index 4c5cf7ddd85d60bc0859bdae8480a1939deb2bab..0faef5ace95554a0c4c56fbaa2a3cb509fed6ebb 100644
--- a/source/RobotAPI/components/units/KinematicUnit.h
+++ b/source/RobotAPI/components/units/KinematicUnit.h
@@ -90,41 +90,41 @@ namespace armarx
         /**
          * \return the robot xml filename as specified in the configuration
          */
-        virtual std::string getRobotFilename(const Ice::Current&) const;
+        virtual std::string getRobotFilename(const Ice::Current& = GlobalIceCurrent) const;
 
         /*!
          * \brief getArmarXPackages
          * \return All dependent packages, which might contain a robot file.
          */
-        virtual std::vector< std::string > getArmarXPackages(const Ice::Current&) const;
+        virtual std::vector< std::string > getArmarXPackages(const Ice::Current& = GlobalIceCurrent) const;
 
         /**
          *
          * \return  The name of this robot instance.
          */
-        virtual std::string getRobotName(const Ice::Current&) const;
+        virtual std::string getRobotName(const Ice::Current& = GlobalIceCurrent) const;
         /**
          *
          * \return  The name of this robot instance.
          */
-        virtual std::string getRobotNodeSetName(const Ice::Current&) const;
+        virtual std::string getRobotNodeSetName(const Ice::Current& = GlobalIceCurrent) const;
 
         /**
          *
          * \return  The name of the report topic
          */
-        virtual std::string getReportTopicName(const Ice::Current&) const;
+        virtual std::string getReportTopicName(const Ice::Current& = GlobalIceCurrent) const;
 
         virtual void onInitKinematicUnit() = 0;
         virtual void onStartKinematicUnit() = 0;
         virtual void onExitKinematicUnit() = 0;
 
         // proxy implementation
-        virtual void requestKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c = ::Ice::Current());
-        virtual void releaseKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c = ::Ice::Current());
-        virtual void switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c = ::Ice::Current());
-        virtual void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c = ::Ice::Current());
-        virtual void setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c = ::Ice::Current());
+        virtual void requestKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c = GlobalIceCurrent);
+        virtual void releaseKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c = GlobalIceCurrent);
+        virtual void switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c = GlobalIceCurrent);
+        virtual void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c = GlobalIceCurrent);
+        virtual void setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c = GlobalIceCurrent);
 
 
         /**
diff --git a/source/RobotAPI/components/units/OptoForceUnitObserver.cpp b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0b43e68d91e0b0d50e52fbdcc77f6fb5e5fe6a4d
--- /dev/null
+++ b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
@@ -0,0 +1,147 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package
+ * @author
+ * @date
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#include "OptoForceUnitObserver.h"
+
+
+#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
+#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
+#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
+#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+
+#include <RobotAPI/libraries/core/Pose.h>
+
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+
+
+
+using namespace armarx;
+
+
+void OptoForceUnitObserver::onInitObserver()
+{
+    usingTopic(getProperty<std::string>("OptoForceTopicName").getValue());
+
+    //offerConditionCheck("updated", new ConditionCheckUpdated());
+    //offerConditionCheck("larger", new ConditionCheckLarger());
+    //offerConditionCheck("equals", new ConditionCheckEquals());
+    //offerConditionCheck("smaller", new ConditionCheckSmaller());
+
+    offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
+}
+
+
+
+void OptoForceUnitObserver::onConnectObserver()
+{
+    debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+}
+
+
+void OptoForceUnitObserver::onExitObserver()
+{
+    //debugDrawerPrx->removePoseVisu("IMU", "orientation");
+    //debugDrawerPrx->removeLineVisu("IMU", "acceleration");
+}
+
+void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, float fx, float fy, float fz, const TimestampBasePtr& timestamp, const Ice::Current& c)
+{
+    ScopedLock lock(dataMutex);
+    TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+
+    if (!existsChannel(name))
+    {
+        offerChannel(name, "Force data");
+    }
+
+    offerOrUpdateDataField(name, "name", Variant(name), "Name of the sensor");
+    offerOrUpdateDataField(name, "device", Variant(device), "Device name");
+    offerOrUpdateDataField(name, "fx", Variant(fx), "Force x");
+    offerOrUpdateDataField(name, "fy", Variant(fy), "Force y");
+    offerOrUpdateDataField(name, "fz", Variant(fz), "Force z");
+    offerOrUpdateDataField(name, "timestamp", timestampPtr, "Timestamp");
+    offerOrUpdateDataField(name, "force", Vector3(fx, fy, fz), "Force");
+
+    updateChannel(name);
+}
+
+/*void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c)
+{
+    ScopedLock lock(dataMutex);
+
+    TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+
+    Vector3Ptr acceleration = new Vector3(values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
+    Vector3Ptr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0), values.gyroscopeRotation.at(1), values.gyroscopeRotation.at(2));
+    Vector3Ptr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
+    QuaternionPtr orientationQuaternion =  new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
+
+    if (!existsChannel(device))
+    {
+        offerChannel(device, "IMU data");
+    }
+
+    offerOrUpdateDataField(device, "name", Variant(name), "Name of the IMU sensor");
+    offerValue(device, "acceleration", acceleration);
+    offerValue(device, "gyroscopeRotation", gyroscopeRotation);
+    offerValue(device, "magneticRotation", magneticRotation);
+    offerValue(device, "acceleration", acceleration);
+    offerOrUpdateDataField(device, "orientationQuaternion", orientationQuaternion,  "orientation quaternion values");
+    offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
+
+    updateChannel(device);
+
+    Eigen::Vector3f zero;
+    zero.setZero();
+
+    DrawColor color;
+    color.r = 1;
+    color.g = 1;
+    color.b = 0;
+    color.a = 0.5;
+
+    Eigen::Vector3f ac = acceleration->toEigen();
+    ac *= 10;
+
+    debugDrawerPrx->setLineVisu("IMU", "acceleration", new Vector3(), new Vector3(ac), 2.0f, color);
+
+    PosePtr posePtr = new Pose(orientationQuaternion->toEigen(), zero);
+    debugDrawerPrx->setPoseVisu("IMU", "orientation", posePtr);
+    debugDrawerPrx->setBoxDebugLayerVisu("floor", new Pose(), new Vector3(5000, 5000, 1), DrawColor {0.7f, 0.7f, 0.7f, 1.0f});
+}*/
+
+void OptoForceUnitObserver::offerValue(std::string device, std::string fieldName, Vector3Ptr vec)
+{
+    offerOrUpdateDataField(device, fieldName, vec, fieldName + " values");
+    offerOrUpdateDataField(device, fieldName + "_x", vec->x, fieldName + "_x value");
+    offerOrUpdateDataField(device, fieldName + "_y", vec->y, fieldName + "_y value");
+    offerOrUpdateDataField(device, fieldName + "_z", vec->z, fieldName + "_z value");
+
+}
+
+
+PropertyDefinitionsPtr OptoForceUnitObserver::createPropertyDefinitions()
+{
+    return PropertyDefinitionsPtr(new OptoForceUnitObserverPropertyDefinitions(getConfigIdentifier()));
+}
+
diff --git a/source/RobotAPI/components/units/OptoForceUnitObserver.h b/source/RobotAPI/components/units/OptoForceUnitObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..a91a482fd9b087499131a0c41dd4a9605e290e13
--- /dev/null
+++ b/source/RobotAPI/components/units/OptoForceUnitObserver.h
@@ -0,0 +1,93 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::units
+ * @author     David Schiebener <schiebener at kit dot edu>
+ * @date       2014
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_OptoForce_OBSERVER_H
+#define _ARMARX_ROBOTAPI_OptoForce_OBSERVER_H
+
+#include <RobotAPI/interface/units/OptoForceUnit.h>
+#include <ArmarXCore/observers/Observer.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+
+namespace armarx
+{
+    /**
+     * \class OptoForceUnitObserverPropertyDefinitions
+     * \brief
+     */
+    class OptoForceUnitObserverPropertyDefinitions:
+        public ObserverPropertyDefinitions
+    {
+    public:
+        OptoForceUnitObserverPropertyDefinitions(std::string prefix):
+            ObserverPropertyDefinitions(prefix)
+        {
+            defineOptionalProperty<std::string>("OptoForceTopicName", "OptoForceValues", "Name of the OptoForce Topic");
+            defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
+        }
+    };
+
+
+    /**
+     * \class OptoForceUnitObserver
+     * \ingroup RobotAPI-SensorActorUnits-observers
+     * \brief Observer monitoring @IMU sensor values
+     *
+     * The OptoForceUnitObserver monitors @IMU sensor values published by OptoForceUnit-implementations and offers condition checks on these values.
+     * Available condition checks are: *updated*, *larger*, *equals* and *smaller*.
+     */
+    class OptoForceUnitObserver :
+        virtual public Observer,
+        virtual public OptoForceUnitObserverInterface
+    {
+    public:
+        OptoForceUnitObserver() {}
+
+        virtual std::string getDefaultName() const
+        {
+            return "OptoForceUnitObserver";
+        }
+        virtual void onInitObserver();
+        virtual void onConnectObserver();
+        virtual void onExitObserver();
+
+        void reportSensorValues(const std::string& device, const std::string& name, float fx, float fy, float fz, const TimestampBasePtr& timestamp, const Ice::Current& c = ::Ice::Current());
+
+        /**
+         * @see PropertyUser::createPropertyDefinitions()
+         */
+        virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+
+    private:
+        Mutex dataMutex;
+        DebugDrawerInterfacePrx debugDrawerPrx;
+
+
+        void offerValue(std::string device, std::string fieldName, Vector3Ptr vec);
+    };
+}
+
+#endif
diff --git a/source/RobotAPI/drivers/CMakeLists.txt b/source/RobotAPI/drivers/CMakeLists.txt
index d1dea0719aa7e69f881053576ef44296a5996d45..8270167be4736817298509e27d230e444cf6bc74 100644
--- a/source/RobotAPI/drivers/CMakeLists.txt
+++ b/source/RobotAPI/drivers/CMakeLists.txt
@@ -1,3 +1,5 @@
 add_subdirectory(WeissHapticSensor)
 add_subdirectory(XsensIMU)
+add_subdirectory(OptoForceUnit)
 add_subdirectory(OrientedTactileSensor)
+
diff --git a/source/RobotAPI/drivers/CMakeLists.txt.orig b/source/RobotAPI/drivers/CMakeLists.txt.orig
new file mode 100644
index 0000000000000000000000000000000000000000..41cef5bc459cacf6e4c6a88f50eec1befada44cb
--- /dev/null
+++ b/source/RobotAPI/drivers/CMakeLists.txt.orig
@@ -0,0 +1,8 @@
+add_subdirectory(WeissHapticSensor)
+add_subdirectory(XsensIMU)
+<<<<<<< HEAD
+add_subdirectory(OrientedTactileSensor)
+=======
+add_subdirectory(OptoForceUnit)
+
+>>>>>>> master
diff --git a/source/RobotAPI/drivers/OptoForce/CMakeLists.txt b/source/RobotAPI/drivers/OptoForce/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..69ebc2ee58ac092c170ded635229f10655dfe01b
--- /dev/null
+++ b/source/RobotAPI/drivers/OptoForce/CMakeLists.txt
@@ -0,0 +1,24 @@
+armarx_component_set_name("OptoForce")
+
+find_package(OptoForceOMD QUIET)
+armarx_build_if(OptoForceOMD_FOUND "OptoForceOMD not available 11elf!")
+
+if(OptoForceOMD_FOUND)
+    include_directories(${OptoForceOMD_INCLUDE_DIR})
+endif()
+
+set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore OMD)
+
+set(SOURCES
+OptoForce.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(HEADERS
+OptoForce.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/drivers/OptoForce/OptoForce.cpp b/source/RobotAPI/drivers/OptoForce/OptoForce.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f4583843a0b503724fcf040cb225f37e2259d14e
--- /dev/null
+++ b/source/RobotAPI/drivers/OptoForce/OptoForce.cpp
@@ -0,0 +1,57 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::OptoForce
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "OptoForce.h"
+
+
+using namespace armarx;
+
+
+void OptoForce::onInitComponent()
+{
+
+}
+
+
+void OptoForce::onConnectComponent()
+{
+
+}
+
+
+void OptoForce::onDisconnectComponent()
+{
+
+}
+
+
+void OptoForce::onExitComponent()
+{
+
+}
+
+armarx::PropertyDefinitionsPtr OptoForce::createPropertyDefinitions()
+{
+    return armarx::PropertyDefinitionsPtr(new OptoForcePropertyDefinitions(
+                                      getConfigIdentifier()));
+}
+
diff --git a/source/RobotAPI/drivers/OptoForce/OptoForce.h b/source/RobotAPI/drivers/OptoForce/OptoForce.h
new file mode 100644
index 0000000000000000000000000000000000000000..d8cffde689d631b3385b7a647384c531f2274025
--- /dev/null
+++ b/source/RobotAPI/drivers/OptoForce/OptoForce.h
@@ -0,0 +1,99 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::OptoForce
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_RobotAPI_OptoForce_H
+#define _ARMARX_COMPONENT_RobotAPI_OptoForce_H
+
+
+#include <ArmarXCore/core/Component.h>
+#include <opto.h>
+
+namespace armarx
+{
+    /**
+     * @class OptoForcePropertyDefinitions
+     * @brief
+     */
+    class OptoForcePropertyDefinitions:
+        public armarx::ComponentPropertyDefinitions
+    {
+    public:
+        OptoForcePropertyDefinitions(std::string prefix):
+            armarx::ComponentPropertyDefinitions(prefix)
+        {
+            //defineRequiredProperty<std::string>("PropertyName", "Description");
+            //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
+        }
+    };
+
+    /**
+     * @defgroup Component-OptoForce OptoForce
+     * @ingroup RobotAPI-Components
+     * A description of the component OptoForce.
+     * 
+     * @class OptoForce
+     * @ingroup Component-OptoForce
+     * @brief Brief description of class OptoForce.
+     * 
+     * Detailed description of class OptoForce.
+     */
+    class OptoForce :
+        virtual public armarx::Component
+    {
+    public:
+        /**
+         * @see armarx::ManagedIceObject::getDefaultName()
+         */
+        virtual std::string getDefaultName() const
+        {
+            return "OptoForce";
+        }
+
+    protected:
+        /**
+         * @see armarx::ManagedIceObject::onInitComponent()
+         */
+        virtual void onInitComponent();
+
+        /**
+         * @see armarx::ManagedIceObject::onConnectComponent()
+         */
+        virtual void onConnectComponent();
+
+        /**
+         * @see armarx::ManagedIceObject::onDisconnectComponent()
+         */
+        virtual void onDisconnectComponent();
+
+        /**
+         * @see armarx::ManagedIceObject::onExitComponent()
+         */
+        virtual void onExitComponent();
+
+        /**
+         * @see PropertyUser::createPropertyDefinitions()
+         */
+        virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions();
+    };
+}
+
+#endif
diff --git a/source/RobotAPI/drivers/OptoForce/test/CMakeLists.txt b/source/RobotAPI/drivers/OptoForce/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..752baf57ed0a71efa1839d086abc830efd2fc363
--- /dev/null
+++ b/source/RobotAPI/drivers/OptoForce/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore OptoForce)
+ 
+armarx_add_test(OptoForceTest OptoForceTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/drivers/OptoForce/test/OptoForceTest.cpp b/source/RobotAPI/drivers/OptoForce/test/OptoForceTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..721c33f3de840451e4ef7ce2b6092d8ede4c0a80
--- /dev/null
+++ b/source/RobotAPI/drivers/OptoForce/test/OptoForceTest.cpp
@@ -0,0 +1,37 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::OptoForceOMD
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::OptoForceOMD
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/OptoForceOMD/OptoForceOMD.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+    armarx::OptoForceOMD instance;
+
+    BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/drivers/OptoForceUnit/CMakeLists.txt b/source/RobotAPI/drivers/OptoForceUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b1a56eba222d6b24adbcf6099a195c108059aad9
--- /dev/null
+++ b/source/RobotAPI/drivers/OptoForceUnit/CMakeLists.txt
@@ -0,0 +1,27 @@
+armarx_component_set_name("OptoForceUnit")
+
+find_package(OptoForceOMD QUIET)
+armarx_build_if(OptoForceOMD_FOUND "OptoForceOMD not available")
+
+if(OptoForceOMD_FOUND)
+    include_directories(${OptoForceOMD_INCLUDE_DIR})
+endif()
+
+#message("OptoForceOMD_INCLUDE_DIR: " ${OptoForceOMD_INCLUDE_DIR})
+#message("OptoForceOMD_LIBRARIES: " ${OptoForceOMD_LIBRARIES})
+
+set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ArmarXCoreEigen3Variants RobotAPIInterfaces ${OptoForceOMD_LIBRARIES})
+
+set(SOURCES
+./OptoForceUnit.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(HEADERS
+./OptoForceUnit.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9fdfeaf16f6b73dea6b9d87a6a5b7959d70ca704
--- /dev/null
+++ b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
@@ -0,0 +1,220 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::OptoForceUnit
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "OptoForceUnit.h"
+
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+
+
+
+using namespace armarx;
+
+
+void OptoForceUnit::onInitComponent()
+{
+    std::string calibrationFilePath = getProperty<std::string>("CalibrationFilePath").getValue();
+    if(!ArmarXDataPath::getAbsolutePath(calibrationFilePath, calibrationFilePath))
+    {
+        throw LocalException("Could not find calibration file '") << calibrationFilePath << "'";
+    }
+    ARMARX_INFO << "reading config " << calibrationFilePath;
+    RapidXmlReaderPtr reader = RapidXmlReader::FromFile(calibrationFilePath);
+    RapidXmlReaderNode rootNode = reader->getRoot("Calibration");
+
+    auto findDaqNode = [&](std::string serialNumber)
+    {
+        for(RapidXmlReaderNode daqNode : rootNode.nodes("Daq"))
+        {
+            if(daqNode.attribute_value("serialNumber") == serialNumber)
+            {
+                return daqNode;
+            }
+        }
+        return RapidXmlReaderNode::NullNode();
+    };
+
+    offeringTopic(getProperty<std::string>("OptoForceTopicName").getValue());
+
+    OPort* portlist = ports.listPorts(true);
+    ARMARX_INFO << "Found " << ports.getLastSize() << " Optoforce device(s).";
+
+
+    portlist = ports.listPorts(true);
+    ARMARX_INFO << "Found " << ports.getLastSize() << " Optoforce device(s):";
+
+    for (int i = 0; i < ports.getLastSize(); i++)
+    {
+        std::string deviceName(portlist[i].name);
+        std::string serialNumber(portlist[i].serialNumber);
+        ARMARX_INFO << "Found DAQ: deviceName='" << deviceName << "', serialNumber='" << serialNumber << "'";
+        RapidXmlReaderNode daqNode = findDaqNode(serialNumber);
+        if(!daqNode.is_valid())
+        {
+            throw LocalException("Could not find config node for device deviceName='") << deviceName << "', serialNumber='" << serialNumber << "'";
+        }
+        DaqWrapperPtr daqPtr(new DaqWrapper(deviceName, serialNumber, daqNode));
+        daqList.push_back(daqPtr);
+    }
+
+    ARMARX_INFO << "number of ports: " << ports.getLastSize();
+
+    for (int i = 0; i < ports.getLastSize(); ++i)
+    {
+        const DaqWrapperPtr& daq = daqList.at(i);
+        ARMARX_IMPORTANT << "Opening port #" << i << " " << portlist[i].name;
+        daq->daq.open(portlist[i]);
+        daq->printInfo();
+        daq->checkSensorCount();
+    }
+
+    readTask = new RunningTask<OptoForceUnit>(this, &OptoForceUnit::run, "OptoForceUnit");
+
+}
+
+
+void OptoForceUnit::onConnectComponent()
+{
+    topicPrx = getTopic<OptoForceUnitListenerPrx>(getProperty<std::string>("OptoForceTopicName").getValue());
+    readTask->start();
+}
+
+void OptoForceUnit::run()
+{
+    ARMARX_IMPORTANT << "run";
+
+
+    OptoForceUnitListenerPrx batchPrx = topicPrx->ice_batchOneway();
+    ARMARX_IMPORTANT << "got batch Proxy";
+
+    while(readTask->isRunning())
+    {
+        bool flushNeeded = false;
+
+        for(const DaqWrapperPtr& daqPtr : daqList)
+        {
+            OptoPackage* pa = 0;
+            int size = daqPtr->daq.readAll(pa, false);
+            if (size == 0)
+            {
+                // size == 0: no new data for daq
+                continue;
+            }
+            if(size < 0)
+            {
+                // size == -1: buffer full
+                ARMARX_WARNING << "buffer full of daq " << daqPtr->deviceName;
+            }
+            if (daqPtr->daq.getSize() > 0)
+            {
+                IceUtil::Time now = IceUtil::Time::now();
+                TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
+
+                for (int i = 0; i < daqPtr->daq.getSensorSize(); i++)
+                {
+                    for (int n = 0; n < size; n++)
+                    {
+                        float countsPerN = daqPtr->countsPerN.at(i);
+                        float x = pa[i * size + n].x / countsPerN;
+                        float y = pa[i * size + n].y / countsPerN;
+                        float z = pa[i * size + n].z / countsPerN;
+
+                        batchPrx->reportSensorValues(daqPtr->deviceName + ":" + std::to_string(i), daqPtr->sensorNames.at(i), x, y, z, nowTimestamp);
+                        flushNeeded = true;
+                    }
+                }
+
+                delete[] pa;
+            }
+        }
+        if(flushNeeded)
+        {
+            batchPrx->ice_flushBatchRequests();
+        }
+        else
+        {
+            usleep(1000); // 1ms
+        }
+    }
+}
+
+
+void OptoForceUnit::onDisconnectComponent()
+{
+
+}
+
+
+void OptoForceUnit::onExitComponent()
+{
+    readTask->stop();
+}
+
+armarx::PropertyDefinitionsPtr OptoForceUnit::createPropertyDefinitions()
+{
+    return armarx::PropertyDefinitionsPtr(new OptoForceUnitPropertyDefinitions(
+                                      getConfigIdentifier()));
+}
+
+
+
+OptoForceUnit::DaqWrapper::DaqWrapper(const std::string& deviceName, const std::string& serialNumber, const RapidXmlReaderNode& daqNode)
+    : deviceName(deviceName), serialNumber(serialNumber)
+{
+    int i = 0;
+    for(RapidXmlReaderNode sensorNode : daqNode.nodes("Sensor"))
+    {
+        float counts_at_nc = sensorNode.attribute_as_float("counts_at_nc");
+        float nominalCapacity = sensorNode.attribute_as_float("nominalCapacity");
+        std::string sensorName = sensorNode.attribute_value_or_default("name", serialNumber + "-" + std::to_string(i));
+        countsPerN.push_back(counts_at_nc / nominalCapacity);
+        sensorNames.push_back(sensorName);
+        i++;
+    }
+}
+
+void OptoForceUnit::DaqWrapper::printInfo()
+{
+    SensorConfig config = daq.getConfig();
+    int state = config.getState();
+    int speed = config.getSpeed();
+    int filter = config.getFilter();
+    int mode = config.getMode();
+
+    ARMARX_IMPORTANT_S << "device: " << deviceName << " serialNumber:" << serialNumber;
+    ARMARX_IMPORTANT_S << "state: " << state;
+    ARMARX_IMPORTANT_S << "speed: " << speed;
+    ARMARX_IMPORTANT_S << "filter: " << filter;
+    ARMARX_IMPORTANT_S << "mode: " << mode;
+    ARMARX_IMPORTANT_S << "getBytesPerRead: " << daq.getBytesPerRead();
+    ARMARX_IMPORTANT_S << "getSensorSize: " << daq.getSensorSize();
+    ARMARX_IMPORTANT_S << "getSize: " << daq.getSize();
+    ARMARX_IMPORTANT_S << "getVersion: " << daq.getVersion();
+}
+
+void OptoForceUnit::DaqWrapper::checkSensorCount()
+{
+    if((int)countsPerN.size() != daq.getSensorSize())
+    {
+        throw LocalException("Sensor count mismatch. Configured: ") << ((int)countsPerN.size()) << " found: " << daq.getSensorSize();
+    }
+    ARMARX_INFO_S << "Configured: " << ((int)countsPerN.size()) << " found: " << daq.getSensorSize() << " sensors";
+}
diff --git a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..50c7fccdd3404fba75dd010cde72e7f37c92cc7a
--- /dev/null
+++ b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
@@ -0,0 +1,131 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::OptoForceUnit
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_RobotAPI_OptoForceUnit_H
+#define _ARMARX_COMPONENT_RobotAPI_OptoForceUnit_H
+
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <RobotAPI/interface/units/OptoForceUnit.h>
+#include <OptoForce/omd/opto.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+
+namespace armarx
+{
+    /**
+     * @class OptoForceUnitPropertyDefinitions
+     * @brief
+     */
+    class OptoForceUnitPropertyDefinitions:
+        public armarx::ComponentPropertyDefinitions
+    {
+    public:
+        OptoForceUnitPropertyDefinitions(std::string prefix):
+            armarx::ComponentPropertyDefinitions(prefix)
+        {
+            //defineRequiredProperty<std::string>("PropertyName", "Description");
+            defineOptionalProperty<std::string>("OptoForceTopicName", "OptoForceValues", "Name of the OptoForce Topic");
+            defineOptionalProperty<std::string>("CalibrationFilePath", "RobotAPI/sensors/OptoForceCalibration.xml", "Path of the Calibration File");
+        }
+    };
+
+    /**
+     * @defgroup Component-OptoForceUnit OptoForceUnit
+     * @ingroup RobotAPI-Components
+     * A description of the component OptoForceUnit.
+     * 
+     * @class OptoForceUnit
+     * @ingroup Component-OptoForceUnit
+     * @brief Brief description of class OptoForceUnit.
+     * 
+     * Detailed description of class OptoForceUnit.
+     */
+    class OptoForceUnit :
+        virtual public armarx::Component
+    {
+    private:
+        class DaqWrapper
+        {
+        public:
+            DaqWrapper(const std::string& deviceName, const std::string& serialNumber, const RapidXmlReaderNode& daqNode);
+
+            OptoDAQ daq;
+            std::string deviceName;
+            std::string serialNumber;
+            std::vector<float> countsPerN;
+            std::vector<std::string> sensorNames;
+
+            void printInfo();
+            void checkSensorCount();
+        };
+        typedef boost::shared_ptr<DaqWrapper> DaqWrapperPtr;
+
+    public:
+        /**
+         * @see armarx::ManagedIceObject::getDefaultName()
+         */
+        virtual std::string getDefaultName() const
+        {
+            return "OptoForceUnit";
+        }
+
+    protected:
+        /**
+         * @see armarx::ManagedIceObject::onInitComponent()
+         */
+        virtual void onInitComponent();
+
+        /**
+         * @see armarx::ManagedIceObject::onConnectComponent()
+         */
+        virtual void onConnectComponent();
+
+        /**
+         * @see armarx::ManagedIceObject::onDisconnectComponent()
+         */
+        virtual void onDisconnectComponent();
+
+        /**
+         * @see armarx::ManagedIceObject::onExitComponent()
+         */
+        virtual void onExitComponent();
+
+        /**
+         * @see PropertyUser::createPropertyDefinitions()
+         */
+        virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions();
+
+    private:
+        void run();
+
+        OptoForceUnitListenerPrx topicPrx;
+
+        OptoPorts ports;
+        //OptoDAQ daq;
+        std::vector<DaqWrapperPtr> daqList;
+        RunningTask<OptoForceUnit>::pointer_type readTask;
+
+    };
+}
+
+#endif
diff --git a/source/RobotAPI/drivers/OptoForceUnit/test/CMakeLists.txt b/source/RobotAPI/drivers/OptoForceUnit/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d98753d86660f78faaee7e476511d3af7e9daa29
--- /dev/null
+++ b/source/RobotAPI/drivers/OptoForceUnit/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore OptoForceUnit)
+ 
+armarx_add_test(OptoForceUnitTest OptoForceUnitTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/drivers/OptoForceUnit/test/OptoForceUnitTest.cpp b/source/RobotAPI/drivers/OptoForceUnit/test/OptoForceUnitTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3f12161380ccbae533b0d649ac5dd957ad57b086
--- /dev/null
+++ b/source/RobotAPI/drivers/OptoForceUnit/test/OptoForceUnitTest.cpp
@@ -0,0 +1,37 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::OptoForceUnit
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::OptoForceUnit
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+    armarx::OptoForceUnit instance;
+
+    BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index eee3075258955e3a8c5b3be0f1d33351d3b0b7f9..cc2ef98360a49b7d25c6761d3709a2fd87608b7d 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -23,6 +23,7 @@ set(SLICE_FILES
 
     units/ForceTorqueUnit.ice
     units/InertialMeasurementUnit.ice
+    units/OptoForceUnit.ice
     units/HandUnitInterface.ice
     units/HapticUnit.ice
     units/WeissHapticUnit.ice
@@ -39,6 +40,10 @@ set(SLICE_FILES
     components/ViewSelectionInterface.ice
 
     visualization/DebugDrawerInterface.ice
+
+    libraries/RTControllers/LVL1Controller.ice
+    libraries/RTControllers/PassThroughController.ice
+    libraries/RTControllers/RobotUnitInterface.ice
 )
     #core/RobotIK.ice
 
diff --git a/source/RobotAPI/interface/CMakeLists.txt.orig b/source/RobotAPI/interface/CMakeLists.txt.orig
new file mode 100644
index 0000000000000000000000000000000000000000..e28b236db5f3a6535a9007c2f835c1e56a0c651d
--- /dev/null
+++ b/source/RobotAPI/interface/CMakeLists.txt.orig
@@ -0,0 +1,54 @@
+###
+### CMakeLists.txt file for ArmarX Interfaces
+###
+
+set(ROBOTAPI_INTERFACE_DEPEND ArmarXCore)
+
+set(SLICE_FILES
+    observers/KinematicUnitObserverInterface.ice
+    observers/PlatformUnitObserverInterface.ice
+    observers/ObserverFilters.ice
+
+    core/PoseBase.ice
+    core/OrientedPoint.ice
+    core/LinkedPoseBase.ice
+    core/FramedPoseBase.ice
+    core/RobotState.ice
+    core/RobotStateObserverInterface.ice
+    core/Trajectory.ice
+
+    selflocalisation/SelfLocalisationProcess.ice
+
+    speech/SpeechInterface.ice
+
+    units/ForceTorqueUnit.ice
+    units/InertialMeasurementUnit.ice
+    units/OptoForceUnit.ice
+    units/HandUnitInterface.ice
+    units/HapticUnit.ice
+    units/WeissHapticUnit.ice
+    units/HeadIKUnit.ice
+    units/KinematicUnitInterface.ice
+    units/PlatformUnitInterface.ice
+    units/RobotPoseUnitInterface.ice
+    units/TCPControlUnit.ice
+    units/TCPMoverUnitInterface.ice
+    units/UnitInterface.ice
+    units/ATINetFTUnit.ice
+<<<<<<< HEAD
+    units/OrientedTactileSensorUnit.ice
+=======
+>>>>>>> master
+
+    components/ViewSelectionInterface.ice
+
+    visualization/DebugDrawerInterface.ice
+
+    libraries/RTControllers/LVL1Controller.ice
+    libraries/RTControllers/PassThroughController.ice
+    libraries/RTControllers/RobotUnitInterface.ice
+)
+    #core/RobotIK.ice
+
+# generate the interface library
+armarx_interfaces_generate_library(RobotAPI "${ROBOTAPI_INTERFACE_DEPEND}")
diff --git a/source/RobotAPI/interface/libraries/RTControllers/LVL1Controller.ice b/source/RobotAPI/interface/libraries/RTControllers/LVL1Controller.ice
new file mode 100644
index 0000000000000000000000000000000000000000..a229edc6ead598adad1cf61d0af6f630b616a389
--- /dev/null
+++ b/source/RobotAPI/interface/libraries/RTControllers/LVL1Controller.ice
@@ -0,0 +1,45 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::LVL1ControllerInterface
+ * @author     Raphael Grimm ( raphael dot grimm at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_LVL1ControllerInterface_SLICE_
+#define _ARMARX_ROBOTAPI_LVL1ControllerInterface_SLICE_
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+
+module armarx
+{
+    dictionary<string, string> JointNameToControlModeDictionary;
+    class LVL1ControllerConfig
+    {
+    };
+
+    interface LVL1ControllerInterface
+    {
+        ["cpp:const"] idempotent string getClassName();
+        ["cpp:const"] idempotent string  getInstanceName();
+        ["cpp:const"] idempotent JointNameToControlModeDictionary getJointControlModeMap();
+        ["cpp:const"] idempotent bool isControllerActive();
+    };
+
+    dictionary<string, LVL1ControllerInterface*> StringLVL1ControllerPrxDictionary;
+};
+#endif
diff --git a/source/RobotAPI/interface/libraries/RTControllers/PassThroughController.ice b/source/RobotAPI/interface/libraries/RTControllers/PassThroughController.ice
new file mode 100644
index 0000000000000000000000000000000000000000..7566f8c3249b8f7cf46e6986c478f8bc4d02252b
--- /dev/null
+++ b/source/RobotAPI/interface/libraries/RTControllers/PassThroughController.ice
@@ -0,0 +1,42 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::PassThroughController
+ * @author     Raphael Grimm ( raphael dot grimm at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_PassThroughController_SLICE_
+#define _ARMARX_ROBOTAPI_PassThroughController_SLICE_
+
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <RobotAPI/interface/libraries/RTControllers/LVL1Controller.ice>
+
+module armarx
+{
+    class PassThroughControllerConfig extends LVL1ControllerConfig
+    {
+        Ice::StringSeq jointNames;
+    };
+
+    interface PassThroughControllerInterface extends LVL1ControllerInterface
+    {
+        idempotent void setJoint(string joint, float value) throws InvalidArgumentException;
+        idempotent void setJoints(StringFloatDictionary values) throws InvalidArgumentException;
+    };
+};
+#endif
diff --git a/source/RobotAPI/interface/libraries/RTControllers/RobotUnitInterface.ice b/source/RobotAPI/interface/libraries/RTControllers/RobotUnitInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..aeeae71ccd7ecec1d873a1cde1b740deaf501ae0
--- /dev/null
+++ b/source/RobotAPI/interface/libraries/RTControllers/RobotUnitInterface.ice
@@ -0,0 +1,77 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::RobotUnit
+ * @author     Raphael Grimm ( raphael dot grimm at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_UNITS_RobotUnit_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_RobotUnit_SLICE_
+
+#include <ArmarXCore/interface/core/UserException.ice>
+
+#include <RobotAPI/interface/libraries/RTControllers/LVL1Controller.ice>
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+#include <RobotAPI/interface/units/ForceTorqueUnit.ice>
+#include <RobotAPI/interface/units/InertialMeasurementUnit.ice>
+#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+
+module armarx
+{
+    dictionary<string, Ice::StringSeq> JointNameToControlModesDictionary;
+
+    dictionary<string, string> JointNameToLVL1Dictionary;
+
+    interface RobotUnitInterface
+    {
+        //controllers
+        //names
+        ["cpp:const"] idempotent Ice::StringSeq getControllersKnown();
+        ["cpp:const"] idempotent Ice::StringSeq getControllerNamesLoaded();
+        ["cpp:const"] idempotent Ice::StringSeq getControllerNamesRequested();
+        ["cpp:const"] idempotent Ice::StringSeq getControllerNamesActivated();
+        //proxy
+        ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getControllersLoaded();
+        ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getControllersRequested();
+        ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getControllersActivated();
+        //assignement
+        ["cpp:const"] idempotent JointNameToLVL1Dictionary getControllerJointAssignment();
+
+        //modes
+        ["cpp:const"] idempotent JointNameToControlModeDictionary  getJointControlModesRequested();
+        ["cpp:const"] idempotent JointNameToControlModeDictionary  getJointControlModesActivated();
+        ["cpp:const"] idempotent JointNameToControlModesDictionary getJointControlModesSupported();
+
+        //loading
+        void switchSetup(Ice::StringSeq controllerInstanceNames) throws InvalidArgumentException;
+        LVL1ControllerInterface* loadController(string className, string instanceName, LVL1ControllerConfig config) throws InvalidArgumentException;
+
+        bool loadLibFromPath(string path);
+        bool loadLibFromPackage(string package, string lib);
+
+
+        //units
+        ["cpp:const"] idempotent KinematicUnitInterface* getKinematicUnit();
+        ["cpp:const"] idempotent ForceTorqueUnitInterface* getForceTorqueUnit();
+        ["cpp:const"] idempotent InertialMeasurementUnitInterface* getInertialMeasurementUnit();
+        ["cpp:const"] idempotent PlatformUnitInterface* getPlatformUnitInterface();
+    };
+};
+
+#endif
diff --git a/source/RobotAPI/interface/units/KinematicUnitInterface.ice b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
index eb6acb2d500d5fe224ebc5d4af091bcb5d06f386..09d6ecfddf00e654c9dd8699bbdbaf1ff7265c69 100644
--- a/source/RobotAPI/interface/units/KinematicUnitInterface.ice
+++ b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
@@ -128,9 +128,13 @@ module armarx
 	**/
     dictionary<string, float> NameValueMap;
     /**
-	* [NameControlModeMap] defined. This data container is mostly used to assign control modes to e.g. joints which are identified by name.
-	**/
+    * [NameControlModeMap] defined. This data container is mostly used to assign control modes to e.g. joints which are identified by name.
+    **/
     dictionary<string, ControlMode> NameControlModeMap;
+    /**
+    * [ControlModeBoolMap] defined. This data container is mostly used to signal for each control mode, whether it is supported by some component (e.g. a RobotUnit).
+    **/
+    dictionary<ControlMode, bool> ControlModeBoolMap;
     /**
 	* [NameStatusMap] defined. This data container is mostly used to a status to e.g. a joint which is identified by name.
 	**/
diff --git a/source/RobotAPI/interface/units/OptoForceUnit.ice b/source/RobotAPI/interface/units/OptoForceUnit.ice
new file mode 100644
index 0000000000000000000000000000000000000000..c0085c06fa1a9838ad6e790440ffb3ff0a457a7c
--- /dev/null
+++ b/source/RobotAPI/interface/units/OptoForceUnit.ice
@@ -0,0 +1,71 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI
+ * @author     Markus Grotz <markus dot grotz at kit dot edu>
+ * @date       2015
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_UNITS_OPTOFORCETUNIT_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_OPTOFORCETUNIT_SLICE_
+
+
+#include <RobotAPI/interface/units/UnitInterface.ice>
+#include <RobotAPI/interface/core/RobotState.ice>
+
+#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/observers/Matrix.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+
+
+
+module armarx
+{
+	/**
+	* Implements an interface to an OptoForceUnit.
+	**/
+    interface OptoForceUnitInterface extends armarx::SensorActorUnitInterface
+    {
+    };
+	/**
+	* Implements an interface to an OptoForceUnitListener.
+	**/
+    interface OptoForceUnitListener
+    {	
+		/**
+		* reportSensorValues reports the IMU sensor values from a given sensor device.
+		* @param device Name of IMU sensor device.
+		* @param values IMU sensor data.
+		* @param timestamp Timestamp of the measurement.
+		**/
+        void reportSensorValues(string device, string name, float fx, float fy, float fz, TimestampBase timestamp);
+    };
+	/**
+	* Implements an interface to an OptoForceUnitObserver.
+	**/
+    interface OptoForceUnitObserverInterface extends ObserverInterface, OptoForceUnitListener
+    {
+    };
+
+};
+
+#endif
diff --git a/source/RobotAPI/libraries/BasicRTControllers/CMakeLists.txt b/source/RobotAPI/libraries/BasicRTControllers/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5ed672502d3e1d99a07e1a9bd47b88b1440dfe24
--- /dev/null
+++ b/source/RobotAPI/libraries/BasicRTControllers/CMakeLists.txt
@@ -0,0 +1,20 @@
+armarx_component_set_name("BasicRTControllers")
+armarx_set_target("Library: BasicRTControllers")
+
+set(LIB_NAME       BasicRTControllers)
+
+set(LIBS
+	ArmarXCoreInterfaces 
+	ArmarXCore
+    RobotAPIInterfaces
+    RobotRTControllers
+)
+
+set(LIB_FILES
+    PassThroughController.cpp
+)
+set(LIB_HEADERS
+    PassThroughController.h
+)
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.cpp b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2685867430a98e18b647cc34ed3cc96723c5042f
--- /dev/null
+++ b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.cpp
@@ -0,0 +1,25 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::PassThroughController
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "PassThroughController.h"
+
+using namespace armarx;
diff --git a/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
new file mode 100644
index 0000000000000000000000000000000000000000..acb5582b8f8ba728bd45f9e099ade3cd07bd5a14
--- /dev/null
+++ b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
@@ -0,0 +1,221 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::PassThroughController
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_PassThroughController_H
+#define _ARMARX_LIB_RobotAPI_PassThroughController_H
+
+#include <atomic>
+#include <ArmarXCore/core/util/algorithm.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/RobotRTControllers/LVL1Controller.h>
+#include <RobotAPI/libraries/RobotRTControllers/RobotUnit.h>
+#include <RobotAPI/interface/libraries/RTControllers/PassThroughController.h>
+
+#include <RobotAPI/libraries/RobotRTControllers/Targets/JointPositionTarget.h>
+#include <RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h>
+#include <RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h>
+#include <RobotAPI/libraries/RobotRTControllers/DataUnits/KinematicDataUnit.h>
+
+namespace armarx
+{
+    template <typename T>
+    struct AtomicWrapper
+    {
+        std::atomic<T> val;
+
+        AtomicWrapper(): val {                } {}
+        AtomicWrapper(const T& v): val {v               } {}
+        AtomicWrapper(const std::atomic<T>& val): val {val.load()      } {}
+        AtomicWrapper(const AtomicWrapper& other): val {other.val.load()} {}
+
+        AtomicWrapper& operator=(const AtomicWrapper& other)
+        {
+            val.store(other.val.load());
+            return *this;
+        }
+    };
+
+    template <typename TargetType>
+    struct PassThroughControllerTargetTypeTraits;
+
+    template <typename TargetType>
+    class PassThroughController:
+        virtual public LVL1Controller,
+        virtual public PassThroughControllerInterface
+    {
+    public:
+        using Traits = PassThroughControllerTargetTypeTraits<TargetType>;
+
+        inline PassThroughController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config);
+
+        inline virtual void rtRun(const IceUtil::Time&, const IceUtil::Time&) override;
+        inline virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+        inline virtual void rtPreActivateController() override;
+
+        //ice interface
+        inline virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override;
+        inline virtual void setJoint(const std::string& name, Ice::Float value, const Ice::Current& = GlobalIceCurrent) override;
+        inline virtual void setJoints(const StringFloatDictionary& values, const Ice::Current& = GlobalIceCurrent) override;
+    protected:
+        std::vector<float*> lvl0Targets;
+        std::vector<const float*> jointStates;
+        std::vector<AtomicWrapper<float>> iceTargets;
+        std::unordered_map<std::string, std::size_t> indices;
+
+        virtual void onInitComponent() override {}
+        virtual void onConnectComponent() override {}
+    };
+
+    template <>
+    struct PassThroughControllerTargetTypeTraits<JointVelocityTarget>
+    {
+        static float* getTargetDatamember(JointVelocityTarget& t)
+        {
+            return  &t.velocity;
+        }
+        static std::vector<const float*> getSensorValues(const KinematicDataUnitInterface* ku, const std::vector<std::string>& v)
+        {
+            return ku->getJointVelocities(v);
+        }
+        static  std::string getControlMode()
+        {
+            return ControlModes::VelocityMode;
+        }
+    };
+    template <>
+    struct PassThroughControllerTargetTypeTraits<JointTorqueTarget>
+    {
+        static float* getTargetDatamember(JointTorqueTarget& t)
+        {
+            return  &t.torque;
+        }
+        static std::vector<const float*> getSensorValues(const KinematicDataUnitInterface* ku, const std::vector<std::string>& v)
+        {
+            return ku->getJointTorques(v);
+        }
+        static  std::string getControlMode()
+        {
+            return ControlModes::TorqueMode;
+        }
+    };
+    template <>
+    struct PassThroughControllerTargetTypeTraits<JointPositionTarget>
+    {
+        static float* getTargetDatamember(JointPositionTarget& t)
+        {
+            return  &t.position;
+        }
+        static std::vector<const float*> getSensorValues(const KinematicDataUnitInterface* ku, const std::vector<std::string>& v)
+        {
+            return ku->getJointAngles(v);
+        }
+        static std::string getControlMode()
+        {
+            return ControlModes::PositionMode;
+        }
+    };
+
+    LVL1ControllerRegistration<PassThroughController<JointPositionTarget>> registrationSomeControllerPositionPassThroughController("PositionPassThroughController");
+    LVL1ControllerRegistration<PassThroughController<JointVelocityTarget>> registrationSomeControllerVelocityPassThroughController("VelocityPassThroughController");
+    LVL1ControllerRegistration<PassThroughController<JointTorqueTarget  >> registrationSomeControllerJointPassThroughController("JointPassThroughController");
+
+    template <typename TargetType>
+    std::string PassThroughController<TargetType>::getClassName(const Ice::Current&) const
+    {
+        return "PassThroughController_" + Traits::getControlMode();
+    }
+
+    template <typename TargetType>
+    PassThroughController<TargetType>::PassThroughController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
+    {
+        PassThroughControllerConfigPtr cfg = PassThroughControllerConfigPtr::dynamicCast(config);
+        ARMARX_CHECK_EXPRESSION_W_HINT(cfg,
+                                       "The provided config has the wrong type! The type is " << config->ice_id()
+                                       << " instead of " << PassThroughControllerConfig::ice_staticId());
+
+        //make sure the used units are supported
+        auto kinunit = prov->getRTKinematicDataUnit();
+        ARMARX_CHECK_EXPRESSION_W_HINT(kinunit, "The DataProvider " << prov->getName() << " has no kinematic data unit");
+        //get pointers to sensor values from units
+        ARMARX_INFO << "PassThroughController for " << cfg->jointNames;
+        jointStates = Traits::getSensorValues(kinunit, cfg->jointNames) ;
+
+        //not initialized, this is done in rtPreActivateController
+        iceTargets.resize(cfg->jointNames.size(), 0);
+
+        //get pointers for the results of this controller
+        lvl0Targets.reserve(cfg->jointNames.size());
+        for (const auto & j : cfg->jointNames)
+        {
+            auto target = dynamic_cast<TargetType*>(prov->getJointTarget(j, Traits::getControlMode()));
+            ARMARX_CHECK_EXPRESSION_W_HINT(target, "The joint " << j << " has no controll mode " << Traits::getControlMode());
+            lvl0Targets.emplace_back(Traits::getTargetDatamember(*target));
+        }
+        indices = toIndexMap(cfg->jointNames);
+    }
+
+    template <typename TargetType>
+    void PassThroughController<TargetType>::rtRun(const IceUtil::Time&, const IceUtil::Time&)
+    {
+        for (std::size_t i = 0; i < iceTargets.size(); ++i)
+        {
+            *lvl0Targets.at(i) = iceTargets.at(i).val.load();
+        }
+    }
+
+    template <typename TargetType>
+    void PassThroughController<TargetType>::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+    {
+        rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+    }
+
+    template <typename TargetType>
+    void PassThroughController<TargetType>::rtPreActivateController()
+    {
+        for (std::size_t i = 0; i < jointStates.size(); ++i)
+        {
+            iceTargets.at(i).val.store(*jointStates.at(i));
+        }
+    }
+
+    template <typename TargetType>
+    void PassThroughController<TargetType>::setJoint(const std::string& name, Ice::Float value, const Ice::Current&)
+    {
+        auto targetIt = indices.find(name);
+        if (targetIt == indices.end())
+        {
+            throw InvalidArgumentException {"The joint " + name + " is not controlled by this (" + getName() + ") controller"};
+        }
+        iceTargets.at(targetIt->second).val.store(value);
+    }
+
+    template <typename TargetType>
+    void PassThroughController<TargetType>::setJoints(const StringFloatDictionary& values, const Ice::Current&)
+    {
+        for (const auto & value : values)
+        {
+            setJoint(value.first, value.second);
+        }
+    }
+
+}
+#endif
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index c91c3c67b7b19691e2999f179a31cd990020fcbd..6d274fa763823147869ee4ab4c02a1098396bec4 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -1,3 +1,5 @@
 add_subdirectory(core)
 add_subdirectory(widgets)
+add_subdirectory(RobotRTControllers)
+add_subdirectory(BasicRTControllers)
 
diff --git a/source/RobotAPI/libraries/RobotRTControllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotRTControllers/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..289708c82bd97fc382f77bef1c020e8ac970f132
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/CMakeLists.txt
@@ -0,0 +1,44 @@
+armarx_component_set_name("RobotRTControllers")
+armarx_set_target("Library: RobotRTControllers")
+
+set(LIB_NAME       RobotRTControllers)
+
+set(LIBS
+	ArmarXCoreInterfaces 
+        ArmarXCore
+        RobotAPIUnits
+        RobotAPIInterfaces
+)
+
+set(LIB_FILES
+    SyntaxCheck.cpp
+
+    LVL0Controller.cpp
+    LVL1Controller.cpp
+
+    RobotUnit.cpp
+)
+set(LIB_HEADERS
+    Constants.h
+    ControlModes.h
+		Targets/TargetBase.h
+    Targets/JointPositionTarget.h
+    Targets/JointVelocityTarget.h
+    Targets/JointTorqueTarget.h
+		Targets/PlatformWheelVelocityTarget.h
+
+    DataUnits/ForceTorqueDataUnit.h
+    DataUnits/HapticDataUnit.h
+    DataUnits/IMUDataUnit.h
+    DataUnits/KinematicDataUnit.h
+    DataUnits/PlatformDataUnit.h
+
+    LVL0Controller.h
+    LVL1Controller.h
+
+    RobotUnit.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
diff --git a/source/RobotAPI/libraries/RobotRTControllers/Constants.h b/source/RobotAPI/libraries/RobotRTControllers/Constants.h
new file mode 100644
index 0000000000000000000000000000000000000000..a56fce762581992a0e915d79f2a5a47efd29c3a7
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/Constants.h
@@ -0,0 +1,40 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_ControllerConstants_H
+#define _ARMARX_LIB_RobotAPI_ControllerConstants_H
+
+#include <cmath>
+#include <string>
+
+namespace armarx
+{
+    namespace ControllerConstants
+    {
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+        static const float ValueNotSetNaN = std::nanf(std::to_string((1 << 16) - 1).c_str());
+#pragma GCC diagnostic pop
+    }
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/ControlModes.h b/source/RobotAPI/libraries/RobotRTControllers/ControlModes.h
new file mode 100644
index 0000000000000000000000000000000000000000..c81774b411cf09550c6de32edce61938be640a44
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/ControlModes.h
@@ -0,0 +1,39 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_ControlModes_H
+#define _ARMARX_LIB_RobotAPI_ControlModes_H
+
+#include <string>
+
+namespace armarx
+{
+    namespace ControlModes
+    {
+        static const std::string VelocityMode      = "VelocityMode";
+        static const std::string PositionMode      = "PositionMode";
+        static const std::string TorqueMode        = "TorqueMode";
+        static const std::string EmergencyStopMode = "EmergencyStopMode";
+    }
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/DataUnits/ForceTorqueDataUnit.h b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/ForceTorqueDataUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..a297a4879603508ad6a1098a76b8bcffb3d0c4bd
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/ForceTorqueDataUnit.h
@@ -0,0 +1,39 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::ForceTorqueDataUnit
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_ForceTorqueDataUnit_H
+#define _ARMARX_LIB_RobotAPI_ForceTorqueDataUnit_H
+
+namespace armarx
+{
+    class ForceTorqueDataUnitInterface
+    {
+    public:
+    };
+
+    class ForceTorqueDataUnitPtrProvider: virtual public ForceTorqueDataUnitInterface
+    {
+    public:
+    };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/DataUnits/HapticDataUnit.h b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/HapticDataUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..de02fbe004fb45c8b8f08eafeac404bae1d9200c
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/HapticDataUnit.h
@@ -0,0 +1,39 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::HapticDataUnit
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_HapticDataUnit_H
+#define _ARMARX_LIB_RobotAPI_HapticDataUnit_H
+
+namespace armarx
+{
+    class HapticDataUnitInterface
+    {
+    public:
+    };
+
+    class HapticDataUnitPtrProvider: virtual public HapticDataUnitInterface
+    {
+    public:
+    };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/DataUnits/IMUDataUnit.h b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/IMUDataUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e7433cd1f1d9d69ede38d384433a4238a6cae09
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/IMUDataUnit.h
@@ -0,0 +1,39 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::IMUDataUnit
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_IMUDataUnit_H
+#define _ARMARX_LIB_RobotAPI_IMUDataUnit_H
+
+namespace armarx
+{
+    class IMUDataUnitInterface
+    {
+    public:
+    };
+
+    class IMUDataUnitPtrProvider: virtual public IMUDataUnitInterface
+    {
+    public:
+    };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/DataUnits/KinematicDataUnit.h b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/KinematicDataUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb5c6851f10434f6d54a6c7e4730df5f4b928d76
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/KinematicDataUnit.h
@@ -0,0 +1,232 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::KinematicDataUnit
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_KinematicDataUnit_H
+#define _ARMARX_LIB_RobotAPI_KinematicDataUnit_H
+
+#include <vector>
+#include <string>
+#include <unordered_map>
+#include <algorithm>
+
+#include <ArmarXCore/core/util/algorithm.h>
+
+namespace armarx
+{
+    class KinematicDataUnitInterface
+    {
+    public:
+        virtual const std::vector<std::string>& getJointNames() const = 0;
+
+        virtual std::vector<const float*> getJointAngles(const std::vector<std::string>& joints) const = 0;
+
+        virtual std::vector<const float*> getJointVelocities(const std::vector<std::string>& joints) const = 0;
+
+        virtual std::vector<const float*> getJointTorques(const std::vector<std::string>& joints) const = 0;
+
+        virtual std::vector<const float*> getJointCurrents(const std::vector<std::string>& joints) const = 0;
+
+        virtual std::vector<const float*> getJointMotorTemperatures(const std::vector<std::string>& joints) const = 0;
+
+        virtual std::vector<const float*> getJointGearTemperatures(const std::vector<std::string>& joints) const = 0;
+
+        virtual std::vector<const int32_t*>
+        getJointRawPositionValues(const std::vector<std::string>& joints) const = 0;
+
+        virtual std::vector<const int32_t*>
+        getJointRawVelocityValues(const std::vector<std::string>& joints) const = 0;
+
+        virtual std::vector<const int16_t*> getJointRawCurrentValues(const std::vector<std::string>& joints) const = 0;
+
+        virtual std::vector<const int32_t*> getJointRawTorqueValues(const std::vector<std::string>& joints) const = 0;
+
+        virtual std::vector<const int32_t*>
+        getJointRawGearTemperatures(const std::vector<std::string>& joints) const = 0;
+
+        virtual std::vector<const int32_t*>
+        getJointRawAbsPositionValues(const std::vector<std::string>& joints) const = 0;
+
+        virtual std::vector<const int16_t*>
+        getJointRawMotorTemperatureValues(const std::vector<std::string>& joints) const = 0;
+    };
+
+    class KinematicDataUnitPtrProvider : virtual public KinematicDataUnitInterface
+    {
+    public:
+        KinematicDataUnitPtrProvider(const std::vector<std::string>& jointNames) :
+            jointAngles(jointNames.size(), nullptr),
+            jointVelocites(jointNames.size(), nullptr),
+            jointTorques(jointNames.size(), nullptr),
+            jointCurrents(jointNames.size(), nullptr),
+            jointMotorTemperatures(jointNames.size(), nullptr),
+            jointGearTemperatures(jointNames.size(), nullptr),
+            jointAbsPositions(jointNames.size(), nullptr),
+            jointRawPositionValues(jointNames.size(), nullptr),
+            jointRawVelocityValues(jointNames.size(), nullptr),
+            jointRawCurrentValues(jointNames.size(), nullptr),
+            jointRawTorqueValues(jointNames.size(), nullptr),
+            jointRawGearTemperatures(jointNames.size(), nullptr),
+            jointRawAbsPositionValues(jointNames.size(), nullptr),
+            jointRawMotorTemperatureValues(jointNames.size(), nullptr),
+            jointNames {jointNames},
+        jointIndices {toIndexMap(jointNames)} {}
+
+        KinematicDataUnitPtrProvider() = default;
+
+        KinematicDataUnitPtrProvider(const KinematicDataUnitPtrProvider&) = default;
+
+        void setJointNames(std::vector<std::string>& names)
+        {
+            jointNames = names;
+            jointIndices = toIndexMap(names);
+
+            jointAngles = std::vector<float*>(names.size(), nullptr);
+            jointVelocites = std::vector<float*>(jointNames.size(), nullptr);
+            jointTorques = std::vector<float*>(jointNames.size(), nullptr);
+            jointCurrents = std::vector<float*>(jointNames.size(), nullptr);
+            jointMotorTemperatures = std::vector<float*>(jointNames.size(), nullptr);
+            jointGearTemperatures = std::vector<float*>(jointNames.size(), nullptr);
+            jointAbsPositions = std::vector<float*>(jointNames.size(), nullptr);
+        }
+
+        const std::vector<std::string>& getJointNames() const
+        {
+            return jointNames;
+        }
+
+        std::size_t getJointIndex(const std::string& joint) const
+        {
+            return jointIndices.at(joint);
+        }
+
+        virtual std::vector<const float*> getJointAngles(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointAngles);
+        }
+
+        virtual std::vector<const float*> getJointVelocities(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointVelocites);
+        }
+
+        virtual std::vector<const float*> getJointTorques(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointTorques);
+        }
+
+        virtual std::vector<const float*> getJointCurrents(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointCurrents);
+        }
+
+        virtual std::vector<const float*> getJointMotorTemperatures(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointMotorTemperatures);
+        }
+
+        virtual std::vector<const float*> getJointGearTemperatures(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointGearTemperatures);
+        }
+
+        virtual std::vector<const float*> getJointAbsPositions(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointAbsPositions);
+        }
+
+        virtual std::vector<const int32_t*> getJointRawPositionValues(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointRawPositionValues);
+        }
+
+        virtual std::vector<const int32_t*> getJointRawVelocityValues(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointRawVelocityValues);
+        }
+
+        virtual std::vector<const int16_t*> getJointRawCurrentValues(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointRawCurrentValues);
+        }
+
+        virtual std::vector<const int32_t*> getJointRawTorqueValues(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointRawTorqueValues);
+        }
+
+        virtual std::vector<const int32_t*> getJointRawGearTemperatures(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointRawGearTemperatures);
+        }
+
+        virtual std::vector<const int32_t*>
+        getJointRawAbsPositionValues(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointRawAbsPositionValues);
+        }
+
+        virtual std::vector<const int16_t*>
+        getJointRawMotorTemperatureValues(const std::vector<std::string>& joints) const
+        {
+            return getPointers(joints, jointRawMotorTemperatureValues);
+        }
+
+
+    protected:
+        std::vector<float*> jointAngles;
+        std::vector<float*> jointVelocites;
+        std::vector<float*> jointTorques;
+        std::vector<float*> jointCurrents;
+        std::vector<float*> jointMotorTemperatures;
+        std::vector<float*> jointGearTemperatures;
+        std::vector<float*> jointAbsPositions;
+
+        std::vector<int32_t*> jointRawPositionValues;
+        std::vector<int32_t*> jointRawVelocityValues;
+        std::vector<int16_t*> jointRawCurrentValues;
+        std::vector<int32_t*> jointRawTorqueValues;
+        std::vector<int32_t*> jointRawGearTemperatures;
+        std::vector<int32_t*> jointRawAbsPositionValues;
+        std::vector<int16_t*> jointRawMotorTemperatureValues;
+    private:
+        template<class T>
+        std::vector<const T*>
+        getPointers(const std::vector<std::string>& joints, const std::vector<T*>& targets) const
+        {
+            std::vector<const T*> result;
+            result.reserve(joints.size());
+            std::transform(
+                joints.begin(), joints.end(), std::back_inserter(result),
+                [targets, this](const std::string & joint)
+            {
+                return targets.at(getJointIndex(joint));
+            }
+            );
+            return result;
+        }
+
+        std::vector<std::string> jointNames;
+        std::unordered_map<std::string, std::size_t> jointIndices;
+    };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/DataUnits/PlatformDataUnit.h b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/PlatformDataUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..0f965ade3d392ea3ee1d6ab7b886f67a93fd142e
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/PlatformDataUnit.h
@@ -0,0 +1,88 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::PlatformDataUnit
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_PlatformDataUnit_H
+#define _ARMARX_LIB_RobotAPI_PlatformDataUnit_H
+
+namespace armarx
+{
+    class PlatformDataUnitInterface
+    {
+    public:
+        virtual const float* getFrontRightWheelVelocity() const = 0;
+        virtual const float* getFrontLeftWheelVelocity() const = 0;
+        virtual const float* getRearRightWheelVelocity() const = 0;
+        virtual const float* getRearLeftWheelVelocity() const = 0;
+    };
+
+    class PlatformDataUnitPtrProvider : public virtual PlatformDataUnitInterface
+    {
+    public:
+        const float* getFrontRightWheelVelocity() const override
+        {
+            return frontRightVelocity;
+        }
+
+        const float* getFrontLeftWheelVelocity() const override
+        {
+            return frontLeftVelocity;
+        }
+
+        const float* getRearRightWheelVelocity() const override
+        {
+            return rearRightVelocity;
+        }
+
+        const float* getRearLeftWheelVelocity() const override
+        {
+            return rearLeftVelocity;
+        }
+
+    protected:
+        void setFrontRightVelocityPtr(float* velocity)
+        {
+            frontRightVelocity = velocity;
+        }
+
+        void setFrontLeftVelocityPtr(float* velocity)
+        {
+            frontRightVelocity = velocity;
+        }
+
+        void setRearRightVelocityPtr(float* velocity)
+        {
+            frontRightVelocity = velocity;
+        }
+
+        void setRearLeftVelocityPtr(float* velocity)
+        {
+            frontRightVelocity = velocity;
+        }
+
+        float* frontRightVelocity;
+        float* frontLeftVelocity;
+        float* rearRightVelocity;
+        float* rearLeftVelocity;
+    };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.cpp b/source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f1314225db47875c050245ae3a477a13cad453ef
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.cpp
@@ -0,0 +1,28 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::LVL0Controller
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "LVL0Controller.h"
+
+
+using namespace armarx;
+
+
diff --git a/source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.h b/source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..707f920088ed506caa7d8abfd4aab15a961a75e5
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.h
@@ -0,0 +1,58 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::LVL0Controller
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_LVL0Controller_H
+#define _ARMARX_LIB_RobotAPI_LVL0Controller_H
+
+#include "Targets/TargetBase.h"
+
+#include <memory>
+
+namespace armarx
+{
+    class LVL0ControllerBase;
+    typedef std::shared_ptr<LVL0ControllerBase> LVL0ControllerBasePtr;
+
+    class LVL0ControllerBase
+    {
+    public:
+        virtual void run(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+        virtual TargetBase* getTarget() const = 0;
+
+
+        virtual void resetTarget()
+        {
+            getTarget()->reset();
+        }
+
+        virtual bool isTargetVaild() const
+        {
+            return getTarget()->isValid();
+        }
+
+        virtual std::string getControlMode() const
+        {
+            return getTarget()->getControlMode();
+        }
+    };
+}
+#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.cpp b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0b8b25547253c80beff59dd74c323fe2b7a7ecb9
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.cpp
@@ -0,0 +1,47 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::LVL1Controller
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "LVL1Controller.h"
+
+
+using namespace armarx;
+
+
+
+void LVL1Controller::rtActivateController()
+{
+    if (!isActive)
+    {
+        rtPreActivateController();
+        isActive = true;
+    }
+}
+
+void LVL1Controller::rtDeactivateController()
+{
+    if (isActive)
+    {
+        isActive = false;
+        rtPostDeactivateController();
+    }
+}
+
diff --git a/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..7c525d1525de28f8bdf93001ae9d6ae3183c660c
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
@@ -0,0 +1,340 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::LVL1Controller
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_LVL1Controller_H
+#define _ARMARX_LIB_RobotAPI_LVL1Controller_H
+
+#include <map>
+#include <atomic>
+#include <mutex>
+#include <functional>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/util/Registrar.h>
+#include <ArmarXCore/core/util/TripleBuffer.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/interface/libraries/RTControllers/LVL1Controller.h>
+
+#include "Targets/TargetBase.h"
+
+//units
+#include "DataUnits/ForceTorqueDataUnit.h"
+#include "DataUnits/HapticDataUnit.h"
+#include "DataUnits/IMUDataUnit.h"
+#include "DataUnits/KinematicDataUnit.h"
+#include "DataUnits/PlatformDataUnit.h"
+
+namespace armarx
+{
+    class LVL1ControllerDataProviderInterface: virtual public Ice::Object
+    {
+    public:
+        virtual const ForceTorqueDataUnitInterface* getRTForceTorqueDataUnit() const = 0;
+        virtual const      HapticDataUnitInterface* getRTHapticDataUnit() const = 0;
+        virtual const         IMUDataUnitInterface* getRTIMUDataUnit() const = 0;
+        virtual const   KinematicDataUnitInterface* getRTKinematicDataUnit() const = 0;
+        virtual const    PlatformDataUnitInterface* getRTPlatformDataUnit() const = 0;
+
+        virtual TargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode) = 0;
+
+        virtual std::string getName() const = 0;
+    };
+    using LVL1ControllerDataProviderInterfacePtr = IceInternal::Handle<LVL1ControllerDataProviderInterface>;
+
+    /**
+    * @defgroup Library-RobotRTControllers RobotRTControllers
+    * @ingroup RobotAPI
+    * A description of the library RobotRTControllers.
+    */
+
+    /**
+    * @ingroup Library-RobotRTControllers
+    * @brief A high level controller writing its results into targes.
+    *
+    * This is the abstract base class for all LVL1Controllers.
+    * It implements basic management routines required by the RobotUnit and some basic ice calls.
+    * LVL1Controllers are intantiated and managed by RobotUnits.
+    *
+    * Each LVL1Controller has to do the following in their constructor:
+    * \li Request joints in exactly one control mode from the robot unit (the control mode can't be changed later)
+    *     You will receive pointer targets where it can store its results.
+    * \li Get pointer to the state of required joints / sensors. (you can't request other data at a later point)
+    *     You will receive pointer to the values.
+    *
+    * Each controller has two parts:
+    * \li The RT controll loop
+    * \li The NonRT ice communication
+    *
+    * \section rtcontrollloop The RT controll loop (rtRun)
+    * Here you have access to the robots current state and write your results into targets.
+    * \warning This part has to satisfy realtime conditions!
+    * All realtime able functions of this class have the 'rt' prefix.
+    *
+    * You must not:
+    * \li call any blocking operation
+    * \li allocate ram on the heap (since this is a blocking operation)
+    * \li resize any datastructure (e.g. vector::resize) (this sometimes allocates ram on the heap)
+    * \li insert new datafields into datastructures (e.g. vector::push_back) (this sometimes allocates ram on the heap)
+    * \li write to any stream (e.g. ARMARX_VERBOSE, std::cout, print, filestreams) (this sometimes blocks/allocates ram on the heap)
+    * \li make network calls (e.g. through ice) (this blocks/allocates ram on the heap) (using begin_... end_... version of ice calls IS NO SOLUTION)
+    * \li do any expensive calculations (e.g. calculate IK, run some solver, invert big matrices)
+    *
+    * \section nonrtpart The NonRT ice communication
+    * This part consits of any ice communication.
+    * Here you could get new controll parameters or targets from other components.
+    *
+    * \section rtnrtcomm Communication between RT and NonRT
+    * All communication between RT and NonRT has to be lockfree.
+    * So you have to use constructs like atomics or TripleBuffers (See armarx::LVL1ControllerWithTripleBuffer).
+    *
+    * \image html LVL1ControllerGeneralDataFlow.svg "General Dataflow in a LVL1Controller"
+    *
+    *
+    *
+    *
+    * \image html LVL1ControllerAtomicDataFlow.svg "Dataflow in a LVL1Controller using atomics for communication between RT and NonRT"
+    *
+    *
+    *
+    *
+    * \image html LVL1ControllerTripleBufferDataFlow.svg "Dataflow in a LVL1Controller using a triple buffer for communication between RT and NonRT"
+    *
+    * \section expensivework How to do expensive calculations
+    * You have to execute expensive calculations in a different worker thread.
+    * This thread could calculate target values at its own speed (e.g. 100 Hz).
+    *
+    * While rtRun runs at a higher frequency (e.g. 1000 Hz) and:
+    * \li reads target values
+    * \li optionally passes the target values to a PID controller
+    * \li writes them to the targets
+    * \li sends the sensor values to the worker thread.
+    *
+    * If you do some additional calculation in rtRun, you maybe need to pass config parameters from NonRT to RT using a nonblocking method.
+    *
+    * \image html LVL1ControllerWorkerThreadDataFlow.svg "Dataflow in a LVL1Controller using a worker thread"
+    *
+    */
+    class LVL1Controller:
+        virtual public LVL1ControllerInterface,
+        virtual public Component
+    {
+    public:
+        //ice interface (must not be called in the rt thread)
+        virtual bool isControllerActive(const Ice::Current& = GlobalIceCurrent) const final
+        {
+            return isActive;
+        }
+        virtual std::string getInstanceName(const Ice::Current& = GlobalIceCurrent) const final
+        {
+            return getName();
+        }
+        virtual StringStringDictionary getJointControlModeMap(const Ice::Current& = GlobalIceCurrent) const final
+        {
+            return jointControlModeMap;
+        }
+        virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override  = 0;
+        //c++ interface (local calls) (must not be called in the rt thread)
+        //c++ interface (local calls) (can only be called in the rt thread)
+        virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+        virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+        /**
+         * @brief This function is called before the controller is activated.
+         * You can use it to activate a thread again (DO NOT SPAWN NEW THREADS!) e.g. via a std::atomic_bool.
+         */
+        virtual void rtPreActivateController() {}
+        /**
+         * @brief This function is called after the controller is deactivated.
+         * You can use it to deactivate a thread (DO NOT JOIN THREADS!) e.g. via a std::atomic_bool.
+         */
+        virtual void rtPostDeactivateController() {}
+
+        bool rtUsesJoint(std::size_t jointindex) const
+        {
+            return std::find(jointIndices.begin(), jointIndices.end(), jointindex) == jointIndices.end();
+        }
+        const std::vector<std::size_t>& rtGetJointIndices() const
+        {
+            return jointIndices;
+        }
+    private:
+        friend class RobotUnit;
+        void rtActivateController();
+        void rtDeactivateController();
+
+        //this data is filled by the robot unit to provide convenience functions
+        std::atomic_bool isActive {false};
+        JointNameToControlModeDictionary jointControlModeMap;
+        std::vector<std::size_t> jointIndices;
+
+        // ManagedIceObject interface
+    protected:
+        std::string getDefaultName() const
+        {
+            return getClassName();
+        }
+    };
+    using LVL1ControllerPtr = IceInternal::Handle<LVL1Controller>;
+
+    using LVL1ControllerRegistry = Registrar<std::function<LVL1ControllerPtr(LVL1ControllerDataProviderInterfacePtr robotUnit, LVL1ControllerConfigPtr config)>>;
+    template<class ControllerType>
+    struct LVL1ControllerRegistration
+    {
+        LVL1ControllerRegistration(const std::string& name)
+        {
+            LVL1ControllerRegistry::registerElement(name, [](LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
+            {
+                ARMARX_CHECK_EXPRESSION_W_HINT(prov, "DataProvider is NULL!");
+                return new ControllerType(prov, config);
+            });
+        }
+    };
+
+    /**
+    * @ingroup Library-RobotRTControllers
+    * @brief Brief description of class LVL1Controller.
+    *
+    * Detailed description of class LVL1Controller.
+    *
+    * \code{cpp}
+    //in ice
+    module armarx
+    {
+        struct SomeControllerConfig extends LVL1ControllerConfig
+        {
+            float paramSetViaConfig;
+        };
+
+        interface SomeControllerInterface extends LVL1ControllerInterface
+        {
+            void setSomeParamChangedViaIce(float param);
+        };
+    };
+
+
+    //in c++
+    #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+    namespace armarx
+    {
+        struct SomeControlDataStruct
+        {
+            float parameterChangedViaIceCalls;
+        };
+
+        class SomeController : virtual public LVL1Controller<SomeControlDataStruct>,
+            public SomeControllerInterface
+        {
+            JointVelocityTargetPtr targetJointXPtr;
+            std::vector<const float*> jointValuePtrs;
+            float someParamSetViaConfig;
+            SomeController(RobotUnitPtr robotUnit, LVL1ControllerConfigPtr config):
+                LVL1Controller<SomeControlDataStruct>(SomeControlDataStruct(1)) //initial parameter value
+            {
+                //cast the config to your config type
+                SomeControlDataStructPtr someConfig = SomeControlDataStructPtr::dynamicCast(config);
+                ARMARX_CHECK_EXPRESSION_W_HINT(someConfig,
+                                               "The provided config has the wrong type! The type is " << config->ice_id()
+                                               << " instead of " << SomeControllerConfig::ice_staticId());
+                //read the config
+                someParamSetViaConfig = someConfig->parameterSetViaIceCalls
+                //make sure the used units are supported
+                ARMARX_CHECK_EXPRESSION_W_HINT(robotUnit->getRTKinematicDataUnit(), "The RobotUnit " << robotUnit->getName() << " has no kinematic data unit");
+                //get pointers to sensor values from units
+                jointValuePtrs = robotUnit->getRTKinematicDataUnit()->getJointAngles({"jointX", "jointY"});
+
+                //get pointers for the results of this controller
+                targetJointXPtr = dynamic_cast<JointVelocityTarget*>(robotUnit->getJointTarget("jointX", ControlModes::VelocityMode));
+                ARMARX_CHECK_EXPRESSION_W_HINT(targetJointXPtr, "The Joint jointX does not support the mode " + ControlModes::VelocityMode);
+            }
+
+            SomeController(JointVelocityTargetPtr targetPtr, ControllerConfigPtr config):
+                targetJointXPtr{targetPtr}
+            {
+
+            }
+
+            virtual void run(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
+            {
+                pid.update(
+                            rtGetControlStruct().parameterChangedViaIceCalls,
+                            jointValuePtrs.at(0),
+                            timeSinceLastIteration.toMicroSeconds());
+                *targetJointXPtr = pid.getValue() + someParamSetViaConfig;
+            }
+
+            virtual void setSomeParamChangedViaIce(float param, const Ice::Current& = GlobalIceCurrent) override
+            {
+                std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
+                getWriterControlStruct().parameterChangedViaIceCalls = param;
+                writeControlStruct();
+            }
+        }
+        //register the controller
+        //this line has to appear in some cpp of your lib to do its registration (including it is enough)
+        LVL1ControllerRegistration<SomeController> registrationSomeController("SomeController");
+    }
+    * \endcode
+    */
+    template <typename ControlDataStruct>
+    class LVL1ControllerWithTripleBuffer: virtual public LVL1Controller
+    {
+    public:
+        LVL1ControllerWithTripleBuffer(const ControlDataStruct& initialCommands = ControlDataStruct()):
+            controlDataTripleBuffer {initialCommands}
+        {
+        }
+    protected:
+        const ControlDataStruct& rtGetControlStruct() const
+        {
+            return controlDataTripleBuffer.getReadBuffer();
+        }
+        bool                     rtUpdateControlStruct()
+        {
+            return controlDataTripleBuffer.updateReadBuffer();
+        }
+        virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
+        {
+            rtUpdateControlStruct();
+            rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+        }
+
+        void writeControlStruct()
+        {
+            //just lock to be save and reduce the impact of an error
+            //also this allows code to unlock the mutex before calling this function
+            //(can happen if some lockguard in a scope is used)
+            std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
+            controlDataTripleBuffer.commitWrite();
+        }
+        ControlDataStruct& getWriterControlStruct()
+        {
+            return controlDataTripleBuffer.getWriteBuffer();
+        }
+
+        mutable std::recursive_mutex controlDataMutex;
+    private:
+        WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
+    };
+    template <typename ControlDataStruct>
+    using LVL1ControllerTemplatePtr = IceInternal::Handle<LVL1ControllerWithTripleBuffer<ControlDataStruct>>;
+}
+#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c10b03c2f32e69f20bd6b215d60c5a6fd8750801
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
@@ -0,0 +1,600 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::RobotUnit
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "RobotUnit.h"
+
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+#include <ArmarXCore/core/system/DynamicLibrary.h>
+
+armarx::PropertyDefinitionsPtr armarx::RobotUnit::createPropertyDefinitions()
+{
+    return armarx::PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
+}
+
+void armarx::RobotUnit::addLVL0Controller(const std::string& jointName, armarx::LVL0ControllerBase& lvl0Controller)
+{
+    std::string controlMode = lvl0Controller.getControlMode();
+    GuardType guard {dataMutex};
+    ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
+    if (hasLVL0Controller(jointName, controlMode))
+    {
+        ARMARX_ERROR << "A LVL0Controller for " + jointName + "(" + controlMode + ") does already exist!";
+        throw std::invalid_argument {"A LVL0Controller for " + jointName + "(" + controlMode + ") does already exist!"};
+    }
+    lvl0Controllers[jointName][controlMode] = &lvl0Controller;
+    if (controlMode == ControlModes::EmergencyStopMode)
+    {
+        lvl0ControllersEmergencyStop.at(jointNameIndices.at(jointName)) = &lvl0Controller;
+    }
+}
+
+const armarx::LVL0ControllerBase* armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode) const
+{
+    GuardType guard {dataMutex};
+    ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
+    return lvl0Controllers.at(jointName).at(controlMode);
+}
+
+armarx::LVL0ControllerBase* armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode)
+{
+    GuardType guard {dataMutex};
+    ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
+    return lvl0Controllers.at(jointName).at(controlMode);
+}
+
+Ice::StringSeq armarx::RobotUnit::getControllersKnown(const Ice::Current&) const
+{
+    return LVL1ControllerRegistry::getKeys();
+}
+
+Ice::StringSeq armarx::RobotUnit::getControllerNamesLoaded(const Ice::Current&) const
+{
+    GuardType guard {dataMutex};
+    Ice::StringSeq result;
+    result.reserve(lvl1Controllers.size());
+    for (const auto & lvl1 : lvl1Controllers)
+    {
+        result.emplace_back(lvl1.first);
+    }
+    return result;
+}
+Ice::StringSeq armarx::RobotUnit::getControllerNamesRequested(const Ice::Current&) const
+{
+    GuardType guard {dataMutex};
+    Ice::StringSeq result;
+    result.reserve(lvl1Controllers.size());
+    for (const auto & lvl1 : getRequestedLVL1Controllers())
+    {
+        if (lvl1)
+        {
+            result.emplace_back(lvl1->getName());
+        }
+    }
+    return result;
+}
+Ice::StringSeq armarx::RobotUnit::getControllerNamesActivated(const Ice::Current&) const
+{
+    GuardType guard {dataMutex};
+    Ice::StringSeq result;
+    result.reserve(lvl1Controllers.size());
+    for (const auto & lvl1 : getActivatedLVL1Controllers())
+    {
+        if (!lvl1)
+        {
+            continue;
+        }
+        result.emplace_back(lvl1->getName());
+    }
+    return result;
+}
+
+armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersLoaded(const Ice::Current&) const
+{
+    GuardType guard {dataMutex};
+    StringLVL1ControllerPrxDictionary result;
+    for (const auto & lvl1 : lvl1Controllers)
+    {
+        result[lvl1.first] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1.second->getProxy());
+    }
+    return result;
+}
+armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersRequested(const Ice::Current&) const
+{
+    GuardType guard {dataMutex};
+    StringLVL1ControllerPrxDictionary result;
+    for (const auto & lvl1 : getRequestedLVL1Controllers())
+    {
+        if (lvl1)
+        {
+            result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
+        }
+    }
+    return result;
+}
+armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersActivated(const Ice::Current&) const
+{
+    GuardType guard {dataMutex};
+    StringLVL1ControllerPrxDictionary result;
+    for (const auto & lvl1 : getActivatedLVL1Controllers())
+    {
+        result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
+    }
+    return result;
+}
+
+armarx::JointNameToLVL1Dictionary armarx::RobotUnit::getControllerJointAssignment(const Ice::Current&) const
+{
+    GuardType guard {dataMutex};
+    JointNameToLVL1Dictionary result;
+    for (const auto & joint : jointNames)
+    {
+        result[joint] = "";
+    }
+    for (const auto & lvl1 : getActivatedLVL1Controllers())
+    {
+        if (!lvl1)
+        {
+            continue;
+        }
+        for (const auto & jointMode : lvl1->jointControlModeMap)
+        {
+            result[jointMode.first] = lvl1->getName();
+        }
+    }
+    return result;
+}
+
+armarx::JointNameToControlModeDictionary armarx::RobotUnit::getJointControlModesRequested(const Ice::Current&) const
+{
+    GuardType guard {dataMutex};
+    JointNameToControlModeDictionary result;
+    const auto& requestedModes = getRequestedLVL0Controllers();
+    ARMARX_CHECK_AND_THROW(jointNames.size() == requestedModes.size(), std::logic_error);
+    for (std::size_t i = 0; i < jointNames.size(); ++i)
+    {
+        if (requestedModes.at(i))
+        {
+            result[jointNames.at(i)] = requestedModes.at(i)->getControlMode();
+        }
+        else
+        {
+            result[jointNames.at(i)] = "<NO LVL0CONTROLLER SET>";
+        }
+    }
+    return result;
+}
+armarx::JointNameToControlModeDictionary armarx::RobotUnit::getJointControlModesActivated(const Ice::Current&) const
+{
+    GuardType guard {dataMutex};
+    JointNameToControlModeDictionary result;
+    const auto& activatedModes = getActivatedLVL0Controllers();
+    ARMARX_CHECK_AND_THROW(jointNames.size() == activatedModes.size(), std::logic_error);
+    for (std::size_t i = 0; i < jointNames.size(); ++i)
+    {
+        if (activatedModes.at(i))
+        {
+            result[jointNames.at(i)] = activatedModes.at(i)->getControlMode();
+        }
+        else
+        {
+            result[jointNames.at(i)] = "<NO LVL0CONTROLLER SET>";
+        }
+
+    }
+    return result;
+}
+armarx::JointNameToControlModesDictionary armarx::RobotUnit::getJointControlModesSupported(const Ice::Current&) const
+{
+    GuardType guard {dataMutex};
+    JointNameToControlModesDictionary result;
+    for (const auto & joint : lvl0Controllers)
+    {
+        std::vector<std::string> modes;
+        modes.reserve(joint.second.size());
+        for (const auto & mode : joint.second)
+        {
+            modes.emplace_back(mode.first);
+        }
+        result[joint.first] = std::move(modes);
+    }
+    return result;
+}
+
+void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNames, const Ice::Current&)
+{
+    GuardType guard {dataMutex};
+    ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
+    //check if all of these controllers exist
+    for (const auto & lvl1 : controllerRequestedNames)
+    {
+        if (!hasLVL1Controller(lvl1))
+        {
+            throw InvalidArgumentException {"No controller of the name '" + lvl1 + "' is loaded!"};
+        }
+    }
+    //check controllers (is there a collision/which controllers need to be deactivated)
+    const auto currentActiveLVL1Controllers = getControllerNamesActivated();
+    auto lvl1ControllerAssignement = getControllerJointAssignment();
+
+    ARMARX_INFO << "current active lvl1 controllers:\n" << currentActiveLVL1Controllers;
+
+    std::set<std::string> controllersToActivate {currentActiveLVL1Controllers.begin(), currentActiveLVL1Controllers.end()}; //these controllers will be set as active controllers
+
+    for (const auto & toActivate : controllerRequestedNames)
+    {
+        if (controllersToActivate.count(toActivate))
+        {
+            ARMARX_INFO << "controller already active: " << toActivate;
+            continue;
+        }
+        controllersToActivate.emplace(toActivate);
+
+        ARMARX_INFO << "activate '" << toActivate << "'";
+
+        const auto& lvl1 = lvl1Controllers.at(toActivate);
+        //check and update the assignement map
+        for (const auto & jointControlMode : lvl1->jointControlModeMap)
+        {
+            const auto& joint = jointControlMode.first;
+            const auto& currentAssignedLVL1 = lvl1ControllerAssignement.at(joint);
+            if (toActivate == currentAssignedLVL1)
+            {
+                continue;
+            }
+            if (currentAssignedLVL1.empty())
+            {
+                lvl1ControllerAssignement[joint] = toActivate;
+                continue;
+            }
+            //deactivate other controller
+            for (auto & assignement : lvl1ControllerAssignement)
+            {
+                if (assignement.second == currentAssignedLVL1)
+                {
+                    assignement.second.clear();
+                }
+            }
+            controllersToActivate.erase(currentAssignedLVL1);
+            ARMARX_INFO << "deactivated '" << currentAssignedLVL1 << "' (caused by activation of '" << toActivate << "')";
+            lvl1ControllerAssignement[joint] = toActivate;
+        }
+    }
+    //verify (exception for collision of requested lvl1)
+    for (const auto & toActivate : controllerRequestedNames)
+    {
+        if (!controllersToActivate.count(toActivate))
+        {
+            switchSetupError = true;
+            ARMARX_ERROR << "The given set of controllers is in conflict and could not be activated! (conflicting is: " + toActivate + ")";
+            throw InvalidArgumentException {"The given set of controllers is in conflict and could not be activated! (conflicting is: " + toActivate + ")"};
+        }
+    }
+    //verify (warn for orphant joints) + populate controllersRequested.lvl0Controllers
+    getRequestedLVL0Controllers().resize(jointNames.size());
+    for (std::size_t i = 0; i < jointNames.size(); ++i)
+    {
+        const auto& joint = jointNames.at(i);
+        if (lvl1ControllerAssignement[joint].empty())
+        {
+            getRequestedLVL0Controllers().at(i) = getLVL0Controller(joint, ControlModes::EmergencyStopMode);
+            ARMARX_WARNING << "Joint '" << joint << "' has no lvl1 controller assigned!";
+            continue;
+        }
+        const auto& lvl0Mode = lvl1Controllers.at(lvl1ControllerAssignement[joint])->jointControlModeMap.at(joint);
+        getRequestedLVL0Controllers().at(i) = getLVL0Controller(joint, lvl0Mode);
+    }
+    //populate controllersRequested.lvl1Controllers
+    getRequestedLVL1Controllers().clear();
+    getRequestedLVL1Controllers().reserve(controllersToActivate.size());
+    for (const auto & lvl1 : controllersToActivate)
+    {
+        getRequestedLVL1Controllers().emplace_back(lvl1Controllers.at(lvl1));
+    }
+
+    //now change the assignement
+    controllersRequested.commitWrite();
+    ARMARX_INFO << "requested lvl1 controllers:\n" << getControllerNamesRequested();
+}
+
+armarx::LVL1ControllerInterfacePrx armarx::RobotUnit::loadController(const std::string& className, const std::string& instanceName, const armarx::LVL1ControllerConfigPtr& config, const Ice::Current&)
+{
+    if (instanceName.empty())
+    {
+        ARMARX_ERROR << "The instance name is empty!";
+        throw InvalidArgumentException {"The instance name is empty!"};
+    }
+    //check if we would be able to create the class
+    if (!LVL1ControllerRegistry::has(className))
+    {
+        std::stringstream ss;
+        ss << "Requested controller class '" << className << "' unknown! Known classes:" << LVL1ControllerRegistry::getKeys();
+        ARMARX_ERROR << ss.str();
+        throw InvalidArgumentException {ss.str()};
+    }
+    auto factory = LVL1ControllerRegistry::get(className);
+
+    GuardType guard {dataMutex};
+    //check if the instance name is already in use
+    if (hasLVL1Controller(instanceName))
+    {
+        std::stringstream ss;
+        ss << "There already is a controller instance with the name '" << instanceName << "'";
+        ARMARX_ERROR << ss.str();
+        throw InvalidArgumentException {ss.str()};
+    }
+    //create the controller
+    jointsUsedByLVL1Controler.clear();
+    RobotUnitInterfacePtr::dynamicCast(RobotUnitPtr {this});
+    LVL1ControllerDataProviderInterfacePtr::dynamicCast(RobotUnitPtr {this});
+    LVL1ControllerPtr lvl1 = factory(LVL1ControllerDataProviderInterfacePtr::dynamicCast(RobotUnitPtr {this}), config);
+    lvl1->jointControlModeMap = jointsUsedByLVL1Controler;
+    lvl1->jointIndices.clear();
+    lvl1->jointIndices.reserve(jointsUsedByLVL1Controler.size());
+    for (const auto & j : jointsUsedByLVL1Controler)
+    {
+        lvl1->jointIndices.emplace_back(jointNameIndices.at(j.first));
+    }
+
+    getArmarXManager()->addObject(lvl1, instanceName);
+    const auto prx = lvl1->getProxy(-1);
+    lvl1Controllers[instanceName] = lvl1;
+    return LVL1ControllerInterfacePrx::uncheckedCast(prx);
+}
+
+armarx::TargetBase* armarx::RobotUnit::getJointTarget(const std::string& jointName, const std::string& controlMode)
+{
+    GuardType guard {dataMutex};
+    if (!hasLVL0Controller(jointName, controlMode))
+    {
+        return nullptr;
+    }
+    const auto lvl0 = getLVL0Controller(jointName, controlMode);
+    ARMARX_CHECK_EXPRESSION(lvl0->getControlMode() == controlMode);
+    jointsUsedByLVL1Controler[jointName] = controlMode;
+    return lvl0->getTarget();
+}
+
+void armarx::RobotUnit::setJointNames(const std::vector<std::string>& names)
+{
+    GuardType guard {dataMutex};
+    ARMARX_CHECK_EXPRESSION_W_HINT(!areJointNamesSet(), "JointNames were already set!");
+    jointNames = names;
+    jointNameIndices = toIndexMap(jointNames);
+    lvl0ControllersEmergencyStop.resize(jointNames.size());
+    controllersRequested.reinitAllBuffers(LVL0AndLVL1ControllerList(jointNames.size()));
+    controllersActivated.reinitAllBuffers(LVL0AndLVL1ControllerList(jointNames.size()));
+}
+
+bool armarx::RobotUnit::hasLVL1Controller(const std::string& name) const
+{
+    GuardType guard {dataMutex};
+    ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
+    return lvl1Controllers.end() != lvl1Controllers.find(name);
+}
+
+void armarx::RobotUnit::rtSetLVL1ControllerActive(const armarx::LVL1ControllerPtr& lvl1, bool isActive)
+{
+    if (isActive)
+    {
+        lvl1->rtActivateController();
+    }
+    else
+    {
+        lvl1->rtDeactivateController();
+    }
+}
+
+bool armarx::RobotUnit::areJointNamesSet() const
+{
+    GuardType guard {dataMutex};
+    return !jointNames.empty();
+}
+
+bool armarx::RobotUnit::hasLVL0Controller(const std::string& jointName, const std::string& controlMode) const
+{
+    GuardType guard {dataMutex};
+    ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
+    return  lvl0Controllers.end()               != lvl0Controllers.find(jointName) &&
+            lvl0Controllers.at(jointName).end() != lvl0Controllers.at(jointName).find(controlMode) &&
+            lvl0Controllers.at(jointName).at(controlMode);
+}
+
+
+bool armarx::RobotUnit::loadLibFromAbsolutePath(const std::string& path)
+{
+    GuardType guard {dataMutex};
+    if (loadedLibs.count(path) > 0)
+    {
+        return true;
+    }
+    DynamicLibraryPtr lib(new DynamicLibrary());
+    try
+    {
+        lib->load(path);
+    }
+    catch (...)
+    {
+        handleExceptions();
+        return false;
+    }
+
+    if (lib->isLibraryLoaded())
+    {
+        loadedLibs[path] = lib;
+    }
+    else
+    {
+        ARMARX_ERROR << "Could not load lib " + path + ": " + lib->getErrorMessage();
+        return false;
+    }
+
+
+    return true;
+}
+
+bool armarx::RobotUnit::loadLibFromPath(const std::string& path, const Ice::Current&)
+{
+    std::string absPath;
+    if (ArmarXDataPath::getAbsolutePath(path, absPath))
+    {
+        return loadLibFromAbsolutePath(absPath);
+    }
+    else
+    {
+        ARMARX_ERROR << "Could not find library " + path;
+        return false;
+    }
+}
+
+bool armarx::RobotUnit::loadLibFromPackage(const std::string& package, const std::string& name, const Ice::Current&)
+{
+    CMakePackageFinder finder(package);
+    if (!finder.packageFound())
+    {
+        ARMARX_ERROR << "Could not find package '" << package << "'";
+        return false;
+    }
+
+    for (auto libDirPath : armarx::Split(finder.getLibraryPaths(), ";"))
+    {
+        boost::filesystem::path fullPath = libDirPath;
+        fullPath /= "lib" + name  + "." + DynamicLibrary::GetSharedLibraryFileExtension();
+        if (!boost::filesystem::exists(fullPath))
+        {
+            fullPath = libDirPath;
+            fullPath /= name;
+            if (!boost::filesystem::exists(fullPath))
+            {
+                continue;
+            }
+        }
+        if (loadLibFromAbsolutePath(fullPath.string()))
+        {
+            return true;
+        }
+    }
+    ARMARX_ERROR << "Could not find library " <<  name << " in package " << package;
+    return false;
+}
+
+bool armarx::RobotUnit::rtSwitchSetup()
+{
+    if (!rtControllersWereSwitched())
+    {
+        return false;
+    }
+
+    //handle lvl1
+    for (std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
+    {
+        //deactivate old
+        if (rtGetActivatedLVL1Controller(i))
+        {
+            rtGetActivatedLVL1Controller(i)->rtDeactivateController();
+        }
+
+        //activate new
+        if (rtGetRequestedLVL1Controller(i))
+        {
+            rtGetRequestedLVL1Controller(i)->rtActivateController();
+        }
+        //update activated
+        rtGetActivatedLVL1Controller(i) = rtGetRequestedLVL1Controller(i);
+    }
+    //handle lvl0
+    for (std::size_t i = 0; i < rtGetRequestedLVL0Controllers().size(); ++i)
+    {
+        if (rtGetRequestedLVL0Controller(i) != rtGetActivatedLVL0Controller(i))
+        {
+            rtSwitchLVL0Controller(i, rtGetActivatedLVL0Controller(i), rtGetRequestedLVL0Controller(i));
+            rtGetActivatedLVL0Controller(i) = rtGetRequestedLVL0Controller(i);
+        }
+
+        if (!rtGetRequestedLVL0Controller(i))
+        {
+            //no lvl0 controller is set!
+            rtSwitchLVL0Controller(i, rtGetActivatedLVL0Controller(i), rtGetEmergencyStopLVL0Controller(i));
+            rtGetActivatedLVL0Controller(i) = rtGetActivatedLVL0Controller(i);
+        }
+    }
+
+
+    return true;
+}
+
+void armarx::RobotUnit::rtRunLVL1Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+{
+    for (LVL1ControllerPtr & lvl1 : rtGetActivatedLVL1Controllers())
+    {
+        if (!lvl1)
+        {
+            continue;
+        }
+        lvl1->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration);
+    }
+}
+
+void armarx::RobotUnit::rtDeactivateAssignedLVL1Controller(std::size_t index)
+{
+    for (std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
+    {
+        if (!rtGetRequestedLVL1Controller(i))
+        {
+            continue;
+        }
+        if (rtGetRequestedLVL1Controller(i)->rtUsesJoint(index))
+        {
+            rtGetRequestedLVL1Controller(i)->rtDeactivateController();
+            for (auto used : rtGetRequestedLVL1Controller(i)->rtGetJointIndices())
+            {
+                rtSwitchLVL0Controller(used, rtGetActivatedLVL0Controller(i), rtGetEmergencyStopLVL0Controller(i));
+                rtGetActivatedLVL0Controller(i) = rtGetEmergencyStopLVL0Controller(i);
+            }
+            rtGetActivatedLVL1Controller(i) = nullptr;
+            return;
+        }
+    }
+}
+
+void armarx::RobotUnit::rtRunLVL0Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+{
+    for (LVL0ControllerBase * lvl0 : rtGetActivatedLVL0Controllers())
+    {
+        lvl0->run(sensorValuesTimestamp, timeSinceLastIteration);
+    }
+}
+
+bool armarx::RobotUnit::validateAddedLVL0Controllers() const
+{
+    GuardType guard {dataMutex};
+    ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
+    for (const auto & lvl0 : lvl0ControllersEmergencyStop)
+    {
+        if (!lvl0)
+        {
+            return false;
+        }
+    }
+    return true;
+
+}
diff --git a/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..29b64a714f907e0204d84b1d7aaf1ef59fede61d
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
@@ -0,0 +1,456 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::RobotUnit
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_UNIT_RobotAPI_RobotUnit_H
+#define _ARMARX_UNIT_RobotAPI_RobotUnit_H
+
+#include <thread>
+#include <atomic>
+#include <chrono>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/algorithm.h>
+#include <ArmarXCore/core/util/TripleBuffer.h>
+
+#include <RobotAPI/interface/libraries/RTControllers/RobotUnitInterface.h>
+#include "LVL0Controller.h"
+#include "LVL1Controller.h"
+
+namespace armarx
+{
+    class DynamicLibrary;
+    typedef boost::shared_ptr<DynamicLibrary> DynamicLibraryPtr;
+    /**
+     * @class RobotUnitPropertyDefinitions
+     * @brief
+     */
+    class RobotUnitPropertyDefinitions:
+        public armarx::ComponentPropertyDefinitions
+    {
+    public:
+        RobotUnitPropertyDefinitions(std::string prefix):
+            armarx::ComponentPropertyDefinitions(prefix)
+        {
+            //defineRequiredProperty<std::string>("PropertyName", "Description");
+            //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
+        }
+    };
+
+    /**
+     * @defgroup Component-RobotUnit RobotUnit
+     * @ingroup RobotAPI-Components
+     * A description of the component RobotUnit.
+     *
+     * @class RobotUnit
+     * @ingroup Component-RobotUnit
+     * @brief Brief description of class RobotUnit.
+     *
+     * \section Datastructures you should have
+     * \li A thread for RT
+     * \li A thread for publishing + errorreporting
+     * \li A Tripplebuffer to publish the current robot state + current targets
+     * \li A WriteBufferedTripleBuffer containing an error and logging structure (to communicate such information to the publisher thread)
+     *
+     * \section RobotUnit-tasks-to-implement Tasks to implement when deriving from this class
+     * When implementing this class you have to handle the following tasks:
+     * \li Implement the real time thread.
+     * \li Implement the publisher thread.
+     * \li Implement the communication between both threads.
+     * \li Load all LVL0 controllers and add them to the unit.
+     *
+     * \subsection RobotUnit-tasks-to-implement-rt Implementing the RT thread
+     * The thread has to be RT.
+     * So you must not do anything of the following:
+     * \li Call blocking actions (e.g. mutex::lock)
+     * \li allocate ram using new (this calls lock on a mutex)
+     * \li print output (this may allocate ram or block)
+     * \li change the size of a datasructure (strings, vectors, etc) (this may allocate ram)
+     *
+     * The structure for a thread is:
+     * \code{.cpp}
+     * void rtThread()
+     * {
+     *      try
+     *      {
+     *          initYourCommunication();//implement this
+     *          passYourLVL0ControllersToTheRobotUnit();//implement this
+     *          if(!validateAddedLVL0Controllers())
+     *          {
+     *              //report errors
+     *              throw std::logic_error{"lvl0 controller setup invalid"};
+     *          }
+     *
+     *          //at this point the RT thread becomes RT (calls before this may not be rt)
+     *          IceUtil::Time currentStateTimestamp = TimeUtil::GetTime();
+     *          IceUtil::Time lastStateTimestamp = TimeUtil::GetTime();
+     *          getTheCurrentRobotState();//implement this
+     *          while(isNotStopped())//implement this
+     *          {
+     *              const IceUtil::Time  startIteration = TimeUtil::GetTime();
+     *              //switching controllers
+     *              if(rtSwitchSetup())
+     *              {
+     *                  if(switchSetupError)
+     *                  {
+     *                      //report this error and exit
+     *                      throw std::logic_error{"switching the controller setup failed"};
+     *                  }
+     *                  //validate the setup if you need to
+     *                  //if you had to change something, call rtCommitActivatedControllers();
+     *              }
+     *              //run the lvl1 controllers
+     *              rtRunLVL1Controllers(currentStateTimestamp, currentStateTimestamp - lastStateTimestamp);
+     *              //validate targets
+     *              for(std::size_t i = 0; i<rtGetActivatedLVL0Controllers().size(); ++i)
+     *              {
+     *                  if(!rtGetActivatedLVL0Controller(i)->isTargetVaild())
+     *                  {
+     *                      //handle this error!
+     *                      //you probably should log this error to some error struct
+     *                      rtDeactivateAssignedLVL1Controller(i);
+     *                      rtCommitActivatedControllers();
+     *                  }
+     *              }
+     *              //run lvl0
+     *              rtRunLVL0Controllers();
+     *
+     *              //time keeping + communicating
+     *              lastStateTimestamp = currentStateTimestamp;
+     *              currentStateTimestamp = TimeUtil::GetTime();
+     *              getTheCurrentRobotStateAndSendTheCurrentControllCommands();//implement this
+     *              communicateWithYourPublisherThread();//implement this
+     *              //maybe meassure how long a loop took and log this information to your error struct
+     *              TimeUtil::Sleep(IceUtil::Time::microSeconds(1000)-(currentStateTimestamp-startIteration));
+     *          }
+     *      }
+     *      catch(...)
+     *      {
+     *          emergencyStop();//implement this
+     *          dumpDebuggingData();//implement this
+     *          doSomeErrorHandling();//implement this
+     *          shutEverythingGracefullyDown();//implement this
+     *      }
+     * }
+     * \endcode
+     *
+     * \subsection RobotUnit-tasks-to-implement-pub Implementing the publisher thread
+     *
+     * \subsection RobotUnit-tasks-to-implement-com Implementing the communication between both threads
+     * The communication between both threads has to be RT.
+     * So you have to use atomics or TrippleBuffers (or any other non-blocking method)
+     * Data you should communicate is:
+     * \li Current state (joint modes/values)
+     * \li Current goals
+     * \li all error messages.
+     *
+     * \subsection RobotUnit-tasks-to-implement-lvl0 Adding LVL0 controllers
+     * Add LVL0Controllers by calling
+     * \code{.cpp}
+     * addLVL0Controller(const std::string& jointName, LVL0ControllerBase& lvl0Controller);
+     * \endcode
+     * If you add two controllers withe the same control mode for one joint, an std::invalid_argument exception is thrown.
+     *
+     * Each joint needs a LVL0Controller for the mode ControlModes::EmergencyStopMode.
+     * This controller should activate the motor brakes.
+     */
+    class RobotUnit :
+        virtual public Component,
+        virtual public RobotUnitInterface,
+        virtual public LVL1ControllerDataProviderInterface
+    {
+    public:
+        using MutexType = std::recursive_mutex;
+        using GuardType = std::lock_guard<MutexType>;
+
+        // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+        //ice LVL1ControllerDataProviderInterface
+        // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+        virtual std::string getName() const override
+        {
+            return Component::getName();
+        }
+        // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+        //ice interface
+        // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+        //controllers
+        virtual Ice::StringSeq getControllersKnown(const Ice::Current& = GlobalIceCurrent) const override;
+        virtual Ice::StringSeq getControllerNamesLoaded(const Ice::Current& = GlobalIceCurrent) const override;
+        virtual Ice::StringSeq getControllerNamesRequested(const Ice::Current& = GlobalIceCurrent) const override;
+        virtual Ice::StringSeq getControllerNamesActivated(const Ice::Current& = GlobalIceCurrent) const override;
+
+        virtual StringLVL1ControllerPrxDictionary getControllersLoaded(const Ice::Current& = GlobalIceCurrent) const override;
+        virtual StringLVL1ControllerPrxDictionary getControllersRequested(const Ice::Current& = GlobalIceCurrent) const override;
+        virtual StringLVL1ControllerPrxDictionary getControllersActivated(const Ice::Current& = GlobalIceCurrent) const override;
+        virtual JointNameToLVL1Dictionary getControllerJointAssignment(const Ice::Current& = GlobalIceCurrent) const override;
+        //modes
+        virtual JointNameToControlModeDictionary  getJointControlModesRequested(const Ice::Current& = GlobalIceCurrent) const override;
+        virtual JointNameToControlModeDictionary  getJointControlModesActivated(const Ice::Current& = GlobalIceCurrent) const override;
+        virtual JointNameToControlModesDictionary getJointControlModesSupported(const Ice::Current& = GlobalIceCurrent) const override;
+        //loading and switching
+        virtual void switchSetup(const Ice::StringSeq& controllerRequestedNames, const Ice::Current& = GlobalIceCurrent) override;
+        virtual LVL1ControllerInterfacePrx loadController(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config, const Ice::Current& = GlobalIceCurrent) override;
+        virtual bool loadLibFromPath(const std::string& path, const Ice::Current& = GlobalIceCurrent) override;
+        virtual bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = GlobalIceCurrent) override;
+
+        //units
+        virtual           KinematicUnitInterfacePrx getKinematicUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
+        virtual         ForceTorqueUnitInterfacePrx getForceTorqueUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
+        virtual InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
+        virtual            PlatformUnitInterfacePrx getPlatformUnitInterface(const Ice::Current& = GlobalIceCurrent) const = 0;
+        // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+        // used by lvl1 controllers
+        // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+        //units (nonrt but results are used in rt)
+        virtual const ForceTorqueDataUnitInterface* getRTForceTorqueDataUnit() const = 0;
+        virtual const      HapticDataUnitInterface* getRTHapticDataUnit() const = 0;
+        virtual const         IMUDataUnitInterface* getRTIMUDataUnit() const = 0;
+        virtual const   KinematicDataUnitInterface* getRTKinematicDataUnit() const = 0;
+        virtual const    PlatformDataUnitInterface* getRTPlatformDataUnit() const = 0;
+        //targets (nonrt but results are used in rt)
+        virtual TargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode) override;
+
+    protected:
+        // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+        // NonRT
+        // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+        // setup the data structures
+        void setJointNames(const std::vector<std::string>& names);
+
+        // Component
+        /// @see PropertyUser::createPropertyDefinitions()
+        virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+        //get buffers
+        /// @return The requested lvl0 controllers (none is null)
+        std::vector<LVL0ControllerBase*>& getRequestedLVL0Controllers()
+        {
+            return controllersRequested.getWriteBuffer().lvl0Controllers;
+        }
+        /// @return The requested lvl1 controllers (some may be null)
+        std::vector<LVL1ControllerPtr  >& getRequestedLVL1Controllers()
+        {
+            return controllersRequested.getWriteBuffer().lvl1Controllers;
+        }
+        /// @return The requested lvl0 controllers (none is null)
+        const std::vector<LVL0ControllerBase*>& getRequestedLVL0Controllers() const
+        {
+            return controllersRequested.getWriteBuffer().lvl0Controllers;
+        }
+        /// @return The requested lvl1 controllers (some may be null)
+        const std::vector<LVL1ControllerPtr  >& getRequestedLVL1Controllers() const
+        {
+            return controllersRequested.getWriteBuffer().lvl1Controllers;
+        }
+        /// @return The activated lvl0 controllers (none is null)
+        const std::vector<LVL0ControllerBase*>& getActivatedLVL0Controllers() const
+        {
+            return controllersActivated.getUpToDateReadBuffer().lvl0Controllers;
+        }
+        /// @return The activated lvl1 controllers (some may be null)
+        const std::vector<LVL1ControllerPtr  >& getActivatedLVL1Controllers() const
+        {
+            return controllersActivated.getUpToDateReadBuffer().lvl1Controllers;
+        }
+
+        //checks
+        /// @return Return whether the set of added LVL0Controllers is ok (e.g. if each joint has one EmergencyStop controller)
+        virtual bool validateAddedLVL0Controllers() const;
+
+        /**
+         * @brief Add a LVL0Controller for a specified joint.
+         * @param jointName The joint controlled by the controller
+         * @param lvl0Controller The controller to add.
+         * @throw If you add two controllers withe the same control mode for one joint, an std::invalid_argument exception is thrown.
+         */
+        void addLVL0Controller(const std::string& jointName, LVL0ControllerBase& lvl0Controller);
+
+        /// @return The LVL0Controller for given joint and control mode.
+        const LVL0ControllerBase* getLVL0Controller(const std::string& jointName, const std::string& controlMode) const;
+
+        /// @return The LVL0Controller for given joint and control mode.
+        LVL0ControllerBase* getLVL0Controller(const std::string& jointName, const std::string& controlMode);
+
+        LVL0ControllerBase* rtGetEmergencyStopLVL0Controller(std::size_t index)
+        {
+            return lvl0ControllersEmergencyStop.at(index);
+        }
+
+        /// @return Whether a LVL0Controller for given joint and control mode exists.
+        bool hasLVL0Controller(const std::string& jointName, const std::string& controlMode) const;
+
+        /// @return Whether a LVL1Controller for this name is loaded.
+        bool hasLVL1Controller(const std::string& name) const;
+
+        static void rtSetLVL1ControllerActive(const LVL1ControllerPtr& lvl1, bool isActive = 1);
+
+        //other
+    private:
+        bool areJointNamesSet() const;
+        bool loadLibFromAbsolutePath(const std::string& path);
+
+    protected:
+        // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+        // RT
+        // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+        // get buffers (rt)
+        bool rtControllersWereSwitched() const
+        {
+            return controllersRequested.updateReadBuffer();
+        }
+        /// @return The requested lvl0 controllers (none is null)
+        const std::vector<LVL0ControllerBase*>& rtGetRequestedLVL0Controllers() const
+        {
+            return controllersRequested.getReadBuffer().lvl0Controllers;
+        }
+        /// @return The requested lvl1 controllers (some may be null)
+        const std::vector<LVL1ControllerPtr  >& rtGetRequestedLVL1Controllers() const
+        {
+            return controllersRequested.getReadBuffer().lvl1Controllers;
+        }
+        /// @return The activated lvl0 controllers (none is null)
+        std::vector<LVL0ControllerBase*>& rtGetActivatedLVL0Controllers()
+        {
+            return controllersActivated.getWriteBuffer().lvl0Controllers;
+        }
+        /// @return The activated lvl1 controllers (some may be null)
+        std::vector<LVL1ControllerPtr  >& rtGetActivatedLVL1Controllers()
+        {
+            return controllersActivated.getWriteBuffer().lvl1Controllers;
+        }
+        /// @return The activated lvl0 controllers (none is null)
+        const std::vector<LVL0ControllerBase*>& rtGetActivatedLVL0Controllers() const
+        {
+            return controllersActivated.getWriteBuffer().lvl0Controllers;
+        }
+        /// @return The activated lvl1 controllers (some may be null)
+        const std::vector<LVL1ControllerPtr  >& rtGetActivatedLVL1Controllers() const
+        {
+            return controllersActivated.getWriteBuffer().lvl1Controllers;
+        }
+        //get controllers (rt)
+        LVL0ControllerBase* const& rtGetActivatedLVL0Controller(std::size_t index) const
+        {
+            return rtGetActivatedLVL0Controllers().at(index);
+        }
+        const LVL1ControllerPtr& rtGetActivatedLVL1Controller(std::size_t index) const
+        {
+            return rtGetActivatedLVL1Controllers().at(index);
+        }
+        LVL0ControllerBase*& rtGetActivatedLVL0Controller(std::size_t index)
+        {
+            return rtGetActivatedLVL0Controllers().at(index);
+        }
+        LVL1ControllerPtr& rtGetActivatedLVL1Controller(std::size_t index)
+        {
+            return rtGetActivatedLVL1Controllers().at(index);
+        }
+
+
+        LVL0ControllerBase* rtGetRequestedLVL0Controller(std::size_t index) const
+        {
+            return rtGetRequestedLVL0Controllers().at(index);
+        }
+        const LVL1ControllerPtr&   rtGetRequestedLVL1Controller(std::size_t index) const
+        {
+            return getRequestedLVL1Controllers().at(index);
+        }
+
+        void rtCommitActivatedControllers()
+        {
+            controllersActivated.commitWrite();
+        }
+
+        //switching (rt)
+        bool rtSwitchSetup();
+        void rtRunLVL1Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+        void rtDeactivateAssignedLVL1Controller(std::size_t index);
+        void rtRunLVL0Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+        /**
+         * @brief Hook for switching the lvl0 controller (this changes the controll mode)
+         * @param index The index of the lvl0 controller
+         * @param oldLVL0 The old lvl0 controller (will be deactivated)
+         * @param newLVL0 The new lvl0 controller (will be activated)
+         */
+        virtual void rtSwitchLVL0Controller(std::size_t index, LVL0ControllerBase* oldLVL0, LVL0ControllerBase* newLVL0) = 0;
+
+
+    protected:
+        // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+        // data
+        // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+        mutable MutexType dataMutex;
+        //data accessible in RT and nonRT
+        std::vector<std::string> jointNames;
+        std::unordered_map<std::string, std::size_t> jointNameIndices;
+        /// @brief True if some error occured in switch LVL1Controllers
+        std::atomic_bool switchSetupError {false};
+
+    private:
+        //controllers
+        struct LVL0AndLVL1ControllerList
+        {
+            LVL0AndLVL1ControllerList(std::size_t size):
+                lvl0Controllers {size, nullptr},
+                            lvl1Controllers {size, nullptr}
+            {}
+
+            LVL0AndLVL1ControllerList() = default;
+            LVL0AndLVL1ControllerList(const LVL0AndLVL1ControllerList&) = default;
+
+            std::vector<LVL0ControllerBase*> lvl0Controllers;
+            std::vector<LVL1ControllerPtr  > lvl1Controllers;
+        };
+        /**
+         * @brief Holds pointer to all lvl0 controllers. (index: [jointname][controlmode])
+         * Is initialized by derived classes.
+         * May not be accessed when the controll loop is running
+         */
+        std::map<std::string, std::map<std::string, LVL0ControllerBase*>> lvl0Controllers;
+        /**
+         * @brief Holds all currently loaded LVL1 controllers (index: [instancename])
+         * May not be accessed in rt.
+         */
+        std::map<std::string, LVL1ControllerPtr> lvl1Controllers;
+        /// @brief LVL0Controllers for Emergency stop
+        std::vector<LVL0ControllerBase*> lvl0ControllersEmergencyStop;
+
+        //used to communicate with rt
+        /// @brief Controllers the RT thread is supposed to activate (direction nonRT -> RT) (some lvl1 controllers may be null)
+        WriteBufferedTripleBuffer<LVL0AndLVL1ControllerList> controllersRequested;
+        /// @brief Controllers the RT thread is currently running (direction RT -> nonRT) (some lvl1 controllers may be null)
+        WriteBufferedTripleBuffer<LVL0AndLVL1ControllerList> controllersActivated;
+
+        //other
+        /// @brief Stores joints accessed via getJointTarget
+        JointNameToControlModeDictionary jointsUsedByLVL1Controler;
+        std::map<std::string, DynamicLibraryPtr> loadedLibs;
+    };
+
+    using RobotUnitPtr = IceInternal::Handle<RobotUnit>;
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/SyntaxCheck.cpp b/source/RobotAPI/libraries/RobotRTControllers/SyntaxCheck.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..537a95b05e346be4e757283dee548af8e73bdfe0
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/SyntaxCheck.cpp
@@ -0,0 +1,38 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "Constants.h"
+#include "LVL0Controller.h"
+#include "LVL1Controller.h"
+
+#include "DataUnits/ForceTorqueDataUnit.h"
+#include "DataUnits/HapticDataUnit.h"
+#include "DataUnits/IMUDataUnit.h"
+#include "DataUnits/KinematicDataUnit.h"
+#include "DataUnits/PlatformDataUnit.h"
+
+#include "Targets/JointPositionTarget.h"
+#include "Targets/TargetBase.h"
+#include "Targets/JointTorqueTarget.h"
+#include "Targets/JointVelocityTarget.h"
+
+//this file is only for syntax checks
diff --git a/source/RobotAPI/libraries/RobotRTControllers/Targets/JointPositionTarget.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointPositionTarget.h
new file mode 100644
index 0000000000000000000000000000000000000000..2fe706d361672c09f0d8b0c1e9e548e76b893e58
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointPositionTarget.h
@@ -0,0 +1,61 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::JointPositionTarget
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_JointPositionTarget_H
+#define _ARMARX_LIB_RobotAPI_JointPositionTarget_H
+
+#include "TargetBase.h"
+
+namespace armarx
+{
+    /**
+    * @defgroup Library-RobotRTControllers RobotRTControllers
+    * @ingroup RobotAPI
+    * A description of the library RobotRTControllers.
+    *
+    * @class JointPositionTarget
+    * @ingroup Library-RobotRTControllers
+    * @brief Brief description of class JointPositionTarget.
+    *
+    * Detailed description of class JointPositionTarget.
+    */
+    class JointPositionTarget: public TargetBase
+    {
+    public:
+        float position = ControllerConstants::ValueNotSetNaN;
+        virtual std::string getControlMode() const override
+        {
+            return ControlModes::PositionMode;
+        }
+        virtual void reset() override
+        {
+            position = ControllerConstants::ValueNotSetNaN;
+        }
+        virtual bool isValid() const override
+        {
+            return std::isfinite(position);
+        }
+    };
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h
new file mode 100644
index 0000000000000000000000000000000000000000..0c70ed9a03735db69c57beb3a01c666f107304c6
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h
@@ -0,0 +1,61 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::JointTorqueTarget
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_JointTorqueTarget_H
+#define _ARMARX_LIB_RobotAPI_JointTorqueTarget_H
+
+#include "TargetBase.h"
+
+namespace armarx
+{
+    /**
+    * @defgroup Library-RobotRTControllers RobotRTControllers
+    * @ingroup RobotAPI
+    * A description of the library RobotRTControllers.
+    *
+    * @class JointTorqueTarget
+    * @ingroup Library-RobotRTControllers
+    * @brief Brief description of class JointTorqueTarget.
+    *
+    * Detailed description of class JointTorqueTarget.
+    */
+    class JointTorqueTarget: public TargetBase
+    {
+    public:
+        float torque = ControllerConstants::ValueNotSetNaN;
+        virtual std::string getControlMode() const override
+        {
+            return ControlModes::TorqueMode;
+        }
+        virtual void reset() override
+        {
+            torque = ControllerConstants::ValueNotSetNaN;
+        }
+        virtual bool isValid() const override
+        {
+            return std::isfinite(torque);
+        }
+    };
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h
new file mode 100644
index 0000000000000000000000000000000000000000..1c6296d950b7d1a1a7642f768409b2e1f662ad15
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h
@@ -0,0 +1,60 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::JointVelocityTarget
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_JointVelocityTarget_H
+#define _ARMARX_LIB_RobotAPI_JointVelocityTarget_H
+
+#include "TargetBase.h"
+
+namespace armarx
+{
+    /**
+    * @defgroup Library-RobotRTControllers RobotRTControllers
+    * @ingroup RobotAPI
+    * A description of the library RobotRTControllers.
+    *
+    * @class JointVelocityTarget
+    * @ingroup Library-RobotRTControllers
+    * @brief Brief description of class JointVelocityTarget.
+    *
+    * Detailed description of class JointVelocityTarget.
+    */
+    class JointVelocityTarget: public TargetBase
+    {
+    public:
+        float velocity = ControllerConstants::ValueNotSetNaN;
+        virtual std::string getControlMode() const override
+        {
+            return ControlModes::VelocityMode;
+        }
+        virtual void reset() override
+        {
+            velocity = ControllerConstants::ValueNotSetNaN;
+        }
+        virtual bool isValid() const override
+        {
+            return std::isfinite(velocity);
+        }
+    };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RobotRTControllers/Targets/PlatformWheelVelocityTarget.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/PlatformWheelVelocityTarget.h
new file mode 100644
index 0000000000000000000000000000000000000000..c2bafb0adb4cb68407667fbf5db85c2eb9eb7ea6
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/PlatformWheelVelocityTarget.h
@@ -0,0 +1,57 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Markus Swarowsky (markus dot swarowsky at student dot kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+
+#ifndef _ARMARX_LIB_RobotAPI_PLATFORMWHEELTARGET_H
+#define _ARMARX_LIB_RobotAPI_PLATFORMWHEELTARGET_H
+
+
+#include "TargetBase.h"
+
+
+namespace armarx
+{
+
+    class PlatformWheelVelocityTarget : public TargetBase
+    {
+    public:
+        float velocity = ControllerConstants::ValueNotSetNaN;
+        virtual std::string getControlMode() const override
+        {
+            return ControlModes::VelocityMode;
+        }
+        virtual void reset() override
+        {
+            velocity = ControllerConstants::ValueNotSetNaN;
+        }
+        virtual bool isValid() const override
+        {
+            return std::isfinite(velocity);
+        }
+
+    private:
+    };
+
+}
+
+#endif //_ARMARX_LIB_RobotAPI_PLATFORMWHEELTARGET_H
diff --git a/source/RobotAPI/libraries/RobotRTControllers/Targets/TargetBase.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/TargetBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..d303e24ebcb1ff546bcb8bd68d485f50f6b1fb12
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/TargetBase.h
@@ -0,0 +1,52 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::JointTargetBase
+ * @author     Raphael ( ufdrv at student dot kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_JointTargetBase_H
+#define _ARMARX_LIB_RobotAPI_JointTargetBase_H
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include "../Constants.h"
+#include "../ControlModes.h"
+
+namespace armarx
+{
+    /**
+    * @defgroup Library-RobotRTControllers RobotRTControllers
+    * @ingroup RobotAPI
+    * A description of the library RobotRTControllers.
+    *
+    * @class TargetBase
+    * @ingroup Library-RobotRTControllers
+    * @brief Brief description of class JointTargetBase.
+    *
+    * Detailed description of class TargetBase.
+    */
+    class TargetBase
+    {
+    public:
+        virtual std::string getControlMode() const = 0;
+        virtual void reset() = 0;
+        virtual bool isValid() const = 0;
+    };
+}
+
+#endif