diff --git a/.gitignore b/.gitignore
index eecacf5486e21fa012af192d1dcae6574a9d4cfe..505aa0582bff911578c2b61139bddc917efcabbf 100644
--- a/.gitignore
+++ b/.gitignore
@@ -60,3 +60,4 @@ scenarios/*/*/ttyACM*.log
 scenarios/*/*/config/datapath.cfg
 
 *.icegrid.xml
+data/RobotAPI/logs
diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index bdfd4460bdbaddacd22a1b59de3f4746741d43fa..0537f558b61fc877967992b3a2ca57e1eb959362 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -57,6 +57,14 @@
             propertyName="IMUObserverName"
             propertyIsOptional="true"
             propertyDefaultValue="InertialMeasurementUnitObserver" />
+        <Proxy include="RobotAPI/interface/units/OrientedTactileSensorUnit.h"
+            humanName="Oriented Tactile Sensor Unit Observer"
+            typeName="OrientedTactileSensorUnitObserverInterfacePrx"
+            memberName="orientedTactileSensorUnitObserver"
+            getterName="getOrientedTactileSensorUnitObserver"
+            propertyName="OrientedTactileSensorUnitObserverName"
+            propertyIsOptional="true"
+            propertyDefaultValue="OrientedTactileSensorUnitObserver" />
         <Proxy include="RobotAPI/interface/units/ForceTorqueUnit.h"
             humanName="Force Torque Unit Observer"
             typeName="ForceTorqueUnitObserverInterfacePrx"
diff --git a/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
index 443162d742ef69c2d3d6bedc728ac05494254fc2..23c06bee20258339aa1284f1d0c6d9ba2bef21ff 100644
--- a/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
+++ b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="utf-8"?>
-<scenario name="OrientedTactileSensor" lastChange="2017-03-06.17:30:06" creation="2017-02-27.01:48:55 PM" globalConfigName="./config/global.cfg" package="RobotAPI">
-	<application name="OrientedTactileSensorUnitApp" instance="" package="RobotAPI"/>
-	<application name="OrientedTactileSensorUnitObserverApp" instance="" package="RobotAPI"/>
+<scenario name="OrientedTactileSensor" creation="2017-02-27.01:48:55 PM" globalConfigName="./config/global.cfg" package="RobotAPI">
+	<application name="OrientedTactileSensorUnitApp" instance="" package="RobotAPI" enabled="true"/>
+	<application name="OrientedTactileSensorUnitObserverApp" instance="" package="RobotAPI" enabled="true"/>
 </scenario>
 
diff --git a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
index 1ec6764f36d51be5c6b8501bb6ef5e5046114c79..01ddb0e7eb3ba7c1413f097f884ed698ec30bd0c 100644
--- a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
+++ b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
@@ -79,7 +79,15 @@
 #  - Default:            65524 3 12 65534 65534 1 1208 119 58726 1000 943 
 #  - Case sensitivity:   no
 #  - Required:           no
-ArmarX.OrientedTactileSensorUnit.CalibrationData = 65524 3 12 65534 65534 1 1208 119 58726 1000 943
+ArmarX.OrientedTactileSensorUnit.CalibrationData = 65535 65535 65535 65535 65535 65535 65535 65535 65535 65535 65535
+
+
+# ArmarX.OrientedTactileSensorUnit.DebugDrawerTopicName:  Name of the debug drawer topic that should be used
+#  Attributes:
+#  - Default:            DebugDrawerUpdates
+#  - Case sensitivity:   no
+#  - Required:           no
+# ArmarX.OrientedTactileSensorUnit.DebugDrawerTopicName = DebugDrawerUpdates
 
 
 # ArmarX.OrientedTactileSensorUnit.EnableProfiling:  enable profiler which is used for logging performance events
@@ -143,7 +151,7 @@ ArmarX.OrientedTactileSensorUnit.SerialInterfaceDevice = /dev/ttyACM0
 #  - Default:            OrientedTactileSensorValues
 #  - Case sensitivity:   no
 #  - Required:           no
-# ArmarX.OrientedTactileSensorUnit.TopicName = OrientedTactileSensorValues
+ArmarX.OrientedTactileSensorUnit.TopicName = OrientedTactileSensorValues
 
 
 # ArmarX.OrientedTactileSensorUnit.calibrateSensor:  
@@ -154,6 +162,14 @@ ArmarX.OrientedTactileSensorUnit.SerialInterfaceDevice = /dev/ttyACM0
 ArmarX.OrientedTactileSensorUnit.calibrateSensor = 0
 
 
+# ArmarX.OrientedTactileSensorUnit.logData:  Custom Property
+#  Attributes:
+#  - Default:            ::NOT_DEFINED::
+#  - Case sensitivity:   no
+#  - Required:           no
+ArmarX.OrientedTactileSensorUnit.logData = 1
+
+
 # ArmarX.RedirectStdout:  Redirect std::cout and std::cerr to ArmarXLog
 #  Attributes:
 #  - Default:            1
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 0940cd7b20da5c3612ec4da86a2bcf4516d91e47..b262e15b770a516fa618752979ff36ce2e398bb5 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -265,7 +265,7 @@ namespace armarx
                     double influencePrev = 1.0f - (double)(time - prevIt->first) / deltaT;
                     //                    ARMARX_INFO_S << "Interpolating: " << time << " prev: " << time - prevIt->first << " next: " << it->first - time << VAROUT(influenceNext) << VAROUT(influencePrev) << " sum: " << (influenceNext + influencePrev);
                     auto jointIt = prevIt->second.jointMap.begin();
-                    for (auto & joint : config.jointMap)
+                    for (auto& joint : config.jointMap)
                     {
                         joint.second = joint.second * influenceNext + jointIt->second * influencePrev;
                         jointIt++;
@@ -295,7 +295,7 @@ namespace armarx
         {
             timestamp = IceUtil::Time::now().toMicroSeconds();
         }
-
+        ARMARX_DEBUG << deactivateSpam(1) << "Got new jointangles: " << jointAngles << " from timestamp " << IceUtil::Time::microSeconds(timestamp).toDateTime() << " a value changed: " << aValueChanged;
         if (aValueChanged)
         {
             {
@@ -352,7 +352,7 @@ namespace armarx
         auto packages = armarx::Application::GetProjectDependencies();
         packages.push_back(Application::GetProjectName());
 
-        for (const std::string & projectName : packages)
+        for (const std::string& projectName : packages)
         {
             if (projectName.empty())
             {
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index 10e5e49547a834e2b6577333eb63433ef0d38a5c..bdf8dc94e2068fb373f5f5984604d262bb78d1a1 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -296,14 +296,14 @@ void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelN
         boost::unordered_map< ::std::string, ::armarx::VariantBasePtr> map;
         if (timestamp < 0)
         {
-            for (const auto & it : nameValueMap)
+            for (const auto& it : nameValueMap)
             {
                 map[it.first] = new Variant(it.second);
             }
         }
         else
         {
-            for (const auto & it : nameValueMap)
+            for (const auto& it : nameValueMap)
             {
                 map[it.first] = new TimedVariant(new Variant(it.second), IceUtil::Time::microSeconds(timestamp));
             }
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
index 906e90f79bb7593b36e23e985357c78076fc2512..7762139ad1d7ac849a06f111ecccd5982fe8bf7e 100644
--- a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
@@ -74,7 +74,7 @@ void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressur
 
     offerOrUpdateDataField(channelName, "pressure", Variant(pressure), "current pressure");
     QuaternionPtr orientationQuaternion =  new Quaternion(qw, qx, qy, qz);
-    offerOrUpdateDataField(channelName, "orientation", orientationQuaternion, "current oriantation");
+    offerOrUpdateDataField(channelName, "orientation", orientationQuaternion, "current orientation");
     offerOrUpdateDataField(channelName, "rotationRate", Variant(rotationRate), "current rotationRate");
     offerOrUpdateDataField(channelName, "pressureRate", Variant(pressureRate), "current pressureRate");
     offerOrUpdateDataField(channelName, "accelerationRate", Variant(accelerationRate), "current accelerationRate");
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index 8ca00d27d61b8207e51550420e4701f75321ad97..adc615c85bd1fb9281e75e21fb91dea991c30c18 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -414,7 +414,7 @@ namespace armarx
             //            ARMARX_INFO << deactivateSpam(1) << "clamped new Vel: " << VAROUT(nextv);
             if (sign(currentV) != sign(nextv))
             {
-                ARMARX_INFO << deactivateSpam(1) << "wrong sign: stopping"; //stop now
+                //                ARMARX_INFO << deactivateSpam(1) << "wrong sign: stopping"; //stop now
                 nextv = 0;
             }
         }
@@ -429,7 +429,7 @@ namespace armarx
         {
             //the area between soft and hard limits is sticky
             //the controller can only move out of it (not further in)
-            ARMARX_DEBUG << deactivateSpam(1) << "Soft limit violation. " << softLimitViolation << VAROUT(nextv);
+            //            ARMARX_DEBUG << deactivateSpam(1) << "Soft limit violation. " << softLimitViolation << VAROUT(nextv);
             return 0;
         }
 
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
index 1fdb9568e51096ab1b9bbfe72358840e5c289c95..1d1b5d255d3bf9fb139013ce65cfb73b9830e360 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
@@ -57,6 +57,18 @@ namespace armarx
 
         virtual void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
     };
+
+    template<class SensorValueType>
+    class SensorDeviceTemplate : public virtual SensorDevice
+    {
+        static_assert(std::is_base_of<SensorValueBase, SensorValueType>::value, "SensorValueType has to inherit SensorValueBase");
+    public:
+        SensorDeviceTemplate(const std::string& name): DeviceBase(name), SensorDevice(name) {}
+
+        virtual const SensorValueType* getSensorValue() const final;
+
+        SensorValueType sensorValue;
+    };
 }
 
 //inline functions
@@ -73,6 +85,12 @@ namespace armarx
     }
 
     inline void SensorDevice::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) {}
+
+    template<class SensorValueType>
+    inline const SensorValueType* SensorDeviceTemplate<SensorValueType>::getSensorValue() const
+    {
+        return &sensorValue;
+    }
 }
 
 #endif
