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/LaserScannerTest/LaserScannerTest.scx b/scenarios/LaserScannerTest/LaserScannerTest.scx
new file mode 100644
index 0000000000000000000000000000000000000000..dde93318980244be817875d4c6053720d0455418
--- /dev/null
+++ b/scenarios/LaserScannerTest/LaserScannerTest.scx
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="LaserScannerTest" lastChange="2017-03-09.17:02:34" creation="2017-03-07.16:12:47" globalConfigName="./config/global.cfg" package="RobotAPI">
+	<application name="HokuyoLaserUnitApp" instance="" package="RobotAPI"/>
+	<application name="LaserScannerUnitObserverApp" instance="" package="RobotAPI"/>
+</scenario>
+
diff --git a/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg b/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..6f8512d97047c86bb3fe4cadba24fad5fb6f5255
--- /dev/null
+++ b/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg
@@ -0,0 +1,183 @@
+# ==================================================================
+# HokuyoLaserUnitApp 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.HokuyoLaserUnit.AngleOffset:  Offset is applied the raw angles before reporting them
+#  Attributes:
+#  - Default:            -2.3561945
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.HokuyoLaserUnit.AngleOffset = -2.3561945
+
+
+# ArmarX.HokuyoLaserUnit.Devices:  List of devices in form of 'IP1,port1;IP2,port2;...'
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.HokuyoLaserUnit.Devices = "192.168.0.10,10940"
+
+
+# ArmarX.HokuyoLaserUnit.EnableProfiling:  enable profiler which is used for logging performance events
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.HokuyoLaserUnit.EnableProfiling = 0
+
+
+# ArmarX.HokuyoLaserUnit.LaserScannerTopicName:  Name of the laser scan topic.
+#  Attributes:
+#  - Default:            LaserScans
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.HokuyoLaserUnit.LaserScannerTopicName = LaserScans
+
+
+# ArmarX.HokuyoLaserUnit.MinimumLoggingLevel:  Local logging level only for this component
+#  Attributes:
+#  - Default:            Undefined
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.HokuyoLaserUnit.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.HokuyoLaserUnit.ObjectName:  Name of IceGrid well-known object
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.HokuyoLaserUnit.ObjectName = ""
+
+
+# ArmarX.HokuyoLaserUnit.UpdatePeriod:  Update period for laser scans
+#  Attributes:
+#  - Default:            25
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.HokuyoLaserUnit.UpdatePeriod = 25
+
+
+# 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/LaserScannerTest/config/LaserScannerUnitObserverApp.cfg b/scenarios/LaserScannerTest/config/LaserScannerUnitObserverApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..5d4e2e744baf2c25a947db4ca667f51c39fcd31d
--- /dev/null
+++ b/scenarios/LaserScannerTest/config/LaserScannerUnitObserverApp.cfg
@@ -0,0 +1,183 @@
+# ==================================================================
+# LaserScannerUnitObserverApp 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.LaserScannerUnitObserver.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.LaserScannerUnitObserver.CreateUpdateFrequenciesChannel = 0
+
+
+# ArmarX.LaserScannerUnitObserver.EnableProfiling:  enable profiler which is used for logging performance events
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.LaserScannerUnitObserver.EnableProfiling = 0
+
+
+# ArmarX.LaserScannerUnitObserver.LaserScannerTopicName:  Name of the laser scan topic.
+#  Attributes:
+#  - Default:            LaserScans
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.LaserScannerUnitObserver.LaserScannerTopicName = LaserScans
+
+
+# ArmarX.LaserScannerUnitObserver.MaxHistoryRecordFrequency:  The Observer history is written with this maximum frequency. Everything faster is being skipped.
+#  Attributes:
+#  - Default:            50
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.LaserScannerUnitObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.LaserScannerUnitObserver.MaxHistorySize:  Maximum number of entries in the Observer history
+#  Attributes:
+#  - Default:            5000
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.LaserScannerUnitObserver.MaxHistorySize = 5000
+
+
+# ArmarX.LaserScannerUnitObserver.MinimumLoggingLevel:  Local logging level only for this component
+#  Attributes:
+#  - Default:            Undefined
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.LaserScannerUnitObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.LaserScannerUnitObserver.ObjectName:  Name of IceGrid well-known object
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.LaserScannerUnitObserver.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/LaserScannerTest/config/global.cfg b/scenarios/LaserScannerTest/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..e7cd7f67055d284001fc7ac3ad93ed5b65719366
--- /dev/null
+++ b/scenarios/LaserScannerTest/config/global.cfg
@@ -0,0 +1,7 @@
+# ==================================================================
+# Global Config from Scenario LaserScannerTest
+# ==================================================================
+
+
+
+
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/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
new file mode 100644
index 0000000000000000000000000000000000000000..443162d742ef69c2d3d6bedc728ac05494254fc2
--- /dev/null
+++ b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="OrientedTactileSensor" lastChange="2017-03-06.17:30:06" creation="2017-02-27.01:48:55 PM" globalConfigName="./config/global.cfg" package="RobotAPI">
+	<application name="OrientedTactileSensorUnitApp" instance="" package="RobotAPI"/>
+	<application name="OrientedTactileSensorUnitObserverApp" instance="" package="RobotAPI"/>
+</scenario>
+
diff --git a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..1ec6764f36d51be5c6b8501bb6ef5e5046114c79
--- /dev/null
+++ b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
@@ -0,0 +1,207 @@
+# ==================================================================
+# OrientedTactileSensorUnitApp 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.OrientedTactileSensorUnit.CalibrationData:  Sensor Register Data to calibrate the sensor
+#  Attributes:
+#  - Default:            65524 3 12 65534 65534 1 1208 119 58726 1000 943 
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.OrientedTactileSensorUnit.CalibrationData = 65524 3 12 65534 65534 1 1208 119 58726 1000 943
+
+
+# ArmarX.OrientedTactileSensorUnit.EnableProfiling:  enable profiler which is used for logging performance events
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OrientedTactileSensorUnit.EnableProfiling = 0
+
+
+# ArmarX.OrientedTactileSensorUnit.MinimumLoggingLevel:  Local logging level only for this component
+#  Attributes:
+#  - Default:            Undefined
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OrientedTactileSensorUnit.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.OrientedTactileSensorUnit.ObjectName:  Name of IceGrid well-known object
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OrientedTactileSensorUnit.ObjectName = ""
+
+
+# ArmarX.OrientedTactileSensorUnit.SamplesAcceleration:  number of pressure values to differentiate
+#  Attributes:
+#  - Default:            20
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.OrientedTactileSensorUnit.SamplesAcceleration = 20
+
+
+# ArmarX.OrientedTactileSensorUnit.SamplesPressure:  number of pressure values to differentiate
+#  Attributes:
+#  - Default:            10
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.OrientedTactileSensorUnit.SamplesPressure = 20
+
+
+# ArmarX.OrientedTactileSensorUnit.SamplesRotation:  number of orientation values to differentiate
+#  Attributes:
+#  - Default:            20
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.OrientedTactileSensorUnit.SamplesRotation = 20
+
+
+# ArmarX.OrientedTactileSensorUnit.SerialInterfaceDevice:  The serial device the arduino is connected to.
+#  Attributes:
+#  - Default:            /dev/ttyACM0
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.OrientedTactileSensorUnit.SerialInterfaceDevice = /dev/ttyACM0
+
+
+# ArmarX.OrientedTactileSensorUnit.TopicName:  Name of the topic on which the sensor values are provided
+#  Attributes:
+#  - Default:            OrientedTactileSensorValues
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OrientedTactileSensorUnit.TopicName = OrientedTactileSensorValues
+
+
+# ArmarX.OrientedTactileSensorUnit.calibrateSensor:  
+#  Attributes:
+#  - Default:            1
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.OrientedTactileSensorUnit.calibrateSensor = 0
+
+
+# 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/OrientedTactileSensor/config/OrientedTactileSensorUnitObserverApp.cfg b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitObserverApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..5f35ab28581e987287c8ed60661bc575f4a73d26
--- /dev/null
+++ b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitObserverApp.cfg
@@ -0,0 +1,191 @@
+# ==================================================================
+# OrientedTactileSensorUnitObserverApp 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.OrientedTactileSensorUnitObserver.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.OrientedTactileSensorUnitObserver.CreateUpdateFrequenciesChannel = 0
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.DebugDrawerTopic:  Name of the DebugDrawerTopic
+#  Attributes:
+#  - Default:            DebugDrawerUpdates
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OrientedTactileSensorUnitObserver.DebugDrawerTopic = DebugDrawerUpdates
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.EnableProfiling:  enable profiler which is used for logging performance events
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OrientedTactileSensorUnitObserver.EnableProfiling = 0
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.MaxHistoryRecordFrequency:  The Observer history is written with this maximum frequency. Everything faster is being skipped.
+#  Attributes:
+#  - Default:            50
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OrientedTactileSensorUnitObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.MaxHistorySize:  Maximum number of entries in the Observer history
+#  Attributes:
+#  - Default:            5000
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OrientedTactileSensorUnitObserver.MaxHistorySize = 5000
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.MinimumLoggingLevel:  Local logging level only for this component
+#  Attributes:
+#  - Default:            Undefined
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OrientedTactileSensorUnitObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.ObjectName:  Name of IceGrid well-known object
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OrientedTactileSensorUnitObserver.ObjectName = ""
+
+
+# ArmarX.OrientedTactileSensorUnitObserver.SensorTopicName:  Name of the Sensor Topic.
+#  Attributes:
+#  - Default:            OrientedTactileSensorValues
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OrientedTactileSensorUnitObserver.SensorTopicName = OrientedTactileSensorValues
+
+
+# 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/OrientedTactileSensor/config/global.cfg b/scenarios/OrientedTactileSensor/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..e3da6095b2689ee8257c2909a5ed2df0107a2d64
--- /dev/null
+++ b/scenarios/OrientedTactileSensor/config/global.cfg
@@ -0,0 +1,7 @@
+# ==================================================================
+# Global Config from Scenario OrientedTactileSensor
+# ==================================================================
+
+
+
+
diff --git a/scenarios/SensorPackage/SensorPackage.scx b/scenarios/SensorPackage/SensorPackage.scx
new file mode 100644
index 0000000000000000000000000000000000000000..3732d9201b2e4cff86d1eee6f1386569922a659e
--- /dev/null
+++ b/scenarios/SensorPackage/SensorPackage.scx
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="SensorPackage" lastChange="2017-02-27.11:57:43" creation="2017-02-27.11:57:30" globalConfigName="./config/global.cfg" package="RobotAPI">
+	<application name="SensorPackageUnitApp" instance="" package="RobotAPI"/>
+</scenario>
+
diff --git a/scenarios/SensorPackage/config/SensorPackageUnitApp.cfg b/scenarios/SensorPackage/config/SensorPackageUnitApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..f427f4873dee5f7dad887780f41da8447f82131a
--- /dev/null
+++ b/scenarios/SensorPackage/config/SensorPackageUnitApp.cfg
@@ -0,0 +1,172 @@
+# ==================================================================
+# SensorPackageUnitApp 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.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.SensorPackageUnit.EnableProfiling:  enable profiler which is used for logging performance events
+#  Attributes:
+#  - Default:            0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.SensorPackageUnit.EnableProfiling = 0
+
+
+# ArmarX.SensorPackageUnit.MinimumLoggingLevel:  Local logging level only for this component
+#  Attributes:
+#  - Default:            Undefined
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.SensorPackageUnit.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.SensorPackageUnit.ObjectName:  Name of IceGrid well-known object
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.SensorPackageUnit.ObjectName = ""
+
+
+# ArmarX.SensorPackageUnit.SerialInterfaceDevice:  The serial device the arduino is connected to.
+#  Attributes:
+#  - Default:            /dev/ttyACM0
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.SensorPackageUnit.SerialInterfaceDevice = /dev/ttyACM0
+
+
+# ArmarX.SensorPackageUnit.TopicName:  Name of the topic on which the sensor values are provided
+#  Attributes:
+#  - Default:            SensorPackageUnit
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.SensorPackageUnit.TopicName = SensorPackageUnit
+
+
+# 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/SensorPackage/config/global.cfg b/scenarios/SensorPackage/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..62f0a2b32fc1bfe619ddb6d311cd6169ebb188fe
--- /dev/null
+++ b/scenarios/SensorPackage/config/global.cfg
@@ -0,0 +1,7 @@
+# ==================================================================
+# Global Config from Scenario SensorPackage
+# ==================================================================
+
+
+
+
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 91731e3bdc411e53d2d585349873524b6831b8b7..afbd2702c93994f6eb71deec1c2e9ef711e4d768 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -21,3 +21,12 @@ add_subdirectory(XsensIMU)
 add_subdirectory(InertialMeasurementUnitObserver)
 
 add_subdirectory(ViewSelection)
