diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index 088be6ebe83e6fa29a877c92eeb62548115d8492..5b81f9c9832e4dc364808b690025613613a0b91d 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -34,13 +34,14 @@
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
 
+
 #include <algorithm>
 
 using namespace armarx;
 
 void KinematicUnitSimulation::onInitKinematicUnit()
 {
-
+    usePDControllerForPosMode = getProperty<bool>("UsePDControllerForJointControl").getValue();
     for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
     {
         std::string jointName = (*it)->getName();
@@ -68,6 +69,10 @@ void KinematicUnitSimulation::onInitKinematicUnit()
             std::string jointName = (*it)->getName();
             jointStates[jointName] = KinematicUnitSimulationJointState();
             jointStates[jointName].init();
+            positionControlPIDController[jointName] = PIDControllerPtr(new PIDController(
+                        getProperty<float>("Kp").getValue(),
+                        getProperty<float>("Ki").getValue(),
+                        getProperty<float>("Kd").getValue()));
         }
     }
 
@@ -102,6 +107,10 @@ void KinematicUnitSimulation::simulationFunction()
 
     bool aPosValueChanged = false;
     bool aVelValueChanged = false;
+    auto signedMin = [](float newValue, float minAbsValue)
+    {
+        return std::copysign(std::min<float>(fabs(newValue), fabs(minAbsValue)), newValue);
+    };
 
     {
         boost::mutex::scoped_lock lock(jointStatesMutex);
@@ -117,12 +126,29 @@ void KinematicUnitSimulation::simulationFunction()
                 double change = (it->second.velocity * timeDeltaInSeconds);
 
                 double randomNoiseValue = distribution(generator);
-                change += randomNoiseValue;
+                change *= 1 + randomNoiseValue;
                 newAngle = it->second.angle + change;
             }
             else if (it->second.controlMode == ePositionControl)
             {
-                newAngle = it->second.angle;
+                if (!usePDControllerForPosMode)
+                {
+                    newAngle = it->second.angle;
+                }
+                else
+                {
+                    PIDControllerPtr pid = positionControlPIDController[it->first];
+                    if (pid)
+                    {
+                        float velValue = (it->second.velocity != 0.0f ? signedMin(pid->getControlValue(), it->second.velocity) : pid->getControlValue()); //restrain to max velocity
+                        velValue *= 1 + distribution(generator);
+                        pid->update(it->second.angle);
+                        newAngle = it->second.angle +
+                                   velValue *
+                                   timeDeltaInSeconds;
+
+                    }
+                }
             }
 
             // constrain the angle
@@ -233,8 +259,21 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
                 targetJointAngle = std::min<float>(targetJointAngle, jointInfos[targetJointName].limitHi);
             }
 
-            iterJoints->second.angle = targetJointAngle;
-            actualJointAngles[targetJointName] = iterJoints->second.angle;
+            if (!usePDControllerForPosMode)
+            {
+                iterJoints->second.angle = targetJointAngle;
+                actualJointAngles[targetJointName] = iterJoints->second.angle;
+            }
+            else
+            {
+                PIDControllerPtr pid = positionControlPIDController[it->first];
+                if (pid)
+                {
+                    pid->reset();
+                    pid->setTarget(targetJointAngle);
+
+                }
+            }
         }
         else
         {
@@ -362,7 +401,6 @@ bool mapEntryEqualsString(std::pair<std::string, KinematicUnitSimulationJointSta
 
 void KinematicUnitSimulation::requestJoints(const StringSequence& joints, const Ice::Current& c)
 {
-    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
     KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
 
@@ -374,7 +412,6 @@ void KinematicUnitSimulation::requestJoints(const StringSequence& joints, const
 
 void KinematicUnitSimulation::releaseJoints(const StringSequence& joints, const Ice::Current& c)
 {
-    ARMARX_INFO << flush;
     // if one of the joints belongs to this unit, unlock access
     KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
 
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.h b/source/RobotAPI/components/units/KinematicUnitSimulation.h
index 57e3c9f4cc9cee0dde3ab5cdd621aa6e2bc99f5f..4b730a0f42b34b056ddce468fb2fa6cbb4249a10 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.h
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.h
@@ -29,6 +29,7 @@
 
 #include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 #include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <RobotAPI/libraries/core/PIDController.h>
 #include <random>
 
 #include <IceUtil/Time.h>
@@ -111,7 +112,12 @@ namespace armarx
             KinematicUnitPropertyDefinitions(prefix)
         {
             defineOptionalProperty<int>("IntervalMs", 10, "The time in milliseconds between two calls to the simulation method.");
-            defineOptionalProperty<float>("Noise", 0.0f, "Noise is added in velocity mode with (rand()/RAND_MAX-0.5)*2*Noise. Value in Degree");
+            defineOptionalProperty<float>("Noise", 0.0f, "Gaussian noise is added to the velocity. Value in Degree");
+            defineOptionalProperty<bool>("UsePDControllerForJointControl", true, "If true a PD controller is also used in Position Mode instead of setting the joint angles instantly");
+            defineOptionalProperty<float>("Kp", 3, "proportional gain of the PID position controller.");
+            defineOptionalProperty<float>("Ki", 0.001, "integral gain of the PID position controller.");
+            defineOptionalProperty<float>("Kd", 0.0, "derivative gain of the PID position controller.");
+
         }
     };
 
@@ -173,7 +179,8 @@ namespace armarx
         int intervalMs;
         std::normal_distribution<double> distribution;
         std::default_random_engine generator;
-
+        bool usePDControllerForPosMode;
+        std::map<std::string, PIDControllerPtr> positionControlPIDController;
         PeriodicTask<KinematicUnitSimulation>::pointer_type simulationTask;
     };
 }
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 7a82a5d6a5722b11b06568c33b28483548078aa7..441fb1177b43c79308575dc3a1bdea8072216b95 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -66,6 +66,19 @@ void PIDController::update(double measuredValue, double targetValue)
     lastUpdateTime = now;
 }
 
+
+void PIDController::update(double measuredValue)
+{
+    ScopedRecursiveLock lock(mutex);
+    update(measuredValue, target);
+}
+
+void PIDController::setTarget(double newTarget)
+{
+    ScopedRecursiveLock lock(mutex);
+    target = newTarget;
+}
+
 void PIDController::update(double deltaSec, double measuredValue, double targetValue)
 {
     ScopedRecursiveLock lock(mutex);
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index c1c1529a4202cb6436720416222ff9d119c071fc..8e9578b0257259ce74a2fa8944ede3c75b13abed 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -38,6 +38,8 @@ namespace armarx
         PIDController(float Kp, float Ki, float Kd);
         void update(double deltaSec, double measuredValue, double targetValue);
         void update(double measuredValue, double targetValue);
+        void update(double measuredValue);
+        void setTarget(double newTarget);
         double getControlValue() const;
 
         void reset();