diff --git a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
index 9af95336c1343d0f1ea4691627ed7b4499b6c406..8ee7ee817971ce0dcb13a9cc90c296f32672777c 100644
--- a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
@@ -75,6 +75,18 @@ namespace armarx
         void rtActivate();
         void rtDeactivate();
     };
+
+    template<class ControlTargetType>
+    class JointControllerTemplate : public virtual JointController
+    {
+        static_assert(std::is_base_of<ControlTargetBase, ControlTargetType>::value, "ControlTargetType has to inherit SensorValueBase");
+    public:
+        using JointController::JointController;
+
+        virtual ControlTargetType* getControlTarget() final;
+
+        ControlTargetType controlTarget;
+    };
 }
 
 //inline functions
@@ -152,5 +164,11 @@ namespace armarx
         rtPostDeactivateController();
         rtResetTarget();
     }
+
+    template<class ControlTargetType>
+    inline ControlTargetType* JointControllerTemplate<ControlTargetType>::getControlTarget()
+    {
+        return &controlTarget;
+    }
 }
 #endif
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 849bfbca61ec6bcb9c6a595c49307d6530dbc969..358bdd97eea725b3c203d7520cb26a44d033d93c 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -28,7 +28,10 @@
 #include <VirtualRobot/Nodes/ForceTorqueSensor.h>
 #include <VirtualRobot/Robot.h>
 #include <VirtualRobot/RobotNodeSet.h>
-
+#include <VirtualRobot/Visualization/VisualizationNode.h>
+#include <VirtualRobot/Obstacle.h>
+#include <VirtualRobot/SceneObjectSet.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 #include <ArmarXCore/core/system/DynamicLibrary.h>
 #include <ArmarXCore/observers/variant/TimestampVariant.h>
@@ -39,6 +42,7 @@
 #include "Units/InertialMeasurementSubUnit.h"
 #include "Units/KinematicSubUnit.h"
 #include "Units/PlatformSubUnit.h"
+#include "SensorValues/SensorValue1DoFActuator.h"
 
 namespace std
 {
@@ -320,8 +324,14 @@ namespace armarx
             for (const auto& dev : sensorDevices.values())
             {
                 sensorDevicesConstPtr[dev->getDeviceName()] = dev;
+                const SensorValue1DoFActuatorPosition* sv = dev->getSensorValue()->asA<SensorValue1DoFActuatorPosition>();
+                if (sv && rtRobot->hasRobotNode(dev->getDeviceName()))
+                {
+                    nodePositionSensorMap[rtRobot->getRobotNode(dev->getDeviceName())] = sv;
+                }
             }
         }
+
         ARMARX_VERBOSE << "ControlDevices:\n" << controlDevices.keys();
         ARMARX_VERBOSE << "SensorDevices:\n" << sensorDevices.keys();
         ctrlModeGroups.groupIndices.assign(getNumberOfControlDevices(), IndexSentinel());
@@ -712,7 +722,7 @@ namespace armarx
         publishNewSensorDataTime = TimeUtil::GetTime();
         publisherTask = new PublisherTaskT([&] {publish({});}, pubtimestep, false, getName() + "_PublisherTask");
         ARMARX_INFO << "starting publisher with timestep " << pubtimestep;
-        publisherTask->setDelayWarningTolerance(15);
+        publisherTask->setDelayWarningTolerance(100);
         publisherTask->start();
         ARMARX_INFO << "finishUnitInitialization...done! " << (MicroNow() - beg).count() << " us";
     }
@@ -932,7 +942,17 @@ namespace armarx
             {
                 if (nJointCtrl)
                 {
+                    auto start = MicroNow();
                     nJointCtrl->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration);
+                    auto duration = MicroNow() - start;
+                    if (duration.count() > 500)
+                    {
+                        ARMARX_ERROR << deactivateSpam(5) << "The NJointController " << nJointCtrl->getClassName() << " took " << duration.count() << " µs to run!";
+                    }
+                    else if (duration.count() > 50)
+                    {
+                        ARMARX_WARNING << deactivateSpam(5) << "The NJointController " << nJointCtrl->getClassName() << " took " << duration.count() << " µs to run!";
+                    }
                 }
             }
             catch (...)
@@ -1567,6 +1587,7 @@ namespace armarx
             {
                 if (robotUnitObs->getState() >= eManagedIceObjectStarted)
                 {
+                    //                    TIMING_START(RobotUnitObsUpdate);
                     robotUnitObs->offerOrUpdateDataFieldsFlatCopy(robotUnitObs->additionalChannel, debugObserverMap);
                     robotUnitObs->updateChannel(robotUnitObs->additionalChannel);
                     robotUnitObs->offerOrUpdateDataFieldsFlatCopy(robotUnitObs->timingChannel, timingMap);
@@ -1575,6 +1596,7 @@ namespace armarx
                     robotUnitObs->updateChannel(robotUnitObs->controlDevicesChannel);
                     robotUnitObs->offerOrUpdateDataFieldsFlatCopy(robotUnitObs->sensorDevicesChannel, sensorDevMap);
                     robotUnitObs->updateChannel(robotUnitObs->sensorDevicesChannel);
+                    //                    TIMING_CEND(RobotUnitObsUpdate, 2);
                 }
                 debugObserverBatchPrx->ice_flushBatchRequests();
             };
@@ -2282,6 +2304,219 @@ namespace armarx
         addPropertyUser(PropertyUserPtr::dynamicCast(robotUnitObs));
     }
 