+
+add_subdirectory(OrientedTactileSensorUnit)
+add_subdirectory(OrientedTactileSensorUnitObserver)
+
+add_subdirectory(OptoForceUnit)
+add_subdirectory(OptoForceUnitObserver)
+
+add_subdirectory(HokuyoLaserUnit)
+add_subdirectory(LaserScannerUnitObserver)
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/HokuyoLaserUnit/CMakeLists.txt b/source/RobotAPI/applications/HokuyoLaserUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5cbb7a44126940f6ca1c0c476189689794010e0a
--- /dev/null
+++ b/source/RobotAPI/applications/HokuyoLaserUnit/CMakeLists.txt
@@ -0,0 +1,22 @@
+armarx_component_set_name("HokuyoLaserUnitApp")
+
+#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()
+
+find_package(HokuyoLaserScannerDriver QUIET)
+
+set(COMPONENT_LIBS
+    ArmarXCoreInterfaces
+    ArmarXCore
+    HokuyoLaserUnit
+)
+
+set(EXE_SOURCE main.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/HokuyoLaserUnit/main.cpp b/source/RobotAPI/applications/HokuyoLaserUnit/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..111a542188d5d46aa6f5664aa5476d9c7dc6f88e
--- /dev/null
+++ b/source/RobotAPI/applications/HokuyoLaserUnit/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::HokyouLaserUnit
+ * @author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.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::HokuyoLaserUnit > (argc, argv, "HokuyoLaserUnit");
+}
diff --git a/source/RobotAPI/applications/LaserScannerUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/LaserScannerUnitObserver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..510e19879be5a995f91d9b5f4795e53ec547adf1
--- /dev/null
+++ b/source/RobotAPI/applications/LaserScannerUnitObserver/CMakeLists.txt
@@ -0,0 +1,20 @@
+armarx_component_set_name("LaserScannerUnitObserverApp")
+
+#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/LaserScannerUnitObserver/main.cpp b/source/RobotAPI/applications/LaserScannerUnitObserver/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ecae3d6b3b6a05d96acf8d1e3b661d2ed47e5f58
--- /dev/null
+++ b/source/RobotAPI/applications/LaserScannerUnitObserver/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::LaserScannerUnitObserver
+ * @author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <RobotAPI/components/units/LaserScannerUnitObserver.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::LaserScannerUnitObserver > (argc, argv, "LaserScannerUnitObserver");
+}
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/applications/OrientedTactileSensorUnit/CMakeLists.txt b/source/RobotAPI/applications/OrientedTactileSensorUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f7bb09c22acd41ad580a15ec936f92603f397aff
--- /dev/null
+++ b/source/RobotAPI/applications/OrientedTactileSensorUnit/CMakeLists.txt
@@ -0,0 +1,15 @@
+armarx_component_set_name("OrientedTactileSensorUnitApp")
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
+if (Eigen3_FOUND)
+    include_directories(
+        ${Eigen3_INCLUDE_DIR})
+endif()
+
+set(COMPONENT_LIBS RobotAPIUnits OrientedTactileSensor )
+
+set(EXE_SOURCE main.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/OrientedTactileSensorUnit/main.cpp b/source/RobotAPI/applications/OrientedTactileSensorUnit/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f4ab124f78163892921a4330c9680f618be3aef0
--- /dev/null
+++ b/source/RobotAPI/applications/OrientedTactileSensorUnit/main.cpp
@@ -0,0 +1,9 @@
+#include <ArmarXCore/core/application/Application.h>
+#include <RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/Component.h>
+
+int main(int argc, char* argv[])
+{
+    return armarx::runSimpleComponentApp < armarx::OrientedTactileSensorUnit > (argc, argv, "OrientedTactileSensorUnit");
+}
diff --git a/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ec058b57f5d32a7ed5a08cd0efa385d31766432a
--- /dev/null
+++ b/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/CMakeLists.txt
@@ -0,0 +1,20 @@
+armarx_component_set_name("OrientedTactileSensorUnitObserverApp")
+
+#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/OrientedTactileSensorUnitObserver/main.cpp b/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e400516ab5faa9c676abd943be2a35afa25c3db3
--- /dev/null
+++ b/source/RobotAPI/applications/OrientedTactileSensorUnitObserver/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::OrientedTactileSensorUnitObserver
+ * @author     andreeatulbure ( andreea_tulbure at yahoo dot de )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <RobotAPI/components/units/OrientedTactileSensorUnitObserver.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::OrientedTactileSensorUnitObserver > (argc, argv, "OrientedTactileSensorUnitObserver");
+}
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/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 1506e7acb0a4b6630f70b1f7bc20a60436646a7c..9dabab63daeba10e73ac9734297a9eacbf4dc16a 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -982,8 +982,9 @@ namespace armarx
         SoSeparator* sep = new SoSeparator();
         SoNode* c = CoinVisualizationFactory::getCoinVisualization(triMesh);
         sep->addChild(c);
-        layer.addedPolygonVisualizations[d.name] = sep;
+        layer.addedTriMeshVisualizations[d.name] = sep;
         layer.mainNode->addChild(sep);
+
     }
 
     void DebugDrawerComponent::drawArrow(const DebugDrawerComponent::ArrowData& d)
@@ -1007,7 +1008,6 @@ namespace armarx
 
         SoSeparator* sepArrow = VirtualRobot::CoinVisualizationFactory::CreateArrow(d.direction, d.length, d.width, d.color);
         sep->addChild(sepArrow);
-
         layer.addedArrowVisualizations[d.name] = sep;
         layer.mainNode->addChild(sep);
     }
@@ -2054,13 +2054,11 @@ namespace armarx
             ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
             std::string entryName = boost::replace_all_copy(std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
             ArrowData& d = accumulatedUpdateData.arrows[entryName];
-
             d.position = Vector3Ptr::dynamicCast(position)->toEigen();
             d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
             d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);;
             d.length = length;
             d.width = width;
-
             d.layerName = layerName;
             d.name = arrowName;
             d.active = true;
@@ -2387,7 +2385,6 @@ namespace armarx
     {
         ScopedRecursiveLockPtr lockData = getScopedAccumulatedDataLock();
         ScopedRecursiveLockPtr lockVisu = getScopedVisuLock();
-
         // check for clear&remove
         //(updates only contain elements to add for each layer after the last clear for this layer was executed)
         //this prevents a sequence add,clear,add to result in an empty layer
@@ -2470,9 +2467,9 @@ namespace armarx
         }
         accumulatedUpdateData.arrows.clear();
 
