diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index 5b81f9c9832e4dc364808b690025613613a0b91d..978fedcf56a92dea71785161f663647355b87a9d 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -119,7 +119,7 @@ void KinematicUnitSimulation::simulationFunction()
         {
 
             float newAngle = 0.0f;
-
+            KinematicUnitSimulationJointInfo& jointInfo = jointInfos[it->first];
             // calculate joint positions if joint is in velocity control mode
             if (it->second.controlMode == eVelocityControl)
             {
@@ -152,11 +152,14 @@ void KinematicUnitSimulation::simulationFunction()
             }
 
             // constrain the angle
-            KinematicUnitSimulationJointInfo& jointInfo = jointInfos[it->first];
-            float maxAngle = (jointInfo.hasLimits()) ? jointInfo.limitHi : 2.0 * M_PI;
-            float minAngle = (jointInfo.hasLimits()) ? jointInfo.limitLo : -2.0 * M_PI;
-            newAngle = std::max<float>(newAngle, minAngle);
-            newAngle = std::min<float>(newAngle, maxAngle);
+            if (!jointInfo.continuousJoint())
+            {
+                float maxAngle = (jointInfo.hasLimits()) ? jointInfo.limitHi : M_PI;
+                float minAngle = (jointInfo.hasLimits()) ? jointInfo.limitLo : -M_PI;
+
+                newAngle = std::max<float>(newAngle, minAngle);
+                newAngle = std::min<float>(newAngle, maxAngle);
+            }
 
             // Check if joint existed before
             KinematicUnitSimulationJointStates::iterator itPrev = previousJointStates.find(it->first);
@@ -180,6 +183,18 @@ void KinematicUnitSimulation::simulationFunction()
                 aVelValueChanged = true;
             }
 
+            if (jointInfo.continuousJoint())
+            {
+                if (newAngle < 0)
+                {
+                    newAngle = fmod(newAngle - M_PI, 2 * M_PI) + M_PI;
+                }
+                else
+                {
+                    newAngle = fmod(newAngle + M_PI, 2 * M_PI) - M_PI;
+                }
+
+            }
 
             actualJointAngles[it->first] = newAngle;
             actualJointVelocities[it->first] = it->second.velocity;
@@ -252,11 +267,11 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
             {
                 aValueChanged = true;
             }
-
-            if (jointInfos[targetJointName].hasLimits())
+            KinematicUnitSimulationJointInfo& jointInfo = jointInfos[targetJointName];
+            if (jointInfo.hasLimits() && !jointInfo.continuousJoint())
             {
-                targetJointAngle = std::max<float>(targetJointAngle, jointInfos[targetJointName].limitLo);
-                targetJointAngle = std::min<float>(targetJointAngle, jointInfos[targetJointName].limitHi);
+                targetJointAngle = std::max<float>(targetJointAngle, jointInfo.limitLo);
+                targetJointAngle = std::min<float>(targetJointAngle, jointInfo.limitHi);
             }
 
             if (!usePDControllerForPosMode)
@@ -270,8 +285,16 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
                 if (pid)
                 {
                     pid->reset();
+                    if (jointInfo.continuousJoint())
+                    {
+                        float delta = targetJointAngle - iterJoints->second.angle;
+                        float signedSmallestDelta = atan2(sin(delta), cos(delta));
+                        targetJointAngle = iterJoints->second.angle + signedSmallestDelta;
+                    }
+
                     pid->setTarget(targetJointAngle);
 
+
                 }
             }
         }
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.h b/source/RobotAPI/components/units/KinematicUnitSimulation.h
index 4b730a0f42b34b056ddce468fb2fa6cbb4249a10..87cea5bde4109336bcaab519e72ac9be59854e6a 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.h
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.h
@@ -94,6 +94,10 @@ namespace armarx
         {
             return (limitLo != limitHi);
         }
+        bool continuousJoint() const
+        {
+            return !hasLimits() || limitLo - limitHi >= 360.0f;
+        }
 
         float limitLo;
         float limitHi;