diff --git a/etc/doxygen/pages/5.Components.dox b/etc/doxygen/pages/5.Components.dox
index b076255b458833746ada7e9e9fc082d4b7f6ae26..023de80200bd3c6cbcadd9a163710562f4305b11 100644
--- a/etc/doxygen/pages/5.Components.dox
+++ b/etc/doxygen/pages/5.Components.dox
@@ -21,15 +21,26 @@ There are several types of special components:
     There has to be a specific observer implementation for each base unit.
 
 
+\defgroup RobotAPI-SensorActorUnits Sensor-Actor Unit
+\ingroup RobotAPI-Components
+\brief Components that provide interfaces to associated hardware devices.
+
+Sensor-Actor Units implement a corresponding Ice interface which makes them accessible for other running components.
+
+
 \defgroup RobotAPI-SensorActorUnits-simulation Sensor-Actor Unit Simulations
 \ingroup RobotAPI-Components
-Basic simulation implementations of the interfaces that do not consider any physical laws.
+\brief Simulated Sensor-Actor Units do not connected to any hardware.
+
 
 \defgroup RobotAPI-SensorActorUnits-observers Sensor-Actor Unit Observers
 \ingroup RobotAPI-Components
-Almost any Sensor-Actor Unit has an observer, that observes the sensor value of this unit and
+\brief Component monitoring sensor values reported from Sensor-Actor Units.
+
+Any Sensor-Actor Unit can have an observer, that monitors the reported sensor values of this unit and
 provides datafields for the sensor data. All unit observers implement the \ref armarx::Observer interface.
 
+
 \defgroup RobotAPI-SensorActorUnits-util Utilities
 \ingroup RobotAPI-Components
 Utility and helper classes associated with Sensor-Actor Units.
diff --git a/etc/doxygen/pages/SensorActorUnits.dox b/etc/doxygen/pages/SensorActorUnits.dox
deleted file mode 100644
index 147a15e14db7f05b27f84d086403d1c3705a2aa5..0000000000000000000000000000000000000000
--- a/etc/doxygen/pages/SensorActorUnits.dox
+++ /dev/null
@@ -1,10 +0,0 @@
-/**
-\defgroup RobotAPI-SensorActorUnits Sensor-Actor Unit
-\ingroup RobotAPI-Components
-
-\brief Sensor-Actor Units are components that provide an interface to an associated hardware device.
-
-Sensor-Actor Units implement a corresponding Ice interface which makes them accessible for other running components.
- */
-
-
diff --git a/source/RobotAPI/components/RobotIK/RobotIK.cpp b/source/RobotAPI/components/RobotIK/RobotIK.cpp
index 788d6e31c6ed10c96f3be8d0732af829ef1a654f..cdab9cff84f9da6f1b81614ddf267ec65a89ada5 100644
--- a/source/RobotAPI/components/RobotIK/RobotIK.cpp
+++ b/source/RobotAPI/components/RobotIK/RobotIK.cpp
@@ -527,7 +527,7 @@ namespace armarx
         GenericIKSolver ikSolver(nodeSet, JacobiProvider::eSVDDamped);
         bool success = ikSolver.solve(tcpPose, convertCartesianSelection(cs), _numIKTrials);
         // Read solution from node set
-        if (success)
+        if (true)
         {
             const std::vector<RobotNodePtr> robotNodes = nodeSet->getAllRobotNodes();
             BOOST_FOREACH(RobotNodePtr rnode, robotNodes)
@@ -538,7 +538,8 @@ namespace armarx
             ikSolution.isReachable = true;
             ikSolution.error = 0;
         }