+    const RobotUnit::SelfCollisionAvoidanceData& RobotUnit::getSelfCollisionAvoidanceData() const
+    {
+        return selfCollisionAvoidanceData;
+    }
+
+    const VirtualRobot::RobotPtr& RobotUnit::getRtRobot() const
+    {
+        return rtRobot;
+    }
+
+    void armarx::RobotUnit::startSelfCollisionAvoidance()
+    {
+        // fill self collision avoidance data
+        // clone robot
+        //        VirtualRobot::CollisionCheckerPtr cc(new VirtualRobot::CollisionChecker());
+        selfCollisionAvoidanceData.robot = this->robot->clone(robot->getName() + "_Clone_SelfCollisionAvoidance");
+
+        selfCollisionAvoidanceData.minDistance = getProperty<float>("MinSelfDistance").getValue();
+
+        // set collision models
+        std::map<std::string, VirtualRobot::SceneObjectSetPtr> colModels;
+        const std::string colModelsString = getProperty<std::string>("SelfCollisionModelPairs").getValue();
+        std::vector<std::string> entries;
+        if (!colModelsString.empty())
+        {
+            entries = armarx::Split(colModelsString, ";", true, true);
+        }
+        for (std::string entry : entries)
+        {
+            // Removing parentheses
+            boost::trim_if(entry, boost::is_any_of("{}"));
+            std::string first = armarx::Split(entry, ",", true, true)[0];
+            std::string second = armarx::Split(entry, ",", true, true)[1];
+
+            if (!selfCollisionAvoidanceData.robot->hasRobotNodeSet(first) && first != "FLOOR")
+            {
+                ARMARX_WARNING << "No RobotNodeSet with name '" << first << "' defined in " << robotFileName << ". Skipping.";
+                continue;
+            }
+            if (!selfCollisionAvoidanceData.robot->hasRobotNodeSet(second) && second != "FLOOR")
+            {
+                ARMARX_WARNING << "No RobotNodeSet with name '" << second << "' defined in " << robotFileName << ". Skipping.";
+                continue;
+            }
+
+            selfCollisionAvoidanceData.modelPairs.push_back(std::make_pair(first, second));
+
+            auto inflateNodeSet = [&](VirtualRobot::RobotNodeSetPtr nodeset)
+            {
+                for (int i = 0; i < static_cast<int>(nodeset->getSize()); ++i)
+                {
+                    if (nodeset->getNode(i)->getCollisionModel())
+                    {
+                        nodeset->getNode(i)->getCollisionModel()->inflateModel(selfCollisionAvoidanceData.minDistance / 2.f);
+                    }
+                    else
+                    {
+                        ARMARX_WARNING << "Self Collision Avoidance: No collision model found for '" << nodeset->getNode(i)->getName() << "'";
+                    }
+                }
+            };
+
+            if (colModels.find(first) == colModels.end() && first != "FLOOR")
+            {
+                inflateNodeSet(selfCollisionAvoidanceData.robot->getRobotNodeSet(first));
+                colModels[first] = selfCollisionAvoidanceData.robot->getRobotNodeSet(first);
+            }
+            if (colModels.find(second) == colModels.end() && second != "FLOOR")
+            {
+                inflateNodeSet(selfCollisionAvoidanceData.robot->getRobotNodeSet(second));
+                colModels[second] = selfCollisionAvoidanceData.robot->getRobotNodeSet(second);
+            }
+        }
+
+        // Adding scene-object representing the floor, to avoid collisions with the floor
+        VirtualRobot::SceneObjectSetPtr boxSet(new VirtualRobot::SceneObjectSet("FLOOR"));
+        float floorHeight = 10.0f;
+        VirtualRobot::ObstaclePtr boxOb = VirtualRobot::Obstacle::createBox(20000, 20000, floorHeight);
+        boxOb->getCollisionModel()->inflateModel(selfCollisionAvoidanceData.minDistance / 2);
+        Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
+        //        globalPose(2, 3) = floorHeight / 2 - (selfCollisionAvoidanceData.minDistance + 0.1f);
+
+        //        VirtualRobot::SceneObjectPtr box(new VirtualRobot::SceneObject("FloorCollisionBox", boxOb->getVisualization(), boxOb->getCollisionModel()));
+        boxOb->setGlobalPose(globalPose);
+        boxSet->addSceneObject(boxOb);
+        colModels["FLOOR"] = boxSet;
+
+        selfCollisionAvoidanceData.collisionModels = colModels;
+
+        // start the task
+        int colChecksPerSecond = getProperty<int>("SelfCollisionChecksPerSecond").getValue();
+        int intervalMs = (1.0 / (float) colChecksPerSecond) * 1000;
+        selfCollisionAvoidanceTask = new PeriodicTask<RobotUnit>(this, &RobotUnit::updateSelfCollisionAvoidance, intervalMs, false, "SelfCollisionAvoidance");
+        selfCollisionAvoidanceTask->setDelayWarningTolerance(200);
+        selfCollisionAvoidanceTask->start();
+
+        // logging
+        std::string validCollisionModelsString;
+        for (auto it = selfCollisionAvoidanceData.collisionModels.begin(); it != selfCollisionAvoidanceData.collisionModels.end(); ++it)
+        {
+            validCollisionModelsString += it->first;
+            validCollisionModelsString += " ";
+        }
+        ARMARX_IMPORTANT << "Started Self Collision Avoidance:" <<
+                         "\nRobot: " << selfCollisionAvoidanceData.robot->getName() <<
+                         "\nMin. allowed distance: " << selfCollisionAvoidanceData.minDistance << " mm" <<
+                         "\nCollision models: " << validCollisionModelsString <<
+                         "\nCollision checks per second: " << colChecksPerSecond << " (every " << intervalMs << " ms)";
+    }
+
+    void armarx::RobotUnit::stopSelfCollisionAvoidance()
+    {
+        ARMARX_IMPORTANT << "Stopping Self Collision Avoidance.";
+        if (selfCollisionAvoidanceTask)
+        {
+            selfCollisionAvoidanceTask->stop();
+        }
+    }
+
+    void armarx::RobotUnit::updateSelfCollisionAvoidance()
+    {
+        if (emergencyStop || !getProperty<bool>("EnableSelfCollisionAvoidance").getValue())
+        {
+            return;
+        }
+        if (!devicesReadyStates.count(state) || state != RobotUnitState::Running)
+        {
+            ARMARX_VERBOSE << deactivateSpam(1) << "Self Distance Check: Waiting for device initialization.";
+            return;
+        }
+
+        auto startTime = std::chrono::high_resolution_clock::now();
+        sensorAndControlBufferChanged(); // update the sensor read buffer
+        const SensorAndControl& sc =  getSensorAndControlBuffer();
+        // set joint values
+        {
+            auto guard = getGuard();
+            for (std::size_t i = 0; i < sc.sensors.size(); i++)
+            {
+                const std::unique_ptr<SensorValueBase>& sensorValueBase = sc.sensors.at(i);
+                // skip, if the current sensor device does not correspond to a joint.
+                std::string deviceName = sensorDevices.at(i)->getDeviceName();
+                if (!selfCollisionAvoidanceData.robot->hasRobotNode(deviceName))
+                {
+                    continue;
+                }
+                // skip, if the sensor value of the current sensor device can't provide a position.
+                if (!sensorValueBase->isA<SensorValue1DoFActuatorPosition>())
+                {
+                    continue;
+                }
+                SensorValue1DoFActuatorPosition* sv = sensorValueBase->asA<SensorValue1DoFActuatorPosition>();
+                if (sv)
+                {
+                    float position = sv->position;
+                    // set the joint value
+                    selfCollisionAvoidanceData.robot->getRobotNode(deviceName)->setJointValueNoUpdate(position);
+                }
+            }
+        }
+        selfCollisionAvoidanceData.robot->applyJointValues();
+        // check distances between all collision models
+        std::vector<VirtualRobot::SceneObjectSetPtr> conflictingColModels;
+        //        std::vector<float> distances;
+        std::string distancesString("All collision states:\n");
+        bool anyCollision = false;
+        for (auto it = selfCollisionAvoidanceData.modelPairs.begin(); it != selfCollisionAvoidanceData.modelPairs.end(); ++it)
+        {
+            bool collision = selfCollisionAvoidanceData.robot->getCollisionChecker()->checkCollision(selfCollisionAvoidanceData.collisionModels[it->first], selfCollisionAvoidanceData.collisionModels[it->second]);
+            anyCollision |= collision;
+            distancesString += "---- " + it->first + " <-> " + it->second + " in Collsition: " + (collision ? "True" : "False") +  "\n";
+            //            distances.push_back(distance);
+            if (collision)
+            {
+                conflictingColModels.push_back(selfCollisionAvoidanceData.collisionModels[it->first]);
+                conflictingColModels.push_back(selfCollisionAvoidanceData.collisionModels[it->second]);
+            }
+        }
+
+        auto endTime = std::chrono::high_resolution_clock::now();
+        std::chrono::duration<double> duration = std::chrono::duration_cast<std::chrono::duration<double>>(endTime - startTime);
+
+        if (anyCollision)
+        {
+            // collision
+            std::string conflictingColModelsString = "";
+            for (std::size_t i = 0; i < conflictingColModels.size(); i++)
+            {
+                conflictingColModelsString += conflictingColModels.at(i)->getName() + ((i % 2) ? ";" : "<->");
+            }
+
+            ARMARX_WARNING << "Self Distance Check: Collision. Entering EmergencyStop." <<
+                           "\nmin. allowed distance: " << selfCollisionAvoidanceData.minDistance << " mm" <<
+                           "\nConflicting collision models: " << conflictingColModelsString <<
+                           "\n" << distancesString <<
+                           "\nComputation time: " << (duration.count() * 1000) << " ms (max. allowed duration (as specified): " << (1000.0 / getProperty<int>("SelfCollisionChecksPerSecond").getValue()) << " ms)";;
+
+            setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+            getEmergencyStopMaster()->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+            // since at least one of the NJoint-Controllers is going to cause a collision, we just kick them all.
+            setActivateControllersRequest({});
+        }
+        else
+        {
+            // no collision
+            ARMARX_DEBUG << deactivateSpam(5) << "Self Distance Check: OK." <<
+                         "\nmin. allowed distance: " << selfCollisionAvoidanceData.minDistance << " mm" <<
+                         "\n" << distancesString <<
+                         "\nComputation time: " << (duration.count() * 1000) << " ms (max. allowed duration (as specified): " << (1000.0 / getProperty<int>("SelfCollisionChecksPerSecond").getValue()) << " ms)";
+        }
+
+    }
+
     void armarx::RobotUnit::icePropertiesUpdated(const std::set<std::string>& changedProperties)
     {
 
@@ -2346,6 +2581,10 @@ namespace armarx
                 try
                 {
                     robot = VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
+                    rtRobot = robot->clone(robot->getName());
+                    rtRobot->setUpdateCollisionModel(false);
+                    rtRobot->setUpdateVisualization(false);
+                    rtRobot->setThreadsafe(false);
                 }
                 catch (VirtualRobot::VirtualRobotException& e)
                 {
@@ -2438,12 +2677,14 @@ namespace armarx
         debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue());
         debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverTopicName").getValue());
         onConnectRobotUnit();
+        startSelfCollisionAvoidance();
         ARMARX_DEBUG << "RobotUnit::onConnectComponent()...done!";
     }
 
     void armarx::RobotUnit::onDisconnectComponent()
     {
         ARMARX_DEBUG << "RobotUnit::onDisconnectComponent()";
+        stopSelfCollisionAvoidance();
         onDisconnectRobotUnit();
         ARMARX_DEBUG << "RobotUnit::onDisconnectComponent()...done!";
     }
@@ -2488,6 +2729,13 @@ namespace armarx
             }
         }
         sensorAndControl.commitWrite();
+
+        for (auto& node : nodePositionSensorMap)
+        {
+            node.first->setJointValueNoUpdate(node.second->position);
+        }
+        rtRobot->applyJointValues();
+
         rtThreadTimingsSensorDevice->rtMarkRtUpdateSensorAndControlBufferEnd();
     }
 
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index 30f01306e708463772e4c75b906b39e3e021eef9..4dfa536d87343a7f7b604442c966801d08c7f2da 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -43,6 +43,9 @@
 
 #include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
 
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/SceneObjectSet.h>
+
 #include "Units/RobotUnitSubUnit.h"
 #include "JointControllers/JointController.h"
 #include "NJointControllers/NJointController.h"
@@ -61,6 +64,8 @@ namespace armarx
 
     class DynamicLibrary;
     typedef boost::shared_ptr<DynamicLibrary> DynamicLibraryPtr;