-        for (auto i = accumulatedUpdateData.triMeshes.begin(); i != accumulatedUpdateData.triMeshes.end(); i++)
+        for (auto & e : accumulatedUpdateData.triMeshes)
         {
-            drawTriMesh(i->second);
+            drawTriMesh(e.second);
         }
         accumulatedUpdateData.triMeshes.clear();
 
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 90567cf96caddb0d174100b504237265888f06a2..ad87ce0aa80fdb948e57bae2a6cb4d8fd7579f91 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -32,6 +32,9 @@ set(LIB_HEADERS
     HapticObserver.h
     InertialMeasurementUnit.h
     InertialMeasurementUnitObserver.h
+    OptoForceUnitObserver.h
+    OrientedTactileSensorUnitObserver.h
+    LaserScannerUnitObserver.h
     TCPControlUnit.h
     TCPControlUnitObserver.h
 
@@ -57,6 +60,9 @@ set(LIB_FILES
     HapticObserver.cpp
     InertialMeasurementUnit.cpp
     InertialMeasurementUnitObserver.cpp
+    OptoForceUnitObserver.cpp
+    OrientedTactileSensorUnitObserver.cpp
+    LaserScannerUnitObserver.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/LaserScannerUnitObserver.cpp b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7e5980094db9c2eeac1409e2ba1544e8c4538331
--- /dev/null
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
@@ -0,0 +1,113 @@
+/*
+ * 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/>.
+ *
+ * @author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#include "LaserScannerUnitObserver.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 LaserScannerUnitObserver::onInitObserver()
+{
+    usingTopic(getProperty<std::string>("LaserScannerTopicName").getValue());
+
+    offerConditionCheck("updated", new ConditionCheckUpdated());
+    offerConditionCheck("larger", new ConditionCheckLarger());
+    offerConditionCheck("equals", new ConditionCheckEquals());
+    offerConditionCheck("smaller", new ConditionCheckSmaller());
+}
+
+
+
+void LaserScannerUnitObserver::onConnectObserver()
+{
+}
+
+
+void LaserScannerUnitObserver::onExitObserver()
+{
+}
+
+
+PropertyDefinitionsPtr LaserScannerUnitObserver::createPropertyDefinitions()
+{
+    return PropertyDefinitionsPtr(new LaserScannerUnitObserverPropertyDefinitions(getConfigIdentifier()));
+}
+
+void LaserScannerUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan, const TimestampBasePtr& timestamp, const Ice::Current& c)
+{
+    ScopedLock lock(dataMutex);
+
+    if (!existsChannel(device))
+    {
+        offerChannel(device, "laser scans");
+    }
+
+    TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+    offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
+
+    // Calculate some statistics on the laser scan
+    float minDistance = FLT_MAX;
+    float minAngle = 0.0f;
+    float maxDistance = -FLT_MAX;
+    float maxAngle = 0.0f;
+    float distanceSum = 0.0f;
+    for (LaserScanStep const & step : scan)
+    {
+        distanceSum += step.distance;
+        if (step.distance < minDistance)
+        {
+            minDistance = step.distance;
+            minAngle = step.angle;
+        }
+        if (step.distance > maxDistance)
+        {
+            maxDistance = step.distance;
+            maxAngle = step.angle;
+        }
+    }
+
+    if (scan.size() > 0)
+    {
+        offerOrUpdateDataField(device, "minDistance", minDistance, "minimal distance in scan");
+        offerOrUpdateDataField(device, "minAngle", minAngle, "angle with minimal distance in scan");
+        offerOrUpdateDataField(device, "maxDistance", maxDistance, "maximal distance in scan");
+        offerOrUpdateDataField(device, "maxAngle", maxAngle, "angle with maximal distance in scan");
+        float averageDistance = distanceSum / scan.size();
+        offerOrUpdateDataField(device, "averageDistance", averageDistance, "average distance in scan");
+    }
+
+    updateChannel(device);
+}
+
+
diff --git a/source/RobotAPI/components/units/LaserScannerUnitObserver.h b/source/RobotAPI/components/units/LaserScannerUnitObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..0696ec089541842bca8c24e34030c696f2b885b9
--- /dev/null
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.h
@@ -0,0 +1,87 @@
+/*
+ * 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     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_LASER_SCANNER_UNIT_OBSERVER_H
+#define _ARMARX_ROBOTAPI_LASER_SCANNER_UNIT_OBSERVER_H
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <ArmarXCore/observers/Observer.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+
+namespace armarx
+{
+    /**
+     * \class LaserScannerUnitObserverPropertyDefinitions
+     * \brief
+     */
+    class LaserScannerUnitObserverPropertyDefinitions:
+        public ObserverPropertyDefinitions
+    {
+    public:
+        LaserScannerUnitObserverPropertyDefinitions(std::string prefix):
+            ObserverPropertyDefinitions(prefix)
+        {
+            defineOptionalProperty<std::string>("LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
+        }
+    };
+
+
+    /**
+     * \class LaserScannerUnitObserver
+     * \ingroup RobotAPI-SensorActorUnits-observers
+     * \brief Observer monitoring laser scanner values
+     *
+     * The LaserScannerUnitObserver monitors laser scanner values published by LaserScannerUnit-implementations.
+     */
+    class LaserScannerUnitObserver :
+        virtual public Observer,
+        virtual public LaserScannerUnitObserverInterface
+    {
+    public:
+        LaserScannerUnitObserver() {}
+
+        virtual std::string getDefaultName() const
+        {
+            return "LaserScannerUnitObserver";
+        }
+        virtual void onInitObserver();
+        virtual void onConnectObserver();
+        virtual void onExitObserver();
+        /**
+         * @see PropertyUser::createPropertyDefinitions()
+         */
+        virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+        void reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan,
+                                const TimestampBasePtr& timestamp, const Ice::Current& c) override;
+
+    private:
+        Mutex dataMutex;
+
+    };
+}
+
+#endif
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/components/units/OrientedTactileSensorUnitObserver.cpp b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..906e90f79bb7593b36e23e985357c78076fc2512
--- /dev/null
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
@@ -0,0 +1,109 @@
+/*
+ * 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 "OrientedTactileSensorUnitObserver.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 OrientedTactileSensorUnitObserver::onInitObserver()
+{
+    usingTopic(getProperty<std::string>("SensorTopicName").getValue());
+    offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
+}
+
+
+
+void OrientedTactileSensorUnitObserver::onConnectObserver()
+{
+    debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+}
+
+
+void OrientedTactileSensorUnitObserver::onExitObserver()
+{
+    //debugDrawerPrx->removePoseVisu("IMU", "orientation");
+    //debugDrawerPrx->removeLineVisu("IMU", "acceleration");
+}
+
+void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, float accelerationRate, float accelx, float accely, float accelz, const TimestampBasePtr& timestamp, const Ice::Current&)
+{
+    ScopedLock lock(dataMutex);
+    TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+
+    std::stringstream ss;
+    ss << "sensor" << id;
+    std::string channelName = ss.str();
+
+    if (!existsChannel(channelName))
+    {
+        offerChannel(channelName, "Sensor Data");
+    }
+
+    offerOrUpdateDataField(channelName, "pressure", Variant(pressure), "current pressure");
+    QuaternionPtr orientationQuaternion =  new Quaternion(qw, qx, qy, qz);
+    offerOrUpdateDataField(channelName, "orientation", orientationQuaternion, "current oriantation");
+    offerOrUpdateDataField(channelName, "rotationRate", Variant(rotationRate), "current rotationRate");
+    offerOrUpdateDataField(channelName, "pressureRate", Variant(pressureRate), "current pressureRate");
+    offerOrUpdateDataField(channelName, "accelerationRate", Variant(accelerationRate), "current accelerationRate");
+    offerOrUpdateDataField(channelName, "linear acceleration", Variant(new Vector3(accelx, accely, accelz)), "current linear acceleration");
+}
+/* TODO: for integration with debug drawer
+    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});
+*/
+
+
+PropertyDefinitionsPtr OrientedTactileSensorUnitObserver::createPropertyDefinitions()
+{
+    return PropertyDefinitionsPtr(new OrientedTactileSensorUnitObserverPropertyDefinitions(getConfigIdentifier()));
+}
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..7945c13847534201bc9d41f9b169e41ab4ca3b38
--- /dev/null
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
@@ -0,0 +1,89 @@
+/*
+ * 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_IMU_OBSERVER_H
+#define _ARMARX_ROBOTAPI_IMU_OBSERVER_H
+
+#include <RobotAPI/interface/units/OrientedTactileSensorUnit.h>
+#include <ArmarXCore/observers/Observer.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+
+namespace armarx
+{
+    /**
+     * \class OrientedTactileSensorUnitObserverPropertyDefinitions
+     * \brief
+     */
+    class OrientedTactileSensorUnitObserverPropertyDefinitions:
+        public ObserverPropertyDefinitions
+    {
+    public:
+        OrientedTactileSensorUnitObserverPropertyDefinitions(std::string prefix):
+            ObserverPropertyDefinitions(prefix)
+        {
+            defineOptionalProperty<std::string>("SensorTopicName", "OrientedTactileSensorValues", "Name of the Sensor Topic.");
+            defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
+        }
+    };
+
+
+    /**
+     * \class OrientedTactileSensorUnitObserver
+     * \ingroup RobotAPI-SensorActorUnits-observers
+     * \brief Observer monitoring @IMU sensor values
+     *
+     * The OrientedTactileSensorUnitObserver monitors @IMU sensor values published by OrientedTactileSensorUnit-implementations and offers condition checks on these values.
+     */
+    class OrientedTactileSensorUnitObserver :
+        virtual public Observer,
+        virtual public OrientedTactileSensorUnitObserverInterface
+    {
+    public:
+        OrientedTactileSensorUnitObserver() {}
+
+        virtual std::string getDefaultName() const
+        {
+            return "OrientedTactileSensorUnitObserver";
+        }
+        virtual void onInitObserver();
+        virtual void onConnectObserver();
+        virtual void onExitObserver();
+
+        void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, float accelerationRate, float accelx, float accely, float accelz, const TimestampBasePtr& timestamp, const Ice::Current&);
+
+        /**
+         * @see PropertyUser::createPropertyDefinitions()
+         */
+        virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+
+    private:
+        Mutex dataMutex;
+        DebugDrawerInterfacePrx debugDrawerPrx;
+    };
+}
+
+#endif
diff --git a/source/RobotAPI/drivers/CMakeLists.txt b/source/RobotAPI/drivers/CMakeLists.txt
index ce2834d8731aa2a82f549313634a7d28b2277084..7d5899a24dbbd16720e616420c57146ec963a7ab 100644
--- a/source/RobotAPI/drivers/CMakeLists.txt
+++ b/source/RobotAPI/drivers/CMakeLists.txt
@@ -1,2 +1,5 @@
 add_subdirectory(WeissHapticSensor)
 add_subdirectory(XsensIMU)