-        else {
+        else
+        {
             ikSolution.isReachable = false;
             ikSolution.error = 0;
         }
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index dcf5ecabdbbf798ee1294240753c8ade61e3c148..540e657d73028dbb89f89b3494fd98e325a266fd 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -1,3 +1,26 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package    ArmarX::
+* @author     Mirko Waechter ( mirko.waechter at kit dot edu)
+* @date       2013
+* @copyright  http://www.gnu.org/licenses/gpl.txt
+*             GNU General Public License
+*/
+
 #include "ForceTorqueObserver.h"
 
 #include <Core/observers/checks/ConditionCheckUpdated.h>
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.h b/source/RobotAPI/components/units/ForceTorqueObserver.h
index 0272f731e05919d4ea83c28bde89816cb157c055..4554f0e98c69bd6a7ea13dd426ec50fdd4e1c701 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.h
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.h
@@ -1,3 +1,26 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package    ArmarX::
+* @author     Mirko Waechter ( mirko.waechter at kit dot edu)
+* @date       2013
+* @copyright  http://www.gnu.org/licenses/gpl.txt
+*             GNU General Public License
+*/
+
 #ifndef _ARMARX_ROBOTAPI_FORCETORQUEOBSERVER_H
 #define _ARMARX_ROBOTAPI_FORCETORQUEOBSERVER_H
 
@@ -7,7 +30,10 @@
 
 namespace armarx
 {
-
+    /**
+     * \class ForceTorqueObserverPropertyDefinitions
+     * \brief
+     */
     class ForceTorqueObserverPropertyDefinitions:
             public ComponentPropertyDefinitions
     {
@@ -20,10 +46,12 @@ namespace armarx
     };
 
     /**
-     * @class ForceTorqueObserver
-     * @ingroup RobotAPI-SensorActorUnits-observers
+     * \class ForceTorqueObserver
+     * \ingroup RobotAPI-SensorActorUnits-observers
+     * \brief Observer monitoring Force/Torque values
+     *
      * The ForceTorqueObserver monitors F/T values published by ForceTorqueUnit-implementations and offers condition checks on these values.
-     * Available condition checks are: updated, larger, equals, smaller, magnitudelarger.
+     * Available condition checks are: *updated*, *larger*, *equals*, *smaller* and *magnitudelarger*.
      */
     class ForceTorqueObserver :
             virtual public Observer,
diff --git a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
index 59c7382083e35a49f816047d0128828acf1435a4..c7594288e2b1a099d6711f17be936d0ab62e3d10 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
+++ b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
@@ -37,8 +37,8 @@
 namespace armarx
 {
     /**
-     * @class ForceTorqueUnitSimulationPropertyDefinitions
-     * @brief
+     * \class ForceTorqueUnitSimulationPropertyDefinitions
+     * \brief
      */
     class ForceTorqueUnitSimulationPropertyDefinitions :
             public ForceTorqueUnitPropertyDefinitions
@@ -53,11 +53,10 @@ namespace armarx
     };
 
     /**
-     * @class ForceTorqueUnitSimulation
-     * @brief
-     * @ingroup RobotAPI-SensorActorUnits-simulation
+     * \class ForceTorqueUnitSimulation
+     * \brief Simulates a set of Force/Torque sensors.
+     * \ingroup RobotAPI-SensorActorUnits-simulation
      *
-     * Simulates a set of Force/Torque sensors.
      * The unit is given a list of sensor names as a property. It then publishes force/torque values under these names.
      * The published values will always be zero.
      */
@@ -65,7 +64,6 @@ namespace armarx
         virtual public ForceTorqueUnit
     {
     public:
-        // inherited from Component
         virtual std::string getDefaultName() const
         {
             return "ForceTorqueUnitSimulation";
@@ -76,11 +74,19 @@ namespace armarx
         virtual void onExitForceTorqueUnit();
 
         void simulationFunction();
+
+        /**
+         * \warning Not implemented yet
+         */
         virtual void setOffset(const FramedDirectionBasePtr &forceOffsets, const FramedDirectionBasePtr &torqueOffsets, const Ice::Current& c = ::Ice::Current());
+
+        /**
+         * \warning Not implemented yet
+         */
         virtual void setToNull(const Ice::Current& c = ::Ice::Current());
 
         /**
-         * @see PropertyUser::createPropertyDefinitions()
+         * \see PropertyUser::createPropertyDefinitions()
          */
         virtual PropertyDefinitionsPtr createPropertyDefinitions();
 
@@ -89,8 +95,6 @@ namespace armarx
         armarx::FramedDirectionPtr torques;
 
         PeriodicTask<ForceTorqueUnitSimulation>::pointer_type simulationTask;
-
-
     };
 }
 
diff --git a/source/RobotAPI/components/units/HandUnitSimulation.h b/source/RobotAPI/components/units/HandUnitSimulation.h
index ea51a6a2199535d00d173b5554c2317704a81cb4..e37f66e510c7730a8bebeb5d91e4685527242c1b 100644
--- a/source/RobotAPI/components/units/HandUnitSimulation.h
+++ b/source/RobotAPI/components/units/HandUnitSimulation.h
@@ -30,8 +30,8 @@
 namespace armarx
 {
     /**
-     * @class HandUnitSimulationPropertyDefinitions
-     * @brief Defines all necessary properties for armarx::HandUnitSimulation
+     * \class HandUnitSimulationPropertyDefinitions
+     * \brief Defines all necessary properties for armarx::HandUnitSimulation
      */
     class HandUnitSimulationPropertyDefinitions:
             public ComponentPropertyDefinitions
@@ -47,13 +47,14 @@ namespace armarx
 
 
     /**
-     * @class HandUnitSimulation
-     * @brief This class defines an interface for providing high level access to robot hands
-     * @ingroup RobotAPI-SensorActorUnits-simulation
+     * \class HandUnitSimulation
+     * \ingroup RobotAPI-SensorActorUnits-simulation
+     * \brief Simulates a robot hand.
      *
      * An instance of a HandUnitSimulation provides means to open, close, and shape hands.
      * It uses the HandUnitListener Ice interface to report updates of its current state
      *
+     * \warning This component is not yet fully functional
      */
     class HandUnitSimulation :
         virtual public HandUnit
@@ -65,25 +66,20 @@ namespace armarx
             return "HandUnitSimulation";
         }
 
-        /**
-         *
-         */
         virtual void onInitHandUnit();
-        /**
-         *
-         */
         virtual void onStartHandUnit();
-        /**
-         *
-         */
         virtual void onExitHandUnit();
 
-
-
         /**
          * Send command to the hand to form a specific shape position.
+         *
+         * \warning Not implemented yet!
          */
         virtual void setShape(const std::string& shapeName, const Ice::Current& c = ::Ice::Current());
+
+        /**
+         * \warning Not implemented yet!
+         */
         virtual void setJointAngles(const NameValueMap& jointAngles, const Ice::Current& c = ::Ice::Current());
 
     };
diff --git a/source/RobotAPI/components/units/HapticObserver.h b/source/RobotAPI/components/units/HapticObserver.h
index 4e167780a5f638c0302a97264df33501c536eadf..dd9cb014c5580962c4b091a61b83b981ed09407b 100644
--- a/source/RobotAPI/components/units/HapticObserver.h
+++ b/source/RobotAPI/components/units/HapticObserver.h
@@ -27,14 +27,16 @@
 #include <RobotAPI/interface/units/HapticUnit.h>
 #include <Core/observers/Observer.h>
 #include <Core/util/variants/eigen3/MatrixVariant.h>
-//#include <Core/util/variants/eigen3/Eigen3LibRegistry.h>
 #include <Core/util/variants/eigen3/Eigen3VariantObjectFactories.h>
 #include <Core/core/services/tasks/PeriodicTask.h>
 
 
 namespace armarx
 {
-
+    /**
+     * \class HapticObserverPropertyDefinitions
+     * \brief
+     */
     class HapticObserverPropertyDefinitions:
             public ComponentPropertyDefinitions
     {
@@ -46,11 +48,9 @@ namespace armarx
         }
     };
 
-
-    /*!
-     * @class HapticSampleStatistics
-     *
-     * \brief The HapticSampleStatistics class
+    /**
+     * \class HapticSampleStatistics
+     * \brief
      */
     class HapticSampleStatistics
     {
@@ -109,9 +109,12 @@ namespace armarx
     };
 
     /**
-     * @class HapticObserver
-     * @ingroup RobotAPI-SensorActorUnits-observers
-     * @brief The HapticObserver class
+     * \class HapticObserver
+     * \ingroup RobotAPI-SensorActorUnits-observers
+     * \brief Observer monitoring haptic sensor values
+     *
+     * The HapticObserver monitors haptic sensor values published by HapticUnit-implementations and offers condition checks on these values.
+     * Available condition checks are: *updated*, *larger*, *equals* and *smaller*.
      */
     class HapticObserver :
             virtual public Observer,
@@ -131,7 +134,7 @@ namespace armarx
         void reportSensorValues(const ::std::string& device, const ::std::string& name, const ::armarx::MatrixFloatBasePtr& values, const ::armarx::TimestampBasePtr& timestamp, const ::Ice::Current& = ::Ice::Current());
 
         /**
-         * @see PropertyUser::createPropertyDefinitions()
+         * \see PropertyUser::createPropertyDefinitions()
          */
         virtual PropertyDefinitionsPtr createPropertyDefinitions();
     private:
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
index 0a1d93318f23bac0d42da4810862e758a5007780..291e4dcda27186bc28e6cad3cb622b4560e333d7 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
@@ -31,7 +31,10 @@
 
 namespace armarx
 {
-
+    /**
+     * \class InertialMeasurementUnitObserverPropertyDefinitions
+     * \brief
+     */
     class InertialMeasurementUnitObserverPropertyDefinitions:
             public ComponentPropertyDefinitions
     {
@@ -45,10 +48,13 @@ namespace armarx
     };
 
 
-    /*!
-     * @class InertialMeasurementUnitObserver
-     * @ingroup RobotAPI-SensorActorUnits-observers
-     * \brief The InertialMeasurementUnitObserver class
+    /**
+     * \class InertialMeasurementUnitObserver
+     * \ingroup RobotAPI-SensorActorUnits-observers
+     * \brief Observer monitoring @IMU sensor values
+     *
+     * The InertialMeasurementUnitObserver monitors @IMU sensor values published by InertialMeasurementUnit-implementations and offers condition checks on these values.
+     * Available condition checks are: *updated*, *larger*, *equals* and *smaller*.
      */
     class InertialMeasurementUnitObserver :
             virtual public Observer,
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.h b/source/RobotAPI/components/units/KinematicUnitObserver.h
index 01dea5f222d9e7e33939b9bb08c53aee341c18f7..37e3637c9f65f77e9218b331b441348fc51af676 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.h
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.h
@@ -41,14 +41,17 @@ namespace armarx
     ARMARX_CREATE_CHECK(KinematicUnitObserver, larger)
 
    /**
-    * @class KinematicUnitObserver
-    * @ingroup RobotAPI-SensorActorUnits-observers
+    * \class KinematicUnitObserver
+    * \ingroup RobotAPI-SensorActorUnits-observers
+    * \brief Observer monitoring kinematic sensor and actor values
     *
     * The KinematicUnitObserver allows to install conditions on all channel reported by the KinematicUnit.
     * These include joint angles, velocities, torques and motor temperatures
     *
     * The KinematicUnitObserver retrieves its configuration from a VirtualRobot robot model. Within the model, the joints
-    * which are observer by the unit are define by a robotnodeset
+    * which are observer by the unit are define by a robotnodeset.
+    *
+    * Available condition checks are: *valid*, *updated*, *equals*, *inrange*, *approx*, *larger* and *smaller*.
     */
     class ARMARXCORE_IMPORT_EXPORT KinematicUnitObserver :
         virtual public Observer,
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index bd85de32f88a41d5cc6ed903c8c4d7af5f41ed9c..0e309977a4f026e1d3bd2cb0c3530069063da5da 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -61,7 +61,7 @@ void KinematicUnitSimulation::onInitKinematicUnit()
         for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
         {
             std::string jointName = (*it)->getName();
-            jointStates[jointName] = JointState();
+            jointStates[jointName] = KinematicUnitSimulationJointState();
             jointStates[jointName].init();
         }
     }
@@ -99,7 +99,7 @@ void KinematicUnitSimulation::simulationFunction()
 
     {
         boost::mutex::scoped_lock lock(jointStatesMutex);
-        for ( JointStates::iterator it = jointStates.begin(); it!=jointStates.end(); it++ )
+        for ( KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it!=jointStates.end(); it++ )
         {
 
             float newAngle = 0.0f;
@@ -118,14 +118,14 @@ void KinematicUnitSimulation::simulationFunction()
             }
 
             // constrain the angle
-            JointInfo & jointInfo = jointInfos[it->first];
+            KinematicUnitSimulationJointInfo & jointInfo = jointInfos[it->first];
             float maxAngle = (jointInfo.hasLimits()) ? jointInfo.limitHi : 2.0 * M_PI;
             float minAngle = (jointInfo.hasLimits()) ? jointInfo.limitLo : -2.0 * M_PI;
             newAngle = std::max<float>(newAngle, minAngle);
             newAngle = std::min<float>(newAngle, maxAngle);
 
             // Check if joint existed before
-            JointStates::iterator itPrev = previousJointStates.find(it->first);
+            KinematicUnitSimulationJointStates::iterator itPrev = previousJointStates.find(it->first);
             if(itPrev == previousJointStates.end())
             {
                 aPosValueChanged = aVelValueChanged = true;
@@ -167,7 +167,7 @@ void KinematicUnitSimulation::switchControlMode(const NameControlModeMap& target
             std::string targetJointName = it->first;
             ControlMode targetControlMode = it->second;
 
-            JointStates::iterator iterJoints = jointStates.find(targetJointName);
+            KinematicUnitSimulationJointStates::iterator iterJoints = jointStates.find(targetJointName);
 
             // check whether there is a joint with this name and set joint angle
             if(iterJoints != jointStates.end())
@@ -200,7 +200,7 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
         float targetJointAngle = it->second;
 
         // check whether there is a joint with this name and set joint angle
-        JointStates::iterator iterJoints = jointStates.find(targetJointName);
+        KinematicUnitSimulationJointStates::iterator iterJoints = jointStates.find(targetJointName);
         if(iterJoints != jointStates.end())
         {
             if (fabs(iterJoints->second.angle - targetJointAngle)>0)
@@ -231,7 +231,7 @@ void KinematicUnitSimulation::setJointVelocities(const NameValueMap& targetJoint
         std::string targetJointName = it->first;
         float targetJointVelocity = it->second;
 
-        JointStates::iterator iterJoints = jointStates.find(targetJointName);
+        KinematicUnitSimulationJointStates::iterator iterJoints = jointStates.find(targetJointName);
         // check whether there is a joint with this name and set joint angle
         if(iterJoints != jointStates.end())
         {
@@ -256,7 +256,7 @@ void KinematicUnitSimulation::setJointTorques(const NameValueMap& targetJointTor
         std::string targetJointName = it->first;
         float targetJointTorque = it->second;
 
-        JointStates::iterator iterJoints = jointStates.find(targetJointName);
+        KinematicUnitSimulationJointStates::iterator iterJoints = jointStates.find(targetJointName);
         // check whether there is a joint with this name and set joint angle
         if(iterJoints != jointStates.end())
         {
@@ -282,7 +282,7 @@ void KinematicUnitSimulation::stop(const Ice::Current &c)
 {
 
     boost::mutex::scoped_lock lock(jointStatesMutex);
-    for ( JointStates::iterator it = jointStates.begin(); it!=jointStates.end(); it++ )
+    for ( KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it!=jointStates.end(); it++ )
     {
         it->second.velocity = 0;
     }
@@ -297,7 +297,7 @@ PropertyDefinitionsPtr KinematicUnitSimulation::createPropertyDefinitions()
 }
 
 
-bool mapEntryEqualsString(std::pair<std::string, JointState> iter, std::string compareString)
+bool mapEntryEqualsString(std::pair<std::string, KinematicUnitSimulationJointState> iter, std::string compareString)
 {
     return (iter.first == compareString);
 }
@@ -306,7 +306,7 @@ void KinematicUnitSimulation::requestJoints(const StringSequence& joints, const
 {
     ARMARX_INFO << flush;
     // if one of the joints belongs to this unit, lock access to this unit for other components except for the requesting one
-    JointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
+    KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
     if (jointStates.end() != it)
     {
         KinematicUnit::request(c);
@@ -317,7 +317,7 @@ void KinematicUnitSimulation::releaseJoints(const StringSequence& joints, const
 {
     ARMARX_INFO << flush;
     // if one of the joints belongs to this unit, unlock access
-    JointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
+    KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
     if (jointStates.end() != it)
     {
         KinematicUnit::release(c);
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.h b/source/RobotAPI/components/units/KinematicUnitSimulation.h
index c4c11f71e5af8f9d694366b80e2861e2dffd4983..4b15853425deae3cff664c3875fa3ae3d3b6f702 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.h
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.h
@@ -35,17 +35,17 @@
 namespace armarx
 {
     /**
-     * @class JointState.
-     * @brief State of a joint.
-     * @ingroup RobotAPI-SensorActorUnits-util
+     * \class KinematicUnitSimulationJointState.
+     * \brief State of a joint.
+     * \ingroup RobotAPI-SensorActorUnits-util
      *
-     * Includes the control mode, the joint angle, velocitym torquem and motor temperature.
-     * Controlmode defaults to ePositionControl. All other values to zero.
+     * Includes the control mode, the joint angle, velocity, torque, and motor temperature.
+     * Controlmode defaults to ePositionControl, all other values default to zero.
      */
-    class JointState
+    class KinematicUnitSimulationJointState
     {
     public:
-        JointState()
+        KinematicUnitSimulationJointState()
         {
             init();
         }
@@ -60,25 +60,25 @@ namespace armarx
         }
 
         ControlMode controlMode;
-        float       angle;
-        float       velocity;
-        float       torque;
-        float       temperature;
+        float angle;
+        float velocity;
+        float torque;
+        float temperature;
     };
-    typedef std::map<std::string, JointState> JointStates;
-
+    typedef std::map<std::string, KinematicUnitSimulationJointState> KinematicUnitSimulationJointStates;
 
     /**
-     * @class JointInfo.
-     * @brief
-     * @ingroup RobotAPI-SensorActorUnits-util
+     * \class JointInfo.
+     * \brief Additional information about a joint.
+     * \ingroup RobotAPI-SensorActorUnits-util
      *
-     * Joint info including lower and upper joint limits (rad). No limits set by default (lo=hi).
+     * Joint info including lower and upper joint limits (rad).
+     * No limits are set by default (lo = hi).
      */
-    class JointInfo
+    class KinematicUnitSimulationJointInfo
     {
     public:
-        JointInfo()
+        KinematicUnitSimulationJointInfo()
         {
             reset();
         }
@@ -96,11 +96,11 @@ namespace armarx
         float limitLo;
         float limitHi;
     };
-    typedef std::map<std::string, JointInfo> JointInfos;
+    typedef std::map<std::string, KinematicUnitSimulationJointInfo> KinematicUnitSimulationJointInfos;
 
     /**
-     * @class KinematicUnitSimulationPropertyDefinitions
-     * @brief
+     * \class KinematicUnitSimulationPropertyDefinitions
+     * \brief
      */
     class KinematicUnitSimulationPropertyDefinitions :
             public KinematicUnitPropertyDefinitions
@@ -115,9 +115,9 @@ namespace armarx
     };
 
     /**
-     * @class KinematicUnitSimulation
-     * @brief
-     * @ingroup RobotAPI-SensorActorUnits-simulation
+     * \class KinematicUnitSimulation
+     * \brief Simulates robot kinematics
+     * \ingroup RobotAPI-SensorActorUnits-simulation
      */
     class KinematicUnitSimulation :
         virtual public KinematicUnit
@@ -139,19 +139,28 @@ namespace armarx
         virtual void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c = ::Ice::Current());
         virtual void setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c = ::Ice::Current());
         virtual void setJointTorques(const NameValueMap& targetJointTorques, const Ice::Current& c = ::Ice::Current());
+
+        /**
+         * \warning Not implemented yet!
+         */
         virtual void setJointAccelerations(const NameValueMap& targetJointAccelerations, const Ice::Current& c = ::Ice::Current());
+
+        /**
+         * \warning Not implemented yet!
+         */
         virtual void setJointDecelerations(const NameValueMap& targetJointDecelerations, const Ice::Current& c = ::Ice::Current());
+
         void stop(const Ice::Current &c = Ice::Current());
 
         /**
-         * @see PropertyUser::createPropertyDefinitions()
+         * \see PropertyUser::createPropertyDefinitions()
          */
         virtual PropertyDefinitionsPtr createPropertyDefinitions();
 
     protected:
-        JointStates jointStates;
-        JointStates previousJointStates;
-        JointInfos jointInfos;
+        KinematicUnitSimulationJointStates jointStates;
+        KinematicUnitSimulationJointStates previousJointStates;
+        KinematicUnitSimulationJointInfos jointInfos;
         boost::mutex jointStatesMutex;
         IceUtil::Time lastExecutionTime;
         float noise;
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.h b/source/RobotAPI/components/units/PlatformUnitObserver.h
index 3dacf641de0bba563a15ab9db00447499f8083ca..d07da91a4a16875949df07b272b90c8f065df2a3 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.h
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.h
@@ -43,15 +43,17 @@ namespace armarx
     ARMARX_CREATE_CHECK(PlatformUnitObserver, inrange)
 
    /**
-    * @class PlatformUnitObserver
-    * @brief
-    * @ingroup RobotAPI-SensorActorUnits-observers
+    * \class PlatformUnitObserver
+    * \ingroup RobotAPI-SensorActorUnits-observers
+    * \brief Observer monitoring platform-related sensor values.
     *
     * The PlatformUnitObserver allows the installation of conditions on all channel reported by the PlatformUnit.
     * These include current and target position.
     *
     * The PlatformUnitObserver retrieves its platform configuration via properties.
     * The name must exist in the used Simox robot model file.
+    *
+    * Available condition checks are: *valid*, *updated*, *equals*, *inrange*, *approx*, *larger* and *smaller*.
     */
     class ARMARXCORE_IMPORT_EXPORT PlatformUnitObserver :
         virtual public Observer,
@@ -70,10 +72,10 @@ namespace armarx
         virtual std::string getDefaultName() const
         {
             return "PlatformUnitObserver";
-        };
+        }
 
         /**
-         * @see PropertyUser::createPropertyDefinitions()
+         * \see PropertyUser::createPropertyDefinitions()
          */
         virtual PropertyDefinitionsPtr createPropertyDefinitions();
 
@@ -85,9 +87,9 @@ namespace armarx
     };
 
     /**
-      @class PlatformUnitDatafieldCreator
-      @brief
-      @ingroup RobotAPI-SensorActorUnits-util
+     * \class PlatformUnitDatafieldCreator
+     * \brief
+     * \ingroup RobotAPI-SensorActorUnits-util
      */
     class PlatformUnitDatafieldCreator
     {
@@ -97,11 +99,11 @@ namespace armarx
         {}
 
         /**
-         * @brief Function to create a Channel Representation for a PlatformUnitObserver
-         * @param platformUnitOberserverName Name of the PlatformUnitObserver
-         * @param platformName Name of the platform of the robot like it is specified
-         * in the simox-robot-xml-file
-         * @return returns a ChannelRepresentationPtr
+         * \brief Function to create a Channel Representation for a PlatformUnitObserver
+         * \param platformUnitOberserverName Name of the PlatformUnitObserver
+         * \param platformName Name of the platform of the robot like it is specified
+         *        in the simox-robot-xml-file
+         * \return returns a ChannelRepresentationPtr
          */
         DataFieldIdentifier getDatafield(const std::string& platformUnitObserverName, const std::string& platformName) const
         {
@@ -126,7 +128,6 @@ namespace armarx
             const PlatformUnitDatafieldCreator platformVelocity("platformVelocity");
         }
     }
-
 }
 
 #endif
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.h b/source/RobotAPI/components/units/PlatformUnitSimulation.h
index f5910e0750e4c9c7544139dcad98a38375f84289..dfd0e18ab04cfa3ae59a2ac94a05b524e3a42a1b 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.h
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.h
@@ -36,8 +36,8 @@
 namespace armarx
 {
     /**
-     * @class PlatformUnitSimulationPropertyDefinitions
-     * @brief
+     * \class PlatformUnitSimulationPropertyDefinitions
+     * \brief
      */
     class PlatformUnitSimulationPropertyDefinitions :
             public PlatformUnitPropertyDefinitions
@@ -57,9 +57,9 @@ namespace armarx
     };
 
     /**
-     * @class PlatformUnitSimulation
-     * @brief
-     * @ingroup RobotAPI-SensorActorUnits-simulation
+     * \class PlatformUnitSimulation
+     * \brief Simulates a robot platform.
+     * \ingroup RobotAPI-SensorActorUnits-simulation
      */
     class PlatformUnitSimulation :
         virtual public PlatformUnit
@@ -80,11 +80,17 @@ namespace armarx
 
         // proxy implementation
         void moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = ::Ice::Current());
+
+        /**
+         * \warning Not yet implemented!
+         */
         void move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current &c = Ice::Current());
+
         void moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current &c = Ice::Current());
         void setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current &c = Ice::Current());
+
         /**
-         * @see PropertyUser::createPropertyDefinitions()
+         * \see PropertyUser::createPropertyDefinitions()
          */
         virtual PropertyDefinitionsPtr createPropertyDefinitions();
 
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index c0c5cf9a1d447452f17ad040fef9c87ed73c50d3..aa6741a0afbc81db4707103af0cd8a33a4b9a3ec 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -402,6 +402,7 @@ namespace armarx
         for(; itDIK != localDIKMap.end(); itDIK++)
         {
             RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(itDIK->first);
+
             EDifferentialIKPtr dIK = boost::dynamic_pointer_cast<EDifferentialIK>(itDIK->second);
 //            ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << robotNodeSet->getTCP()->getGlobalPose();
 //            dIK->setVerbose(true);
@@ -414,7 +415,7 @@ namespace armarx
             Eigen::VectorXf jointLimitCompensation = CalcJointLimitAvoidanceDeltas(robotNodeSet, fullJac, fullJacInv);
 
             // for debugging only!
-            {
+            /*{
                 MatrixXf posJac = fullJac.block(0, 0, fullJac.rows(), 3);
                 MatrixXf oriJac = fullJac.block(0, 3, fullJac.rows(), 3);
                 JacobiSVD<MatrixXf> svdPos(posJac);
@@ -427,18 +428,22 @@ namespace armarx
                 {
                     ARMARX_IMPORTANT << deactivateSpam() << "The robot is close to a singularity! Minimal singular value of the orientation Jacobian: " << svdOri.singularValues()[2];
                 }
-            }
+            }*/
 
             // added by David S
             const float maxJointLimitCompensationRatio = 0.8;
-            if (jointLimitCompensation.norm() > maxJointLimitCompensationRatio*jointDelta.norm())
+
+            if (jointLimitCompensation(0) == jointLimitCompensation(0))
             {
-                jointLimitCompensation = maxJointLimitCompensationRatio*jointDelta.norm()/jointLimitCompensation.norm() * jointLimitCompensation;
+                if (jointLimitCompensation.norm() > maxJointLimitCompensationRatio*jointDelta.norm())
+                {
+                    jointLimitCompensation = maxJointLimitCompensationRatio*jointDelta.norm()/jointLimitCompensation.norm() * jointLimitCompensation;
+                }
+                jointDelta += jointLimitCompensation;
             }
 
-            jointDelta += jointLimitCompensation;
-
             jointDelta /=(cycleTime*0.001);
+
             jointDelta = applyMaxJointVelocity(jointDelta, maxJointVelocity);
 
 //            Eigen::Vector3f currentTCPPosition = robotNodeSet->getTCP()->getGlobalPose().block(0,3,3,1);
@@ -692,6 +697,8 @@ namespace armarx
             VectorXf newJointValues;
             rns->getJointValues(newJointValues);
             resultJointDelta = newJointValues - oldJointValues;
+
+
 //            ARMARX_DEBUG << "joint angle deltas:\n" << dThetaSum;
 
             // check tolerances
@@ -726,6 +733,7 @@ namespace armarx
         }
         while (step<maxNStep);
 
+
         float bestJVError = std::numeric_limits<float>::max();
         for(unsigned int i=0; i < jointDeltaIterations.size(); i++)
         {
@@ -737,6 +745,7 @@ namespace armarx
 
             }
         }
+
         robot->setJointValues(rns,oldJointValues + resultJointDelta);
 
 //        ARMARX_DEBUG << "best try: " <<  bestIndex << " with error: " << bestJVError;
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index 030891c76bf6b8e02d928317e07a3f168d746a4f..24ce0d04a82ee3cf2de97e8d71950c23c0bc92f3 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -15,7 +15,7 @@
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarX::
-* @author     Mirko Waechter ( mirko.waechter at kit dot edu)
+* @author     Mirko Waechter (mirko.waechter at kit dot edu)
 * @date       2013
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
diff --git a/source/RobotAPI/components/units/TCPControlUnitObserver.h b/source/RobotAPI/components/units/TCPControlUnitObserver.h
index 8efaf63e6a8e789dfc8cc9aabc1bc8eababdd76c..e8af6aaf86700be5e58d956b405cf2005356ca61 100644
--- a/source/RobotAPI/components/units/TCPControlUnitObserver.h
+++ b/source/RobotAPI/components/units/TCPControlUnitObserver.h
@@ -27,7 +27,10 @@
 #include <RobotAPI/interface/units/TCPControlUnit.h>
 
 namespace armarx {
-
+    /**
+     * \class TCPControlUnitObserverPropertyDefinitions
+     * \brief
+     */
     class TCPControlUnitObserverPropertyDefinitions:
             public ComponentPropertyDefinitions
     {
@@ -39,6 +42,16 @@ namespace armarx {
         }
     };
 
+    /**
+     * \class TCPControlUnitObserver
+     * \ingroup RobotAPI-SensorActorUnits-observers
+     * \brief Observer monitoring TCP-related sensor values.
+     *
+     * The TCPControlUnitObserver allows the installation of conditions on all channel reported by the TCPControlUnit.
+     * These include TCP pose and velocity.
+     *
+     * Available condition checks are: *updated*, *equals*, *approx*, *larger*, *smaller* and *magnitudelarger*.
+     */
     class TCPControlUnitObserver :
             virtual public Observer,
             virtual public TCPControlUnitObserverInterface
@@ -52,12 +65,12 @@ namespace armarx {
         void onConnectObserver();
 
         /**
-         * @see PropertyUser::createPropertyDefinitions()
+         * \see PropertyUser::createPropertyDefinitions()
          */
         virtual PropertyDefinitionsPtr createPropertyDefinitions();
 
-        // TCPControlUnitListener interface
     public:
+        // TCPControlUnitListener interface
         void reportTCPPose(const FramedPoseBaseMap &poseMap, const Ice::Current &c = Ice::Current());
         void reportTCPVelocities(const FramedDirectionMap &tcpTranslationVelocities, const FramedDirectionMap &tcpOrientationVelocities, const Ice::Current &c = Ice::Current());
 
diff --git a/source/RobotAPI/interface/core/FramedPoseBase.ice b/source/RobotAPI/interface/core/FramedPoseBase.ice
index e618d59af3d9170047c6b7351f38eef27e91cb3d..acc1f7b9d4d8330cb10e3efdc72ece0201c279f4 100644
--- a/source/RobotAPI/interface/core/FramedPoseBase.ice
+++ b/source/RobotAPI/interface/core/FramedPoseBase.ice
@@ -30,7 +30,12 @@
 
 module armarx
 {
-
+	/**
+	* [FramedDirectionBase] defines a vector with regard to an agent and a frame within the agent.
+	* If agent is empty and frame is set to armarx::GlobalFrame, vector is defined in global frame.
+	* @param frame Name of frame.
+	* @param frame Name of agent.
+	*/
     ["cpp:virtual"]
     class FramedDirectionBase extends Vector3Base
     {
@@ -38,14 +43,25 @@ module armarx
         string agent;
     };
 
-
+	/**
+	* [FramedPoseBase] defines a pose with regard to an agent and a frame within the agent.
+	* If agent is empty and frame is set to armarx::GlobalFrame, pose is defined in global frame.
+	* @param frame Name of frame.
+	* @param frame Name of agent.
+	*/
     ["cpp:virtual"]
     class FramedPoseBase extends PoseBase
     {
         string frame;
         string agent;
     };
-
+    
+	/**
+	* [FramedPositionBase] defines a position with regard to an agent and a frame within the agent.
+	* If agent is empty and frame is set to armarx::GlobalFrame, position is defined in global frame.
+	* @param frame Name of frame.
+	* @param frame Name of agent.
+	*/
     ["cpp:virtual"]
     class FramedPositionBase extends Vector3Base
     {
@@ -53,16 +69,32 @@ module armarx
         string agent;
     };
 
+	/**
+	* [FramedOrientationBase] defines an orientation in quaternion with regard to an agent and a frame within the agent.
+	* If agent is empty and frame is set to armarx::GlobalFrame, orientation in quaternion is defined in global frame.
+	* @param frame Name of frame.
+	* @param frame Name of agent.
+	*/
    ["cpp:virtual"]
     class FramedOrientationBase extends QuaternionBase
     {
         string frame;
         string agent;
     };
-
+	/**
+	* FramedDirectionMap is a map of FramedDirectionBases and corresponding agent nodes identified by name.
+	* @see FramedDirectionBase
+	*/
     dictionary<string, FramedDirectionBase> FramedDirectionMap;
+    /**
+	* FramedPositionBase is a map of FramedPositionBases and corresponding agent nodes identified by name.
+	* @see FramedPositionBase
+	*/
     dictionary<string, FramedPositionBase> FramedPositionMap;
-
+    
+	/**
+	* PoseMedianFilterBase filters poses with a median filter.
+	*/
     ["cpp:virtual"]
     class PoseMedianFilterBase extends MedianFilterBase
     {
diff --git a/source/RobotAPI/interface/core/LinkedPoseBase.ice b/source/RobotAPI/interface/core/LinkedPoseBase.ice
index 14f78c6d1b7056ce15fd7d9164d464352e180631..3f465967c721200bd717266150797b14a5bb3125 100644
--- a/source/RobotAPI/interface/core/LinkedPoseBase.ice
+++ b/source/RobotAPI/interface/core/LinkedPoseBase.ice
@@ -29,17 +29,31 @@
 
 module armarx
 {
+	/**
+	* [LinkedPoseBase] 
+	*/
     ["cpp:virtual"]
     class LinkedPoseBase extends FramedPoseBase
     {
         SharedRobotInterface* referenceRobot;
+        /**
+        * changeFrame describes pose on new frame.
+        * @param newFrame Name of new frame node.
+        */
         void changeFrame(string newFrame);
     };
-
+    
+	/**
+	* [LinkedDirectionBase] 
+	*/
     ["cpp:virtual"]
     class LinkedDirectionBase extends FramedDirectionBase
     {
         SharedRobotInterface* referenceRobot;
+        /**
+        * changeFrame describes direction on new frame.
+        * @param newFrame Name of new frame node.
+        */
         void changeFrame(string newFrame);
     };
 };
diff --git a/source/RobotAPI/interface/core/PoseBase.ice b/source/RobotAPI/interface/core/PoseBase.ice
index 3634981d22bf805cb43711bd9c7f323dedee6a6b..7cadb9e161d83712db74606c666e103adabae6a1 100644
--- a/source/RobotAPI/interface/core/PoseBase.ice
+++ b/source/RobotAPI/interface/core/PoseBase.ice
@@ -29,22 +29,35 @@
 
 module armarx
 {
-
+	/**
+	* [Vector2Base] defines a 2D vector with x and y.
+	*/
     class Vector2Base extends VariantDataClass
     {
         float x = zeroInt;
         float y = zeroInt;
     };
-
+	/**
+	* [Vector3Base] defines a 3D vector with x, y, and z.
+	*/
     class Vector3Base extends VariantDataClass
     {
         float x = zeroInt;
         float y = zeroInt;
         float z = zeroInt;
     };
-
+	/**
+	* List of [Vector3Base].
+	*/
     sequence<Vector3Base> Vector3BaseList;
 
+	/**
+	* [QuaternionBase] defines a quaternion with
+	* @param qw Rotation angle.
+	* @param qx x-coordinate of normalized rotation axis.
+	* @param qy y-coordinate of normalized rotation axis.
+	* @param qz z-coordinate of normalized rotation axis.
+	*/
     class QuaternionBase extends VariantDataClass
     {
         float qw = zeroInt;
@@ -53,13 +66,20 @@ module armarx
         float qz = zeroInt;
     };
 
+	/**
+	* [PoseBase] defines a pose with position in [Vector3Base] and orientation in [QuaternionBase].
+	* @param position Position as 3D vector in x, y, and z [Vector3Base].
+	* @param orientation Orientation as quaternion [QuaternionBase].
+	*/
     ["cpp:virtual"]
     class PoseBase extends VariantDataClass
     {
         Vector3Base position;
         QuaternionBase orientation;
     };
-
+	/**
+	* List of [PoseBase].
+	*/
     sequence<PoseBase> PoseBaseList;
 
 };
diff --git a/source/RobotAPI/interface/observers/ObserverFilters.ice b/source/RobotAPI/interface/observers/ObserverFilters.ice
index d233e745d2a83ceff17e5190f3d099beddaac6c9..b668c374f15836a31a72c8120f7afde299913004 100644
--- a/source/RobotAPI/interface/observers/ObserverFilters.ice
+++ b/source/RobotAPI/interface/observers/ObserverFilters.ice
@@ -36,42 +36,66 @@ module armarx
         const string magnitudeinrange = "magnitudeinrange";
     };
 
+	/**
+	* OffsetFilterBase implements a filter which stores the measurement in the first run as an offset.
+	* This offset is subtracted from subsequent measurements.
+	*/
     ["cpp:virtual"]
     class OffsetFilterBase extends DatafieldFilterBase
     {
 
     };
-
+	/**
+	* MatrixMaxFilterBase implements a filter which returns the maximum coefficient of a matrix.
+	*/
     ["cpp:virtual"]
     class MatrixMaxFilterBase extends DatafieldFilterBase
     {
 
     };
-
+	/**
+	* MatrixMinFilterBase implements a filter which returns the minimum coefficient of a matrix.
+	*/
     ["cpp:virtual"]
     class MatrixMinFilterBase extends DatafieldFilterBase
     {
 
     };
-
+	
+	/**
+	* MatrixMinFilterBase implements a filter which returns the mean value of all coefficients of a matrix.
+	*/
     ["cpp:virtual"]
     class MatrixAvgFilterBase extends DatafieldFilterBase
     {
 
     };
-
+	/**
+	* MatrixPercentileFilterBase implements a filter which returns a subset of the smallest coefficients of a matrix.
+	* The matrix is transformed int a vector which is sorted in a ascending order. The subset consists of the first entries
+	* of the sorted vector. The size of the subset is defined by parameter percentile which ranges from 0.0 to 1.0.
+	*/
     ["cpp:virtual"]
     class MatrixPercentileFilterBase extends DatafieldFilterBase
     {
         float percentile;
     };
 
+	/**
+	* MatrixPercentilesFilterBase implements a filter which returns a subset of the smallest coefficients of a matrix.
+	* The matrix is transformed int a vector which is sorted in a ascending order. The subset consists of the first entries
+	* of the sorted vector. The size of the subset is defined by parameter percentiles.
+	*/
     ["cpp:virtual"]
     class MatrixPercentilesFilterBase extends DatafieldFilterBase
     {
         int percentiles;
     };
-
+    
+	/**
+	* MatrixCumulativeFrequencyFilterBase implements a filter which returns a matrix filtered by Cumulative Frequency filter.
+	* The entries of the filtered matrix range between the provided min and max values.
+	*/
     ["cpp:virtual"]
     class MatrixCumulativeFrequencyFilterBase extends DatafieldFilterBase
     {
diff --git a/source/RobotAPI/interface/units/ForceTorqueUnit.ice b/source/RobotAPI/interface/units/ForceTorqueUnit.ice
index 52ef14f2d69388f1a875c003f02553f27ee376a2..5e09c93abec1ddb3256c0dceb62bee6e80f65f5c 100644
--- a/source/RobotAPI/interface/units/ForceTorqueUnit.ice
+++ b/source/RobotAPI/interface/units/ForceTorqueUnit.ice
@@ -19,7 +19,7 @@
  * @copyright  2013 Mirko Waechter
  * @license    http://www.gnu.org/licenses/gpl.txt
  *             GNU General Public License
- */
+ **/
 
 #ifndef _ARMARX_ROBOTAPI_UNITS_FORCETORQUE_SLICE_
 #define _ARMARX_ROBOTAPI_UNITS_FORCETORQUE_SLICE_
@@ -34,28 +34,55 @@
 #include <Core/interface/observers/ObserverInterface.ice>
 
 module armarx
-{
+{    
+     /**
+     * Implements an interface to a ForceTorqueUnit.
+     **/
     interface ForceTorqueUnitInterface extends armarx::SensorActorUnitInterface
     {
+	/** 
+	 * setOffset allows to define offset forces and torques which are subtracted from the measured values.
+	 * @param forceOffsets Offset forces in x, y, and z.
+	 * @param torqueOffsets Offset torques in x, y, and z.
+	 **/
         void setOffset(FramedDirectionBase forceOffsets, FramedDirectionBase torqueOffsets);
+     /** 
+	 * setToNull allows to reset the ForceTorqueUnit by setting the measured values to zero.
+	 **/
         void setToNull();
     };
-
+	/**
+     * Implements an interface to a ForceTorqueUnitListener
+     **/
     interface ForceTorqueUnitListener
     {
-		/*!
-			\param sensorNodeName The RobotNode on which the sensor is mounted.
-			\param forces Set to zero if not available.
-			\param torques Set to zero if not available.
-		*/
+		/**
+			* reportSensorValues reports measured forces and torques from the ForceTorqueUnit to ForceTorqueObserver.
+			* @param sensorNodeName Name of force/torque sensor.
+			* @param forces Measured forces.
+			* @param torques Measured torques.
+		**/
         void reportSensorValues(string sensorNodeName, FramedDirectionBase forces, FramedDirectionBase torques);
     };
-
+	/**
+     * Implements an interface to a ForceTorqueUnitObserver. Provides topics on measured forces and torques which can be subscribed.
+     **/
     interface ForceTorqueUnitObserverInterface extends ObserverInterface, ForceTorqueUnitListener
     {
+		/**
+			* getForceDatafield returns a reference on the force topic.
+			* @param nodeName Name of force sensor topic.
+		**/
         DatafieldRefBase getForceDatafield(string nodeName) throws UserException;
+        /** 
+			* getTorqueDatafield returns a reference on the torque topic.
+			* @param nodeName Name of torque sensor topic.
+		**/
         DatafieldRefBase getTorqueDatafield(string nodeName) throws UserException;
-
+		/**
+			* createNulledDatafield resets the force/torque topic.
+			* @param forceTorqueDatafieldRef Reference of torque/torque sensor topic.
+		**/
         DatafieldRefBase createNulledDatafield(DatafieldRefBase forceTorqueDatafieldRef);
     };
 };
diff --git a/source/RobotAPI/interface/units/HandUnitInterface.ice b/source/RobotAPI/interface/units/HandUnitInterface.ice
index c66df1a86d42e1ec6f17d265b846641fd0655e49..8f989f0cf45a14185645c6c97dc89e5f8b5b1699 100644
--- a/source/RobotAPI/interface/units/HandUnitInterface.ice
+++ b/source/RobotAPI/interface/units/HandUnitInterface.ice
@@ -35,40 +35,76 @@
 
 module armarx
 {
-
+	/**
+     * Implements an interface to a HandUnit.
+     */
     interface HandUnitInterface extends SensorActorUnitInterface
     {
+		/**
+         * setShape sets the name of a known hand shape for the hand unit.
+         * @param shapeName Name of the hand shape.
+         */
         void setShape(string shapeName);
+        /**
+         * getShapeNames returns a list of names of hand shapes known to the hand unit.
+         * @return List of known hand shape names.
+         */
         SingleTypeVariantListBase getShapeNames();
         NameValueMap getShapeJointValues(string shapeName);
         string getHandName();
 
 
-        /*!
-         * \brief Inform the hand unit that an object has been successfully grasped.
-         * \param objectName The name of the object
-        *  E.g. a state that verifies if a grasp was successful can call this routine.
-        */
+        /**
+         * setObjectGrasped informs the hand unit that an object has been successfully grasped.
+         * @param objectName Name of the object
+         * E.g. a state that verifies if a grasp was successful can call this routine.
+         */
         void setObjectGrasped(string objectName);
 
-        /*!
-         * \brief setObjectReleased Inform the hand unit that an object has been released.
-         * \param objectName The name of the object
+        /**
+         * setObjectReleased informs the hand unit that an object has been released.
+         * @param objectName Name of the object
+         * E.g. a state that verifies if a grasp was successful can call this routine.
          */
         void setObjectReleased(string objectName);
-
+		/**
+         * setJointAngles controls the joints of the hand unit.
+         * @param targetJointAngles Map of joint names and corresponding joint angle values.
+         */
         void setJointAngles(NameValueMap targetJointAngles);
     };
-
+	/**
+     * Implements an interface to a HandUnitListener.
+     */
     interface HandUnitListener
     {
+		/**
+		* reportNewHandShapeName reports the current hand shape name.
+		* @param handName Name of the hand to be controlled.
+		* @param handName Name of the current hand shape name.
+		**/
         void reportHandShaped(string handName, string handShapeName);
+        /**
+		* reportNewHandShapeName reports the target hand shape name.
+		* @param handName Name of the hand to be controlled.
+		* @param handName Name of the target hand shape name.
+		**/
         void reportNewHandShapeName(string handName, string handShapeName);
+        /**
+		* reportJointAngles reports sensed joint angle values in the hand joints.
+		* @param actualJointAngles Map of joint names and corresponding sensed joint angle values.
+		**/
         void reportJointAngles(NameValueMap actualJointAngles);
+        /**
+		* reportJointPressures reports sensed pressure values in the hand joints.
+		* @param actualJointPressures Map of joint names and corresponding sensed pressure values.
+		**/
         void reportJointPressures(NameValueMap actualJointPressures);
 
     };
-
+	/**
+     * Implements an interface to a HandUnitObserver.
+     */
     interface HandUnitObserverInterface extends ObserverInterface, HandUnitListener
     {
     };
diff --git a/source/RobotAPI/interface/units/HapticUnit.ice b/source/RobotAPI/interface/units/HapticUnit.ice
index b7dfe40d35ea7bfda302521cdbf3be7088fa73f5..8afe57f9e7f90be40b84be195245216b8b3c800c 100644
--- a/source/RobotAPI/interface/units/HapticUnit.ice
+++ b/source/RobotAPI/interface/units/HapticUnit.ice
@@ -36,15 +36,28 @@
 
 module armarx
 {
+	/**
+     * Implements an interface to a HapitcUnit.
+     */
     interface HapticUnitInterface extends armarx::SensorActorUnitInterface
     {
     };
-
+	/**
+     * Implements an interface to a HapitcUnitListener.
+     */
     interface HapticUnitListener
     {
+		/**
+		* reportSensorValues reports the tactile sensor values from a given sensor device.
+		* @param device Name of tactile sensor device.
+		* @param values Matrix of taxels.
+		* @param timestamp Timestamp of the measurement.
+		**/
         void reportSensorValues(string device, string name, MatrixFloatBase values, TimestampBase timestamp);
     };
-
+	/**
+     * Implements an interface to a HapitcUnitObserver.
+     */
     interface HapticUnitObserverInterface extends ObserverInterface, HapticUnitListener
     {
     };
diff --git a/source/RobotAPI/interface/units/HeadIKUnit.ice b/source/RobotAPI/interface/units/HeadIKUnit.ice
index 2fa681eeb7b87a40b9b18e8476671c9723d95dcc..48ebc04eb7fe43c6cc1a6229b294a8c8a5a17d75 100644
--- a/source/RobotAPI/interface/units/HeadIKUnit.ice
+++ b/source/RobotAPI/interface/units/HeadIKUnit.ice
@@ -35,10 +35,21 @@
 
 module armarx
 {
-
+	/**
+	* Implements an interface to a HeadIKUnit.
+	**/
     interface HeadIKUnitInterface extends armarx::SensorActorUnitInterface
     {
+		/**
+		* setCycleTime allows to set the cycle time with which the head IK is solved and the head is controlled within a periodic task.
+		* @param milliseconds Cycle time in milliseconds.
+		**/
         void setCycleTime(int milliseconds);
+        /**
+		* setHeadTarget allows to set a new target position for the head.
+		* @param robotNodeSetName Name of kinematic chain/nodeset with head as TCP.
+		* @param targetPosition in x, y, and z.
+		**/
         void setHeadTarget(string robotNodeSetName, FramedPositionBase targetPosition);
     };
 
diff --git a/source/RobotAPI/interface/units/InertialMeasurementUnit.ice b/source/RobotAPI/interface/units/InertialMeasurementUnit.ice
index 8ab4784cc562b1e478b192f2bf50ebbe052751f5..834e077298ff469edd84ec284070b2dbe86e2242 100644
--- a/source/RobotAPI/interface/units/InertialMeasurementUnit.ice
+++ b/source/RobotAPI/interface/units/InertialMeasurementUnit.ice
@@ -39,22 +39,41 @@
 
 module armarx
 {
+	/**
+	* Struct IMUData with which IMU sensor data is represented. It incorporates following entries:
+	* @param orientationQuaternion Orientation in quaternion.
+	* @param magneticRotation Magnetic rotation.
+	* @param gyroscopeRotation Rotation of gyroscope.
+	* @param acceleration Acceleration of IMU sensor device.
+	**/
     struct IMUData {
         FloatSequence orientationQuaternion;
         FloatSequence magneticRotation;
         FloatSequence gyroscopeRotation; 
         FloatSequence acceleration;
     };
-
+	/**
+	* Implements an interface to an InertialMeasurementUnit.
+	**/
     interface InertialMeasurementUnitInterface extends armarx::SensorActorUnitInterface
     {
     };
-
+	/**
+	* Implements an interface to an InertialMeasurementUnitListener.
+	**/
     interface InertialMeasurementUnitListener
-    {
+    {	
+		/**
+		* reportSensorValues reports the IMU sensor values from a given sensor device.
+		* @param device Name of IMU sensor device.
+		* @param values IMU sensor data.
+		* @param timestamp Timestamp of the measurement.
+		**/
         void reportSensorValues(string device, string name, IMUData values, TimestampBase timestamp);
     };
-
+	/**
+	* Implements an interface to an InertialMeasurementUnitObserver.
+	**/
     interface InertialMeasurementUnitObserverInterface extends ObserverInterface, InertialMeasurementUnitListener
     {
     };
diff --git a/source/RobotAPI/interface/units/InertialMeasurementUnitInterface.ice b/source/RobotAPI/interface/units/InertialMeasurementUnitInterface.ice
deleted file mode 100644
index 8ab4784cc562b1e478b192f2bf50ebbe052751f5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/interface/units/InertialMeasurementUnitInterface.ice
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as
- * published by the Free Software Foundation; either version 2 of
- * the License, or (at your option) any later version.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package    RobotAPI
- * @author     Markus Grotz <markus dot grotz at kit dot edu>
- * @date       2015
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#ifndef _ARMARX_ROBOTAPI_UNITS_INERTIALMEASUREMENTUNIT_SLICE_
-#define _ARMARX_ROBOTAPI_UNITS_INERTIALMEASUREMENTUNIT_SLICE_
-
-
-#include <RobotAPI/interface/units/UnitInterface.ice>
-#include <RobotAPI/interface/core/RobotState.ice>
-
-#include <Core/interface/core/UserException.ice>
-#include <Core/interface/core/BasicTypes.ice>
-#include <Core/interface/observers/VariantBase.ice>
-#include <Core/interface/observers/Matrix.ice>
-#include <Core/interface/observers/Timestamp.ice>
-#include <Core/interface/observers/ObserverInterface.ice>
-
-
-
-module armarx
-{
-    struct IMUData {
-        FloatSequence orientationQuaternion;
-        FloatSequence magneticRotation;
-        FloatSequence gyroscopeRotation; 
-        FloatSequence acceleration;
-    };
-
-    interface InertialMeasurementUnitInterface extends armarx::SensorActorUnitInterface
-    {
-    };
-
-    interface InertialMeasurementUnitListener
-    {
-        void reportSensorValues(string device, string name, IMUData values, TimestampBase timestamp);
-    };
-
-    interface InertialMeasurementUnitObserverInterface extends ObserverInterface, InertialMeasurementUnitListener
-    {
-    };
-
-};
-
-#endif
diff --git a/source/RobotAPI/interface/units/KinematicUnitInterface.ice b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
index e960915f8145d993ee61da8fce2a5b95beaa6fff..5d0011a86373663433eb474534a5b51bea07692e 100644
--- a/source/RobotAPI/interface/units/KinematicUnitInterface.ice
+++ b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
@@ -31,20 +31,35 @@
 
 module armarx
 {
+	/**
+	* Struct RangeViolation for checking whether a given value is in given bounds:
+	**/
     struct RangeViolation
     {
+		/**
+		* @param rangeFrom Lower bound.
+		* @param rangeTo Upper bound.
+		* @param actualValue Actual value to be checked.
+		**/
         float rangeFrom;
         float rangeTo;
         float actualValue;
     };
 
+	/**
+	* @param RangeViolationSequence Sequence of values and corresponding bounds to be checked.
+	**/
     sequence<RangeViolation> RangeViolationSequence;
-
+	/**
+	* @throws OutOfRangeException Raised if value is violating the bounds.
+	**/
     exception OutOfRangeException extends UserException
     {
         RangeViolationSequence violation;
     };
-
+	/**
+	* [ControlMode] defines the different modes which a KinematicUnit can be controlled.
+	**/
     enum ControlMode
     {
         eDisabled,
@@ -54,21 +69,29 @@ module armarx
         eTorqueControl,
         ePositionVelocityControl
     };
-
+	/**
+	* [OperationStatus] defines whether a KinematicUnit is online, offline, or initialized.
+	**/
     enum OperationStatus
     {
         eOffline,
         eOnline,
         eInitialized
     };
-    
+    /**
+	* [ErrorStatus] defines the error status of a KinematicUnit.
+	**/
     enum ErrorStatus
     {
         eOk,
         eWarning,
         eError
     };
-    
+    /**
+	* JointStatus combines OperationStatus and ErrorStatus to describe the status of a joint.
+	* @see OperationStatus
+	* @see ErrorStatus
+	**/
     struct JointStatus
     {
         OperationStatus operation;
@@ -77,39 +100,63 @@ module armarx
         bool enabled;
         bool emergencyStop;
     };
-
+	/**
+	* ControlModeNotSupportedException Raised if a mode is requested which is not supported
+	* @see ControlMode
+	**/
     exception ControlModeNotSupportedException extends UserException
     {
         ControlMode mode;
     };
-
+	/**
+	* KinematicUnitUnavailable Raised if the resource KinematicUnit is not available.
+	**/
     exception KinematicUnitUnavailable extends ResourceUnavailableException
     {
         StringStringDictionary nodeOwners;
     };
-
+	/**
+	* KinematicUnitNotOwnedException Raised if the resource KinematicUnit is not owned.
+	**/
     exception KinematicUnitNotOwnedException extends ResourceNotOwnedException
     {
         StringSequence nodes;
     };
-
+	/**
+	* [NameValueMap] defined. This data container is mostly used to assign values to e.g. joints which are identified by name.
+	**/
     dictionary<string, float> NameValueMap;
+    /**
+	* [NameControlModeMap] defined. This data container is mostly used to assign control modes to e.g. joints which are identified by name.
+	**/
     dictionary<string, ControlMode> NameControlModeMap;
+    /**
+	* [NameStatusMap] defined. This data container is mostly used to a status to e.g. a joint which is identified by name.
+	**/
     dictionary<string, JointStatus> NameStatusMap;
-
+	/**
+	* Implements an interface to an KinematicUnit.
+	**/
     interface KinematicUnitInterface extends SensorActorUnitInterface
     {
-        /*
+        /**
          * Requesting and releasing joints guarantees that a joint is controlled by exactly
          * one component at any time
          * 
          * The request only affects the active access, sensor values can always be read.
-         * 
-         * TODO: The actual access has to be checked using a token.
+         *
          */
         void requestJoints(StringSequence joints) throws KinematicUnitUnavailable;
+        /**
+         * After usage joints should be released so that another component can get access to these joints.
+         */
         void releaseJoints(StringSequence joints) throws KinematicUnitNotOwnedException;
         
+        /**
+         * 
+         * switchControlMode allows switching control modes of joints specified in targetJointModes.
+         * @param targetJointModes defines target control modes and corresponding joints.
+         */
         void switchControlMode(NameControlModeMap targetJointModes) throws ControlModeNotSupportedException;
         
         /*
@@ -129,17 +176,64 @@ module armarx
          */
         //void set Trajectory(...);
     };
-
+	/**
+	* Implements an interface to an KinematicUnitListener.
+	**/
     interface KinematicUnitListener
     {
+		/**
+		* reportControlModeChanged reports if a ControlMode has changed.
+		* @param actualJointModes Map of control modes and corresponding joint names.
+		* @param aValueChanged Is set to true if a mode has changed.
+		**/
         void reportControlModeChanged(NameControlModeMap actualJointModes, bool aValueChanged);
-
+		/**
+		* reportJointAngles reports joint angle values.
+		* @param actualJointAngles Map of joint angle values and corresponding joint names.
+		* @param aValueChanged Is set to true if a joint angle value has changed.
+		**/
         void reportJointAngles(NameValueMap actualJointAngles, bool aValueChanged);
+        
+		/**
+		* reportJointVelocities reports joint angle velocities.
+		* @param actualJointVelocities Map of joint angle velocities and corresponding joint names.
+		* @param aValueChanged Is set to true if a joint angle velocity has changed.
+		**/
         void reportJointVelocities(NameValueMap actualJointVelocities, bool aValueChanged);
+        
+		/**
+		* reportJointTorques reports joint torques.
+		* @param actualJointTorques Map of joint torques and corresponding joint names.
+		* @param aValueChanged Is set to true if a joint torque has changed.
+		**/
         void reportJointTorques(NameValueMap actualJointTorques, bool aValueChanged);
+        
+		/**
+		* reportJointAccelerations reports joint accelerations.
+		* @param actualJointAccelerations Map of joint accelerations and corresponding joint names.
+		* @param aValueChanged Is set to true if a joint acceleration has changed.
+		**/
         void reportJointAccelerations(NameValueMap actualJointAccelerations, bool aValueChanged);
+        
+		/**
+		* reportJointCurrents reports joint currents.
+		* @param actualJointCurrents Map of joint currents and corresponding joint names.
+		* @param aValueChanged Is set to true if a joint current has changed.
+		**/
         void reportJointCurrents(NameValueMap actualJointCurrents, bool aValueChanged);
+		
+		/**
+		* reportJointMotorTemperatures reports joint motor temperatures.
+		* @param actualJointMotorTemperatures Map of joint motor temperatures and corresponding joint names.
+		* @param aValueChanged Is set to true if a joint motor temperature has changed.
+		**/
         void reportJointMotorTemperatures(NameValueMap actualJointMotorTemperatures, bool aValueChanged);
+        
+		/**
+		* reportJointStatuses reports current joint statuses.
+		* @param actualJointStatuses Map of joint statuses and corresponding joint names.
+		* @param aValueChanged Is set to true if a joint status has changed.
+		**/
         void reportJointStatuses(NameStatusMap actualJointStatuses, bool aValueChanged);
     };
     
diff --git a/source/RobotAPI/interface/units/PlatformUnitInterface.ice b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
index 3311d982a49ec15dcecec41268736b05c4b9374d..b7bde4e0880519b4011cd88ab3b3766899f94d5a 100644
--- a/source/RobotAPI/interface/units/PlatformUnitInterface.ice
+++ b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
@@ -30,20 +30,73 @@
 #include <Core/interface/core/BasicTypes.ice>
 
 module armarx
-{
+{	
+	/**
+	* Implements an interface to an PlatformUnit.
+	**/
     interface PlatformUnitInterface extends SensorActorUnitInterface
     {
+		/**
+		* moveTo moves the platform to a global pose specified by:
+		* @param targetPlatformPositionX Global x-coordinate of target position. 
+		* @param targetPlatformPositionY Global y-coordinate of target position.
+		* @param targetPlatformRotation Global orientation of platform, mostly yaw angle (rotation around z-axis). 
+		* @param postionalAccuracy Platform stops translating if distance to target position gets lower than this threshhold.
+		* @param orientationalAccuracy Platform stops rotating if distance from current to target orientation gets lower than this threshhold.
+		**/
         void moveTo(float targetPlatformPositionX, float targetPlatformPositionY, float targetPlatformRotation, float positionalAccuracy, float orientationalAccuracy);
+        /**
+		* move moves the platform with given velocities.
+		* @param targetPlatformVelocityX x-coordinate of target velocity defined in platform root. 
+		* @param targetPlatformVelocityY y-coordinate of target velocity defined in platform root. 
+		* @param targetPlatformVelocityRotation Target orientational velocity defined in platform root. 
+		**/
         void move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation);
+        /**
+		* moveRelative moves to a pose defined in platform coordinates.
+		* @param targetPlatformOffsetX x-coordinate of target position defined in platform root. 
+		* @param targetPlatformOffsetY y-coordinate of target position defined in platform root.
+		* @param targetPlatformOffsetRotation Target orientation of platform defined in platform root. 
+		* @param postionalAccuracy Platform stops translating if distance to target position gets lower than this threshhold.
+		* @param orientationalAccuracy Platform stops rotating if distance from current to target orientation gets lower than this threshhold.
+		**/
         void moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy);
+        /**
+		* setMaxVelocities allows to specify max velocities in translation and orientation.
+		* @param positionalVelocity Max translation velocity. 
+		* @param orientationalVelocity Max orientational velocity. 
+		**/
         void setMaxVelocities(float positionalVelocity, float orientationalVelocity);
+        /**
+		* stopPlatform stops the movements of the platform.
+		**/
         void stopPlatform();
     };
-
+	/**
+	* Implements an interface to an PlatformUnitListener.
+	**/
     interface PlatformUnitListener
     {
+		/**
+		* reportPlatformPose reports current platform pose.
+		* @param currentPlatformPositionX Global x-coordinate of current platform position.
+		* @param currentPlatformPositionY Global y-coordinate of current platform position.
+		* @param currentPlatformRotation Current global orientation of platform.
+		**/
         void reportPlatformPose(float currentPlatformPositionX, float currentPlatformPositionY, float currentPlatformRotation);
+        /**
+		* reportNewTargetPose reports target platform pose.
+		* @param newPlatformPositionX Global x-coordinate of target platform position.
+		* @param newPlatformPositionY Global y-coordinate of target platform position.
+		* @param newPlatformRotation Target global orientation of platform.
+		**/
         void reportNewTargetPose(float newPlatformPositionX, float newPlatformPositionY, float newPlatformRotation);
+        /**
+		* reportPlatformVelocity reports current platform velocities.
+		* @param currentPlatformVelocityX x-coordinate of current velocity defined in platform root. 
+		* @param currentPlatformVelocityY y-coordinate of current velocity defined in platform root. 
+		* @param currentPlatformVelocityRotation Current orientational velocity defined in platform root. 
+		**/
         void reportPlatformVelocity(float currentPlatformVelocityX, float currentPlatformVelocityY, float currentPlatformVelocityRotation);
     };
 
diff --git a/source/RobotAPI/interface/units/TCPControlUnit.ice b/source/RobotAPI/interface/units/TCPControlUnit.ice
index fafcd2735017c5d78a9994eae3177e03b9f3ca0b..a8adef62e2774e6880cf3e8063383fd745be8896 100644
--- a/source/RobotAPI/interface/units/TCPControlUnit.ice
+++ b/source/RobotAPI/interface/units/TCPControlUnit.ice
@@ -35,23 +35,52 @@
 
 module armarx
 {
-
+	/**
+	* Implements an interface to an TCPControlUnit. The TCPControlUnit uses differential IK in order to move the TCP a target pose.
+	**/
     interface TCPControlUnitInterface extends armarx::SensorActorUnitInterface, armarx::KinematicUnitListener
     {
+		/**
+		* setCycleTime allows to set the cycle time with which the TCPControlUnit solves the IK and moves the TCP within a periodic task.
+		* @param milliseconds Cycle time in milliseconds.
+		**/
         void setCycleTime(int milliseconds);
+        /**
+		* setTCPVelocity allows to set a new target velocities in task space for the TCP.
+		* @param robotNodeSetName Name of kinematic chain/nodeset.
+		* @param tcpNodeName Name of TCP node within the kinematic chain/nodeset.
+		* @param translationVelocity Translational velocity in task space in x, y, and z.
+		* @param orientationVelocityRPY Orientational velocity in task space in roll, pitch, yaw.
+		**/
         void setTCPVelocity(string robotNodeSetName, string tcpNodeName, FramedDirectionBase translationVelocity, FramedDirectionBase orientationVelocityRPY);
 
     };
 
-
+	/**
+	* [FramedPoseBaseMap] defines a data container which contains nodes, identified by name, and corresponding FramedPoses.
+	**/
     dictionary<string, FramedPoseBase> FramedPoseBaseMap;
+    /**
+	* Implements an interface to an TCPControlUnitListener. 
+	**/
     interface TCPControlUnitListener
     {
+		/**
+		* reportTCPPose reports TCPs and corresponding current FramedPoses.
+		* @param tcpPoses Map of TCP node names and corresponding current FramedPoses. 
+		**/
         void reportTCPPose(FramedPoseBaseMap tcpPoses);
+        /**
+		* reportTCPVelocities reports current velocities of TCPs.
+		* @param tcpTranslationVelocities Map of TCP node names and corresponding current translational velocities. 
+		* @param tcpOrienationVelocities Map of TCP node names and corresponding current orientational velocities. 
+		**/
         void reportTCPVelocities(FramedDirectionMap tcpTranslationVelocities, FramedDirectionMap tcpOrienationVelocities);
 
     };
-
+	/**
+	* Implements an interface to an TCPControlUnitObserver. 
+	**/
     interface TCPControlUnitObserverInterface extends ObserverInterface, TCPControlUnitListener
     {
 
diff --git a/source/RobotAPI/interface/units/UnitInterface.ice b/source/RobotAPI/interface/units/UnitInterface.ice
index 4ef54fc1cca5fe71e588beb7026efb227bdef341..853e3c755afcf91344b36ab2969e0ce735559f40 100644
--- a/source/RobotAPI/interface/units/UnitInterface.ice
+++ b/source/RobotAPI/interface/units/UnitInterface.ice
@@ -28,14 +28,22 @@
 
 module armarx
 {
+	/**
+	* @throws ResourceUnavailableException Raised if the unit resource is not available.
+	**/
     exception ResourceUnavailableException extends UserException
     {
     };
 
+	/**
+	* @throws ResourceNotOwnedException Raised if the unit resource is not owned.
+	**/
     exception ResourceNotOwnedException extends UserException
     { 
     };
-
+	/**
+	* [UnitExecutionState] defines whether a Unit is constructed, initialized, started or stopped.
+	**/
     enum UnitExecutionState
     {
         eUnitConstructed,
@@ -43,22 +51,50 @@ module armarx
         eUnitStarted,
         eUnitStopped
     };
-
+    
+	/**
+	* Implements an interface to a UnitExecutionManagement.
+	**/
     interface UnitExecutionManagementInterface
     {
+		/**
+		* init is called to initialize the unit before starting it. Virtual method which has to implemented by components inheriting from this interface.
+		**/
         void init();
+        /**
+		* start is called to start the unit. Virtual method which has to implemented by components inheriting from this interface.
+		**/
         void start();
+        /**
+		* stop is called to stopthe unit. Virtual method which has to implemented by components inheriting from this interface.
+		**/
         void stop();
         
+        /**
+		* getExecutionState returns the execution state of the unit. Virtual method which has to implemented by components inheriting from this interface.
+		* @return UnitExecutionState
+		* @see UnitExecutionState
+		**/
         UnitExecutionState getExecutionState();
     };
-
+    
+	/**
+	* Implements an interface to a UnitResourceManagement.
+	**/
     interface UnitResourceManagementInterface
     {
+		/**
+		* request is called to grant another component exclusive access to this unit.
+		**/
         void request() throws ResourceUnavailableException;
+        /**
+		* release has to be called by an owning unit in order to allow other components to access this unit.
+		**/
         void release() throws ResourceNotOwnedException;
     };
-
+	/**
+	* Implements an interface to a SensorActorUnit. The SensorActorUnit is the the base component for subsequent actuation and sensor components.
+	**/
     interface SensorActorUnitInterface extends UnitExecutionManagementInterface, UnitResourceManagementInterface
     {
     };
diff --git a/source/RobotAPI/interface/units/WeissHapticUnit.ice b/source/RobotAPI/interface/units/WeissHapticUnit.ice
index 447e64d1917db597fe3693db4a4a01fe9cd86f5d..414a1aae8cead59d120307ccdc336d12928232b9 100644
--- a/source/RobotAPI/interface/units/WeissHapticUnit.ice
+++ b/source/RobotAPI/interface/units/WeissHapticUnit.ice
@@ -28,10 +28,24 @@
 
 module armarx
 {
+	/**
+	* Implements an interface to a HapticUnit for the tactile sensors from Weiss Robotics.
+	**/
     interface WeissHapticUnitInterface extends armarx::HapticUnitInterface
     {
+		/**
+		* setDeviceTag allows to rename device.
+		* @param deviceName Original name of tactile sensor device.
+		* @param tag New name of tactile sensor device.
+		**/
         void setDeviceTag(string deviceName, string tag);
+        /**
+		* startLogging starts logging when devices are initialized and started.
+		**/
         void startLogging();
+        /**
+		* stopLogging stop logging when devices are initialized and started.
+		**/
         void stopLogging();
     };