+
+    class SensorValue1DoFActuatorPosition;
     /**
      * @class RobotUnitPropertyDefinitions
      * @brief
@@ -152,6 +157,23 @@ namespace armarx
                 "Syntax: semicolon separated groups , each group is CSV of joints "
                 "e.g. j1,j2;j3,j4,j5 -> Group 1 = j1+j2 Group 2 = j3+j4+j5; No joint may be in two groups!"
             );
+
+
+            defineOptionalProperty<bool>(
+                "EnableSelfCollisionAvoidance", true,
+                "Whether self collision avoidance should be active or not.", PropertyDefinitionBase::eModifiable);
+            defineOptionalProperty<int>(
+                "SelfCollisionChecksPerSecond", 10,
+                "Frequency [Hz] of self collision checking (default = 10).", PropertyDefinitionBase::eConstant);
+            defineOptionalProperty<float>(
+                "MinSelfDistance", 20,
+                "Minimum allowed distance (mm) between each collision model before entering emergency stop (default = 20).", PropertyDefinitionBase::eConstant);
+            defineOptionalProperty<std::string>(
+                "SelfCollisionModelPairs", "",
+                "Comma seperated list of pairs of collision models that should be checked against each other by collision avoidance \n"
+                "(e.g. {rnsColModel1,rnsColModel2};{rnsColModel3,rnsColModel4}; .... "
+                "says that rnsColModel1 will be only checked against rnsColModel2 and rnsColModel3 will only be checked against rnsColModel4). \n"
+                "Following predefined descriptors can be used: 'FLOOR' which represents a virtual collision model of the floor.", PropertyDefinitionBase::eConstant);
         }
     };
 
@@ -345,6 +367,14 @@ namespace armarx
         };
         ControlDeviceHardwareControlModeGroups ctrlModeGroups;
 
+        struct SelfCollisionAvoidanceData
+        {
+            VirtualRobot::RobotPtr robot;
+            std::map<std::string, VirtualRobot::SceneObjectSetPtr> collisionModels;
+            float minDistance {0}; // min allowed distance (mm) between each collision model
+            std::vector<std::pair<std::string, std::string>> modelPairs; // list of pairs of collision models that should be checked agaisnt each other
+        };
+
         //robot
     private:
         boost::shared_ptr<VirtualRobot::Robot> robot;
@@ -353,6 +383,16 @@ namespace armarx
         std::string robotFileName;
         std::string robotPlatformName;
 
+        VirtualRobot::RobotPtr rtRobot;
+        VirtualRobot::RobotNodeSetPtr rtRobotJointSet, rtRobotBodySet;
+        std::map<VirtualRobot::RobotNodePtr, const SensorValue1DoFActuatorPosition*> nodePositionSensorMap;
+
+        SelfCollisionAvoidanceData selfCollisionAvoidanceData;
+        PeriodicTask<RobotUnit>::pointer_type selfCollisionAvoidanceTask;
+        void startSelfCollisionAvoidance();
+        void stopSelfCollisionAvoidance();
+        void updateSelfCollisionAvoidance();
+
         // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
         // util
         // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
@@ -642,6 +682,10 @@ namespace armarx
             return emergencyStopMaster;
         }
 
+        const SelfCollisionAvoidanceData& getSelfCollisionAvoidanceData() const;
+
+        const VirtualRobot::RobotPtr& getRtRobot() const;
+
     protected:
         const RobotUnitListenerPrx& getListenerProxy() const;
         void icePropertiesInitialized() override;
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
index f7b90992c8d7ccb446435bf12958049dd4762bbe..2d2fb710f5b84cba9f84258e8474d6ac629fe660 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
@@ -34,7 +34,7 @@ namespace armarx
     {
     public:
         DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
-        float position;
+        float position = 0.0f;
         virtual std::map<std::string, VariantBasePtr>   toVariants(const IceUtil::Time& timestamp) const override
         {
             return {{"position", {new TimedVariant{position, timestamp}}}};
@@ -44,7 +44,7 @@ namespace armarx
     {
     public:
         DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
-        float velocity;
+        float velocity = 0.0f;
         virtual std::map<std::string, VariantBasePtr>   toVariants(const IceUtil::Time& timestamp) const override
         {
             return {{"velocity", {new TimedVariant{velocity, timestamp}}}};
@@ -54,7 +54,7 @@ namespace armarx
     {
     public:
         DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
-        float acceleration;
+        float acceleration = 0.0f;
         virtual std::map<std::string, VariantBasePtr>   toVariants(const IceUtil::Time& timestamp) const override
         {
             return {{"acceleration", {new TimedVariant{acceleration, timestamp}}}};
@@ -64,7 +64,7 @@ namespace armarx
     {
     public:
         DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
-        float torque;
+        float torque = 0.0f;
         virtual std::map<std::string, VariantBasePtr>   toVariants(const IceUtil::Time& timestamp) const override
         {
             return {{"torque", {new TimedVariant{torque, timestamp}}}};
@@ -95,7 +95,7 @@ namespace armarx
     {
     public:
         DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
-        float motorCurrent;
+        float motorCurrent = 0.0f;
         virtual std::map<std::string, VariantBasePtr>   toVariants(const IceUtil::Time& timestamp) const override
         {
             return {{"motorCurrent", {new TimedVariant{motorCurrent, timestamp}}}};
@@ -105,7 +105,7 @@ namespace armarx
     {
     public:
         DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
-        float motorTemperature;
+        float motorTemperature = 0.0f;
         virtual std::map<std::string, VariantBasePtr>   toVariants(const IceUtil::Time& timestamp) const override
         {
             return {{"motorTemperature", {new TimedVariant{motorTemperature, timestamp}}}};
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index 73226beb6b36c407978a03d59e3ceb9e261d2fd2..4ee7f8ec607a5754a93a30c1a1cb49c040932b69 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -115,6 +115,7 @@ void HokuyoLaserUnit::onConnectComponent()
     }
 
     task = new PeriodicTask<HokuyoLaserUnit>(this, &HokuyoLaserUnit::updateScanData, updatePeriod, false, "HokuyoLaserScanUpdate");
+    task->setDelayWarningTolerance(100);
     task->start();
 }
 
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt b/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt
index eefa4ca3137b61c4c20707c2592e5096437a755e..e362b40cd817888054e4c6f5b2716373f44feeac 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/CMakeLists.txt
@@ -13,7 +13,7 @@ set(LIB_NAME       OrientedTactileSensor)
 
 
 
-set(LIBS RobotAPIUnits ArmarXCoreObservers ArmarXCoreEigen3Variants)
+set(LIBS SimpleJsonLogger RobotAPIUnits ArmarXCoreObservers ArmarXCoreEigen3Variants)
 
 set(LIB_FILES
     OrientedTactileSensorUnit.cpp
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
index 2820435ebabc2a9853c838518430fc9e13f82a22..d1b874e27e70b15e10522a04039359ed0aeba32b 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
@@ -1,10 +1,12 @@
 #include "OrientedTactileSensorUnit.h"
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 #include <termios.h>
 #include <sys/ioctl.h>
 #include <fcntl.h>
 #include <math.h>
 #include <ArmarXCore/core/application/Application.h>
 #include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <RobotAPI/libraries/core/Pose.h>
 
 using namespace armarx;
 
@@ -14,22 +16,42 @@ OrientedTactileSensorUnit::OrientedTactileSensorUnit()
     sampleIndexRotation = 0;
     sampleIndexPressure = 0;
     sampleIndexAcceleration = 0;
+    sampleIndexPressureRate = 0;
 }
 
 void OrientedTactileSensorUnit::onInitComponent()
 {
-    maxSamplesRotation = getProperty<std::size_t>("SamplesRotation").getValue();
-    maxSamplesPressure = getProperty<std::size_t>("SamplesPressure").getValue();
-    maxSamplesAcceleration = getProperty<std::size_t>("SamplesAcceleration").getValue();
+    //logger part
+    if (getProperty<bool>("logData").getValue())
+    {
+        IceUtil::Time now = IceUtil::Time::now();
+        time_t timer = now.toSeconds();
+        struct tm* ts;
+        char buffer[80];
+        ts = localtime(&timer);
+        strftime(buffer, 80, "%Y-%m-%d-%H-%M-%S", ts);
+        std::string packageName = "RobotAPI";
+        armarx::CMakePackageFinder finder(packageName);
+        std::string dataDir = finder.getDataDir() + "/" + packageName + "/logs/";
+        std::string filename = dataDir +  buffer + std::string("_data")  + ".json";
+        //ARMARX_IMPORTANT << filename;
+
+        logger.reset(new SimpleJsonLogger(filename, true));
+        prefix = std::string(buffer);
+    }
+    maxSamplesRotation = stoi(getProperty<std::string>("SamplesRotation").getValue());
+    maxSamplesPressure = stoi(getProperty<std::string>("SamplesPressure").getValue());
+    maxSamplesAcceleration = stoi(getProperty<std::string>("SamplesAcceleration").getValue());
 
     std::string topicName = getProperty<std::string>("TopicName").getValue();
     offeringTopic(topicName);
 
     //open serial port
     std::string portname = getProperty<std::string>("SerialInterfaceDevice").getValue();
-    arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+    arduinoIn.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+    arduinoOut.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
 
-    fd = open(portname.c_str(), O_RDONLY | O_NOCTTY);
+    fd = open(portname.c_str(), O_RDWR | O_NOCTTY);
     struct termios toptions;
 
     /* Get currently set options for the tty */
@@ -55,7 +77,7 @@ void OrientedTactileSensorUnit::onInitComponent()
 
     ARMARX_INFO << "opening device " << getProperty<std::string>("SerialInterfaceDevice").getValue();
 
-    if (!arduino.is_open())
+    if (!arduinoIn.is_open())
     {
 
         throw LocalException("Cannot open Arduino on ") << getProperty<std::string>("SerialInterfaceDevice").getValue();
@@ -66,6 +88,7 @@ void OrientedTactileSensorUnit::onInitComponent()
     //wait for the Arduino to reboot
     usleep(4000000);
     std::string arduinoLine;
+
     //wait for the IMU to be calibrated or load calibration
     ARMARX_INFO << "waiting for IMU calibration - this can take some time";
     if (getProperty<bool>("calibrateSensor").getValue())
@@ -74,22 +97,18 @@ void OrientedTactileSensorUnit::onInitComponent()
 
         while (arduinoLine.find("mode") == std::string::npos)
         {
-            getline(arduino, arduinoLine, '\n');
+            getline(arduinoIn, arduinoLine, '\n');
         }
-        arduino.close();
 
-        arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
-        arduino << "calibrate";
-        arduino.flush();
-        arduino.close();
+        arduinoOut << "calibrate";
+        arduinoOut.flush();
 
-        arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
         while (arduinoLine.find("Calibration Sucessfull") == std::string::npos)
         {
-            getline(arduino, arduinoLine, '\n');
+            getline(arduinoIn, arduinoLine, '\n');
             ARMARX_INFO << arduinoLine;
         }
-        getline(arduino, arduinoLine, '\n');
+        getline(arduinoIn, arduinoLine, '\n');
         if (getCalibrationValues(arduinoLine))
         {
             ARMARX_IMPORTANT << "calibrated sensor";
@@ -102,6 +121,14 @@ void OrientedTactileSensorUnit::onInitComponent()
         if (loadCalibration())
         {
             ARMARX_IMPORTANT << "loaded calibration";
+            first = true;
+            /*std::string line;
+            getline(arduinoIn, line, '\n');
+            ARMARX_IMPORTANT<<line;
+            SensorData dataInit = getValues(line.c_str());
+            ARMARX_IMPORTANT<<dataInit.qw<<" "dataInit.qx<<" "<<dataInit.qy<<" "<<dataInit.qz;
+            Eigen::Quaternionf initialOrientation =  Eigen::Quaternionf(dataInit.qw, dataInit.qx, dataInit.qy, dataInit.qz);
+            inverseOrientation = initialOrientation.inverse();*/
         }
     }
     readTask = new RunningTask<OrientedTactileSensorUnit>(this, &OrientedTactileSensorUnit::run);
@@ -111,6 +138,7 @@ void OrientedTactileSensorUnit::onInitComponent()
 
 void OrientedTactileSensorUnit::onConnectComponent()
 {
+    debugDrawerTopic = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
     std::string topicName = getProperty<std::string>("TopicName").getValue();
     topicPrx = getTopic<OrientedTactileSensorUnitListenerPrx>(topicName);
 }
@@ -126,7 +154,7 @@ void OrientedTactileSensorUnit::run()
     while (readTask->isRunning())
     {
         std::string line;
-        getline(arduino, line, '\n');
+        getline(arduinoIn, line, '\n');
         SensorData data = getValues(line.c_str());
         IceUtil::Time now = IceUtil::Time::now();
         TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
@@ -140,6 +168,7 @@ void OrientedTactileSensorUnit::run()
             RotationRate sampleRotation;
             sampleRotation.timestamp = now;
             sampleRotation.orientation =  Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
+            //sampleRotation.orientation = sampleRotation.orientation * inverseOrientation;
             if (samplesRotation.size() < maxSamplesRotation)
             {
                 samplesRotation.push_back(sampleRotation);
@@ -153,10 +182,10 @@ void OrientedTactileSensorUnit::run()
                 oldsampleRotation.timestamp = samplesRotation.at(sampleIndexRotation).timestamp;
                 oldsampleRotation.orientation = samplesRotation.at(sampleIndexRotation).orientation;
                 Eigen::AngleAxisf aa(sampleRotation.orientation * oldsampleRotation.orientation.inverse());
+                //ARMARX_IMPORTANT << "aa: " << aa.axis() << " " << aa.angle();
                 rotationRate = aa.angle() / (sampleRotation.timestamp - oldsampleRotation.timestamp).toSecondsDouble();
             }
         }
-
         //compute pressureRate
         float pressureRate = 0;
         PressureRate samplePressure;
@@ -194,16 +223,93 @@ void OrientedTactileSensorUnit::run()
             oldsampleAcceleration.rotationRate = samplesAcceleration.at(sampleIndexAcceleration).rotationRate;
             accelerationRate = (sampleAcceleration.rotationRate - oldsampleAcceleration.rotationRate) / (sampleAcceleration.timestamp - oldsampleAcceleration.timestamp).toSecondsDouble();
         }