+add_subdirectory(OptoForceUnit)
+add_subdirectory(OrientedTactileSensor)
+add_subdirectory(HokuyoLaserUnit)
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/HokuyoLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/HokuyoLaserUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d78aae9576595b3e4461178cf8c9c171ae987536
--- /dev/null
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/CMakeLists.txt
@@ -0,0 +1,31 @@
+armarx_component_set_name("HokuyoLaserUnit")
+
+#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()
+
+find_package(HokuyoLaserScannerDriver QUIET)
+armarx_build_if(HokuyoLaserScannerDriver_FOUND "Hokuyo laser scanner driver not found")
+
+set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreEigen3Variants
+    RobotAPICore RobotAPIUnits
+    HokuyoLaserScannerDriver)
+
+set(SOURCES
+./HokuyoLaserUnit.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(HEADERS
+./HokuyoLaserUnit.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/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fca068ee614dcd7e11f4e95ded08c8a0d853cab2
--- /dev/null
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -0,0 +1,207 @@
+/*
+ * 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::HokuyoLaserUnit
+ * @author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "HokuyoLaserUnit.h"
+
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+
+#include <boost/algorithm/string/split.hpp>
+#include <urg_utils.h>
+
+using namespace armarx;
+
+
+void HokuyoLaserUnit::onInitComponent()
+{
+    topicName = getProperty<std::string>("LaserScannerTopicName").getValue();
+    offeringTopic(topicName);
+    updatePeriod = getProperty<int>("UpdatePeriod").getValue();
+    angleOffset = getProperty<float>("AngleOffset").getValue();
+
+    std::string deviceStrings = getProperty<std::string>("Devices").getValue();
+    std::vector<std::string> splitDeviceStrings;
+    boost::split(splitDeviceStrings, deviceStrings, boost::is_any_of(";"));
+    devices.clear();
+    devices.reserve(splitDeviceStrings.size());
+    for (std::string const & deviceString : splitDeviceStrings)
+    {
+        std::vector<std::string> ipAndPort;
+        boost::split(ipAndPort, deviceString, boost::is_any_of(","));
+        if (ipAndPort.size() != 2)
+        {
+            ARMARX_WARNING << "Unexpected format for laser scanner device: " << deviceString
+                           << " (split size: " << ipAndPort.size() << ")";
+            continue;
+        }
+
+        try
+        {
+            HokuyoLaserScanDevice device;
+            device.ip = ipAndPort[0];
+            device.port = std::stoi(ipAndPort[1]);
+            device.connected = false;
+            device.isDummy = boost::starts_with(device.ip, "Dummy");
+            devices.push_back(device);
+        }
+        catch (std::exception const& ex)
+        {
+            ARMARX_WARNING << "Could not convert port to integer for laser scanner device " << deviceString
+                           << " (port is " << ipAndPort[1] << ") : " << ex.what();
+            continue;
+        }
+    }
+}
+
+
+void HokuyoLaserUnit::onConnectComponent()
+{
+    topic = getTopic<LaserScannerUnitListenerPrx>(topicName);
+
+    if (task)
+    {
+        task->stop();
+    }
+
+    for (HokuyoLaserScanDevice & device : devices)
+    {
+        if (device.isDummy)
+        {
+            continue;
+        }
+        int ret = urg_open(&device.urg, URG_ETHERNET, device.ip.c_str(), device.port);
+        device.connected = (ret == 0);
+        if (!device.connected)
+        {
+            ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: "
+                           << device.ip << ", Port: " << device.port << ", Error: " << ret << ")";
+            continue;
+        }
+
+        int lengthDataSize = urg_max_data_size(&device.urg);
+        device.lengthData.resize(lengthDataSize);
+    }
+
+    task = new PeriodicTask<HokuyoLaserUnit>(this, &HokuyoLaserUnit::updateScanData, updatePeriod, false, "HokuyoLaserScanUpdate");
+    task->start();
+}
+
+
+void HokuyoLaserUnit::onDisconnectComponent()
+{
+    if (task)
+    {
+        task->stop();
+    }
+
+    for (HokuyoLaserScanDevice & device : devices)
+    {
+        if (device.connected)
+        {
+            urg_close(&device.urg);
+            device.connected = false;
+        }
+    }
+}
+
+
+void HokuyoLaserUnit::onExitComponent()
+{
+
+}
+
+armarx::PropertyDefinitionsPtr HokuyoLaserUnit::createPropertyDefinitions()
+{
+    return armarx::PropertyDefinitionsPtr(new HokuyoLaserUnitPropertyDefinitions(
+            getConfigIdentifier()));
+}
+
+std::string HokuyoLaserUnit::getReportTopicName(const Ice::Current& c) const
+{
+    return topicName;
+}
+
+void HokuyoLaserUnit::updateScanData()
+{
+    LaserScan scan;
+    TimestampVariantPtr now(new TimestampVariant(TimeUtil::GetTime()));
+    for (HokuyoLaserScanDevice & device : devices)
+    {
+        if (device.isDummy)
+        {
+            static std::mt19937 engine;
+            std::uniform_real_distribution<float> dist(0.0f, 500.0f);
+
+            int lengthDataSize = 1081;
+            scan.clear();
+            scan.reserve(lengthDataSize);
+            for (int stepIndex = 0; stepIndex < lengthDataSize; ++stepIndex)
+            {
+                LaserScanStep step;
+                step.angle = angleOffset + float(0.25 * M_PI / 180.0 * stepIndex);
+                step.distance = stepIndex * 25000.0f / lengthDataSize + dist(engine);
+                scan.push_back(step);
+            }
+
+            // TODO: Better names for the devices?
+            ARMARX_INFO << deactivateSpam(1.0) << "Reporting new laser scan values";
+            topic->reportSensorValues(device.ip, device.ip, scan, now);
+            continue;
+        }
+        if (device.connected)
+        {
+            int ret = urg_start_measurement(&device.urg, URG_DISTANCE, 1, 0);
+            if (ret != 0)
+            {
+                ARMARX_WARNING << deactivateSpam(1) << "Could not start measurement for laser scanner (IP: "
+                               << device.ip << ", Port: " << device.port << ", Error: " << ret << ")";
+                continue;
+            }
+
+            int lengthDataSize = urg_get_distance(&device.urg, device.lengthData.data(), nullptr);
+            if (lengthDataSize < 0)
+            {
+                ARMARX_WARNING << deactivateSpam(1) << "Could not get measurement for laser scanner (IP: "
+                               << device.ip << ", Port: " << device.port << ", Error: " << lengthDataSize << ")";
+                continue;
+            }
+
+            scan.clear();
+            scan.reserve(lengthDataSize);
+            for (int stepIndex = 0; stepIndex < lengthDataSize; ++stepIndex)
+            {
+                LaserScanStep step;
+                long distance = device.lengthData[stepIndex];
+                // TODO: Extract the min and max valid value for distance into parameters
+                if (distance >= 21 && distance <= 30000)
+                {
+                    step.angle = angleOffset + (float)urg_step2rad(&device.urg, stepIndex); // Convert steps to rad
+                    step.distance = (float)distance; // Data is already in mm
+                    scan.push_back(step);
+                }
+            }
+
+            // TODO: Better names for the devices?
+            topic->reportSensorValues(device.ip, device.ip, scan, now);
+        }
+    }
+}
+
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..2445d3a03cea91086a57d7d6026ba98a119ad70e
--- /dev/null
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
@@ -0,0 +1,129 @@
+/*
+ * 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::HokuyoLaserUnit
+ * @author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_RobotAPI_HokuyoLaserUnit_H
+#define _ARMARX_COMPONENT_RobotAPI_HokuyoLaserUnit_H
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <RobotAPI/components/units/SensorActorUnit.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+#include <urg_sensor.h>
+#include <vector>
+
+namespace armarx
+{
+    /**
+     * @class HokuyoLaserUnitPropertyDefinitions
+     * @brief
+     */
+    class HokuyoLaserUnitPropertyDefinitions:
+        public armarx::ComponentPropertyDefinitions
+    {
+    public:
+        HokuyoLaserUnitPropertyDefinitions(std::string prefix):
+            armarx::ComponentPropertyDefinitions(prefix)
+        {
+            defineOptionalProperty<std::string>("LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
+            defineOptionalProperty<int>("UpdatePeriod", 25, "Update period for laser scans");
+            defineOptionalProperty<float>("AngleOffset", -2.3561944902, "Offset is applied the raw angles before reporting them");
+            defineOptionalProperty<std::string>("Devices", "", "List of devices in form of 'IP1,port1;IP2,port2;...'");
+        }
+    };
+
+    struct HokuyoLaserScanDevice
+    {
+        std::string ip;
+        int port = 0;
+        bool connected = false;
+        bool isDummy = false;
+        urg_t urg;
+        std::vector<long> lengthData;
+    };
+
+    /**
+     * @defgroup Component-HokuyoLaserUnit HokuyoLaserUnit
+     * @ingroup RobotAPI-Components
+     * A description of the component HokuyoLaserUnit.
+     *
+     * @class HokuyoLaserUnit
+     * @ingroup Component-HokuyoLaserUnit
+     * @brief Brief description of class HokuyoLaserUnit.
+     *
+     * Detailed description of class HokuyoLaserUnit.
+     */
+    class HokuyoLaserUnit :
+        virtual public armarx::LaserScannerUnitInterface,
+        virtual public armarx::SensorActorUnit
+    {
+    public:
+        /**
+         * @see armarx::ManagedIceObject::getDefaultName()
+         */
+        virtual std::string getDefaultName() const
+        {
+            return "HokuyoLaserUnit";
+        }
+
+    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();
+
+        std::string getReportTopicName(const Ice::Current& c) const override;
+
+    private:
+        void updateScanData();
+
+    private:
+        std::string topicName;
+        LaserScannerUnitListenerPrx topic;
+        int updatePeriod = 25;
+        float angleOffset = 0.0f;
+        std::vector<HokuyoLaserScanDevice> devices;
+        PeriodicTask<HokuyoLaserUnit>::pointer_type task;
+    };
+}
+
+#endif
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/test/CMakeLists.txt b/source/RobotAPI/drivers/HokuyoLaserUnit/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..bac2679024ebfd9630fbf1950557b344cbe82708
--- /dev/null
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore HokuyoLaserUnit)
+ 
+armarx_add_test(HokuyoLaserUnitTest HokuyoLaserUnitTest.cpp "${LIBS}")
\ No newline at end of file
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..581618323d926db09e1559e695df1345ed865190
--- /dev/null
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.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::HokuyoLaserUnit
+ * @author     Fabian Paus ( fabian dot paus 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::HokuyoLaserUnit
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+    armarx::HokuyoLaserUnit instance;
+
+    BOOST_CHECK_EQUAL(true, true);
+}
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..0418549306cecf52ad07e6e7eb8cf3fa8d981621
--- /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 <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/drivers/OrientedTactileSensor/CMakeLists.txt b/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..eefa4ca3137b61c4c20707c2592e5096437a755e
--- /dev/null
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt
@@ -0,0 +1,25 @@
+armarx_set_target("OrientedTactileSensor Library: OrientedTactileSensor")
+
+find_package(Eigen3 QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
+if (Eigen3_FOUND)
+    include_directories(
+        ${Eigen3_INCLUDE_DIR})
+endif()
+
+set(LIB_NAME       OrientedTactileSensor)
+
+
+
+set(LIBS RobotAPIUnits ArmarXCoreObservers ArmarXCoreEigen3Variants)
+
+set(LIB_FILES
+    OrientedTactileSensorUnit.cpp
+)
+set(LIB_HEADERS
+   OrientedTactileSensorUnit.h
+)
+
+armarx_add_library("${LIB_NAME}"  "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eaafd8ecaef369e3126bd3b548a8d21378f3ff1b
--- /dev/null
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
@@ -0,0 +1,287 @@
+#include "OrientedTactileSensorUnit.h"
+#include <termios.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <math.h>
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+
+using namespace armarx;
+
+OrientedTactileSensorUnit::OrientedTactileSensorUnit()
+{
+
+    sampleIndexRotation = 0;
+    sampleIndexPressure = 0;
+    sampleIndexAcceleration = 0;
+}
+
+void OrientedTactileSensorUnit::onInitComponent()
+{
+    maxSamplesRotation = stoi(getProperty<std::string>("SamplesRotation").getValue());
+    maxSamplesPressure = stoi(getProperty<std::string>("SamplesPressure").getValue());
+    maxSamplesAcceleration = stoi(getProperty<std::string>("SamplesAcceleration").getValue());
+
+    std::string topicName = getProperty<std::string>("TopicName").getValue();
+    offeringTopic(topicName);
+
+    //open serial port
+    std::string portname = getProperty<std::string>("SerialInterfaceDevice").getValue();
+    arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+
+    fd = open(portname.c_str(), O_RDONLY | O_NOCTTY);
+    struct termios toptions;
+
+    /* Get currently set options for the tty */
+    tcgetattr(fd, &toptions);
+
+    /* Set custom options */
+    cfsetispeed(&toptions, B115200);
+    cfsetospeed(&toptions, B115200);
+
+    /* 8 bits, no parity, no stop bits */
+    toptions.c_lflag = 0;
+    toptions.c_iflag = 0;
+    toptions.c_oflag = 0;
+
+    /* commit the options */
+    tcsetattr(fd, TCSANOW, &toptions);
+
+    /* Wait for the Arduino to reset */
+    usleep(1000 * 1000);
+
+    /* Flush anything already in the serial buffer */
+    tcflush(fd, TCIFLUSH);
+
+    ARMARX_INFO << "opening device " << getProperty<std::string>("SerialInterfaceDevice").getValue();
+
+    if (!arduino.is_open())
+    {
+
+        throw LocalException("Cannot open Arduino on ") << getProperty<std::string>("SerialInterfaceDevice").getValue();
+    }
+
+    ARMARX_INFO << "Arduino restarts, please wait ...";
+
+    //wait for the Arduino to reboot
+    usleep(4000000);
+    std::string arduinoLine;
+    //wait for the IMU to be calibrated or load calibration
+    ARMARX_INFO << "waiting for IMU calibration - this can take some time";
+    if (getProperty<bool>("calibrateSensor").getValue())
+    {
+        //calibrate
+
+        while (arduinoLine.find("mode") == std::string::npos)
+        {
+            getline(arduino, arduinoLine, '\n');
+        }
+        arduino.close();
+
+        arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
+        arduino << "calibrate";
+        arduino.flush();
+        arduino.close();
+
+        arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+        while (arduinoLine.find("Calibration Sucessfull") == std::string::npos)
+        {
+            getline(arduino, arduinoLine, '\n');
+            ARMARX_INFO << arduinoLine;
+        }
+        getline(arduino, arduinoLine, '\n');
+        if (getCalibrationValues(arduinoLine))
+        {
+            ARMARX_IMPORTANT << "calibrated sensor";
+        }
+    }
+    else
+    {
+        //load calibration
+        ARMARX_INFO << "load calibration data " << getProperty<std::string>("CalibrationData").getValue();
+        if (loadCalibration())
+        {
+            ARMARX_IMPORTANT << "loaded calibration";
+        }
+    }
+    readTask = new RunningTask<OrientedTactileSensorUnit>(this, &OrientedTactileSensorUnit::run);
+    readTask->start();
+
+}
+
+void OrientedTactileSensorUnit::onConnectComponent()
+{
+    std::string topicName = getProperty<std::string>("TopicName").getValue();
+    topicPrx = getTopic<OrientedTactileSensorUnitListenerPrx>(topicName);
+}
+
+PropertyDefinitionsPtr OrientedTactileSensorUnit::createPropertyDefinitions()
+{
+    return PropertyDefinitionsPtr(new OrientedTactileSensorUnitPropertyDefinitions(
+                                      getConfigIdentifier()));
+}
+
+void OrientedTactileSensorUnit::run()
+{
+    while (readTask->isRunning())
+    {
+        std::string line;
+        getline(arduino, line, '\n');
+        SensorData data = getValues(line.c_str());
+        IceUtil::Time now = IceUtil::Time::now();
+        TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
+
+        //compute rotationRate
+        float rotationRate = 0;
+
+        //condition for inverse quaternion
+        if ((pow(data.qw, 2) + pow(data.qx, 2) + pow(data.qy, 2) + pow(data.qz, 2)) != 0)
+        {
+            RotationRate sampleRotation;
+            sampleRotation.timestamp = now;
+            sampleRotation.orientation =  Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
+            if (samplesRotation.size() < maxSamplesRotation)
+            {
+                samplesRotation.push_back(sampleRotation);
+            }
+            else
+            {
+                samplesRotation[sampleIndexRotation].timestamp = sampleRotation.timestamp;
+                samplesRotation[sampleIndexRotation].orientation = sampleRotation.orientation;
+                sampleIndexRotation = (sampleIndexRotation + 1) % maxSamplesRotation;
+                RotationRate oldsampleRotation;
+                oldsampleRotation.timestamp = samplesRotation.at(sampleIndexRotation).timestamp;
+                oldsampleRotation.orientation = samplesRotation.at(sampleIndexRotation).orientation;
+                Eigen::AngleAxisf aa(sampleRotation.orientation * oldsampleRotation.orientation.inverse());
+                rotationRate = aa.angle() / (sampleRotation.timestamp - oldsampleRotation.timestamp).toSecondsDouble();
+            }
+        }
+
+        //compute pressureRate
+        float pressureRate = 0;
+        PressureRate samplePressure;
+        samplePressure.timestamp = now;
+        samplePressure.pressure = data.pressure;
+        if (samplesPressure.size() < maxSamplesPressure)
+        {
+            samplesPressure.push_back(samplePressure);
+        }
+        else
+        {
+            samplesPressure[sampleIndexPressure] = samplePressure;
+            sampleIndexPressure = (sampleIndexPressure + 1) % maxSamplesPressure;
+            PressureRate oldsamplePressure;
+            oldsamplePressure.timestamp = samplesPressure.at(sampleIndexPressure).timestamp;
+            oldsamplePressure.pressure = samplesPressure.at(sampleIndexPressure).pressure;
+            pressureRate = (samplePressure.pressure - oldsamplePressure.pressure) / (samplePressure.timestamp - oldsamplePressure.timestamp).toSecondsDouble();
+        }
+
+        //compute angular accceleration Rate
+        float accelerationRate = 0;
+        AccelerationRate sampleAcceleration;
+        sampleAcceleration.timestamp = now;
+        sampleAcceleration.rotationRate = rotationRate;
+        if (samplesAcceleration.size() < maxSamplesAcceleration)
+        {
+            samplesAcceleration.push_back(sampleAcceleration);
+        }
+        else
+        {
+            samplesAcceleration[sampleIndexAcceleration] = sampleAcceleration;
+            sampleIndexAcceleration = (sampleIndexAcceleration + 1) % maxSamplesAcceleration;
+            AccelerationRate oldsampleAcceleration;
+            oldsampleAcceleration.timestamp = samplesAcceleration.at(sampleIndexAcceleration).timestamp;
+            oldsampleAcceleration.rotationRate = samplesAcceleration.at(sampleIndexAcceleration).rotationRate;
+            accelerationRate = (sampleAcceleration.rotationRate - oldsampleAcceleration.rotationRate) / (sampleAcceleration.timestamp - oldsampleAcceleration.timestamp).toSecondsDouble();
+        }
+        if ((abs(pressureRate) > 50))
+        {
+            ARMARX_IMPORTANT << "contact";
+        }
+
+        if (topicPrx)
+        {
+            topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, pressureRate, rotationRate, accelerationRate, data.accelx, data.accely, data.accelz, nowTimestamp);
+        }
+    }
+}
+
+// get imu values from incoming string
+OrientedTactileSensorUnit::SensorData OrientedTactileSensorUnit::getValues(std::string line)
+{
+    SensorData data;
+    std::vector<std::string> splitValues;
+    boost::split(splitValues, line, boost::is_any_of(" "));
+    data.id = stoi(splitValues.at(0));
+    data.pressure = std::stof(splitValues.at(1));
+    data.qw = std::stof(splitValues.at(2));
+    data.qx = std::stof(splitValues.at(3));
+    data.qy = std::stof(splitValues.at(4));
+    data.qz = std::stof(splitValues.at(5));
+    data.accelx = std::stof(splitValues.at(6));
+    data.accely = std::stof(splitValues.at(7));
+    data.accelz = std::stof(splitValues.at(8));
+    return data;
+}
+
+std::string space = " ";
+bool OrientedTactileSensorUnit::loadCalibration()
+{
+    std::string calibrationStream = getProperty<std::string>("CalibrationData").getValue();
+    std::string arduinoLine;
+    while (arduinoLine.find("mode") == std::string::npos)
+    {
+        getline(arduino, arduinoLine, '\n');
+    }
+    arduino.close();
+
+    arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
+    arduino << "load";
+    arduino.flush();
+    arduino.close();
+
+    arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+    while (arduinoLine.find("calibration data") == std::string::npos)
+    {
+        getline(arduino, arduinoLine, '\n');
+    }
+    arduino.close();
+
+    arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
+    arduino << calibrationStream;
+    arduino.flush();
+    arduino.close();
+
+    arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+
+    while (arduinoLine.find("Calibration Sucessfull") == std::string::npos)
+    {
+        getline(arduino, arduinoLine, '\n');
+    }
+    return true;
+}
+
+bool OrientedTactileSensorUnit::getCalibrationValues(std::string line)
+{
+    std::vector<std::string> splitValues;
+    boost::split(splitValues, line, boost::is_any_of(" "));
+    calibration.accel_offset_x = stoi(splitValues.at(0));
+    calibration.accel_offset_y = stoi(splitValues.at(1));
+    calibration.accel_offset_z = stoi(splitValues.at(2));
+    calibration.gyro_offset_x = stoi(splitValues.at(3));
+    calibration.gyro_offset_y = stoi(splitValues.at(4));
+    calibration.gyro_offset_z = stoi(splitValues.at(5));
+    calibration.mag_offset_x = stoi(splitValues.at(6));
+    calibration.mag_offset_y = stoi(splitValues.at(7));
+    calibration.mag_offset_z = stoi(splitValues.at(8));
+    calibration.accel_radius = stoi(splitValues.at(9));
+    calibration.mag_radius = stoi(splitValues.at(10));
+    std::string space = " ";
+    std::string calibrationStream = "";
+    calibrationStream = calibrationStream + std::to_string(calibration.accel_offset_x) + space + std::to_string(calibration.accel_offset_y) + space + std::to_string(calibration.accel_offset_z) + space + std::to_string(calibration.gyro_offset_x) + space + std::to_string(calibration.gyro_offset_y) + space +
+                        std::to_string(calibration.gyro_offset_z) + space + std::to_string(calibration.mag_offset_x) + space + std::to_string(calibration.mag_offset_y) + space + std::to_string(calibration.mag_offset_z) + space + std::to_string(calibration.accel_radius) + space + std::to_string(calibration.mag_radius) + space;
+    ARMARX_IMPORTANT << "calibration data: " << calibrationStream ;
+    return true;
+}
+
+
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..38aade6a48c0be4e9742ad789dcaf05413410f53
--- /dev/null
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
@@ -0,0 +1,145 @@
+#ifndef SENSORPACKAGEUNIT_H
+#define SENSORPACKAGEUNIT_H
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/units/UnitInterface.h>
+#include <RobotAPI/interface/units/OrientedTactileSensorUnit.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <netinet/in.h>
+#include <fstream>
+#include <stdio.h>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+    class OrientedTactileSensorUnitPropertyDefinitions:
+        public ComponentPropertyDefinitions
+    {
+    public:
+        OrientedTactileSensorUnitPropertyDefinitions(std::string prefix):
+            ComponentPropertyDefinitions(prefix)
+        {
+            defineOptionalProperty<std::string>(
+                "SerialInterfaceDevice",
+                "/dev/ttyACM0",
+                "The serial device the arduino is connected to.");
+
+            defineOptionalProperty<std::string>(
+                "TopicName",
+                "OrientedTactileSensorValues",
+                "Name of the topic on which the sensor values are provided");
+
+            defineOptionalProperty<std::string>(
+                "CalibrationData",
+                "65524 3 12 65534 65534 1 1208 119 58726 1000 943 ",
+                "Sensor Register Data to calibrate the sensor");
+
+            defineOptionalProperty<std::string>(
+                "SamplesRotation",
+                "20",
+                "number of orientation values to differentiate");
+
+            defineOptionalProperty<std::string>(
+                "SamplesPressure",
+                "10",
+                "number of pressure values to differentiate");
+
+            defineOptionalProperty<std::string>(
+                "SamplesAcceleration",
+                "20",
+                "number of pressure values to differentiate");
+
+            defineOptionalProperty<bool>(
+                "calibrateSensor",
+                "false"
+                "Set true to calibrate the sensor and get calibration data and false to use existent calibration data");
+        }
+
+    };
+
+    /**
+     * @class OrientedTactileSensorUnit
+     * @brief ArmarX wrapper for an arduino due with one BNO055 IMU and one BMP280 pressure sensor
+     *
+     */
+    class OrientedTactileSensorUnit:
+        virtual public armarx::Component
+        //TODO: needs interface to send calibration data
+    {
+    public:
+        OrientedTactileSensorUnit();
+
+        virtual std::string getDefaultName() const
+        {
+            return "OrientedTactileSensorUnit";
+        }
+
+        struct SensorData
+        {
+            int id;
+            float pressure;
+            float qw, qx, qy, qz;
+            float accelx, accely, accelz;
+        };
+
+        struct CalibrationData
+        {
+            int accel_offset_x, accel_offset_y, accel_offset_z, gyro_offset_x, gyro_offset_y, gyro_offset_z, mag_offset_x, mag_offset_y, mag_offset_z, accel_radius, mag_radius;
+        };
+
+        struct PressureRate
+        {
+            IceUtil::Time timestamp;
+            float pressure;
+        };
+
+        struct RotationRate
+        {
+            IceUtil::Time timestamp;
+            Eigen::Quaternionf orientation;
+        };
+
+        struct AccelerationRate
+        {
+            IceUtil::Time timestamp;
+            float rotationRate;
+        };
+        struct LinAccRate
+        {
+            IceUtil::Time timestamp;
+            float accx, accy, accz;
+        };
+
+    protected:
+        virtual void onInitComponent();
+        virtual void onConnectComponent();
+
+        virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+    private:
+        std::fstream arduino;
+        RunningTask<OrientedTactileSensorUnit>::pointer_type readTask;
+        OrientedTactileSensorUnitListenerPrx topicPrx;
+        OrientedTactileSensorUnitInterfacePrx interfacePrx;
+
+        void run();
+        SensorData getValues(std::string line);
+        bool getCalibrationValues(std::string line);
+        bool loadCalibration();
+        int fd;
+        CalibrationData calibration;
+
+        std::vector<RotationRate> samplesRotation;
+        std::vector<PressureRate> samplesPressure;
+        std::vector<AccelerationRate> samplesAcceleration;
+        int maxSamplesRotation;
+        int sampleIndexRotation;
+        int maxSamplesPressure;
+        int sampleIndexPressure;
+        int maxSamplesAcceleration;
+        int sampleIndexAcceleration;
+    };
+}
+#endif // SENSORPACKAGEUNIT_H
diff --git a/source/RobotAPI/gui-plugins/CMakeLists.txt b/source/RobotAPI/gui-plugins/CMakeLists.txt
index 8e1b05f0d2b9484747993691c5ece3f36addc723..fc363790ab3bf0da2fcbdd97a12daa1c84050093 100644
--- a/source/RobotAPI/gui-plugins/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/CMakeLists.txt
@@ -10,3 +10,5 @@ add_subdirectory(RobotViewerPlugin)
 
 add_subdirectory(DebugDrawerViewer)
 add_subdirectory(ViewSelection)
+
+add_subdirectory(LaserScannerPlugin)
\ No newline at end of file
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/LaserScannerPlugin/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1c1e4e2822a8c5270ea6f90deb7389d6fbc86667
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/CMakeLists.txt
@@ -0,0 +1,40 @@
+
+armarx_set_target("LaserScannerPluginGuiPlugin")
+
+find_package(Qt4 COMPONENTS QtCore QtGui QtDesigner)
+
+armarx_build_if(QT_FOUND "Qt not available")
+# ArmarXGui gets included through depends_on_armarx_package(ArmarXGui "OPTIONAL")
+# in the toplevel CMakeLists.txt
+armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
+
+
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+if(QT_FOUND)
+    include(${QT_USE_FILE})
+endif()
+
+set(SOURCES
+./LaserScannerPluginGuiPlugin.cpp ./LaserScannerPluginWidgetController.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@GuiPlugin.cpp @COMPONENT_PATH@/@COMPONENT_NAME@WidgetController.cpp
+)
+
+set(HEADERS
+./LaserScannerPluginGuiPlugin.h ./LaserScannerPluginWidgetController.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@GuiPlugin.h @COMPONENT_PATH@/@COMPONENT_NAME@WidgetController.h
+)
+
+set(GUI_MOC_HDRS ${HEADERS})
+
+set(GUI_UIS
+./LaserScannerPluginWidget.ui
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@Widget.ui
+)
+
+# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
+set(COMPONENT_LIBS HokuyoLaserUnit SimpleConfigDialog ${QT_LIBRARIES})
+
+if(ArmarXGui_FOUND)
+	armarx_gui_library(LaserScannerPluginGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+endif()
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.cpp b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c391a0ff66ebe5698e8f4f4b62b2a874374ea7fd
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.cpp
@@ -0,0 +1,34 @@
+/*
+ * 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::gui-plugins::LaserScannerPluginGuiPlugin
+ * \author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * \date       2017
+ * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "LaserScannerPluginGuiPlugin.h"
+
+#include "LaserScannerPluginWidgetController.h"
+
+using namespace armarx;
+
+LaserScannerPluginGuiPlugin::LaserScannerPluginGuiPlugin()
+{
+    addWidget < LaserScannerPluginWidgetController > ();
+}
+
+Q_EXPORT_PLUGIN2(armarx_gui_LaserScannerPluginGuiPlugin, LaserScannerPluginGuiPlugin)
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.h b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..546adaed422fde8fc1812a1928c528c0e5475700
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.h
@@ -0,0 +1,51 @@
+/*
+ * 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::gui-plugins::LaserScannerPlugin
+ * \author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * \date       2017
+ * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_RobotAPI_LaserScannerPlugin_GuiPlugin_H
+#define _ARMARX_RobotAPI_LaserScannerPlugin_GuiPlugin_H
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+namespace armarx
+{
+    /**
+     * \class LaserScannerPluginGuiPlugin
+     * \ingroup ArmarXGuiPlugins
+     * \brief LaserScannerPluginGuiPlugin brief description
+     *
+     * Detailed description
+     */
+    class ARMARXCOMPONENT_IMPORT_EXPORT LaserScannerPluginGuiPlugin:
+        public armarx::ArmarXGuiPlugin
+    {
+    public:
+        /**
+         * All widgets exposed by this plugin are added in the constructor
+         * via calls to addWidget()
+         */
+        LaserScannerPluginGuiPlugin();
+    };
+}
+
+#endif
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidget.ui b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidget.ui
new file mode 100644
index 0000000000000000000000000000000000000000..097f7d74afaba5d7f1d1e8dcd8ce27b31ca3e4a4
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidget.ui
@@ -0,0 +1,63 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>LaserScannerPluginWidget</class>
+ <widget class="QWidget" name="LaserScannerPluginWidget">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>490</width>
+    <height>379</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>LaserScannerPluginWidget</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout">
+   <property name="sizeConstraint">
+    <enum>QLayout::SetMinAndMaxSize</enum>
+   </property>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout" stretch="0,0">
+     <property name="spacing">
+      <number>6</number>
+     </property>
+     <item>
+      <widget class="QLabel" name="label">
+       <property name="sizePolicy">
+        <sizepolicy hsizetype="Ignored" vsizetype="Preferred">
+         <horstretch>0</horstretch>
+         <verstretch>0</verstretch>
+        </sizepolicy>
+       </property>
+       <property name="text">
+        <string>Device:</string>
+       </property>
+       <property name="alignment">
+        <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QComboBox" name="deviceComboBox"/>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <widget class="QGraphicsView" name="graphicsView">
+     <property name="verticalScrollBarPolicy">
+      <enum>Qt::ScrollBarAlwaysOff</enum>
+     </property>
+     <property name="horizontalScrollBarPolicy">
+      <enum>Qt::ScrollBarAlwaysOff</enum>
+     </property>
+     <property name="resizeAnchor">
+      <enum>QGraphicsView::AnchorViewCenter</enum>
+     </property>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c190417b22eeabd2a4f9acd43d755cde65231efb
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
@@ -0,0 +1,150 @@
+/*
+ * 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::gui-plugins::LaserScannerPluginWidgetController
+ * \author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * \date       2017
+ * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "LaserScannerPluginWidgetController.h"
+
+#include <QGraphicsView>
+#include <QGraphicsLineItem>
+
+#include <string>
+
+using namespace armarx;
+
+LaserScannerPluginWidgetController::LaserScannerPluginWidgetController()
+{
+    widget.setupUi(getWidget());
+
+    widget.graphicsView->setScene(&scene);
+}
+
+
+LaserScannerPluginWidgetController::~LaserScannerPluginWidgetController()
+{
+
+}
+
+
+void LaserScannerPluginWidgetController::loadSettings(QSettings* settings)
+{
+
+}
+
+void LaserScannerPluginWidgetController::saveSettings(QSettings* settings)
+{
+
+}
+
+
+void LaserScannerPluginWidgetController::onInitComponent()
+{
+    usingProxy(laserScannerUnitName);
+
+    connect(this, SIGNAL(newSensorValuesReported()), this, SLOT(onNewSensorValuesReported()), Qt::QueuedConnection);
+}
+
+
+void LaserScannerPluginWidgetController::onConnectComponent()
+{
+    laserScannerUnit = getProxy<LaserScannerUnitInterfacePrx>(laserScannerUnitName);
+    std::string topicName = laserScannerUnit->getReportTopicName();
+    usingTopic(topicName);
+}
+
+QPointer<QDialog> LaserScannerPluginWidgetController::getConfigDialog(QWidget* parent)
+{
+    if (!dialog)
+    {
+        dialog = new SimpleConfigDialog(parent);
+        dialog->addProxyFinder<LaserScannerUnitInterfacePrx>({"LaserScannerUnit", "", "*LaserUnit"});
+    }
+    return qobject_cast<SimpleConfigDialog*>(dialog);
+}
+
+void LaserScannerPluginWidgetController::configured()
+{
+    if (dialog)
+    {
+        laserScannerUnitName = dialog->getProxyName("LaserScannerUnit");
+    }
+}
+
+void LaserScannerPluginWidgetController::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& newScan, const TimestampBasePtr& timestamp, const Ice::Current& c)
+{
+    {
+        boost::mutex::scoped_lock lock(scanMutex);
+
+        LaserScan& scan = scans[device];
+        // TODO: Do some filtering? aggregation?
+        scan = newScan;
+    }
+
+    ARMARX_INFO << deactivateSpam(1) << "Got new laser scan values";
+    emit newSensorValuesReported();
+}
+
+void LaserScannerPluginWidgetController::onNewSensorValuesReported()
+{
+    // TODO: Draw something on the canvas
+
+    QComboBox* deviceBox = widget.deviceComboBox;
+
+    boost::mutex::scoped_lock lock(scanMutex);
+    for (auto & pair : scans)
+    {
+        QString deviceName(QString::fromStdString(pair.first.c_str()));
+        if (deviceBox->findText(deviceName) < 0)
+        {
+            deviceBox->addItem(deviceName);
+        }
+    }
+
+    std::string deviceName(deviceBox->currentText().toUtf8().data());
+
+    QGraphicsView* view = widget.graphicsView;
+
+
+    scene.clear();
+    int outerR = std::min(view->width() / 2, view->height() / 2);
+    scene.addEllipse(-outerR, -outerR, 2 * outerR, 2 * outerR, QPen(QColor(255, 255, 255)));
+    int r = outerR - 10;
+    scene.addEllipse(-r, -r, 2 * r, 2 * r, QPen(QColor(200, 200, 200)));
+    QColor stepColor(QColor::fromRgb(100, 100, 255));
+    QPen stepPen(stepColor);
+    QBrush stepBrush(stepColor);
+    auto line = [&](float angle, float d)
+    {
+        float di = d * r;
+        QGraphicsEllipseItem* item = scene.addEllipse(-di, -di, 2 * di, 2 * di, stepPen, stepBrush);
+        // Angles for Qt ellipse are in 16th of degree (who thought that would be a great idea?)
+        item->setStartAngle(std::round(16.0f * angle * 180.0 / M_PI));
+        item->setSpanAngle(std::round(0.25f * 16.0f));
+        //scene.addLine(0, 0, std::sin(angle) * d * r, std::cos(angle) * d * r);
+    };
+
+    LaserScan& scan = scans[deviceName];
+    for (LaserScanStep & step : scan)
+    {
+        line(step.angle, step.distance / 10000.0f);
+    }
+
+    view->fitInView(scene.itemsBoundingRect(), Qt::KeepAspectRatio);
+}
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
new file mode 100644
index 0000000000000000000000000000000000000000..7d35abcbf69903ad0d920073468369b00aa1ce28
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.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::gui-plugins::LaserScannerPluginWidgetController
+ * @author     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_RobotAPI_LaserScannerPlugin_WidgetController_H
+#define _ARMARX_RobotAPI_LaserScannerPlugin_WidgetController_H
+
+#include "ui_LaserScannerPluginWidget.h"
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+namespace armarx
+{
+    /**
+    \page ArmarXGui-GuiPlugins-LaserScannerPlugin LaserScannerPlugin
+    \brief The LaserScannerPlugin allows visualizing ...
+
+    \image html LaserScannerPlugin.png
+    The user can
+
+    API Documentation \ref LaserScannerPluginWidgetController
+
+    \see LaserScannerPluginGuiPlugin
+    */
+
+    /**
+     * \class LaserScannerPluginWidgetController
+     * \brief LaserScannerPluginWidgetController brief one line description
+     *
+     * Detailed description
+     */
+    class ARMARXCOMPONENT_IMPORT_EXPORT
+        LaserScannerPluginWidgetController:
+    public armarx::ArmarXComponentWidgetController,
+    public armarx::LaserScannerUnitListener
+    {
+        Q_OBJECT
+
+    public:
+        /**
+         * Controller Constructor
+         */
+        explicit LaserScannerPluginWidgetController();
+
+        /**
+         * Controller destructor
+         */
+        virtual ~LaserScannerPluginWidgetController();
+
+        /**
+         * @see ArmarXWidgetController::loadSettings()
+         */
+        virtual void loadSettings(QSettings* settings);
+
+        /**
+         * @see ArmarXWidgetController::saveSettings()
+         */
+        virtual void saveSettings(QSettings* settings);
+
+        /**
+         * Returns the Widget name displayed in the ArmarXGui to create an
+         * instance of this class.
+         */
+        virtual QString getWidgetName() const
+        {
+            return "RobotControl.LaserScannerGUI";
+        }
+
+        /**
+         * \see armarx::Component::onInitComponent()
+         */
+        virtual void onInitComponent();
+
+        /**
+         * \see armarx::Component::onConnectComponent()
+         */
+        virtual void onConnectComponent();
+
+        QPointer<QDialog> getConfigDialog(QWidget* parent) override;
+
+        void configured() override;
+
+        void reportSensorValues(const std::string& device, const std::string& name,
+                                const LaserScan& scan, const TimestampBasePtr& timestamp,
+                                const Ice::Current& c) override;
+    public slots:
+        void onNewSensorValuesReported();
+
+    signals:
+        void newSensorValuesReported();
+
+    private:
+        /**
+         * Widget Form
+         */
+        Ui::LaserScannerPluginWidget widget;
+        QPointer<SimpleConfigDialog> dialog;
+
+        std::string laserScannerUnitName;
+        LaserScannerUnitInterfacePrx laserScannerUnit;
+
+        Mutex scanMutex;
+        std::unordered_map<std::string, LaserScan> scans;
+
+        QGraphicsScene scene;
+    };
+}
+
+#endif
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 11dd79e238d44df1f6911128ced13b5f1d1ca43e..35e0c8122b8e61dd55562a2df8d9b1f500cd3729 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -23,6 +23,8 @@ set(SLICE_FILES
 
     units/ForceTorqueUnit.ice
     units/InertialMeasurementUnit.ice
+    units/OptoForceUnit.ice
+    units/LaserScannerUnit.ice
     units/HandUnitInterface.ice
     units/HapticUnit.ice
     units/WeissHapticUnit.ice
@@ -34,11 +36,15 @@ set(SLICE_FILES
     units/TCPMoverUnitInterface.ice
     units/UnitInterface.ice
     units/ATINetFTUnit.ice
-
+    units/OrientedTactileSensorUnit.ice
 
     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/LaserScannerUnit.ice b/source/RobotAPI/interface/units/LaserScannerUnit.ice
new file mode 100644
index 0000000000000000000000000000000000000000..113d5331f49cbebea16ed063e93d45f5469c86ed
--- /dev/null
+++ b/source/RobotAPI/interface/units/LaserScannerUnit.ice
@@ -0,0 +1,72 @@
+/*
+ * 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     Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_UNITS_LASER_SCANNER_UNIT_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_LASER_SCANNER_UNIT_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
+{
+	/**
+    * Struct LaserScanStep with which a single scan step is represented. It incorporates following entries:
+    * @param angle Angle in which direction a distance was measured [rad].
+    * @param distance The measured distance [mm].
+	**/
+    struct LaserScanStep {
+        float angle;
+        float distance;
+    };
+
+    sequence<LaserScanStep> LaserScan;
+
+    interface LaserScannerUnitInterface extends armarx::SensorActorUnitInterface
+    {
+        ["cpp:const"]
+        idempotent string getReportTopicName() throws NotInitializedException;
+    };
+
+    interface LaserScannerUnitListener
+    {	
+        void reportSensorValues(string device, string name, LaserScan values, TimestampBase timestamp);
+    };
+
+    interface LaserScannerUnitObserverInterface extends ObserverInterface, LaserScannerUnitListener
+    {
+    };
+
+};
+
+#endif
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/interface/units/OrientedTactileSensorUnit.ice b/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
new file mode 100644
index 0000000000000000000000000000000000000000..48a0fdd7de288b78b50b4bb67343adfe30275fe1
--- /dev/null
+++ b/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
@@ -0,0 +1,59 @@
+/*
+ * 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     Martin Do ( martin dot do at kit dot edu )
+ * @date       2015
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_UNITS_SENSORPACKAGE_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_SENSORPACKAGE_SLICE_
+
+#include <RobotAPI/interface/core/PoseBase.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
+
+module armarx
+{
+    /**
+     * Will be thrown, if a Arduino error occurs.
+     */
+    exception ArduinoException extends UserException
+    {
+        int errorCode;
+    };
+
+
+    interface OrientedTactileSensorUnitInterface extends armarx::SensorActorUnitInterface
+    {
+        //bool loadCalibration(int accel_offset_x, int accel_offset_y, int accel_offset_z, int gyro_offset_x, int gyro_offset_y, int gyro_offset_z, int mag_offset_x, int mag_offset_y, int mag_offset_z, int accel_radius, int mag_radius );
+    };
+
+   interface OrientedTactileSensorUnitListener
+    {
+        void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, float accelerationRate, float accelx, float accely, float accelz, TimestampBase timestamp);
+    };
+
+
+    interface OrientedTactileSensorUnitObserverInterface extends ObserverInterface, OrientedTactileSensorUnitListener
+    {
+
+    };
+};
+
+#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..3bdf59b6936735dd4d946bd5f13e0b8b15219737
--- /dev/null
+++ b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
@@ -0,0 +1,253 @@
+/*
+ * 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>
+#include <RobotAPI/libraries/RobotRTControllers/Targets/PlatformWheelVelocityTarget.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;
+        }
+    };
+
+    template <>
+    struct PassThroughControllerTargetTypeTraits<PlatformWheelVelocityTarget>
+    {
+        static float* getTargetDatamember(PlatformWheelVelocityTarget& t)
+        {
+            return  &t.velocity;
+        }
+        static const float* getFrontRightWheelVelocity(const PlatformDataUnitInterface* pu)
+        {
+            return pu->getFrontRightWheelVelocity();
+        }
+        static const float* getFrontLeftWheelVelocity(const PlatformDataUnitInterface* pu)
+        {
+            return pu->getFrontLeftWheelVelocity();
+        }
+        static const float* getRearRightWheelVelocity(const PlatformDataUnitInterface* pu)
+        {
+            return pu->getRearRightWheelVelocity();
+        }
+        static const float* getRearLeftWheelVelocity(const PlatformDataUnitInterface* pu)
+        {
+            return pu->getRearLeftWheelVelocity();
+        }
+        static  std::string getControlMode()
+        {
+            return ControlModes::VelocityMode;
+        }
+    };
+
+    LVL1ControllerRegistration<PassThroughController<JointPositionTarget>> registrationSomeControllerPositionPassThroughController("PositionPassThroughController");
+    LVL1ControllerRegistration<PassThroughController<JointVelocityTarget>> registrationSomeControllerVelocityPassThroughController("VelocityPassThroughController");
+    LVL1ControllerRegistration<PassThroughController<JointTorqueTarget  >> registrationSomeControllerJointPassThroughController("TorquePassThroughController");
+
+    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));
+            //set it to zero
+            iceTargets.at(i).val.store(0.0);
+        }
+    }
+
+    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..860a626f9ff9f45b9817f908e3a70e52a2212af1
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
@@ -0,0 +1,363 @@
+/*
+ * 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}
+        {
+        }
+
+        virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
+        {
+            rtUpdateControlStruct();
+            rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+        }
+    protected:
+        const ControlDataStruct& rtGetControlStruct() const
+        {
+            return controlDataTripleBuffer.getReadBuffer();
+        }
+        bool                     rtUpdateControlStruct()
+        {
+            return controlDataTripleBuffer.updateReadBuffer();
+        }
+
+        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>>;
+
+
+    struct PlatformCartesianVelocity
+    {
+        float vx;
+        float vy;
+        float vAngle;
+        PlatformCartesianVelocity() : vx(0), vy(0), vAngle(0)
+        { }
+    };
+
+
+    class AbstractLvl1PlatformVelocityController : public virtual LVL1ControllerWithTripleBuffer<PlatformCartesianVelocity>
+    {
+
+    public:
+        virtual void setTarget(float vx, float vy, float vAngle) = 0;
+    };
+
+    typedef boost::shared_ptr <AbstractLvl1PlatformVelocityController> AbstractLvl1PlatformVelocityControllerPtr;
+
+
+}
+#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
diff --git a/source/RobotAPI/statecharts/CMakeLists.txt b/source/RobotAPI/statecharts/CMakeLists.txt
index 9674d17827b4ccdbbbb27200138ee005c56bd60e..4a443a85124ef05f0647e460d8c0abdd6657eed4 100644
--- a/source/RobotAPI/statecharts/CMakeLists.txt
+++ b/source/RobotAPI/statecharts/CMakeLists.txt
@@ -2,4 +2,5 @@
 add_subdirectory(operations)
 
 add_subdirectory(WeissHapticGroup)
-add_subdirectory(StatechartProfilesTestGroup)
\ No newline at end of file
+add_subdirectory(StatechartProfilesTestGroup)
+add_subdirectory(OrientedTactileSensorGroup)
\ No newline at end of file
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/CMakeLists.txt b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6b74ac6725815d3f89da08fff09c180b909211a1
--- /dev/null
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/CMakeLists.txt
@@ -0,0 +1,48 @@
+armarx_component_set_name("OrientedTactileSensorGroup")
+
+#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()
+
+#find_package(Eigen3 QUIET)
+#find_package(Simox 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(COMPONENT_LIBS
+#   RobotAPIInterfaces RobotAPICore
+    ArmarXCoreInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers)
+
+# Sources
+
+set(SOURCES
+OrientedTactileSensorGroupRemoteStateOfferer.cpp
+./OrientedTactileSensorTest.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+
+set(HEADERS
+OrientedTactileSensorGroupRemoteStateOfferer.h
+OrientedTactileSensorGroup.scgxml
+./OrientedTactileSensorTest.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+./OrientedTactileSensorTest.xml
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.xml
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroup.scgxml b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroup.scgxml
new file mode 100644
index 0000000000000000000000000000000000000000..58dc416b8a4de1b86d3b582acc5d44b048279c9d
--- /dev/null
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroup.scgxml
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8"?>
+<StatechartGroup name="OrientedTactileSensorGroup" package="RobotAPI" generateContext="true">
+	<Proxies/>
+	<Configurations/>
+	<State filename="OrientedTactileSensorTest.xml" visibility="public"/>
+</StatechartGroup>
+
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a016ee99ca74581045089b05f3152589586d442c
--- /dev/null
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.cpp
@@ -0,0 +1,66 @@
+/*
+ * 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::OrientedTactileSensorGroup::OrientedTactileSensorGroupRemoteStateOfferer
+ * @author     andreeatulbure ( andreea_tulbure at yahoo dot de )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "OrientedTactileSensorGroupRemoteStateOfferer.h"
+
+using namespace armarx;
+using namespace OrientedTactileSensorGroup;
+
+// DO NOT EDIT NEXT LINE
+OrientedTactileSensorGroupRemoteStateOfferer::SubClassRegistry OrientedTactileSensorGroupRemoteStateOfferer::Registry(OrientedTactileSensorGroupRemoteStateOfferer::GetName(), &OrientedTactileSensorGroupRemoteStateOfferer::CreateInstance);
+
+
+
+OrientedTactileSensorGroupRemoteStateOfferer::OrientedTactileSensorGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
+    XMLRemoteStateOfferer < OrientedTactileSensorGroupStatechartContext > (reader)
+{
+}
+
+void OrientedTactileSensorGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+{
+
+}
+
+void OrientedTactileSensorGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+{
+
+}
+
+void OrientedTactileSensorGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+{
+
+}
+
+// DO NOT EDIT NEXT FUNCTION
+std::string OrientedTactileSensorGroupRemoteStateOfferer::GetName()
+{
+    return "OrientedTactileSensorGroupRemoteStateOfferer";
+}
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateOffererFactoryBasePtr OrientedTactileSensorGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+{
+    return XMLStateOffererFactoryBasePtr(new OrientedTactileSensorGroupRemoteStateOfferer(reader));
+}
+
+
+
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.h
new file mode 100644
index 0000000000000000000000000000000000000000..c387c8e489f740fcfd795db81d8414d9a6bf9125
--- /dev/null
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.h
@@ -0,0 +1,54 @@
+/*
+ * 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::OrientedTactileSensorGroup
+ * @author     andreeatulbure ( andreea_tulbure at yahoo dot de )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_XMLUSERCODE_RobotAPI_OrientedTactileSensorGroup_REMOTESTATEOFFERER_H
+#define _ARMARX_XMLUSERCODE_RobotAPI_OrientedTactileSensorGroup_REMOTESTATEOFFERER_H
+
+#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+#include "OrientedTactileSensorGroupStatechartContext.generated.h"
+
+namespace armarx
+{
+    namespace OrientedTactileSensorGroup
+    {
+        class OrientedTactileSensorGroupRemoteStateOfferer :
+            virtual public XMLRemoteStateOfferer < OrientedTactileSensorGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+        {
+        public:
+            OrientedTactileSensorGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+
+            // inherited from RemoteStateOfferer
+            void onInitXMLRemoteStateOfferer();
+            void onConnectXMLRemoteStateOfferer();
+            void onExitXMLRemoteStateOfferer();
+
+            // static functions for AbstractFactory Method
+            static std::string GetName();
+            static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+            static SubClassRegistry Registry;
+
+
+        };
+    }
+}
+
+#endif
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.cpp b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ff18ceb1d2d9fed9466b7a6cdf32057cc24d99d1
--- /dev/null
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.cpp
@@ -0,0 +1,74 @@
+/*
+ * 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::OrientedTactileSensorGroup
+ * @author     andreeatulbure ( andreea_tulbure at yahoo dot de )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "OrientedTactileSensorTest.h"
+
+using namespace armarx;
+using namespace OrientedTactileSensorGroup;
+
+// DO NOT EDIT NEXT LINE
+OrientedTactileSensorTest::SubClassRegistry OrientedTactileSensorTest::Registry(OrientedTactileSensorTest::GetName(), &OrientedTactileSensorTest::CreateInstance);
+
+
+
+void OrientedTactileSensorTest::onEnter()
+{
+    //OrientedTactileSensorGroupStatechartContext* context = getContext<OrientedTactileSensorGroupStatechartContext>();
+    //HapticUnitObserverInterfacePrx hapticObserver = context->getHapticObserver();
+    //ChannelRegistry channels = hapticObserver->getAvailableChannels(false);
+    //std::map<std::string, DatafieldRefPtr> tactileDatafields_MaximumValueMap;
+
+    //local.setTactileDatafields_MaximumValue(tactileDatafields_MaximumValueMap);
+}
+
+//void OrientedTactileSensorTest::run()
+//{
+//    // put your user code for the execution-phase here
+//    // runs in seperate thread, thus can do complex operations
+//    // should check constantly whether isRunningTaskStopped() returns true
+//
+//// uncomment this if you need a continous run function. Make sure to use sleep or use blocking wait to reduce cpu load.
+//    while (!isRunningTaskStopped()) // stop run function if returning true
+//    {
+//        // do your calculations
+//    }
+//}
+
+//void OrientedTactileSensorTest::onBreak()
+//{
+//    // put your user code for the breaking point here
+//    // execution time should be short (<100ms)
+//}
+
+void OrientedTactileSensorTest::onExit()
+{
+    // put your user code for the exit point here
+    // execution time should be short (<100ms)
+}
+
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateFactoryBasePtr OrientedTactileSensorTest::CreateInstance(XMLStateConstructorParams stateData)
+{
+    return XMLStateFactoryBasePtr(new OrientedTactileSensorTest(stateData));
+}
+
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.h b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.h
new file mode 100644
index 0000000000000000000000000000000000000000..1fb2ae423a4fe17baa3332bad009f5ba996ea2a2
--- /dev/null
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.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::OrientedTactileSensorGroup
+ * @author     andreeatulbure ( andreea_tulbure at yahoo dot de )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef _ARMARX_XMLUSERCODE_RobotAPI_OrientedTactileSensorGroup_OrientedTactileSensorTest_H
+#define _ARMARX_XMLUSERCODE_RobotAPI_OrientedTactileSensorGroup_OrientedTactileSensorTest_H
+
+#include <RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.generated.h>
+
+namespace armarx
+{
+    namespace OrientedTactileSensorGroup
+    {
+        class OrientedTactileSensorTest :
+            public OrientedTactileSensorTestGeneratedBase <OrientedTactileSensorTest>
+        {
+        public:
+            OrientedTactileSensorTest(const XMLStateConstructorParams& stateData):
+                XMLStateTemplate <OrientedTactileSensorTest> (stateData), OrientedTactileSensorTestGeneratedBase <OrientedTactileSensorTest> (stateData)
+            {
+            }
+
+            // inherited from StateBase
+            void onEnter();
+            // void run();
+            // void onBreak();
+            void onExit();
+
+            // static functions for AbstractFactory Method
+            static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+            static SubClassRegistry Registry;
+
+            // DO NOT INSERT ANY CLASS MEMBERS,
+            // use stateparameters instead,
+            // if classmember are neccessary nonetheless, reset them in onEnter
+        };
+    }
+}
+
+#endif
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.xml b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.xml
new file mode 100644
index 0000000000000000000000000000000000000000..dd45021130cdeb24d567e59466e9600161d7b403
--- /dev/null
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.xml
@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="OrientedTactileSensorTest" uuid="83D59864-EC09-4DA1-8773-7FF6D0E74A6A" width="800" height="600" type="Normal State">
+	<InputParameters/>
+	<OutputParameters/>
+	<LocalParameters/>
+	<Substates/>
+	<Events>
+		<Event name="Failure">
+			<Description>Event for statechart-internal failures or optionally user-code failures</Description>
+		</Event>
+	</Events>
+	<Transitions/>
+</State>
+