From 5f121f8dc41b585b5d471498b50829f6010846c8 Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Tue, 24 Apr 2018 22:01:10 +0200
Subject: [PATCH] Add more doxygen
---
.../images/NJointControllerAtomicDataFlow.svg | 2 +-
.../images/NJointControllerAtomicDataFlow.xml | 2 +-
.../NJointControllerGeneralDataFlow.svg | 2 +-
.../NJointControllerGeneralDataFlow.xml | 2 +-
.../NJointControllerTripleBufferDataFlow.svg | 2 +-
.../NJointControllerTripleBufferDataFlow.xml | 2 +-
.../NJointControllerWorkerThreadDataFlow.svg | 2 +-
.../NJointControllerWorkerThreadDataFlow.xml | 2 +-
etc/doxygen/images/RobotModuleInteraction.svg | 2 +
etc/doxygen/images/RobotModuleInteraction.xml | 1 +
etc/doxygen/images/RobotUnitLifeCycle.svg | 2 +
etc/doxygen/images/RobotUnitLifeCycle.xml | 1 +
.../NJointControllers/NJointController.h | 438 +++++++++++++++++-
13 files changed, 431 insertions(+), 29 deletions(-)
create mode 100644 etc/doxygen/images/RobotModuleInteraction.svg
create mode 100644 etc/doxygen/images/RobotModuleInteraction.xml
create mode 100644 etc/doxygen/images/RobotUnitLifeCycle.svg
create mode 100644 etc/doxygen/images/RobotUnitLifeCycle.xml
diff --git a/etc/doxygen/images/NJointControllerAtomicDataFlow.svg b/etc/doxygen/images/NJointControllerAtomicDataFlow.svg
index ce7ead509..e0cccb8b9 100644
--- a/etc/doxygen/images/NJointControllerAtomicDataFlow.svg
+++ b/etc/doxygen/images/NJointControllerAtomicDataFlow.svg
@@ -1,2 +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="<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>"><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<br></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
+<svg xmlns="http://www.w3.org/2000/svg" style="background-color: rgb(255, 255, 255);" xmlns:xlink="http://www.w3.org/1999/xlink" width="601px" height="301px" version="1.1" content="<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:59.0) Gecko/20100101 Firefox/59.0" version="8.5.12" editor="www.draw.io" type="device"><diagram name="Page-1" id="775d93bb-e439-393a-9cf0-e3d5a865d32a">7Vttc+I2EP41zLSdCSNZfuNj4K7X67Q3N5f07aNiC+OesKgQgdyvP8mWsIVMDoIxtE0+gL0SK2uffVarlTNAk/nmHceL2a8sJXTggXQzQG8GngdRBOWXkjxVkhBFlSDjeao71YK7/AvRQqClqzwlS6ujYIyKfGELE1YUJBGWDHPO1na3KaP2qAucEUdwl2DqSv/IUzGrpB4AoG74ieTZTOy2PODkc8bZqtADDjw0Lf+q5jk2ynT/5QynbN0QobcDNOGMiepqvpkQqoxr7Fb97sc9rdsH56QQB/1ANZYPIp7M7EkqjaFvGRczlrEC07e1dFxOkCgVQN7NxJzKSygvySYXfyrx0Av07V+66W8ixJOGGq8Ek6Ja+S+MLXQ/dwZ6UuqxGgI9n3eEzYngT7IDJxSL/NHGEGtXyLb9tj/9yHI5hAeM24LIG3pxEEFff1YajA/7oa1RYJ4RoZU0TXyc3iAOhjEYBR6qPiN7lCVb8YQ4o8iLxtxrUQnvPqjBK9RVj2gUDEM4An5UfQY7SEcvQ/p5tUEc9gN0qB3sEdOVttCHn5XeCSsEZ5QS7jjCcp3PKS4U4utZLsjdAieqaS0DvI37XtAeCRdk8yxsG2NgYFnGj/X9ug62oYmos0acRUb4DNSf5IKAi0xO7KjxIGwZz0P2cJgKwgssyFixYnkqISE6DyGbfFSdJFL8qWqKAnN/OlmNn1WeW8lG2ucrymhZ1DWrLaM/a+GRQ4WBF1L5YOMpK6lb2z78Z8VMw82yNMit7BAvNnWbvMrU92+LVHqBUSUfo9JWNTqgSloIGytO5AD4oeygEFooepYTDcaD4I2UYJpnhRQk0tySr2is6JXLFOVWN8zzNC0dguIHQsfbvGPCKOPluCbzaOWs8T2HtdusSj+elZi0sVn6W+zptfGlcdl0YdPpkpy80IHov8er0OUVBOhyxPJfidVOLON8XRAr8gI7h4BXRDM3y/h0f0JeMc0pbZg4xSSeJlK+lCnLZ9JoCZOYPLQbf5v5fDsV0Qb1AicTQG2Zh2eSuQMzD6PeTTSC3vMMs/42kLoTDZY98JpgHVD4O1tLJWQLkavAt4fZ3x/K+KOT08M9Ylu9iC3KhS6EUWtqenpUHcUOUPdlvJceAH64hC3svBmaPUsPtoCGhQ1j/K4utS1eXffbruuB/nwXOWh9YIUUqDXBxaqrVWI6JWHSukqk0egBgFNXic3OknDmVWPfcIfBeOZVJHAQ/og5lhoIX57ORwhaCfmYY9l4K9g8T5ZdUY0zufxJTstbZcg2H9m3O9rrIjfQi+xg6Y0c1FoXf7+TaOnyr8c1/oVaLhZZj0ZX7neGjQJu7PuXyhGg5/LQMYjasCwOn/bRO5MbGNoBynPm78OWANXJ/F1PP90haqsevHNwUyHUAjnsZMpn2mxXYfWsm+2VYNVDlD8we29KpqJl5y1UUWS8lLjlRXZfVkhu/JPxsj296aJta2gXeCHYgteFKRr6/VEUuaWBDiiKDj9n6J+iqC0q/b8pegReF6CoWxO5OEVR2CNF3VJDFxTdV/u8Coqe6Szo30zRw/Hqn6JG8TVRNHAT/bNR1D8gizieolurXiNFzasxrxR9CV4XoOjpGxPUWvcZDofXjRYEtUgPM2M8/yKfFFMtODwu7Y8/nh1/YnXSHIceCONRGATQ3Xe3cbOberyL9WSGi0zVLZuFwB14zn3wfoFzd1MJ7f/Y3TxNA4P3iQLgvTrLnqpl4MgVwimF7hTYCUwDErUV2EdhhHDYUYHdD3fOuUO3wt5+ctqJa7vJ8ATTZEXLAiooyFp+bk/ienVw2HTvql8PL2zFl3JwaJSc9R1VcJIVr+Md1RBZL5OObPXdvLEagWcHefEbq/K2fsO96l7/HwF6+xU=</diagram></mxfile>"><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(252.5,6.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="94" 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: 95px; 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;">NJointController</div></div></foreignObject><text x="47" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">NJointController</text></switch></g><path d="M 278 174 L 191 174 L 191 121.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 191 116.12 L 194.5 123.12 L 191 121.37 L 187.5 123.12 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(179.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 175 L 467 175 L 467 121.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 467 116.12 L 470.5 123.12 L 467 121.37 L 463.5 123.12 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(454.5,133.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,31.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,31.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<br></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 175 L 449 175 L 449 174 L 324.37 174" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 319.12 174 L 326.12 170.5 L 324.37 174 L 326.12 177.5 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 174 L 179 174 L 179 177 L 86.37 177" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 81.12 177 L 88.12 173.5 L 86.37 177 L 88.12 180.5 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 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/NJointControllerAtomicDataFlow.xml b/etc/doxygen/images/NJointControllerAtomicDataFlow.xml
index e7ff81b81..1804a9890 100644
--- a/etc/doxygen/images/NJointControllerAtomicDataFlow.xml
+++ b/etc/doxygen/images/NJointControllerAtomicDataFlow.xml
@@ -1 +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
+<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:59.0) Gecko/20100101 Firefox/59.0" version="8.5.12" editor="www.draw.io" type="device"><diagram name="Page-1" id="775d93bb-e439-393a-9cf0-e3d5a865d32a">7Vttc+I2EP41zLSdCSNZfuNj4K7X67Q3N5f07aNiC+OesKgQgdyvP8mWsIVMDoIxtE0+gL0SK2uffVarlTNAk/nmHceL2a8sJXTggXQzQG8GngdRBOWXkjxVkhBFlSDjeao71YK7/AvRQqClqzwlS6ujYIyKfGELE1YUJBGWDHPO1na3KaP2qAucEUdwl2DqSv/IUzGrpB4AoG74ieTZTOy2PODkc8bZqtADDjw0Lf+q5jk2ynT/5QynbN0QobcDNOGMiepqvpkQqoxr7Fb97sc9rdsH56QQB/1ANZYPIp7M7EkqjaFvGRczlrEC07e1dFxOkCgVQN7NxJzKSygvySYXfyrx0Av07V+66W8ixJOGGq8Ek6Ja+S+MLXQ/dwZ6UuqxGgI9n3eEzYngT7IDJxSL/NHGEGtXyLb9tj/9yHI5hAeM24LIG3pxEEFff1YajA/7oa1RYJ4RoZU0TXyc3iAOhjEYBR6qPiN7lCVb8YQ4o8iLxtxrUQnvPqjBK9RVj2gUDEM4An5UfQY7SEcvQ/p5tUEc9gN0qB3sEdOVttCHn5XeCSsEZ5QS7jjCcp3PKS4U4utZLsjdAieqaS0DvI37XtAeCRdk8yxsG2NgYFnGj/X9ug62oYmos0acRUb4DNSf5IKAi0xO7KjxIGwZz0P2cJgKwgssyFixYnkqISE6DyGbfFSdJFL8qWqKAnN/OlmNn1WeW8lG2ucrymhZ1DWrLaM/a+GRQ4WBF1L5YOMpK6lb2z78Z8VMw82yNMit7BAvNnWbvMrU92+LVHqBUSUfo9JWNTqgSloIGytO5AD4oeygEFooepYTDcaD4I2UYJpnhRQk0tySr2is6JXLFOVWN8zzNC0dguIHQsfbvGPCKOPluCbzaOWs8T2HtdusSj+elZi0sVn6W+zptfGlcdl0YdPpkpy80IHov8er0OUVBOhyxPJfidVOLON8XRAr8gI7h4BXRDM3y/h0f0JeMc0pbZg4xSSeJlK+lCnLZ9JoCZOYPLQbf5v5fDsV0Qb1AicTQG2Zh2eSuQMzD6PeTTSC3vMMs/42kLoTDZY98JpgHVD4O1tLJWQLkavAt4fZ3x/K+KOT08M9Ylu9iC3KhS6EUWtqenpUHcUOUPdlvJceAH64hC3svBmaPUsPtoCGhQ1j/K4utS1eXffbruuB/nwXOWh9YIUUqDXBxaqrVWI6JWHSukqk0egBgFNXic3OknDmVWPfcIfBeOZVJHAQ/og5lhoIX57ORwhaCfmYY9l4K9g8T5ZdUY0zufxJTstbZcg2H9m3O9rrIjfQi+xg6Y0c1FoXf7+TaOnyr8c1/oVaLhZZj0ZX7neGjQJu7PuXyhGg5/LQMYjasCwOn/bRO5MbGNoBynPm78OWANXJ/F1PP90haqsevHNwUyHUAjnsZMpn2mxXYfWsm+2VYNVDlD8we29KpqJl5y1UUWS8lLjlRXZfVkhu/JPxsj296aJta2gXeCHYgteFKRr6/VEUuaWBDiiKDj9n6J+iqC0q/b8pegReF6CoWxO5OEVR2CNF3VJDFxTdV/u8Coqe6Szo30zRw/Hqn6JG8TVRNHAT/bNR1D8gizieolurXiNFzasxrxR9CV4XoOjpGxPUWvcZDofXjRYEtUgPM2M8/yKfFFMtODwu7Y8/nh1/YnXSHIceCONRGATQ3Xe3cbOberyL9WSGi0zVLZuFwB14zn3wfoFzd1MJ7f/Y3TxNA4P3iQLgvTrLnqpl4MgVwimF7hTYCUwDErUV2EdhhHDYUYHdD3fOuUO3wt5+ctqJa7vJ8ATTZEXLAiooyFp+bk/ienVw2HTvql8PL2zFl3JwaJSc9R1VcJIVr+Md1RBZL5OObPXdvLEagWcHefEbq/K2fsO96l7/HwF6+xU=</diagram></mxfile>
\ No newline at end of file
diff --git a/etc/doxygen/images/NJointControllerGeneralDataFlow.svg b/etc/doxygen/images/NJointControllerGeneralDataFlow.svg
index a51e800c3..249e30f24 100644
--- a/etc/doxygen/images/NJointControllerGeneralDataFlow.svg
+++ b/etc/doxygen/images/NJointControllerGeneralDataFlow.svg
@@ -1,2 +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="<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>"><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<br></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
+<svg xmlns="http://www.w3.org/2000/svg" style="background-color: rgb(255, 255, 255);" xmlns:xlink="http://www.w3.org/1999/xlink" width="601px" height="301px" version="1.1" content="<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:59.0) Gecko/20100101 Firefox/59.0" version="8.5.12" editor="www.draw.io" type="device"><diagram name="Page-1" id="540e3684-599b-54a4-ef23-75810fbedebd">7Vlfk9o2EP80zLSdOUbyfx4DuabptJlMLm3SR2HLxj1hUSEOrp++K1vCFjKEOzjIdI4HY+0uK2l3f7srNPAn8807QRaz33lG2cBD2Wbgvx14HvZjDF+K8thQIj9uCIUoMy3UEu7Kf6kmIk1dlRldWoKScybLhU1MeVXRVFo0IgRf22I5Z/asC1JQh3CXEuZSv5SZnDVUDyHUMn6hZTGTu5wpSe8LwVeVnnDg+Xn9adhzYpRp+eWMZHzdIfm3A38iOJfN23wzoUwZ19it+d3Pe7jbhQtayaN+oJj1QuSj2T3NwBh6yIWc8YJXhN221HG9QapUIBjN5JzBK4ZXuinlV0UeeqEe/qVZf1MpH7WryUpyILXKf+N8oeXcHehNqWV1CHo/7yifUykeQUBQRmT5YPuQ6FAotnLbn37kJUzhIRO2KPaGXhLGONDPRoOJ4SCyNUoiCiq1kq6Jn6Y3TMJhgkah5zfP2J5lyVcipc4s8NLZe0uq3bvP1ejV1Y1EPAqHER6hIG6e4Y6n4+d5+rDaMIku4+hIB9gDYSttoQ+/Kr0TXknBGaPCCYTlupwzUimPr2elpHcLkirWGhK87fe9TnugQtLNQbdtjIGRZZkg0eN1m2wjk1FnnTzrG+IBV3+CgkCqAjb2pPkw7pnP8+3pCJNUVETSsULF8lRAYv9lANnFoxICT4nHhhWHZnw6WE2cNZHb0EY65hvIaFp8blRbRj9o4ZEDhYEXMVjYOOc1dFvbR/+suGHcLGuDvAGBZLFpefBWqO8/FhlEgVEFy2i0NUzHqQALaftKUJiATGsB5aGFgme90XA8CN8ChbCyqICQgrkBr/5YwauEFuWNZszLLKsDgpEpZeNt3zHhjIt6XtN59GLWxJ6D2m1XpZdnNSZ9aIZ4SzxdG5+bl40Iz/MlPbnQofj/h6vIxRVG/vWAFbwCqx9YJvjOAazYC+0eAn9HMHO7jE+fT+gr8pKxjokzQpM8BfoSWpZ72uFEaUKn/cbfdj7fbkW0Qb3Q6QT8vs7DM83ckZ2HUe82GuHF+wxTfzueupMdlE1FC7AzQPgHW0tD5AtZqsS3B9k/Hov4Jzenx0fE9t+LxIJc5Low7m1NT8+qo8Rx1Oc630MEoJ+uYQu7b8bmzHIBW2CDwo4x/lSv2havofvt0PXQ5WLXd7z1gVdAUDXB9dW5qkSe0yjtrRJZPJoidGqV2OyUhBeuGvumO86NL1xFQsfDH4kgoIGK5el4xKgXkA8lASaE0s2U8fS+rAoYTvh8vqqgZ1PAPBf+BJe1Phgq6/YFzr4j0964ucFevJNBXVf2dgTBWVKoC8oLFv5narlaun2yd+EQNOz8q5sEwbUaB4zc49hkBllFJZUuSi98Kr7Codikqcufic1qOj54nyoHvFcHzVwF4qkpaaf6UZyFNO6rfqMo9kl0puoXRHZg48gtf/3HmrOEttsUTwhLV6xOZKiia3hu2+SLBjjuhncjd4F/U5NrBTg2Sl70AgmdZMXv4wIpCqybnpGt/jzXSTE6OMmzr5Ng2F4/N+LtJb9/+x8=</diagram></mxfile>"><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(252.5,6.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="94" 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: 95px; 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;">NJointController</div></div></foreignObject><text x="47" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">NJointController</text></switch></g><path d="M 278 175 L 191 175 L 191 121.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 191 116.12 L 194.5 123.12 L 191 121.37 L 187.5 123.12 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(179.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><path d="M 579 175 L 467 175 L 467 121.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 467 116.12 L 470.5 123.12 L 467 121.37 L 463.5 123.12 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(454.5,133.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,31.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,31.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<br></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 175 L 324.37 175" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 319.12 175 L 326.12 171.5 L 324.37 175 L 326.12 178.5 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(398.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="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 175 L 179 175 L 179 177 L 86.37 177" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 81.12 177 L 88.12 173.5 L 86.37 177 L 88.12 180.5 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/NJointControllerGeneralDataFlow.xml b/etc/doxygen/images/NJointControllerGeneralDataFlow.xml
index 9dd261ed0..04d06cd22 100644
--- a/etc/doxygen/images/NJointControllerGeneralDataFlow.xml
+++ b/etc/doxygen/images/NJointControllerGeneralDataFlow.xml
@@ -1 +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
+<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:59.0) Gecko/20100101 Firefox/59.0" version="8.5.12" editor="www.draw.io" type="device"><diagram name="Page-1" id="540e3684-599b-54a4-ef23-75810fbedebd">7Vlfk9o2EP80zLSdOUbyfx4DuabptJlMLm3SR2HLxj1hUSEOrp++K1vCFjKEOzjIdI4HY+0uK2l3f7srNPAn8807QRaz33lG2cBD2Wbgvx14HvZjDF+K8thQIj9uCIUoMy3UEu7Kf6kmIk1dlRldWoKScybLhU1MeVXRVFo0IgRf22I5Z/asC1JQh3CXEuZSv5SZnDVUDyHUMn6hZTGTu5wpSe8LwVeVnnDg+Xn9adhzYpRp+eWMZHzdIfm3A38iOJfN23wzoUwZ19it+d3Pe7jbhQtayaN+oJj1QuSj2T3NwBh6yIWc8YJXhN221HG9QapUIBjN5JzBK4ZXuinlV0UeeqEe/qVZf1MpH7WryUpyILXKf+N8oeXcHehNqWV1CHo/7yifUykeQUBQRmT5YPuQ6FAotnLbn37kJUzhIRO2KPaGXhLGONDPRoOJ4SCyNUoiCiq1kq6Jn6Y3TMJhgkah5zfP2J5lyVcipc4s8NLZe0uq3bvP1ejV1Y1EPAqHER6hIG6e4Y6n4+d5+rDaMIku4+hIB9gDYSttoQ+/Kr0TXknBGaPCCYTlupwzUimPr2elpHcLkirWGhK87fe9TnugQtLNQbdtjIGRZZkg0eN1m2wjk1FnnTzrG+IBV3+CgkCqAjb2pPkw7pnP8+3pCJNUVETSsULF8lRAYv9lANnFoxICT4nHhhWHZnw6WE2cNZHb0EY65hvIaFp8blRbRj9o4ZEDhYEXMVjYOOc1dFvbR/+suGHcLGuDvAGBZLFpefBWqO8/FhlEgVEFy2i0NUzHqQALaftKUJiATGsB5aGFgme90XA8CN8ChbCyqICQgrkBr/5YwauEFuWNZszLLKsDgpEpZeNt3zHhjIt6XtN59GLWxJ6D2m1XpZdnNSZ9aIZ4SzxdG5+bl40Iz/MlPbnQofj/h6vIxRVG/vWAFbwCqx9YJvjOAazYC+0eAn9HMHO7jE+fT+gr8pKxjokzQpM8BfoSWpZ72uFEaUKn/cbfdj7fbkW0Qb3Q6QT8vs7DM83ckZ2HUe82GuHF+wxTfzueupMdlE1FC7AzQPgHW0tD5AtZqsS3B9k/Hov4Jzenx0fE9t+LxIJc5Low7m1NT8+qo8Rx1Oc630MEoJ+uYQu7b8bmzHIBW2CDwo4x/lSv2havofvt0PXQ5WLXd7z1gVdAUDXB9dW5qkSe0yjtrRJZPJoidGqV2OyUhBeuGvumO86NL1xFQsfDH4kgoIGK5el4xKgXkA8lASaE0s2U8fS+rAoYTvh8vqqgZ1PAPBf+BJe1Phgq6/YFzr4j0964ucFevJNBXVf2dgTBWVKoC8oLFv5narlaun2yd+EQNOz8q5sEwbUaB4zc49hkBllFJZUuSi98Kr7Codikqcufic1qOj54nyoHvFcHzVwF4qkpaaf6UZyFNO6rfqMo9kl0puoXRHZg48gtf/3HmrOEttsUTwhLV6xOZKiia3hu2+SLBjjuhncjd4F/U5NrBTg2Sl70AgmdZMXv4wIpCqybnpGt/jzXSTE6OMmzr5Ng2F4/N+LtJb9/+x8=</diagram></mxfile>
\ No newline at end of file
diff --git a/etc/doxygen/images/NJointControllerTripleBufferDataFlow.svg b/etc/doxygen/images/NJointControllerTripleBufferDataFlow.svg
index 9b114c273..0abd7e978 100644
--- a/etc/doxygen/images/NJointControllerTripleBufferDataFlow.svg
+++ b/etc/doxygen/images/NJointControllerTripleBufferDataFlow.svg
@@ -1,2 +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="<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>"><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<br></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
+<svg xmlns="http://www.w3.org/2000/svg" style="background-color: rgb(255, 255, 255);" xmlns:xlink="http://www.w3.org/1999/xlink" width="601px" height="301px" version="1.1" content="<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:59.0) Gecko/20100101 Firefox/59.0" version="8.5.12" editor="www.draw.io" type="device"><diagram name="Page-1" id="fc77a887-41d1-ed7b-0bc0-1c9de38fa8ce">7Vpbk9o2FP41zLSdKSPZli+PC9km6bSZzO62aR6FLcCNsKgQC+TXR8KSbSHDcjFsZpp9YK0jWZdzvu/ok6DnD2frtxzPp3+yjNCeB7J1z3/T8zzoR1D+U5ZNaQn9qDRMeJ7pRrXhMf9KtBFo6zLPyMJqKBijIp/bxpQVBUmFZcOcs5XdbMyoPeocT4hjeEwxda2f8kxMS6sHAKgr3pF8MhW7NSOcfplwtiz0gD3PH2//yuoZNp3p9ospztiqYfLve/6QMybKp9l6SKhyrvFb+d5ve2qriXNSiKNeUJXbiYiNWT3JpDN0kXExZRNWYHpfWwfbBRLVBZClqZhR+QjlI1nn4h9l7ntIFz/rqn+JEBsdarwUTJrqzv9gbK7buSvQi1LTahj0et4SNiOCb2QDTigW+bMdQ6yhMKnaVa9+ZLkcwgMGthBEfS9GEQz0Z9mDwXAQ2j0KzCdE6E6aLj6tXxSjfgwS5PnlZ2SPsmBLnhJnlDvO8abRbK4aLA4vzl4OBC/O+tAL8qGcgyk1QlGbtmjbhzxDgf898qIE9UOYgCAqP9EO8HYgcSzwDneL4vAM3J0R6FDj/RnTpfbQh99Vv0NWCM4oJdwBwmKVzyguVMRX01yQxzlOVdVK7jd23PcG7ZlwQdYHw7Y2DgaWZ4JYl1d17g9Ngp820r5vjAdC/SD3J1xM5MJOGg/ClvE83x4OU0F4gQUZKFYsLowThH6nhJTB3ZSMjJApX05Jg6aSBKUtiW7DUw/YgYPxTvy7StYJbB2ns9QLE4eSPS+k0p2DMdvOpMZA+N+SmYpfF9uQ3ckG8Xxd18mnifr/1zyTaDRdyWmUvZWVDrgkPYWNGU7kAHi0baDgpB0lW6NBD72RFkzzSSENqQSEzBv+QNE8l8rtTlfM8izbApPiEaGDSo4NGWV8O64RZK25w3DAyR6V2NTT6zX1WltWAX0Ue1oynIs704SNxwtyaR6uNvSuN9zmfguuz/ySZJr5oZsNIPC7TgeW1w8SK/hBrHZiGfB1QazIQ7aWgd8RzVy18/B0gb4Z55Q2XJxhEo9TaV9I6fSFNGrCNCajdudXCuxlSaQd6iFHkUi106JIjKg8UgGZ7l3Bg26ud4xqaETqUTRYNuI1wTqg8E92L6WRzUWuEt8eZv98LONPFsnHI6K61IktyoVuCKNWiXx5Vk1iJ1BP23wvEQB+eQ1f7MhAc3a6gS+gkaANZ/ytHrUvfkD3ZehWMv4G8fKdaH1ghTSoPcGNVVe7xHhMwrR1l8iiZATApbuErvUjmwjX2jXMcDvHr5ZdpC2MVz41AzfEN9xGzuzl1ci773i1F2NR0m/cnMZB8Fq7EASuth9OJUQVQj9ijmWPhC8cF13liHWdU9S170928wWMAruLPTeb54i70AnW+1RF6r063owVYE/EMmeS1ZI0sqgw5uRcAjNEoracm4SRj8OOcm4Q7hx9wpak2yqmO+GAK8WGmKZLuk14oCAr+VmJsw6ZYK4UqsLnihcd3yXGt+FClZMqLlztAh4G7nXfE8/nlAyWUinwC3RHF4je9USLioDBqSriaEQjV08/EJydmh2mjOdf5U6JqQav65k6DJccwwPU4gnUBbcRdDzxiefi5ETZsSt0bWKnvZt6xrD7qt9UXratfx/fVIa+9ZVictTufuIgETg4SIdpE5lUVhPiXZ5lpLgGI07mA7QPtch3pXBXjJDF+hcppSfr3/34998A</diagram></mxfile>"><defs/><g transform="translate(0.5,0.5)"><path d="M 467 106 L 467 130 L 467 159.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 467 164.88 L 463.5 157.88 L 467 159.63 L 470.5 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(252.5,6.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="94" 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: 95px; 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;">NJointController</div></div></foreignObject><text x="47" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">NJointController</text></switch></g><path d="M 200 180 L 191 180 L 191 121.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 191 116.12 L 194.5 123.12 L 191 121.37 L 187.5 123.12 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(179.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 175 L 502 175 L 502 121.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 502 116.12 L 505.5 123.12 L 502 121.37 L 498.5 123.12 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(489.5,128.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,31.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,31.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<br></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 175 L 475 175 L 475 174 L 376.37 174" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 371.12 174 L 378.12 170.5 L 376.37 174 L 378.12 177.5 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(425.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 230 177 L 86.37 177" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 81.12 177 L 88.12 173.5 L 86.37 177 L 88.12 180.5 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,31.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/NJointControllerTripleBufferDataFlow.xml b/etc/doxygen/images/NJointControllerTripleBufferDataFlow.xml
index af98949fb..6f16331ce 100644
--- a/etc/doxygen/images/NJointControllerTripleBufferDataFlow.xml
+++ b/etc/doxygen/images/NJointControllerTripleBufferDataFlow.xml
@@ -1 +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
+<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:59.0) Gecko/20100101 Firefox/59.0" version="8.5.12" editor="www.draw.io" type="device"><diagram name="Page-1" id="fc77a887-41d1-ed7b-0bc0-1c9de38fa8ce">7Vpbk9o2FP41zLSdKSPZli+PC9km6bSZzO62aR6FLcCNsKgQC+TXR8KSbSHDcjFsZpp9YK0jWZdzvu/ok6DnD2frtxzPp3+yjNCeB7J1z3/T8zzoR1D+U5ZNaQn9qDRMeJ7pRrXhMf9KtBFo6zLPyMJqKBijIp/bxpQVBUmFZcOcs5XdbMyoPeocT4hjeEwxda2f8kxMS6sHAKgr3pF8MhW7NSOcfplwtiz0gD3PH2//yuoZNp3p9ospztiqYfLve/6QMybKp9l6SKhyrvFb+d5ve2qriXNSiKNeUJXbiYiNWT3JpDN0kXExZRNWYHpfWwfbBRLVBZClqZhR+QjlI1nn4h9l7ntIFz/rqn+JEBsdarwUTJrqzv9gbK7buSvQi1LTahj0et4SNiOCb2QDTigW+bMdQ6yhMKnaVa9+ZLkcwgMGthBEfS9GEQz0Z9mDwXAQ2j0KzCdE6E6aLj6tXxSjfgwS5PnlZ2SPsmBLnhJnlDvO8abRbK4aLA4vzl4OBC/O+tAL8qGcgyk1QlGbtmjbhzxDgf898qIE9UOYgCAqP9EO8HYgcSzwDneL4vAM3J0R6FDj/RnTpfbQh99Vv0NWCM4oJdwBwmKVzyguVMRX01yQxzlOVdVK7jd23PcG7ZlwQdYHw7Y2DgaWZ4JYl1d17g9Ngp820r5vjAdC/SD3J1xM5MJOGg/ClvE83x4OU0F4gQUZKFYsLowThH6nhJTB3ZSMjJApX05Jg6aSBKUtiW7DUw/YgYPxTvy7StYJbB2ns9QLE4eSPS+k0p2DMdvOpMZA+N+SmYpfF9uQ3ckG8Xxd18mnifr/1zyTaDRdyWmUvZWVDrgkPYWNGU7kAHi0baDgpB0lW6NBD72RFkzzSSENqQSEzBv+QNE8l8rtTlfM8izbApPiEaGDSo4NGWV8O64RZK25w3DAyR6V2NTT6zX1WltWAX0Ue1oynIs704SNxwtyaR6uNvSuN9zmfguuz/ySZJr5oZsNIPC7TgeW1w8SK/hBrHZiGfB1QazIQ7aWgd8RzVy18/B0gb4Z55Q2XJxhEo9TaV9I6fSFNGrCNCajdudXCuxlSaQd6iFHkUi106JIjKg8UgGZ7l3Bg26ud4xqaETqUTRYNuI1wTqg8E92L6WRzUWuEt8eZv98LONPFsnHI6K61IktyoVuCKNWiXx5Vk1iJ1BP23wvEQB+eQ1f7MhAc3a6gS+gkaANZ/ytHrUvfkD3ZehWMv4G8fKdaH1ghTSoPcGNVVe7xHhMwrR1l8iiZATApbuErvUjmwjX2jXMcDvHr5ZdpC2MVz41AzfEN9xGzuzl1ci773i1F2NR0m/cnMZB8Fq7EASuth9OJUQVQj9ijmWPhC8cF13liHWdU9S170928wWMAruLPTeb54i70AnW+1RF6r063owVYE/EMmeS1ZI0sqgw5uRcAjNEoracm4SRj8OOcm4Q7hx9wpak2yqmO+GAK8WGmKZLuk14oCAr+VmJsw6ZYK4UqsLnihcd3yXGt+FClZMqLlztAh4G7nXfE8/nlAyWUinwC3RHF4je9USLioDBqSriaEQjV08/EJydmh2mjOdf5U6JqQav65k6DJccwwPU4gnUBbcRdDzxiefi5ETZsSt0bWKnvZt6xrD7qt9UXratfx/fVIa+9ZVictTufuIgETg4SIdpE5lUVhPiXZ5lpLgGI07mA7QPtch3pXBXjJDF+hcppSfr3/34998A</diagram></mxfile>
\ No newline at end of file
diff --git a/etc/doxygen/images/NJointControllerWorkerThreadDataFlow.svg b/etc/doxygen/images/NJointControllerWorkerThreadDataFlow.svg
index 1efe44f90..982541f57 100644
--- a/etc/doxygen/images/NJointControllerWorkerThreadDataFlow.svg
+++ b/etc/doxygen/images/NJointControllerWorkerThreadDataFlow.svg
@@ -1,2 +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="<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>"><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<br>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<br></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 <br>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
+<svg xmlns="http://www.w3.org/2000/svg" style="background-color: rgb(255, 255, 255);" xmlns:xlink="http://www.w3.org/1999/xlink" width="701px" height="302px" version="1.1" content="<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:59.0) Gecko/20100101 Firefox/59.0" version="8.5.12" editor="www.draw.io" type="device"><diagram name="Page-1" id="b0089374-96cf-f724-51f3-db687a304ca3">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</diagram></mxfile>"><defs/><g transform="translate(0.5,0.5)"><path d="M 151 158 L 151 95.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 151 90.12 L 154.5 97.12 L 151 95.37 L 147.5 97.12 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(138.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(302.5,6.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="94" 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: 95px; 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;">NJointController</div></div></foreignObject><text x="47" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica" font-weight="bold">NJointController</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,31.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 86.37 151" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 81.12 151 L 88.12 147.5 L 86.37 151 L 88.12 154.5 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<br>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,31.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.89 130.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 372.98 125.12 L 376.35 132.18 L 372.89 130.37 L 369.36 132.05 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(361.5,141.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 L 614 175 L 614 121.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 614 116.12 L 617.5 123.12 L 614 121.37 L 610.5 123.12 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(601.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,31.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<br></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 L 524.37 175" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 519.12 175 L 526.12 171.5 L 524.37 175 L 526.12 178.5 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 <br>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 175 L 260 175 L 260 113 L 251.37 113" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 246.12 113 L 253.12 109.5 L 251.37 113 L 253.12 116.5 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(307.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 238 L 178.13 238" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 183.38 238 L 176.38 241.5 L 178.13 238 L 176.38 234.5 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 338 121 L 338 146 L 337.25 164.64" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 337.04 169.88 L 333.83 162.75 L 337.25 164.64 L 340.82 163.03 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 238 L 140 238 L 140 180.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 140 175.12 L 143.5 182.12 L 140 180.37 L 136.5 182.12 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 245 238 L 360 238 L 360 190.37" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 360 185.12 L 363.5 192.12 L 360 190.37 L 356.5 192.12 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/NJointControllerWorkerThreadDataFlow.xml b/etc/doxygen/images/NJointControllerWorkerThreadDataFlow.xml
index 5304d71bd..8a921963d 100644
--- a/etc/doxygen/images/NJointControllerWorkerThreadDataFlow.xml
+++ b/etc/doxygen/images/NJointControllerWorkerThreadDataFlow.xml
@@ -1 +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">7Vtbk9o2FP41zLSdWUaysWw/LmTbZibNZLJp0jwKLMBdY1HbLGx/fSRbMtbFrAHDJu3uA2sd2Ufy+c5dMHAnq91vGV4v/6ARSQYOiHYD983AcQIPsU9OeKoIPvArwiKLo4oE94T7+F8iiEBQN3FEcuXGgtKkiNcqcUbTlMwKhYazjG7V2+Y0UVdd4wUxCPcznJjUL3FULCuqAwDYT/xO4sWy0GemePawyOgmFQsOHHde/lXTKyyZifvzJY7otkFy7wbuJKO0qK5WuwlJuGyl3Krnfm2ZrTeekbTo9IAzqh55xMmGyD2XOyuepDhIGt1yqbLRLMF5Hs8G7nhZrBJGgOySrZU9/cUGYOh7cvxVTFbcSGRIfL9FCTHOFkSQQr8WBlMyQleEsWT3bPdweEKEywYQkpaRBBfxo7okFlqxqNnVK3ygMduMA4QC+6F4ROgvckcqi5xushkRTzXFqzFCIVAY+b7GqHpngxG7aLz2nlSi14akZ0ESJUwy4zktN7WHFP2zoXLiJi/t75bdEKx3+zl2teD//1xHuCCSFdtGxa2aNHSlILtC1Y6MsAXwtLwBsPGav2z5+t544L1hFJzEi5QrF9MIkjHCI8mKmJnjrZhYxVHEnx8neEqScW1jE5rQrFxXWpncEedAdjZnIXayN0FFEytzMPVOsAJD4LqqbsDzNE5wufHVJ+h8npPzVQIY+HBDvBdDmhVLuqApTu721HEpWRIJtJpmvouLysodTwylkf9NiuJJ+HG8KSgj7Zm/o3StOAOL7Wv+wQTgXIv2vCGCIRj51aen2jd0OpnlkWw9Dw4DEHqOW31qGLc4kROARo5h+u8+v4MTZqgZTRJmU7oa5Nt4leCU471dxgW5X+MZn9qyeK6i3gqZYWKtVoNGqhdk0qrGDW/uA4s7dyXxANAfuU2nC/Zi7esFwFgPQst6jqsuhxPmj1Lm/cbcJvIzUTJB+vjpDGDmcZI0HGCESTDngTlnmD+QxgyaBWQ6t0JZK87zWApZyiizVfIiiyilLXSETrI3kbLF+csCFfoGUvdFIwZOs3346yHA/qRyqYh0XcTcebbE3Z+7xuOjrbu7RojZMFCMTaZKim3bTLtdPRToDuEUGDh9Kh03UwDwywuIAqp+B0J4NVlAaYQNYXzml0IWr5priUuuCpfUp6vAZeruBCezTVL6GZCSrYlZrdw95nSydKsHX8sMzzsrqZNhRanoxAtfOtOro5FE1dNY9Jd2QQcZIH6h2QPJPuH8ocfQ7pEgGtlCe+BMXYQOVT0mLK3moEvOEurdo0N9Z4OQZdWLNiIkoE3FhU5oF9t1ehHQdzR91qTdtRdR51aSUaAx6rEX4Zq57msv4thehHs4eoEh1PtUN2c2I/rtPriuoQT3JM2ZlBwgU5Nz0xIIrHnJY4zZ5Hua3kwTOnuI0wUbTuhqtUkZljw/6SsNyWhR8mNDnix09MTC0bRiezNSk2oYuIYvrht+TaeD+shNHLP+kbkJf1OuX/kmKV7h6wqfZyaWl0NPnrX03fRr9vzAc8H2/NyxCmsid7SFZeBaI/oZCWVnEUPbocn/NbwphiGVr3PMOxDefMdT+6n9tNqhZmS9BDsz1L0vPSXv8JlOsq/CYD4naGbt+UV+OAXgoIF1r5O95wuDXnqAO63IaO8J2sryy/YEoa0c+c7OUnp3iPZCImCbDjwfjsSnVp/oxxxdz1IOs/WRepTS7UD2lOaveYz6AWeYcSDZfzjjsehRdwdxU6eMtRKYNmstkke9ZDyWQuN6/foTubxYm/RodBEcNswyGI0UqFE379xP09TMuyZLFj148DDBbprtlRPiF8iHpd+6fjocml3QtzMOyVueY865Zp7ro7S8h8DII74t7wmR72J0UJBH5D1IVXWILImP9XiylyrcPCG4261JmnPsWDrdrMh/LPU+2IO16rtn6rsIOe0xiaUTAJ5UMtxmGX5q3CDKqNaEqPaBUk9E3tvaidXvh66mGdUOLtudG11CbWBTaU44RTpaNepzT0U3qtbjNVoBMq59Z2XB89+37L0sCH1/OGpk8OrhKhqdWBZwtqjxp33FKkQnfMXqWANne1BfRhQnVoM9qbgMDV//msEqCv1cR/ZgLAg1h+v5V0xaXfTDOVrY6md79xp+oEKDPE3mLX7iWBPW1/FFD7c3E3av0Ha/RBzQYD47cracYgOtqYf0s46ecDYW0oDuvrMTNYMN9z/fqG7f/0bGvfsG</diagram></mxfile>
\ No newline at end of file
+<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:59.0) Gecko/20100101 Firefox/59.0" version="8.5.12" editor="www.draw.io" type="device"><diagram name="Page-1" id="b0089374-96cf-f724-51f3-db687a304ca3">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</diagram></mxfile>
\ No newline at end of file
diff --git a/etc/doxygen/images/RobotModuleInteraction.svg b/etc/doxygen/images/RobotModuleInteraction.svg
new file mode 100644
index 000000000..75d07f378
--- /dev/null
+++ b/etc/doxygen/images/RobotModuleInteraction.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="881px" height="451px" version="1.1" content="<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:59.0) Gecko/20100101 Firefox/59.0" version="8.5.12" editor="www.draw.io" type="device"><diagram id="aa7b51f2-7f26-c434-b112-75c9c6c67d3e" name="Page-1">7Zrdc6M2EMD/Gs8kD/EAwuA82mlyfejNZS7X6d1TRwYZ6AFihBzb/esrkMSXiA22MW6nnkyCVh8s2t+udnEm4CnafSIw8T9jF4UTQ3N3E/DLxMg+OvuTSfZcYgGbCzwSuFykl4K34G8khJqQbgIXpbWBFOOQBkld6OA4Rg6tySAheFsftsZh/a4J9JAieHNgqEr/CFzqc+l8ppXyX1Hg+fLOuiZ6VtD56RG8icX9JgZY5x/eHUG5lhif+tDF24oIPE/AE8GY8qto94TCbG/ltvF5Lx/0FnoTFNMuEww+4R2GG/HokESQMEMu2M8TjhIcZ0txZeleblD+iChbRJuA5dYPKHpLoJP1bhkSTObTKGQtnV2qSgk93xGhaFcRCSU/IRwhSvZsiOh9sGd8yr4Oyra0ji6t41csA4QMCiC8YuVyU9iF2Jf2PZope6RsB3IZPqKJCfWxh2MYPpfSZX3DKpvzF6J0LzwAbihmonKF3zBOxDgUu4uMbNZchdj5yUUvQaZ2viZriWX02aFNT/GGOEJvIHwLEg+JUSYXZU900DAEhZAG73WHOWebQcs2WzDKWAqZbsuvX5ZfvhUiLxP9Hgc3gKbkTqBpghY2tYHYtP+VbKJdQL9nXVNDNn+IkZ2wNVVsrUtjm09lTwX3lQEJDmKaVlZ+zQQVFox6mALN8NscD0zt0AR2wVUoYSiepVvsml2Sh9JudtNuzFxkz/tmsvnjRDy6YncqK+A2YOnJytComEoo+YpXmN5kjAXWFWOsru7MOEG28DD7bBcrPfmkADxXnUpmZCM7VdNLdOuwV1ng0PCznUrXb40dY2x2rJtlRzmNj8HzYBmD0mOq5VmRAMerNMm3XFuTgFmopWOIo1+vmrw86r9X04BqX4EWTlD8zQ/iqx7+LayBx0uz1vUcMQ9UOKXRHBiG6ZWM2cjj9N4Wm/cwmAtTP1dGNl4hpYjEucTQzCMonWpvc5jYosYC/bEePIx5I/fgWolpJTb9awy7cSNwJEopE8zZZcPUYw+wtT8j7G5CtLi7nyabVRg4L5vYoQGO7+6vxng7ZvnATpgO5SMnB7XbqGgegNYTTWWCfVk0LYXM6vvNosD5nCPJhQuFwqtXO2YjDzE0tdwxBit3/sPvO2WiWfUdY6x8QCrTB05+vYQpGp9SvfEC47EF0qHeyZttddX/mXH3Q0SmwVVPmI/lCUBNII56wnJBKSYx2r9gcgMhu+kMrSF7MG9oe0M1TmXxAfLG6KnVx27ayV/MlpOjc9YlIdGmmmYKLzuz3JCvGgrerAZHXHml2lDfQpvaFFQ+Ur9iXXs6r/bb9dtcqKgpDsP2EqXv+HreWM6W6uD1OkV00nSxnmWPen4/sOYrCd4hRbKqycLT3aJwO/77PnM7FsbYZMyWOyt6wTDwMhcK0ZoOFcxavtIcLJjNTzgLRo//xpgZO1CrnAzEpAXEJnhW/tX6irCr/Bv1lnlH50yM7BRRa/kbJlwxVxvgbeZqRtkO5mLN8v95eJgp/2kKPP8D</diagram></mxfile>"><defs/><g transform="translate(0.5,0.5)"><rect x="365" y="0" width="150" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(385.5,8.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="108" 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: 109px; 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;">armarx::Component</div></div></foreignObject><text x="54" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">armarx::Component</text></switch></g><path d="M 440 420 L 440 400 L 440 410 L 440 397.12" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 440 381.12 L 448 397.12 L 432 397.12 Z" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><rect x="390" y="420" width="100" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(400.5,428.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="78" 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: 79px; 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;"><ROBOT>Unit</div></div></foreignObject><text x="39" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica"><ROBOT>Unit</text></switch></g><path d="M 415 350 L 415 310 L 100 310 L 100 237.12" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 100 221.12 L 108 237.12 L 92 237.12 Z" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 465 350 L 465 310 L 780 310 L 780 297.12" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 780 281.12 L 788 297.12 L 772 297.12 Z" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><rect x="390" y="350" width="100" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(412.5,358.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="55" 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: 56px; 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;">RobotUnit</div></div></foreignObject><text x="28" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">RobotUnit</text></switch></g><path d="M 780 190 L 780 150 L 503 150 L 503 127.12" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 503 111.12 L 511 127.12 L 495 127.12 Z" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 100 190 L 100 150 L 378 150 L 378 127.12" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 378 111.12 L 386 127.12 L 370 127.12 Z" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 200 205 L 312.76 205" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 297.88 210.33 L 313.88 205 L 297.88 199.67" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(239.5,199.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="36" 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;"> friend </div></div></foreignObject><text x="18" y="11" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 150 220 L 170 220 L 170 235 L 312.76 235" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="1 4" pointer-events="none"/><path d="M 304.88 238 L 313.88 235 L 304.88 232" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(209.5,229.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="31" 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;"> calls </div></div></foreignObject><text x="16" y="11" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><path d="M 150 220 L 140 220 L 140 265 L 677.76 265" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="1 4" pointer-events="none"/><path d="M 669.88 268 L 678.88 265 L 669.88 262" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(298.5,259.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="168" 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;"> calls _moduleA().publicFunction()</div></div></foreignObject><text x="84" y="11" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica"> calls _moduleA().publicFunction()</text></switch></g><rect x="0" y="190" width="200" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(22.5,198.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="154" 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: 155px; 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;">armarx::RobotUnitModule::A</div></div></foreignObject><text x="77" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">armarx::RobotUnitModule::A</text></switch></g><path d="M 440 80 L 440 47.12" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 440 31.12 L 448 47.12 L 432 47.12 Z" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><rect x="315" y="80" width="250" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(332.5,88.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="215" 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: 216px; 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;">armarx::RobotUnitModule::ModuleBase</div></div></foreignObject><text x="108" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">armarx::RobotUnitModule::ModuleBase</text></switch></g><path d="M 565 205 L 677.76 205" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 662.88 210.33 L 678.88 205 L 662.88 199.67" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(604.5,199.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="36" 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;"> friend </div></div></foreignObject><text x="18" y="11" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="315" y="190" width="250" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(327.5,198.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="225" 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: 226px; 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;">armarx::RobotUnitModule::BAttorneyForA</div></div></foreignObject><text x="113" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">armarx::RobotUnitModule::BAttorneyForA</text></switch></g><path d="M 565 235 L 650 235 L 677.76 235" fill="none" stroke="#000000" stroke-miterlimit="10" stroke-dasharray="1 4" pointer-events="none"/><path d="M 669.88 238 L 678.88 235 L 669.88 232" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><g transform="translate(606.5,229.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="31" 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;"> calls </div></div></foreignObject><text x="16" y="11" fill="#000000" text-anchor="middle" font-size="11px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="315" y="220" width="250" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(317.5,228.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="174" 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: 175px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">- PrivateFunctionForA(A&) : void</div></div></foreignObject><text x="87" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">- PrivateFunctionForA(A&) : void</text></switch></g><rect x="680" y="190" width="200" height="30" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(702.5,198.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="154" 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: 155px; 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;">armarx::RobotUnitModule::B</div></div></foreignObject><text x="77" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">armarx::RobotUnitModule::B</text></switch></g><rect x="680" y="220" width="200" height="60" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(682.5,229.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="157" 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: 158px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">- privateFunctionForA() : void<br />- privateFunction() : void<br />+ publicFunction() : void</div></div></foreignObject><text x="79" y="27" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g></g></svg>
\ No newline at end of file
diff --git a/etc/doxygen/images/RobotModuleInteraction.xml b/etc/doxygen/images/RobotModuleInteraction.xml
new file mode 100644
index 000000000..c38e6c1f4
--- /dev/null
+++ b/etc/doxygen/images/RobotModuleInteraction.xml
@@ -0,0 +1 @@
+<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:59.0) Gecko/20100101 Firefox/59.0" version="8.5.12" editor="www.draw.io" type="device"><diagram id="aa7b51f2-7f26-c434-b112-75c9c6c67d3e" name="Page-1">7Zrdc6M2EMD/Gs8kD/EAwuA82mlyfejNZS7X6d1TRwYZ6AFihBzb/esrkMSXiA22MW6nnkyCVh8s2t+udnEm4CnafSIw8T9jF4UTQ3N3E/DLxMg+OvuTSfZcYgGbCzwSuFykl4K34G8khJqQbgIXpbWBFOOQBkld6OA4Rg6tySAheFsftsZh/a4J9JAieHNgqEr/CFzqc+l8ppXyX1Hg+fLOuiZ6VtD56RG8icX9JgZY5x/eHUG5lhif+tDF24oIPE/AE8GY8qto94TCbG/ltvF5Lx/0FnoTFNMuEww+4R2GG/HokESQMEMu2M8TjhIcZ0txZeleblD+iChbRJuA5dYPKHpLoJP1bhkSTObTKGQtnV2qSgk93xGhaFcRCSU/IRwhSvZsiOh9sGd8yr4Oyra0ji6t41csA4QMCiC8YuVyU9iF2Jf2PZope6RsB3IZPqKJCfWxh2MYPpfSZX3DKpvzF6J0LzwAbihmonKF3zBOxDgUu4uMbNZchdj5yUUvQaZ2viZriWX02aFNT/GGOEJvIHwLEg+JUSYXZU900DAEhZAG73WHOWebQcs2WzDKWAqZbsuvX5ZfvhUiLxP9Hgc3gKbkTqBpghY2tYHYtP+VbKJdQL9nXVNDNn+IkZ2wNVVsrUtjm09lTwX3lQEJDmKaVlZ+zQQVFox6mALN8NscD0zt0AR2wVUoYSiepVvsml2Sh9JudtNuzFxkz/tmsvnjRDy6YncqK+A2YOnJytComEoo+YpXmN5kjAXWFWOsru7MOEG28DD7bBcrPfmkADxXnUpmZCM7VdNLdOuwV1ng0PCznUrXb40dY2x2rJtlRzmNj8HzYBmD0mOq5VmRAMerNMm3XFuTgFmopWOIo1+vmrw86r9X04BqX4EWTlD8zQ/iqx7+LayBx0uz1vUcMQ9UOKXRHBiG6ZWM2cjj9N4Wm/cwmAtTP1dGNl4hpYjEucTQzCMonWpvc5jYosYC/bEePIx5I/fgWolpJTb9awy7cSNwJEopE8zZZcPUYw+wtT8j7G5CtLi7nyabVRg4L5vYoQGO7+6vxng7ZvnATpgO5SMnB7XbqGgegNYTTWWCfVk0LYXM6vvNosD5nCPJhQuFwqtXO2YjDzE0tdwxBit3/sPvO2WiWfUdY6x8QCrTB05+vYQpGp9SvfEC47EF0qHeyZttddX/mXH3Q0SmwVVPmI/lCUBNII56wnJBKSYx2r9gcgMhu+kMrSF7MG9oe0M1TmXxAfLG6KnVx27ayV/MlpOjc9YlIdGmmmYKLzuz3JCvGgrerAZHXHml2lDfQpvaFFQ+Ur9iXXs6r/bb9dtcqKgpDsP2EqXv+HreWM6W6uD1OkV00nSxnmWPen4/sOYrCd4hRbKqycLT3aJwO/77PnM7FsbYZMyWOyt6wTDwMhcK0ZoOFcxavtIcLJjNTzgLRo//xpgZO1CrnAzEpAXEJnhW/tX6irCr/Bv1lnlH50yM7BRRa/kbJlwxVxvgbeZqRtkO5mLN8v95eJgp/2kKPP8D</diagram></mxfile>
\ No newline at end of file
diff --git a/etc/doxygen/images/RobotUnitLifeCycle.svg b/etc/doxygen/images/RobotUnitLifeCycle.svg
new file mode 100644
index 000000000..b15480d07
--- /dev/null
+++ b/etc/doxygen/images/RobotUnitLifeCycle.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="640px" height="1201px" version="1.1" content="<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:59.0) Gecko/20100101 Firefox/59.0" version="8.5.12" editor="www.draw.io" type="device"><diagram id="4786f273-7f41-1df8-2594-ed1a9b5c68ee" name="Page-1">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</diagram></mxfile>"><defs/><g transform="translate(0.5,0.5)"><rect x="98.5" y="769.5" width="250" height="130" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><rect x="98.5" y="789.5" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(147.5,798.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="151" 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: 152px; 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;">_preOnConnectComponent</div></div></foreignObject><text x="76" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_preOnConnectComponent</text></switch></g><rect x="98.5" y="819.5" width="250" height="30" fill="#d5e8d4" stroke="#82b366" pointer-events="none"/><g transform="translate(165.5,828.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="115" 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: 116px; 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;">onConnectRobotUnit</div></div></foreignObject><text x="58" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">onConnectRobotUnit</text></switch></g><rect x="98.5" y="849.5" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(145.5,858.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="156" 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: 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;">_postOnConnectComponent</div></div></foreignObject><text x="78" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_postOnConnectComponent</text></switch></g><rect x="98.5" y="769.5" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(101.5,773.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="152" 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: 153px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">>>> <b>onConnectComponent</b></div></div></foreignObject><text x="76" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">>>> <b>onConnectComponent</b></text></switch></g><rect x="98.5" y="879.5" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(101.5,883.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="152" 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: 153px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><<< <b>onConnectComponent</b></div></div></foreignObject><text x="76" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica"><<< <b>onConnectComponent</b></text></switch></g><rect x="98.5" y="299.71" width="250" height="130" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><rect x="98.5" y="299.71" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(101.5,303.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="169" 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: 170px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><div align="left">>>> <b>onDisconnectComponent</b></div></div></div></foreignObject><text x="85" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica"><div align="left">>>> <b>onDisconnectComponent</b></div></text></switch></g><rect x="98.5" y="409.71" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(101.5,413.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="169" 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: 170px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><<< <b>onDisconnectComponent</b></div></div></foreignObject><text x="85" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica"><<< <b>onDisconnectComponent</b></text></switch></g><rect x="98.5" y="319.5" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(140.5,328.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="166" 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: 167px; 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;">_preOnDisconnectComponent</div></div></foreignObject><text x="83" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_preOnDisconnectComponent</text></switch></g><rect x="98.5" y="349.5" width="250" height="30" fill="#d5e8d4" stroke="#82b366" pointer-events="none"/><g transform="translate(158.5,358.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="130" 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: 131px; 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;">onDisconnectRobotUnit</div></div></foreignObject><text x="65" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">onDisconnectRobotUnit</text></switch></g><rect x="98.5" y="379.5" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(137.5,388.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="171" 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: 172px; 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;">_postOnDisconnectComponent</div></div></foreignObject><text x="86" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_postOnDisconnectComponent</text></switch></g><path d="M 224 430 L 224 763.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 224 768.88 L 220.5 761.88 L 224 763.63 L 227.5 761.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 161 900 L 161 915 L 80 915 L 80 280 L 161 280 L 161 293.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 161 298.88 L 157.5 291.88 L 161 293.63 L 164.5 291.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 286 900 L 286 920 L 307 920 L 307 933.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 307 938.88 L 303.5 931.88 L 307 933.63 L 310.5 931.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 306 260 L 306 280 L 286 280 L 286 293.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 286 298.88 L 282.5 291.88 L 286 293.63 L 289.5 291.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><rect x="388.5" y="300" width="250" height="30" fill="#d5e8d4" stroke="#82b366" pointer-events="none"/><g transform="translate(454.5,308.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="117" 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: 118px; 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;">Implementation Code</div></div></foreignObject><text x="59" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Implementation Code</text></switch></g><path d="M 431 260 L 431 280 L 451 280 L 451 293.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 451 298.88 L 447.5 291.88 L 451 293.63 L 454.5 291.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><rect x="389" y="490" width="250" height="30" fill="#d5e8d4" stroke="#82b366" pointer-events="none"/><g transform="translate(455.5,498.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="117" 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: 118px; 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;">Implementation Code</div></div></foreignObject><text x="59" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Implementation Code</text></switch></g><rect x="389" y="679.5" width="250" height="30" fill="#d5e8d4" stroke="#82b366" pointer-events="none"/><g transform="translate(455.5,688.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="117" 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: 118px; 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;">Implementation Code</div></div></foreignObject><text x="59" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Implementation Code</text></switch></g><rect x="388.5" y="869.5" width="250" height="30" fill="#d5e8d4" stroke="#82b366" pointer-events="none"/><g transform="translate(454.5,878.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="117" 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: 118px; 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;">Implementation Code</div></div></foreignObject><text x="59" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Implementation Code</text></switch></g><rect x="243.5" y="20" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(312.5,28.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="112" 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: 113px; 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;">_preOnInitRobotUnit</div></div></foreignObject><text x="56" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_preOnInitRobotUnit</text></switch></g><rect x="243.5" y="50" width="250" height="30" fill="#d5e8d4" stroke="#82b366" pointer-events="none"/><g transform="translate(325.5,58.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; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">onInitRobotUnit</div></div></foreignObject><text x="43" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">onInitRobotUnit</text></switch></g><rect x="243.5" y="80" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(309.5,88.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="117" 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: 118px; 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;">_postOnInitRobotUnit</div></div></foreignObject><text x="59" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_postOnInitRobotUnit</text></switch></g><rect x="243.5" y="130" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(274.5,138.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="187" 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: 188px; 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;">_preFinishComponentInitialization</div></div></foreignObject><text x="94" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_preFinishComponentInitialization</text></switch></g><rect x="243.5" y="190" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(272.5,198.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="192" 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: 193px; 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;">_postFinishComponentInitialization</div></div></foreignObject><text x="96" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_postFinishComponentInitialization</text></switch></g><rect x="243.5" y="160" width="250" height="30" fill="#ffe6cc" stroke="#d79b00" pointer-events="none"/><g transform="translate(250.5,168.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="236" 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: 237px; 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;">InitializingComponent → InitializingDevices</div></div></foreignObject><text x="118" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">InitializingComponent → InitializingDevices</text></switch></g><rect x="243.5" y="0" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(246.5,3.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="121" 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: 122px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">>>> <b>onInitComponent</b></div></div></foreignObject><text x="61" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">>>> <b>onInitComponent</b></text></switch></g><rect x="243.5" y="240" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(246.5,243.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="121" 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: 122px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><<< <b>onInitComponent</b></div></div></foreignObject><text x="61" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica"><<< <b>onInitComponent</b></text></switch></g><rect x="243.5" y="110" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(246.5,113.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="188" 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: 189px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">>>> <b>finishComponentInitialization</b></div></div></foreignObject><text x="94" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">>>> <b>finishComponentInitialization</b></text></switch></g><rect x="243.5" y="220" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(246.5,223.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="188" 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: 189px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><<< <b>finishComponentInitialization</b></div></div></foreignObject><text x="94" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica"><<< <b>finishComponentInitialization</b></text></switch></g><rect x="389" y="555" width="250" height="90" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><rect x="389" y="555" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(441.5,563.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="145" 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: 146px; 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;">_preFinishUnitInitialization</div></div></foreignObject><text x="73" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_preFinishUnitInitialization</text></switch></g><rect x="389" y="615" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(438.5,623.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="150" 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: 151px; 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;">_postFinishUnitInitialization</div></div></foreignObject><text x="75" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_postFinishUnitInitialization</text></switch></g><rect x="389" y="585" width="250" height="30" fill="#ffe6cc" stroke="#d79b00" pointer-events="none"/><g transform="translate(396.5,593.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="235" 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: 236px; 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;">InitializingUnits → InitializingControlThread</div></div></foreignObject><text x="118" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">InitializingUnits → InitializingControlThread</text></switch></g><rect x="389" y="535.21" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(391.5,538.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="145" 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: 146px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><div align="left">>>> <b>finishUnitInitialization</b></div></div></div></foreignObject><text x="73" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="389" y="645.21" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(391.5,648.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="145" 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: 146px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><<< <b>finishUnitInitialization</b></div></div></foreignObject><text x="73" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica"><<< <b>finishUnitInitialization</b></text></switch></g><path d="M 514 520 L 514 528.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 514 533.88 L 510.5 526.88 L 514 528.63 L 517.5 526.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><rect x="389" y="365" width="250" height="90" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><rect x="389" y="365" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(433.5,373.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="161" 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: 162px; 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;">_preFinishDeviceInitialization</div></div></foreignObject><text x="81" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_preFinishDeviceInitialization</text></switch></g><rect x="389" y="425" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(430.5,433.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="166" 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: 167px; 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;">_postFinishDeviceInitialization</div></div></foreignObject><text x="83" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_postFinishDeviceInitialization</text></switch></g><rect x="389" y="395" width="250" height="30" fill="#ffe6cc" stroke="#d79b00" pointer-events="none"/><g transform="translate(413.5,403.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="200" 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: 201px; 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;">InitializingDevices → InitializingUnits</div></div></foreignObject><text x="100" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">InitializingDevices → InitializingUnits</text></switch></g><rect x="389" y="345.21" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(391.5,348.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="162" 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: 163px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><div align="left">>>> <b>finishDeviceInitialization</b></div></div></div></foreignObject><text x="81" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="389" y="455.21" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(391.5,458.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="162" 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: 163px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><<< <b>finishDeviceInitialization</b></div></div></foreignObject><text x="81" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica"><<< <b>finishDeviceInitialization</b></text></switch></g><path d="M 514 330 L 514 338.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 514 343.88 L 510.5 336.88 L 514 338.63 L 517.5 336.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><rect x="388.5" y="744.5" width="250" height="90" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><rect x="388.5" y="744.5" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(412.5,753.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="202" 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: 203px; 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;">_preFinishControlThreadInitialization</div></div></foreignObject><text x="101" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_preFinishControlThreadInitialization</text></switch></g><rect x="388.5" y="804.5" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(409.5,813.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="207" 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: 208px; 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;">_postFinishControlThreadInitialization</div></div></foreignObject><text x="104" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_postFinishControlThreadInitialization</text></switch></g><rect x="388.5" y="774.5" width="250" height="30" fill="#ffe6cc" stroke="#d79b00" pointer-events="none"/><g transform="translate(413.5,783.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="199" 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: 200px; 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;">InitializingControlThread → Running</div></div></foreignObject><text x="100" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">InitializingControlThread → Running</text></switch></g><rect x="388.5" y="724.71" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(391.5,728.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="204" 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: 205px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><div align="left">>>> <b>finishControlThreadInitialization</b></div></div></div></foreignObject><text x="102" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica"><div align="left">>>> <b>finishControlThreadInitialization</b></div></text></switch></g><rect x="388.5" y="834.71" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(391.5,838.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="204" 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: 205px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><<< <b>finishControlThreadInitialization</b></div></div></foreignObject><text x="102" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica"><<< <b>finishControlThreadInitialization</b></text></switch></g><rect x="244" y="959.71" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(246.5,963.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="102" 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: 103px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><div align="left">>>> <b>finishRunning</b></div></div></div></foreignObject><text x="51" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">[Not supported by viewer]</text></switch></g><rect x="244" y="1069.71" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(246.5,1073.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="102" 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: 103px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><<< <b>finishRunning</b></div></div></foreignObject><text x="51" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica"><<< <b>finishRunning</b></text></switch></g><rect x="244" y="979.5" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(316.5,988.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="105" 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: 106px; 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;">_preFinishRunning</div></div></foreignObject><text x="53" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_preFinishRunning</text></switch></g><rect x="244" y="1039.5" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(313.5,1048.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="110" 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: 111px; 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;">_postFinishRunning</div></div></foreignObject><text x="55" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_postFinishRunning</text></switch></g><rect x="244" y="1089.5" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(306.5,1098.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="125" 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: 126px; 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;">_preOnExitComponent</div></div></foreignObject><text x="63" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_preOnExitComponent</text></switch></g><rect x="244" y="1119.5" width="250" height="30" fill="#d5e8d4" stroke="#82b366" pointer-events="none"/><g transform="translate(324.5,1128.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="89" 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: 90px; 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;">onExitRobotUnit</div></div></foreignObject><text x="45" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">onExitRobotUnit</text></switch></g><rect x="244" y="1149.5" width="250" height="30" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(303.5,1158.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="130" 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: 131px; 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;">_postOnExitComponent</div></div></foreignObject><text x="65" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">_postOnExitComponent</text></switch></g><rect x="244" y="1009.5" width="250" height="30" fill="#ffe6cc" stroke="#d79b00" pointer-events="none"/><g transform="translate(317.5,1018.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="102" 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: 103px; 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;">Running → Exiting</div></div></foreignObject><text x="51" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Running → Exiting</text></switch></g><rect x="244" y="939.5" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(246.5,943.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="126" 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: 127px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">>>> <b>onExitComponent</b></div></div></foreignObject><text x="63" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">>>> <b>onExitComponent</b></text></switch></g><rect x="244" y="1179.5" width="250" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(246.5,1183.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="126" 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: 127px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><<< <b>onExitComponent</b></div></div></foreignObject><text x="63" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica"><<< <b>onExitComponent</b></text></switch></g><path d="M 514 710 L 514 718.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 514 723.88 L 510.5 716.88 L 514 718.63 L 517.5 716.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 514 475 L 514 483.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 514 488.88 L 510.5 481.88 L 514 483.63 L 517.5 481.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 514 665 L 514 673.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 514 678.88 L 510.5 671.88 L 514 673.63 L 517.5 671.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 451 900 L 451 920 L 432 920 L 432 933.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 432 938.88 L 428.5 931.88 L 432 933.63 L 435.5 931.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 514 855 L 514 863.63" fill="none" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><path d="M 514 868.88 L 510.5 861.88 L 514 863.63 L 517.5 861.88 Z" fill="#000000" stroke="#000000" stroke-miterlimit="10" pointer-events="none"/><rect x="0" y="60" width="210" height="20" fill="#d5e8d4" stroke="#82b366" pointer-events="none"/><g transform="translate(2.5,63.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="202" 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: 203px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Implemented by a deriving RobotUnit</div></div></foreignObject><text x="101" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Implemented by a deriving RobotUnit</text></switch></g><rect x="0" y="40" width="210" height="20" fill="#ffe6cc" stroke="#d79b00" pointer-events="none"/><g transform="translate(2.5,43.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="146" 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: 147px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">RobotUnit Phase transition</div></div></foreignObject><text x="73" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">RobotUnit Phase transition</text></switch></g><rect x="0" y="20" width="210" height="20" fill="#f5f5f5" stroke="#666666" pointer-events="none"/><g transform="translate(2.5,23.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="167" 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: 168px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Begin/End marker for functions</div></div></foreignObject><text x="84" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Begin/End marker for functions</text></switch></g><rect x="0" y="80" width="210" height="20" fill="#dae8fc" stroke="#6c8ebf" pointer-events="none"/><g transform="translate(2.5,83.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="151" 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: 152px; white-space: nowrap; overflow-wrap: normal;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;">Hook for RobotUnitModules</div></div></foreignObject><text x="76" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica">Hook for RobotUnitModules</text></switch></g><rect x="0" y="0" width="210" height="20" fill="#ffffff" stroke="#000000" pointer-events="none"/><g transform="translate(2.5,3.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;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;"><b>Legend</b></div></div></foreignObject><text x="21" y="12" fill="#000000" text-anchor="middle" font-size="12px" font-family="Helvetica"><b>Legend</b></text></switch></g></g></svg>
\ No newline at end of file
diff --git a/etc/doxygen/images/RobotUnitLifeCycle.xml b/etc/doxygen/images/RobotUnitLifeCycle.xml
new file mode 100644
index 000000000..19d571000
--- /dev/null
+++ b/etc/doxygen/images/RobotUnitLifeCycle.xml
@@ -0,0 +1 @@
+<mxfile userAgent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:59.0) Gecko/20100101 Firefox/59.0" version="8.5.12" editor="www.draw.io" type="device"><diagram id="4786f273-7f41-1df8-2594-ed1a9b5c68ee" name="Page-1">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</diagram></mxfile>
\ No newline at end of file
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
index ac7b028fe..225baa06b 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
@@ -84,28 +84,87 @@ namespace armarx
/**
* @ingroup Library-RobotUnit
- * @brief A high level controller writing its results into targes.
+ * @brief A high level controller writing its results into \ref ControlTargetBase "ControlTargets".
*
* This is the abstract base class for all NJointControllers.
* It implements basic management routines required by the RobotUnit and some basic ice calls.
- * NJointControllers are intantiated and managed by RobotUnits.
+ * \ref NJointController "NJointControllers" are instantiated and managed by the \ref RobotUnit.
*
- * Each NJointController 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.
+ * A \ref NJointController calculates \ref ControlTargetBase "ControlTargets" for a set of
+ * \ref ControlDevice "ControlDevices" in a specific ControlMode.
+ * This ControlMode is defined for each \ref ControlDevice during construction.
*
- * Each controller has two parts:
+ * \section nj-state Requested and Active
+ * A \ref NJointController can is requested / not requested and active / inactive.
+ * All four combinations are possible.
+ *
+ * \subsection nj-state-req Requested / Not Requested
+ * If the user wants this \ref NJointController to be executed it is in a requested state (see \ref isControllerRequested).
+ * Otherwise the controler is not requested.
+ *
+ * Calling \ref activateController sets the state to requested.
+ * Calling \ref deactivateController sets the state to not requested.
+ * If the \ref NJointController causes an error or a different \ref NJointController using one or more of the same
+ * \ref ControlDevice's is requested, this \ref NJointController is deactivated.
+ *
+ *
+ * \subsection nj-state-act Active / Inactive
+ * Is the \ref NJointController executed by the \ref RobotUnitModules::ControlThread "ControlThread" it is active
+ * (see \ref isControllerActive).
+ *
+ * To be executed by the \ref RobotUnitModules::ControlThread "ControlThread", the \ref NJointController has to be
+ * requested at some point in the past.
+ *
+ * If a controller is active, it has to write a valid \ref ControlTargetBase "ControlTarget"
+ * for each of its \ref ControlDevice "ControlDevicea" (if it fails, it will be deactivated).
+ *
+ * \section Constructor
+ * In the Constructor, a \ref NJointController has to declare all \ref ControlDevice "ControlDevices" it uses.
+ *
+ * The constructor takes a pointer to a configuration structure of the type \ref NJointController::ConfigPtrT.
+ * If an implementation requires a special configuration structure (e.g.: SomeOtherCfg), it has to override this type by calling adding
+ * \code{.cpp}
+ * using ConfigPtrT = SomeOtherCfgPtr;
+ * \endcode
+ * to it's clas definition.
+ * The type SomeOtherCfg has to derive \ref NJointControllerConfigPtr.
+ *
+ * There is a way to generate a small gui widget for controller construction (see this \ref nj-ctor-gui "section").
+ *
+ * \subsection nj-ctor-req-ctrl Using ControlTargets
+ * A \ref NJointController can use \ref peekControlDevice to examine a \ref ControlDevice before using it
+ * (e.g.: checking the supported \ref ControlTargetBase "ControlTargets").
+ *
+ * If a \ref ControlDevice should be used by this \ref NJointController, it has to call \ref useControlDevice.
+ * This sets the ControlMode for the \ref ControlDevice and returns a pointer to the \ref ControlTargetBase "ControlTarget".
+ * This pointer has to be used to send commands in each iteration of \ref rtRun.
+ * The ControlMode can't be changed afterwards (A new \ref NJointController has to be created).
+ *
+ *
+ * \subsection nj-ctor-req-sens Using SensorValues
+ * A \ref NJointController can use \ref peekSensorDevice to examine a \ref SensorDevice before using it.
+ *
+ * If a \ref SensorDevice should be used by this \ref NJointController, it has to call \ref useSensorDevice.
+ * This returns a pointer to the \ref SensorValueBase "SensorValue".
+ *
+ * \subsection nj-ctor-rob A synchronized Virtualrobot
+ * If the controller needs a simox robot in \ref rtRun, it should call \ref useSynchronizedRtRobot.
+ * This will provide a simoxRobot via \ref rtGetRobot.
+ * This robot is synchronized with the real robots state before calling \ref rtRun
+ *
+ * \section nj-parts Rt and non Rt
+ * Each \ref NJointController 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.
+ * \subsection rtcontrollloop The Rt controll loop (\ref rtRun)
* \warning This part has to satisfy realtime conditions!
- * All realtime able functions of this class have the 'rt' prefix.
+ * All realtime functions of \ref NJointController have the 'rt' prefix.
*
- * You must not:
+ * Here the \ref NJointController has access to the robot's current state
+ * and has to write results into \ref ControlTargetBase "ControlTargets".
+ *
+ * It 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)
@@ -114,24 +173,19 @@ namespace armarx
* \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
+ * \subsection nonrtpart The NonRT ice communication
* This part consits of any ice communication.
- * Here you could get new controll parameters or targets from other components.
+ * Here the \ref NJointController can 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::NJointControllerWithTripleBuffer).
+ * The \ref NJointController has to use constructs like atomics or
+ * \ref TripleBuffer "TripleBuffers" (See \ref armarx::NJointControllerWithTripleBuffer).
*
* \image html NJointControllerGeneralDataFlow.svg "General Dataflow in a NJointController"
*
- *
- *
- *
* \image html NJointControllerAtomicDataFlow.svg "Dataflow in a NJointController using atomics for communication between RT and NonRT"
*
- *
- *
- *
* \image html NJointControllerTripleBufferDataFlow.svg "Dataflow in a NJointController using a triple buffer for communication between RT and NonRT"
*
* \section expensivework How to do expensive calculations
@@ -148,6 +202,348 @@ namespace armarx
*
* \image html NJointControllerWorkerThreadDataFlow.svg "Dataflow in a NJointController using a worker thread"
*
+ * \section nj-ctor-gui Providing a gui for controller construction.
+ * By implementing these functions:
+ * \code{.cpp}
+ static WidgetDescription::WidgetPtr GenerateConfigDescription
+ (
+ const boost::shared_ptr<VirtualRobot::Robot>& robot,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
+ ); //describes how the widget is supposed to look
+ static ConfigPtrT GenerateConfigFromVariants(const StringVariantBaseMap&); // turns the resulting variants into a config
+ * \endcode
+ *
+ * The \ref RobotUnitGui will provide a widget to configure and construct a \ref NJointController of this type.
+ *
+ * \section Examples
+ *
+ * \subsection nj-example-1 A simple pass Position controller
+ * \note The code can be found in the Tutorial package
+ *
+ * \subsection nj-example-1-h Header
+ * Include headers
+ * \code{.cpp}
+ #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+ #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+ #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+ * \endcode
+ *
+ * Open the namespace
+ * \code{.cpp}
+ namespace armarx
+ {
+ * \endcode
+ *
+ * Typedef some pointers
+ * \code{.cpp}
+ TYPEDEF_PTRS_HANDLE(NJointPositionPassThroughController);
+ TYPEDEF_PTRS_HANDLE(NJointPositionPassThroughControllerConfig);
+ * \endcode
+ *
+ * The config has to inherit \ref NJointControllerConfig and consists only of the joint name
+ * \code{.cpp}
+ class NJointPositionPassThroughControllerConfig : virtual public NJointControllerConfig
+ {
+ public:
+ NJointPositionPassThroughControllerConfig(const std::string& name): deviceName {name} {}
+ std::string deviceName;
+ };
+ * \endcode
+ *
+ * The controller class has to inherit \ref NJointController
+ * \code{.cpp}
+ class NJointPositionPassThroughController: public NJointController
+ {
+ public:
+ * \endcode
+ *
+ * The \ref NJointController provides a config widget for the \ref RobotUnitGuiPlugin
+ * \code{.cpp}
+ static WidgetDescription::WidgetPtr GenerateConfigDescription
+ (
+ const boost::shared_ptr<VirtualRobot::Robot>&,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
+ );
+ static NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
+ * \endcode
+ *
+ * The ctor receives a pointer to the \ref RobotUnit, a pointer to the config and a pointer to a VirtualRobot model.
+ * \code{.cpp}
+ NJointPositionPassThroughController(
+ const RobotUnitPtr& prov,
+ NJointControllerConfigPtr config,
+ const boost::shared_ptr<VirtualRobot::Robot>& r
+ );
+ * \endcode
+ *
+ * The function to provide the class name.
+ * \code{.cpp}
+ std::string getClassName(const Ice::Current&) const override;
+ * \endcode
+ *
+ * This controller provides widgets for function calls.
+ * \code{.cpp}
+ WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name, const StringVariantBaseMap& value, const Ice::Current&) override;
+ * \endcode
+ *
+ * The run function executed to calculate new targets
+ * \code{.cpp}
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ * \endcode
+ *
+ * Hooks for this \ref NJointController to execute code during publishing.
+ * \code{.cpp}
+ void onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
+ void onPublishDeactivation(const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&) override;
+ void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&) override;
+ * \endcode
+ *
+ * This \ref NJointController uses one position \ref SensorValue1DoFActuatorPosition "SensorValue", calculates one
+ * position \ref ControlTarget1DoFActuatorPosition "ControlTarget" and stores the current position target and position value in atomics.
+ * \code{.cpp}
+ const SensorValue1DoFActuatorPosition* sensor;
+ ControlTarget1DoFActuatorPosition* target;
+ std::atomic<float> targetPos {0};
+ std::atomic<float> currentPos {0};
+ * \endcode
+ *
+ * Close the class and the namespace
+ * \code{.cpp}
+ };
+ }
+ * \endcode
+ *
+ * \subsection nj-example-1-c Source file
+ *
+ * Include the required headers.
+ * \code{.cpp}
+ #include "NJointPositionPassThroughController.h"
+ #include <RobotAPI/libraries/core/Pose.h>
+ * \endcode
+ *
+ * Open the namespace
+ * \code{.cpp}
+ namespace armarx
+ {
+ * \endcode
+ *
+ * This generates a config widget used to display a config gui in \ref RobotUnitGui.
+ * It consist out of a \ref HBoxLayout containing a \ref Label and a \ref StringComboBox
+ * \code{.cpp}
+ WidgetDescription::WidgetPtr NJointPositionPassThroughController::GenerateConfigDescription(const boost::shared_ptr<VirtualRobot::Robot>&, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>& sensorDevices)
+ {
+ using namespace armarx::WidgetDescription;
+ HBoxLayoutPtr layout = new HBoxLayout;
+ LabelPtr label = new Label;
+ label->text = "control device name";
+ layout->children.emplace_back(label);
+ StringComboBoxPtr box = new StringComboBox;
+ box->defaultIndex = 0;
+ * \endcode
+ *
+ * The \ref StringComboBox only contains the names of \ref ControlDevice "ControlDevices" accepting
+ * a position \ref ControlTarget1DoFActuatorPosition "ControlTarget" and providing a position
+ * \ref SensorValue1DoFActuatorPosition "SensorValue"
+ * \code{.cpp}
+ //filter control devices
+ for (const auto& name2dev : controlDevices)
+ {
+ const ConstControlDevicePtr& dev = name2dev.second;
+ const auto& name = name2dev.first;
+ if (
+ dev->hasJointController(ControlModes::Position1DoF) &&
+ dev->getJointController(ControlModes::Position1DoF)->getControlTarget()->isA<ControlTarget1DoFActuatorPosition>() &&
+ sensorDevices.count(name) &&
+ sensorDevices.at(name)->getSensorValue()->isA<SensorValue1DoFActuatorPosition>()
+ )
+ {
+ box->options.emplace_back(name);
+ }
+ }
+ box->name = "name";
+ layout->children.emplace_back(box);
+ return layout;
+ }
+ * \endcode
+ *
+ * This turns a map of variants into the config required by this \ref NJointController.
+ * The \ref Variant for the key 'name' defines the \ref ControlDevice.
+ * \code{.cpp}
+ NJointControllerConfigPtr NJointPositionPassThroughController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
+ {
+ //you should check here for types null additional params etc
+ return new NJointPositionPassThroughControllerConfig {values.at("name")->getString()};
+ }
+ * \endcode
+ *
+ * The constructors implementation.
+ * \code{.cpp}
+ NJointPositionPassThroughController::NJointPositionPassThroughController(
+ const RobotUnitPtr& ru,
+ NJointControllerConfigPtr config,
+ const boost::shared_ptr<VirtualRobot::Robot>&
+ )
+ {
+ ARMARX_CHECK_EXPRESSION(ru);
+ * \endcode
+ *
+ * It checks whether the provided config has the correct type.
+ * \code{.cpp}
+ NJointPositionPassThroughControllerConfigPtr cfg = NJointPositionPassThroughControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "The provided config has the wrong type! The type is " << config->ice_id());
+ * \endcode
+ *
+ * Then it retrieves the \ref SensorValue1DoFActuatorPosition "SensorValue" and
+ * \ref ControlTarget1DoFActuatorPosition "ControlTarget", casts then to the required types and stores them to the member variables.
+ * \code{.cpp}
+ const SensorValueBase* sv = useSensorValue(cfg->deviceName);
+ ControlTargetBase* ct = useControlTarget(cfg->deviceName, ControlModes::Position1DoF);
+ ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorPosition>());
+ ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorPosition>());
+ sensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ target = ct->asA<ControlTarget1DoFActuatorPosition>();
+ }
+ * \endcode
+ *
+ * The class name has to be unique, hence the c++ class name should be used.
+ * \code{.cpp}
+ std::string NJointPositionPassThroughController::getClassName(const Ice::Current&) const
+ {
+ return "NJointPositionPassThroughController";
+ }
+
+ * \endcode
+ *
+ * This \ref NJointController provides two widgets for function calls.
+ * \code{.cpp}
+ WidgetDescription::StringWidgetDictionary NJointPositionPassThroughController::getFunctionDescriptions(const Ice::Current&) const
+ {
+ using namespace armarx::WidgetDescription;
+ * \endcode
+ *
+ * The first widget has a \ref Label and a \ref FloatSpinBox.
+ * \code{.cpp}
+ HBoxLayoutPtr layoutSetPos = new HBoxLayout;
+ {
+ LabelPtr label = new Label;
+ label->text = "positiontarget";
+ layoutSetPos->children.emplace_back(label);
+ FloatSpinBoxPtr spin = new FloatSpinBox;
+ spin->defaultValue = 0;
+ spin->max = 1;
+ spin->min = -1;
+ * \endcode
+ *
+ * The \ref FloatSpinBox creates a variant called 'spinmearound'
+ * \code{.cpp}
+ spin->name = "spinmearound";
+ layoutSetPos->children.emplace_back(spin);
+ }
+ * \endcode
+ *
+ * The second widget also has a \ref Label and a \ref FloatSpinBox.
+ * \code{.cpp}
+ VBoxLayoutPtr layoutSetHalfPos = new VBoxLayout;
+ {
+ LabelPtr label = new Label;
+ label->text = "positiontarget / 2";
+ layoutSetHalfPos->children.emplace_back(label);
+ FloatSpinBoxPtr spin = new FloatSpinBox;
+ spin->defaultValue = 0;
+ spin->max = 0.5;
+ spin->min = -0.5;
+ spin->name = "spinmehalfaround";
+ layoutSetHalfPos->children.emplace_back(spin);
+ }
+ * \endcode
+ *
+ * This returns a map of boths widgets.
+ * The keys will be used to identify the called function.
+ * \code{.cpp}
+ return {{"SetPosition", layoutSetPos}, {"SetPositionHalf", layoutSetHalfPos}};
+ }
+ * \endcode
+ *
+ * This function is called from the \ref RobotUnitGuiPlugin when the prior defined functions are called.
+ * Both functions set the target position.
+ * \code{.cpp}
+ void NJointPositionPassThroughController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& value, const Ice::Current&)
+ {
+ if (name == "SetPosition")
+ {
+ //you should check here for types null additional params etc
+ targetPos = value.at("spinmearound")->getFloat();
+ }
+ else if (name == "SetPositionHalf")
+ {
+ //you should check here for types null additional params etc
+ targetPos = value.at("spinmehalfaround")->getFloat() * 2;
+ }
+ else
+ {
+ ARMARX_ERROR << "CALLED UNKNOWN REMOTE FUNCTION: " << name;
+ }
+ }
+ * \endcode
+ *
+ * The \ref rtRun function sets the \ref ControlTarget1DoFActuatorPosition "ControlTarget" to the
+ * target value set via the atomic and stores the current
+ * \ref SensorValue1DoFActuatorPosition "SensorValue" to the other atomic.
+ * \code{.cpp}
+ void NJointPositionPassThroughController::rtRun(const IceUtil::Time& t, const IceUtil::Time&)
+ {
+ ARMARX_RT_LOGF_ERROR("A MESSAGE PARAMETER %f", t.toSecondsDouble()).deactivateSpam(1);
+ ARMARX_RT_LOGF_IMPORTANT("A MESSAGE WITHOUT PARAMETERS").deactivateSpam(1);
+ target->position = targetPos;
+ currentPos = sensor->position;
+ }
+ * \endcode
+ *
+ * The publish activation \ref onPublishActivation "hook" does nothing, but could be used to call
+ * \ref DebugDraver::setRobotVisu.
+ * \code{.cpp}
+ void NJointPositionPassThroughController::onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
+ {
+ //we could do some setup for the drawer here. e.g. add a robot
+ }
+
+ * \endcode
+ *
+ * The publish deactivation \ref onPublishDeactivation "hook" clears the \ref DebugDrawer layer
+ * \code{.cpp}
+ void NJointPositionPassThroughController::onPublishDeactivation(const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&)
+ {
+ drawer->removeLayer("Layer_" + getInstanceName());
+ }
+
+ * \endcode
+ *
+ * The publish \ref onPublish "hook" draws a sphere with radius of the position stored in the atomic times 2000 to the \ref DebugDrawer.
+ * \code{.cpp}
+ void NJointPositionPassThroughController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&)
+ {
+ drawer->setSphereVisu("Layer_" + getInstanceName(), "positionball", new Vector3, armarx::DrawColor {1, 1, 1, 1}, currentPos * 2000);
+ }
+ * \endcode
+ *
+ * This registers a factory for this \ref NJointController.
+ * This factory is used by the \ref RobotUnit to create the \ref NJointController.
+ * The passed name has to match the name returned by \ref getClassName.
+ * \code{.cpp}
+ NJointControllerRegistration<NJointPositionPassThroughController> registrationControllerNJointPositionPassThroughController("NJointPositionPassThroughController");
+ * \endcode
+ *
+ * This statically asserts a factory for the type NJointPositionPassThroughController is present.
+ * \code{.cpp}
+ ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(NJointPositionPassThroughController);
+ * \endcode
+ * Close the namespace
+ * \code{.cpp}
+ }
+ * \endcode
*/
class NJointController:
virtual public NJointControllerInterface,
--
GitLab