-        if ((std::abs(pressureRate) > 50))
+        if (pressureRates.size() < maxSamplesPressure)
+        {
+            pressureRates.push_back(pressureRate);
+        }
+        else
+        {
+            pressureRates[sampleIndexPressureRate] = pressureRate;
+            sampleIndexPressureRate = (sampleIndexPressureRate + 1) % maxSamplesPressure;
+        }
+        if (pressureRate > 50)
         {
             ARMARX_IMPORTANT << "contact";
         }
 
+        Eigen::Quaternionf orientationQuaternion = Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
+        if (getProperty<bool>("logData").getValue())
+        {
+
+            if (i < 50)
+            {
+                inverseOrientation = orientationQuaternion.inverse();
+                i++;
+            }
+            Eigen::Matrix3f quatMatrix = orientationQuaternion.toRotationMatrix();
+            Eigen::Matrix4f quat4Matrix = Eigen::Matrix4f::Identity();
+
+            quat4Matrix.block(0, 0, 3, 3) = quatMatrix;
+
+            Eigen::Vector3f linearAcceleration(data.accelx, data.accely, data.accelz);
+            SimpleJsonLoggerEntry e;
+            e.AddTimestamp();
+            e.Add("Pressure", data.pressure);
+            e.Add("PressureRate", pressureRate);
+            e.Add("RotationRate", rotationRate);
+            e.AddAsArr("Orientation", quat4Matrix);
+            e.AddAsArr("Linear Acceleration", linearAcceleration);
+            logger->log(e);
+        }
+        /*Eigen::Matrix3f rotZ;
+        rotZ(0, 0) = 0;
+        rotZ(0, 1) = 1;
+        rotZ(0, 2) = 0;
+        rotZ(1, 0) = -1;
+        rotZ(1, 1) = 0;
+        rotZ(1, 2) = 0;
+        rotZ(2, 0) = 0;
+        rotZ(2, 1) = 0;
+        rotZ(2, 2) = 1;
+        Eigen::Matrix3f rotX;
+        rotX(0, 0) = 1;
+        rotX(0, 1) = 0;
+        rotX(0, 2) = 0;
+        rotX(1, 0) = 0;
+        rotX(1, 1) = -1;
+        rotX(1, 2) = 0;
+        rotX(2, 0) = 0;
+        rotX(2, 1) = 0;
+        rotX(2, 2) = -1;*/
+        Eigen::Matrix3f rotY;
+        rotY(0, 0) = 0;
+        rotY(0, 1) = 0;
+        rotY(0, 2) = 1;
+        rotY(1, 0) = 0;
+        rotY(1, 1) = 1;
+        rotY(1, 2) = 0;
+        rotY(2, 0) = -1;
+        rotY(2, 1) = 0;
+        rotY(2, 2) = 0;
+        Eigen::Matrix3f rawOrientation = orientationQuaternion.toRotationMatrix();
+
+        PosePtr pose = new Pose(rawOrientation, Eigen::Vector3f(100.0, 200.0, 0.0));
+        if (debugDrawerTopic)
+        {
+            debugDrawerTopic->setPoseVisu("debugdrawerlayer", "pose", pose);
+        }
+        Eigen::Quaternionf quaternion(rawOrientation);
+        data.qw = quaternion.w();
+        data.qx = quaternion.x();
+        data.qy = quaternion.y();
+        data.qz = quaternion.z();
+        ARMARX_IMPORTANT << "or " << orientationQuaternion.w() << " " << orientationQuaternion.x() << " " << orientationQuaternion.y() << " " << orientationQuaternion.z();
         if (topicPrx)
         {
             topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, pressureRate, rotationRate, accelerationRate, data.accelx, data.accely, data.accelz, nowTimestamp);
         }
     }
+
 }
 
 // get imu values from incoming string
@@ -231,32 +337,24 @@ bool OrientedTactileSensorUnit::loadCalibration()
     std::string arduinoLine;
     while (arduinoLine.find("mode") == std::string::npos)
     {
-        getline(arduino, arduinoLine, '\n');
+        getline(arduinoIn, arduinoLine, '\n');
+        ARMARX_INFO << arduinoIn;
     }
-    arduino.close();
-
-    arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
-    arduino << "load";
-    arduino.flush();
-    arduino.close();
-
-    arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+    arduinoOut << "load";
+    arduinoOut.flush();
     while (arduinoLine.find("calibration data") == std::string::npos)
     {
-        getline(arduino, arduinoLine, '\n');
+        getline(arduinoIn, arduinoLine, '\n');
+        ARMARX_INFO << arduinoIn;
     }
-    arduino.close();
-
-    arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
-    arduino << calibrationStream;
-    arduino.flush();
-    arduino.close();
 
-    arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+    arduinoOut << calibrationStream;
+    arduinoOut.flush();
 
     while (arduinoLine.find("Calibration Sucessfull") == std::string::npos)
     {
-        getline(arduino, arduinoLine, '\n');
+        getline(arduinoIn, arduinoLine, '\n');
+        //ARMARX_INFO << arduinoIn;
     }
     return true;
 }
@@ -283,5 +381,3 @@ bool OrientedTactileSensorUnit::getCalibrationValues(std::string line)
     ARMARX_IMPORTANT << "calibration data: " << calibrationStream ;
     return true;
 }
-
-
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
index c518ac3f186d7e0662a4a7d527beb0cb4853abbf..5bacc20d35be89a959a2cc379eb546c19f82219a 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
@@ -7,10 +7,13 @@
 #include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 #include <ArmarXCore/core/services/tasks/RunningTask.h>
 #include <netinet/in.h>
+#include <iostream>
 #include <fstream>
 #include <stdio.h>
 #include <boost/date_time/posix_time/posix_time.hpp>
 #include <Eigen/Dense>
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
 namespace armarx
 {
@@ -36,27 +39,31 @@ namespace armarx
                 "65524 3 12 65534 65534 1 1208 119 58726 1000 943 ",
                 "Sensor Register Data to calibrate the sensor");
 
-            defineOptionalProperty<std::size_t>(
+            defineOptionalProperty<std::string>(
                 "SamplesRotation",
-                20,
+                "20",
                 "number of orientation values to differentiate");
 
-            defineOptionalProperty<std::size_t>(
+            defineOptionalProperty<std::string>(
                 "SamplesPressure",
-                10,
+                "10",
                 "number of pressure values to differentiate");
 
-            defineOptionalProperty<std::size_t>(
+            defineOptionalProperty<std::string>(
                 "SamplesAcceleration",
-                20,
+                "20",
                 "number of pressure values to differentiate");
 
+            defineOptionalProperty<bool>(
+                "logData",
+                "false",
+                "log data from sensor");
             defineOptionalProperty<bool>(
                 "calibrateSensor",
                 "false"
                 "Set true to calibrate the sensor and get calibration data and false to use existent calibration data");
+            defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates", "Name of the debug drawer topic that should be used");
         }
-
     };
 
     /**
@@ -119,7 +126,8 @@ namespace armarx
         virtual PropertyDefinitionsPtr createPropertyDefinitions();
 
     private:
-        std::fstream arduino;
+        std::ifstream arduinoIn;
+        std::ofstream arduinoOut;
         RunningTask<OrientedTactileSensorUnit>::pointer_type readTask;
         OrientedTactileSensorUnitListenerPrx topicPrx;
         OrientedTactileSensorUnitInterfacePrx interfacePrx;
@@ -131,15 +139,26 @@ namespace armarx
         int fd;
         CalibrationData calibration;
 
+        Eigen::Quaternionf inverseOrientation;
         std::vector<RotationRate> samplesRotation;
         std::vector<PressureRate> samplesPressure;
         std::vector<AccelerationRate> samplesAcceleration;
-        std::size_t maxSamplesRotation;
-        std::size_t sampleIndexRotation;
-        std::size_t maxSamplesPressure;
-        std::size_t sampleIndexPressure;
-        std::size_t maxSamplesAcceleration;
-        std::size_t sampleIndexAcceleration;
+        std::vector<float> pressureRates;
+        //Eigen::AngleAxisf aa;
+        int maxSamplesRotation;
+        int sampleIndexRotation;
+        int maxSamplesPressure;
+        int sampleIndexPressure;
+        int maxSamplesAcceleration;
+        int sampleIndexAcceleration;
+        int sampleIndexPressureRate;
+        float sumPressureRates;
+        Eigen::Matrix3f sumOrientation;
+        bool first;
+        int i = 0;
+        DebugDrawerInterfacePrx debugDrawerTopic;
+        SimpleJsonLoggerPtr logger;
+        std::string prefix;
     };
 }
 #endif // SENSORPACKAGEUNIT_H
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h.orig b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h.orig
new file mode 100644
index 0000000000000000000000000000000000000000..a10822f8e98f1bcbb85d4ed3fd76c2a5810497b6
--- /dev/null
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h.orig
@@ -0,0 +1,169 @@
+#ifndef SENSORPACKAGEUNIT_H
+#define SENSORPACKAGEUNIT_H
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/units/UnitInterface.h>
+#include <RobotAPI/interface/units/OrientedTactileSensorUnit.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+#include <netinet/in.h>
+#include <iostream>
+#include <fstream>
+#include <stdio.h>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <Eigen/Dense>
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+
+namespace armarx
+{
+    class OrientedTactileSensorUnitPropertyDefinitions:
+        public ComponentPropertyDefinitions
+    {
+    public:
+        OrientedTactileSensorUnitPropertyDefinitions(std::string prefix):
+            ComponentPropertyDefinitions(prefix)
+        {
+            defineOptionalProperty<std::string>(
+                "SerialInterfaceDevice",
+                "/dev/ttyACM0",
+                "The serial device the arduino is connected to.");
+
+            defineOptionalProperty<std::string>(
+                "TopicName",
+                "OrientedTactileSensorValues",
+                "Name of the topic on which the sensor values are provided");
+
+            defineOptionalProperty<std::string>(
+                "CalibrationData",
+                "65524 3 12 65534 65534 1 1208 119 58726 1000 943 ",
+                "Sensor Register Data to calibrate the sensor");
+
+            defineOptionalProperty<std::size_t>(
+                "SamplesRotation",
+                20,
+                "number of orientation values to differentiate");
+
+            defineOptionalProperty<std::size_t>(
+                "SamplesPressure",
+                10,
+                "number of pressure values to differentiate");
+
+            defineOptionalProperty<std::size_t>(
+                "SamplesAcceleration",
+                20,
+                "number of pressure values to differentiate");
+
+            defineOptionalProperty<bool>(
+                "logData",
+                "false",
+                "log data from sensor");
+            defineOptionalProperty<bool>(
+                "calibrateSensor",
+                "false"
+                "Set true to calibrate the sensor and get calibration data and false to use existent calibration data");
+        }
+
+    };
+
+    /**
+     * @class OrientedTactileSensorUnit
+     * @brief ArmarX wrapper for an arduino due with one BNO055 IMU and one BMP280 pressure sensor
+     *
+     */
+    class OrientedTactileSensorUnit:
+        virtual public armarx::Component
+        //TODO: needs interface to send calibration data
+    {
+    public:
+        OrientedTactileSensorUnit();
+
+        virtual std::string getDefaultName() const
+        {
+            return "OrientedTactileSensorUnit";
+        }
+
+        struct SensorData
+        {
+            int id;
+            float pressure;
+            float qw, qx, qy, qz;
+            float accelx, accely, accelz;
+        };
+
+        struct CalibrationData
+        {
+            int accel_offset_x, accel_offset_y, accel_offset_z, gyro_offset_x, gyro_offset_y, gyro_offset_z, mag_offset_x, mag_offset_y, mag_offset_z, accel_radius, mag_radius;
+        };
+
+        struct PressureRate
+        {
+            IceUtil::Time timestamp;
+            float pressure;
+        };
+
+        struct RotationRate
+        {
+            IceUtil::Time timestamp;
+            Eigen::Quaternionf orientation;
+        };
+
+        struct AccelerationRate
+        {
+            IceUtil::Time timestamp;
+            float rotationRate;
+        };
+        struct LinAccRate
+        {
+            IceUtil::Time timestamp;
+            float accx, accy, accz;
+        };
+
+    protected:
+        virtual void onInitComponent();
+        virtual void onConnectComponent();
+
+        virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+    private:
+        std::ifstream arduinoIn;
+        std::ofstream arduinoOut;
+        RunningTask<OrientedTactileSensorUnit>::pointer_type readTask;
+        OrientedTactileSensorUnitListenerPrx topicPrx;
+        OrientedTactileSensorUnitInterfacePrx interfacePrx;
+
+        void run();
+        SensorData getValues(std::string line);
+        bool getCalibrationValues(std::string line);
+        bool loadCalibration();
+        int fd;
+        CalibrationData calibration;
+
+        Eigen::Quaternionf inverseOrientation;
+        std::vector<RotationRate> samplesRotation;
+        std::vector<PressureRate> samplesPressure;
+        std::vector<AccelerationRate> samplesAcceleration;
+<<<<<<< HEAD
+        std::vector<float> pressureRates;
+        int maxSamplesRotation;
+        int sampleIndexRotation;
+        int maxSamplesPressure;
+        int sampleIndexPressure;
+        int maxSamplesAcceleration;
+        int sampleIndexAcceleration;
+        int sampleIndexPressureRate;
+        float sumPressureRates;
+        bool first;
+        int i = 0;
+        SimpleJsonLoggerPtr logger;
+        std::string prefix;
+=======
+        std::size_t maxSamplesRotation;
+        std::size_t sampleIndexRotation;
+        std::size_t maxSamplesPressure;
+        std::size_t sampleIndexPressure;
+        std::size_t maxSamplesAcceleration;
+        std::size_t sampleIndexAcceleration;
+>>>>>>> master
+    };
+}
+#endif // SENSORPACKAGEUNIT_H
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index c300da40c6681ad246370b68eb75fbaa2fddf34f..cc713d196386cf202bfd1acb7fde45f09b49ff3f 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -145,6 +145,7 @@ void KinematicUnitWidgetController::onInitComponent()
 
 void KinematicUnitWidgetController::onConnectComponent()
 {
+    ARMARX_INFO << "kinematic unit gui :: onConnectComponent()";
     reportedJointAngles.clear();
     reportedJointVelocities.clear();
     reportedJointControlModes.clear();
@@ -1083,15 +1084,20 @@ void KinematicUnitWidgetController::updateJointAnglesTable()
     float dirty_squaresum = 0;
 
     boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
-    double deltaT = jointAnglesUpdateFrequency->getValue()->getDouble();
-    if (deltaT != 0)
+    if (jointAnglesUpdateFrequency && jointAnglesUpdateFrequency->getValue())
     {
-        ui.labelUpdateFreq->setText(QString::number(static_cast<int>(1.0 / deltaT)) + " Hz");
-    }
-    else
-    {
-        ui.labelUpdateFreq->setText("- Hz");
+
+        double deltaT = jointAnglesUpdateFrequency->getValue()->getDouble();
+        if (deltaT != 0)
+        {
+            ui.labelUpdateFreq->setText(QString::number(static_cast<int>(1.0 / deltaT)) + " Hz");
+        }
+        else
+        {
+            ui.labelUpdateFreq->setText("- Hz");
+        }
     }
+
     if (!robotNodeSet)
     {
         return;
diff --git a/source/RobotAPI/interface/core/Trajectory.ice b/source/RobotAPI/interface/core/Trajectory.ice
index daeb1ce6fca713dab8cc238f90a495aa9380d92d..3bcf4960af7e7571a7fd8199c7e48aa6ee175ac2 100644
--- a/source/RobotAPI/interface/core/Trajectory.ice
+++ b/source/RobotAPI/interface/core/Trajectory.ice
@@ -30,6 +30,15 @@ module armarx
 {
     sequence<DoubleSeqSeq> DoubleSeqSeqSeq;
 
+    struct LimitlessState
+    {
+        bool enabled;
+        double limitLo;
+        double limitHi;
+    };
+
+    sequence<LimitlessState> LimitlessStateSeq;
+
         /**
         * [TrajectoryBase] defines a n-dimension trajectory with n deriviations.
         *
@@ -47,6 +56,8 @@ module armarx
         */
         ["protected"] DoubleSeqSeqSeq dataVec;
         ["protected"] Ice::DoubleSeq timestamps;
+
+        ["protected"] LimitlessStateSeq limitless;
     };
 
 
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 10a98da381b0b0b4da90b92fdecfad188f08a1e7..81101dffb41fd0532989a2354bd8b24183234326 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -1,4 +1,4 @@
 add_subdirectory(core)
 add_subdirectory(widgets)
-
+add_subdirectory(SimpleJsonLogger)
 add_subdirectory(RobotAPIVariantWidget)
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/CMakeLists.txt b/source/RobotAPI/libraries/SimpleJsonLogger/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8c3a421f764e62f3e71b2db92bd47d2874ca78ec
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/CMakeLists.txt
@@ -0,0 +1,44 @@
+armarx_component_set_name("SimpleJsonLogger")
+armarx_set_target("Library: SimpleJsonLogger")
+
+set(LIB_NAME       SimpleJsonLogger)
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+#find_package(HapticExplorationSimulation)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+#armarx_build_if(HapticExplorationSimulation_FOUND "HapticExplorationSimulation not available")
+
+
+if (Eigen3_FOUND AND Simox_FOUND)
+    include_directories(
+        ${Eigen3_INCLUDE_DIR}
+        ${Simox_INCLUDE_DIRS}
+        #${HapticExplorationSimulation_INCLUDE_DIRS}
+    )
+endif()
+
+set(LIBS
+	ArmarXCoreInterfaces 
+	ArmarXCore
+        StructuralJson
+)
+
+set(LIB_FILES
+SimpleJsonLogger.cpp
+SimpleJsonLoggerEntry.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(LIB_HEADERS
+SimpleJsonLogger.h
+SimpleJsonLoggerEntry.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..696acf03ebb7ae0a3940937f5f84deb95e174cc5
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp
@@ -0,0 +1,66 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "SimpleJsonLogger.h"
+
+using namespace armarx;
+
+SimpleJsonLogger::SimpleJsonLogger(const std::string& filename, bool append)
+    : autoflush(true)
+{
+    file.open(filename.c_str(), append ? std::ios_base::app : std::ios_base::out | std::ios_base::trunc);
+}
+
+void SimpleJsonLogger::log(JsonDataPtr entry)
+{
+    file << entry->toJsonString() << "\n";
+    if (autoflush)
+    {
+        file.flush();
+    }
+}
+
+void SimpleJsonLogger::log(const SimpleJsonLoggerEntry& entry)
+{
+    log(entry.obj);
+}
+
+void SimpleJsonLogger::log(SimpleJsonLoggerEntryPtr entry)
+{
+    log(entry->obj);
+}
+
+void SimpleJsonLogger::close()
+{
+    file.flush();
+    file.close();
+}
+
+SimpleJsonLogger::~SimpleJsonLogger()
+{
+    if (file.is_open())
+    {
+        file.close();
+    }
+}
+
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h
new file mode 100644
index 0000000000000000000000000000000000000000..857ae73ce15b83fe440f87b95ce68e6e608421d1
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h
@@ -0,0 +1,61 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef armarx_SimpleJsonLogger_SimpleJsonLogger
+#define armarx_SimpleJsonLogger_SimpleJsonLogger
+
+#include "SimpleJsonLoggerEntry.h"
+
+#include <boost/shared_ptr.hpp>
+
+#include <iostream>
+#include <fstream>
+
+#include <ArmarXGui/libraries/StructuralJson/JsonArray.h>
+#include <ArmarXGui/libraries/StructuralJson/JsonData.h>
+#include <ArmarXGui/libraries/StructuralJson/JsonObject.h>
+
+#include <Eigen/Dense>
+
+namespace armarx
+{
+    class SimpleJsonLogger;
+    typedef boost::shared_ptr<SimpleJsonLogger> SimpleJsonLoggerPtr;
+
+    class SimpleJsonLogger
+    {
+    public:
+        SimpleJsonLogger(const std::string& filename, bool append);
+        void log(JsonDataPtr entry);
+        void log(const SimpleJsonLoggerEntry& entry);
+        void log(SimpleJsonLoggerEntryPtr entry);
+        void close();
+        ~SimpleJsonLogger();
+
+    private:
+        bool autoflush;
+        std::ofstream file;
+    };
+}
+
+#endif // armarx_SimpleJsonLogger_SimpleJsonLogger
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d0ecac38ce590cf222f6149a0fd5715555e1e649
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
@@ -0,0 +1,159 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "SimpleJsonLoggerEntry.h"
+#include <IceUtil/Time.h>
+
+using namespace armarx;
+
+SimpleJsonLoggerEntry::SimpleJsonLoggerEntry()
+    : obj(new JsonObject)
+{
+}
+
+void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::Vector3f &vec)
+{
+    obj->add(key, ToArr((Eigen::VectorXf)vec));
+}
+
+void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::VectorXf &vec)
+{
+    obj->add(key, ToArr(vec));
+}
+
+void SimpleJsonLoggerEntry::AddAsObj(const std::string& key, const Eigen::Vector3f &vec)
+{
+    obj->add(key, ToObj(vec));
+}
+
+void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::Matrix4f &mat)
+{
+    obj->add(key, ToArr(mat));
+}
+
+void SimpleJsonLoggerEntry::AddMatrix(const std::string& key, const Eigen::MatrixXf& mat)
+{
+    obj->add(key, MatrixToArr(mat));
+}
+
+void SimpleJsonLoggerEntry::Add(const std::string& key, const std::string& value)
+{
+    obj->add(key, JsonValue::Create(value));
+}
+
+void SimpleJsonLoggerEntry::Add(const std::string& key, float value)
+{
+    obj->add(key, JsonValue::Create(value));
+}
+
+void SimpleJsonLoggerEntry::Add(const std::string& key, const std::vector<float>& value)
+{
+    obj->add(key, ToArr(value));
+}
+
+
+JsonArrayPtr SimpleJsonLoggerEntry::ToArr(const Eigen::VectorXf& vec)
+{
+    JsonArrayPtr arr(new JsonArray);
+    for(int i = 0; i < vec.rows(); i++)
+    {
+        arr->add(JsonValue::Create(vec(i)));
+    }
+    return arr;
+}
+
+JsonArrayPtr SimpleJsonLoggerEntry::ToArr(const std::vector<float>& vec)
+{
+    JsonArrayPtr arr(new JsonArray);
+    for(float v : vec)
+    {
+        arr->add(JsonValue::Create(v));
+    }
+    return arr;
+}
+
+JsonObjectPtr SimpleJsonLoggerEntry::ToObj(Eigen::Vector3f vec)
+{
+    JsonObjectPtr obj(new JsonObject);
+    obj->add("x", JsonValue::Create(vec(0)));
+    obj->add("y", JsonValue::Create(vec(1)));
+    obj->add("z", JsonValue::Create(vec(2)));
+    return obj;
+}
+
+JsonArrayPtr SimpleJsonLoggerEntry::ToArr(Eigen::Matrix4f mat)
+{
+    JsonArrayPtr arr(new JsonArray);
+    JsonArrayPtr row1(new JsonArray);
+    JsonArrayPtr row2(new JsonArray);
+    JsonArrayPtr row3(new JsonArray);
+    JsonArrayPtr row4(new JsonArray);
+
+    row1->add(JsonValue::Create(mat(0, 0)));
+    row1->add(JsonValue::Create(mat(0, 1)));
+    row1->add(JsonValue::Create(mat(0, 2)));
+    row1->add(JsonValue::Create(mat(0, 3)));
+
+    row2->add(JsonValue::Create(mat(1, 0)));
+    row2->add(JsonValue::Create(mat(1, 1)));
+    row2->add(JsonValue::Create(mat(1, 2)));
+    row2->add(JsonValue::Create(mat(1, 3)));
+
+    row3->add(JsonValue::Create(mat(2, 0)));
+    row3->add(JsonValue::Create(mat(2, 1)));
+    row3->add(JsonValue::Create(mat(2, 2)));
+    row3->add(JsonValue::Create(mat(2, 3)));
+
+    row4->add(JsonValue::Create(mat(3, 0)));
+    row4->add(JsonValue::Create(mat(3, 1)));
+    row4->add(JsonValue::Create(mat(3, 2)));
+    row4->add(JsonValue::Create(mat(3, 3)));
+
+    arr->add(row1);
+    arr->add(row2);
+    arr->add(row3);
+    arr->add(row4);
+
+    return arr;
+}
+
+JsonArrayPtr SimpleJsonLoggerEntry::MatrixToArr(Eigen::MatrixXf mat)
+{
+    JsonArrayPtr arr(new JsonArray);
+    for(int i = 0; i < mat.rows(); i++)
+    {
+        JsonArrayPtr row(new JsonArray);
+        for(int j = 0; j < mat.cols(); j++)
+        {
+            row->add(JsonValue::Create(mat(i, j)));
+        }
+        arr->add(row);
+    }
+    return arr;
+}
+
+void SimpleJsonLoggerEntry::AddTimestamp()
+{
+    IceUtil::Time now = IceUtil::Time::now();
+    obj->add("timestamp", JsonValue::Create(now.toDateTime()));
+}
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h
new file mode 100644
index 0000000000000000000000000000000000000000..3c2a25ff7a1cf00d94224c2cb46d0f7b7cf19d4d
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h
@@ -0,0 +1,63 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#ifndef armarx_SimpleJsonLogger_SimpleJsonLoggerEntry
+#define armarx_SimpleJsonLogger_SimpleJsonLoggerEntry
+
+#include <boost/shared_ptr.hpp>
+
+#include <ArmarXGui/libraries/StructuralJson/JsonObject.h>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+    class SimpleJsonLoggerEntry;
+    typedef boost::shared_ptr<SimpleJsonLoggerEntry> SimpleJsonLoggerEntryPtr;
+
+    class SimpleJsonLoggerEntry
+    {
+    public:
+        SimpleJsonLoggerEntry();
+        void AddAsArr(const std::string& key, const Eigen::Vector3f &vec);
+        void AddAsArr(const std::string& key, const Eigen::VectorXf &vec);
+        void AddAsObj(const std::string& key, const Eigen::Vector3f &vec);
+        void AddAsArr(const std::string& key, const Eigen::Matrix4f &mat);
+        void AddMatrix(const std::string& key, const Eigen::MatrixXf &mat);
+
+        void Add(const std::string& key, const std::string& value);
+        void Add(const std::string& key, float value);
+        void Add(const std::string& key, const std::vector<float>& value);
+
+        static JsonArrayPtr ToArr(const Eigen::VectorXf &vec);
+        static JsonArrayPtr ToArr(const std::vector<float>& vec);
+        static JsonObjectPtr ToObj(Eigen::Vector3f vec);
+        static JsonArrayPtr ToArr(Eigen::Matrix4f mat);
+        static JsonArrayPtr MatrixToArr(Eigen::MatrixXf mat);
+
+        void AddTimestamp();
+
+        JsonObjectPtr obj;
+    };
+}
+
+#endif // armarx_SimpleJsonLogger_SimpleJsonLoggerEntry
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/test/CMakeLists.txt b/source/RobotAPI/libraries/SimpleJsonLogger/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f37d4b381a496aa6f798dd0ed7d7bfc9b0369e28
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+#SET(LIBS ${LIBS} ArmarXCore SimpleJsonLogger)
+ 
+#armarx_add_test(SimpleJsonLoggerTest SimpleJsonLoggerTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/test/SimpleJsonLoggerTest.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/test/SimpleJsonLoggerTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..15c777b73509c2ab2ccc7cd2ecc415819bb8152d
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/test/SimpleJsonLoggerTest.cpp
@@ -0,0 +1,36 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    ReactiveGrasping::ArmarXObjects::SimpleJsonLogger
+ * @author     Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date       2016
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE ReactiveGrasping::ArmarXLibraries::SimpleJsonLogger
+
+#define ARMARX_BOOST_TEST
+
+#include <ReactiveGrasping/Test.h>
+#include "../SimpleJsonLogger.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+    BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 106f8a723c75bc884a6efd8323cb4965cd325a3a..cea8fa2744f760265647b2d7f1ad18f5085733bb 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -25,11 +25,12 @@
 #include "PIDController.h"
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
 
 using namespace armarx;
 
 
-PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation) :
+PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool limitless, double limitlessLimitHi) :
     Kp(Kp),
     Ki(Ki),
     Kd(Kd),
@@ -41,7 +42,9 @@ PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValu
     controlValue(0),
     controlValueDerivation(0),
     maxControlValue(maxControlValue),
-    maxDerivation(maxDerivation)
+    maxDerivation(maxDerivation),
+    limitless(limitless),
+    limitlessLimitHi(limitlessLimitHi)
 {
     reset();
 }
@@ -95,7 +98,14 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
     processValue = measuredValue;
     target = targetValue;
 
+
     double error = target - processValue;
+    //ARMARX_INFO << VAROUT(target) << ", " << VAROUT(processValue) << ", " << VAROUT(error);
+    if (limitless)
+    {
+        error = math::MathUtils::fmod(error, limitlessLimitHi - 2 * M_PI, limitlessLimitHi);
+        //ARMARX_INFO << "Limitless after mod:" << VAROUT(error);
+    }
 
     //double dt = (now - lastUpdateTime).toSecondsDouble();
     //    ARMARX_INFO << deactivateSpam() << VAROUT(dt);
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index cf7373d20fba53ef0da244d75b715f60fb9076a3..4e1f5528b4164f5800a585bb327861e74e2265b7 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -35,7 +35,13 @@ namespace armarx
         public Logging
     {
     public:
-        PIDController(float Kp, float Ki, float Kd, double maxControlValue = std::numeric_limits<double>::max(), double maxDerivation = std::numeric_limits<double>::max());
+        PIDController(float Kp,
+                      float Ki,
+                      float Kd,
+                      double maxControlValue = std::numeric_limits<double>::max(),
+                      double maxDerivation = std::numeric_limits<double>::max(),
+                      bool limitless = false,
+                      double limitlessLimitHi = 2 * M_PI);
         void update(double deltaSec, double measuredValue, double targetValue);
         void update(double measuredValue, double targetValue);
         void update(double measuredValue);
@@ -57,6 +63,8 @@ namespace armarx
         double maxControlValue;
         double maxDerivation;
         bool firstRun;
+        bool limitless;
+        double limitlessLimitHi;
         mutable RecursiveMutex mutex;
     };
     typedef boost::shared_ptr<PIDController> PIDControllerPtr;
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index 33a0698f8c15a77469838339703caeae308a37c1..07530219cc7b65f415117e5cc66617ba1c4d6947 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -25,6 +25,7 @@
 #include <ArmarXCore/core/logging/Logging.h>
 #include "VectorHelpers.h"
 #include <ArmarXCore/observers/AbstractObjectSerializer.h>
+#include "math/MathUtils.h"
 
 namespace armarx
 {
@@ -1265,8 +1266,45 @@ namespace armarx
         }
 
         double duration = tNext - tBefore;
-        double delta = next - before;
+
+        double delta;
+        if (trajDimension < limitless.size() && limitless.at(trajDimension).enabled)
+        {
+            double range = limitless.at(trajDimension).limitHi - limitless.at(trajDimension).limitLo;
+
+            double dist1 = next - before;
+            double dist2 = next - (before + range);
+            double dist3 = next - (before - range);
+
+            if (fabs(dist1) <= fabs(dist2) && fabs(dist1) <= fabs(dist3))
+            {
+                // no issue with bounds
+                //ARMARX_IMPORTANT << "LIMITLESS deriv: checking dim " << trajDimension << ", prev:" << before << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
+                delta = dist1;
+            }
+            else if (fabs(dist2) <= fabs(dist3) && fabs(dist2) <= fabs(dist3))
+            {
+                // go over hi bound
+                //ARMARX_IMPORTANT << "LIMITLESS deriv HIGH: checking dim " << trajDimension << ", prev:" << before << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
+                delta = dist2;
+            }
+            else
+            {
+                // go over lo bound
+                //ARMARX_IMPORTANT << "LIMITLESS deriv LOW: checking dim " << trajDimension << ", prev:" << before << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
+                delta = dist3;
+            }
+
+            //ARMARX_IMPORTANT << "LIMITLESS deriv: checking dim " << trajDimension << ": delta is " << delta;
+        }
+        else
+        {
+            // no limitless joint
+            delta = next - before;
+        }
+
         delta = delta / duration;
+        //ARMARX_IMPORTANT << "LIMITLESS deriv: checking dim " << trajDimension << ": delta/duration is " << delta;
         checkValue(delta);
         return delta;
     }
@@ -1292,8 +1330,6 @@ namespace armarx
     }
 
 
-
-
     void Trajectory::reconstructFromDerivativeForDim(double valueAtFirstTimestamp, size_t trajDimension, size_t sourceDimOfSystemState, size_t targetDimOfSystemState)
     {
         const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -1438,10 +1474,43 @@ namespace armarx
             double weightPrev = fabs(t - itNext->timestamp) / distance;
             double weightNext = fabs(itPrev->timestamp - t) / distance;
 
-            return weightNext * next + weightPrev * previous;
-
+            if (dimension < limitless.size() && limitless.at(dimension).enabled)
+            {
+                double result = 0;
+                double range = limitless.at(dimension).limitHi - limitless.at(dimension).limitLo;
+                double dist1 = next - previous;
+                double dist2 = next - (previous + range);
+                double dist3 = next - (previous - range);
+                //ARMARX_IMPORTANT << "LIMITLESS: checking dim " << dimension << ", prev:" << previous << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
+                if (fabs(dist1) <= fabs(dist2) && fabs(dist1) <= fabs(dist3))
+                {
+                    // no issue with bounds
+                    result = weightNext * next + weightPrev * previous;
+                }
+                else if (fabs(dist2) <= fabs(dist3) && fabs(dist2) <= fabs(dist3))
+                {
+                    // go over hi bound
+                    result = weightNext * next + weightPrev * (previous + range);
+                    result = math::MathUtils::fmod(result, limitless.at(dimension).limitLo, limitless.at(dimension).limitHi);
+                    //ARMARX_IMPORTANT << "LIMITLESS - HIGH: checking dim " << dimension << ": high bounds, result: " << result << ", orig result would be " << weightNext* next + weightPrev* previous;
+                }
+                else
+                {
+                    // go over lo bound
+                    result = weightNext * next + weightPrev * (previous - range);
+                    result = math::MathUtils::fmod(result, limitless.at(dimension).limitLo, limitless.at(dimension).limitHi);
+                    //ARMARX_IMPORTANT << "LIMITLESS - LOW: checking dim " << dimension << ": low bounds, result: " << result << ", orig result would be " << weightNext* next + weightPrev* previous;
+                }
+                return result;
+            }
+            else
+            {
+                //ARMARX_IMPORTANT << "LIMITLESS: dim " << dimension << ": not limitless, limitless.size()=" << limitless.size();
+                return weightNext * next + weightPrev * previous;
+            }
         }
     }
+
     void Trajectory::gaussianFilter(double filterRadius)
     {
         const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -1538,6 +1607,8 @@ namespace armarx
             destination.addDimension(source.getDimensionData(dim), timestamps);
         }
         destination.setDimensionNames(source.getDimensionNames());
+
+        destination.setLimitless(source.getLimitless());
     }
 
     void Trajectory::clear()
@@ -1612,6 +1683,7 @@ namespace armarx
             normExampleTraj.addDimension(dimensionData, normTimestamps);
         }
         normExampleTraj.setDimensionNames(traj.getDimensionNames());
+        normExampleTraj.setLimitless(traj.getLimitless());
         return normExampleTraj;
 
 
@@ -1849,4 +1921,15 @@ namespace armarx
     }
 
 
+    void Trajectory::setLimitless(const LimitlessStateSeq& limitlessStates)
+    {
+        limitless = limitlessStates;
+    }
+
+    LimitlessStateSeq Trajectory::getLimitless() const
+    {
+        return limitless;
+    }
+
+
 } // namespace armarx
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index db38b1e6657b9717dd2f0f98bd418b9d5a02e234..f1b481febf2b8b62786115e3ce287b5d660e0125 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -424,6 +424,8 @@ namespace armarx
             dimensionNames = dimNames;
         }
 
+        void setLimitless(const LimitlessStateSeq& limitlessStates);
+        LimitlessStateSeq getLimitless() const;
 
     protected:
         void __addDimension();
diff --git a/source/RobotAPI/libraries/core/math/MathUtils.h b/source/RobotAPI/libraries/core/math/MathUtils.h
index bef829c58f7f55ae16416e8524a434cce2c9d987..eae0128a8508b7963ea5820394cc9a13cbf71173 100644
--- a/source/RobotAPI/libraries/core/math/MathUtils.h
+++ b/source/RobotAPI/libraries/core/math/MathUtils.h
@@ -124,6 +124,28 @@ namespace armarx
                 return max;
             }
 
+            static float fmod(float value, float boundLow, float boundHigh)
+            {
+                if (value < 0)
+                {
+                    return std::fmod(value + boundLow, boundHigh - boundLow) - boundLow;
+                }
+                else
+                {
+                    return std::fmod(value - boundLow, boundHigh - boundLow) + boundLow;
+                }
+            }
+
+            static float angleMod2PI(float value)
+            {
+                return fmod(value, 0, 2 * M_PI);
+            }
+
+            static float angleModPI(float value)
+            {
+                return angleMod2PI(value) - M_PI;
+            }
+
         };
     }
 }