diff --git a/lwrserv/Trajectroy.cpp b/lwrserv/Trajectroy.cpp
new file mode 100644
index 0000000..b89a5f6
--- /dev/null
+++ b/lwrserv/Trajectroy.cpp
@@ -0,0 +1 @@
+#include "Trajectroy.h"
diff --git a/lwrserv/global.cpp b/lwrserv/global.cpp
index 2bba56b..31c618e 100644
--- a/lwrserv/global.cpp
+++ b/lwrserv/global.cpp
@@ -26,7 +26,7 @@ float USTarget[12];
 float USApproach[12];
 double VELOCITY = 20;
 double ACCELARATION = 10;
-double maxVelJnt[] = {110.0,110.0,128.0,128.0,204.0,184.0,184.0};
+//double maxVelJnt[] = {110.0,110.0,128.0,128.0,204.0,184.0,184.0};
 double maxAccJnt[] = {1.0,1.0,1.0,1.0,2.0,1.0,1.0};
 double maxTrqJnt[] = {176.0,176.0,100.0,100.0,100.0,38.0,38.0};
 double maxRangeJnt[] = {170.0,120.0,170.0,120.0,170.0,120.0,170.0};
diff --git a/lwrserv/include/global.h b/lwrserv/include/global.h
index 3399f10..4a99459 100644
--- a/lwrserv/include/global.h
+++ b/lwrserv/include/global.h
@@ -22,12 +22,6 @@ extern double MSRMSRJACOBIAN[42];
 extern double MSRMSRFTTCP[6];
 extern double MSRCMDJNTPOS[7];
 extern double MSRCMDPOSE[3][4];
-extern double VELOCITY;
-extern double ACCELARATION;
-extern double maxVelJnt[LBR_MNJ];
-extern double maxAccJnt[LBR_MNJ];
-extern double maxTrqJnt[LBR_MNJ];
-extern double maxRangeJnt[LBR_MNJ];
 
 #ifndef SSSS
 #define SSSS
diff --git a/lwrserv/include/mat.h b/lwrserv/include/mat.h
index 221ebae..8f91c7d 100644
--- a/lwrserv/include/mat.h
+++ b/lwrserv/include/mat.h
@@ -145,6 +145,17 @@ public:
                     buf(i,j) += m_aatData[i][a] * mat(a,j);
         return buf;
     }
+    Mat<T, SIZE> &operator = (const T aatData[SIZE*SIZE]) 
+    {
+        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
+            for (unsigned int j=0; j<SIZE; j++) // Spalte j
+                m_aatData[i][j] = aatData[j+(i*4)];
+
+        return (*this); // also an R-value in e.g.
+                        // vec1  = vec2  +   (vec2=atData); // parenthesis () needed to evaluate expression vec2=atData
+                        // L-Val = R-Val +       R-Val
+                        //   "   =   "   + (L-Val = R-Val)
+    }
 
 #if 0
     Mat<T, SIZE> operator * (const Vec<T, SIZE> &vec)
@@ -157,6 +168,15 @@ public:
     }
 #endif
 
+    Mat<T,SIZE> abs()
+    {
+        Mat<T,SIZE> buf;
+        for (unsigned int j=0; j<SIZE; j++) // SPALTE j
+            for (unsigned int i=0; i<SIZE; i++) // ZEILE i
+                buf.m_aatData[i][j] = abs(m_aatData[i][j]);
+        return buf;
+    }
+
     T determinant()
     {
         T buf = 0;
diff --git a/lwrserv/include/vec.h b/lwrserv/include/vec.h
index 4ef4833..6d0d028 100644
--- a/lwrserv/include/vec.h
+++ b/lwrserv/include/vec.h
@@ -2,6 +2,7 @@
 #define _VEC_H_
 #include <math.h>
 #include <ostream>
+#include <cmath>
 #include "mat.h"
 
 template<class T, unsigned SIZE> class Vec;
@@ -172,6 +173,65 @@ public:
         return vec;
     }
 
+    Vec<T, SIZE> celldivide (const Vec<T, SIZE> & vec) 
+    {
+        Vec<T, SIZE> buff;
+        for (int i=0; i<SIZE; i++)
+            buff.m_atData[i] = m_atData[i]/vec.m_atData[i];
+        return buff;
+    }
+    Vec<T, SIZE> cellmultiply (const Vec<T, SIZE> & vec) 
+    {
+        Vec<T, SIZE> buff;
+        for (int i=0; i<SIZE; i++)
+            buff.m_atData[i] = m_atData[i]*vec.m_atData[i];
+        return buff;
+    }
+
+    Vec<T, SIZE> abs ()
+    {
+        Vec<T, SIZE> buff;
+        for (int i=0; i<SIZE; i++)
+            buff.m_atData[i] = std::abs(m_atData[i]);
+        return buff;
+    }
+    
+    Vec<T, SIZE> floor ()
+    {
+        Vec<T, SIZE> buff;
+        for (int i=0; i<SIZE; i++)
+            buff.m_atData[i] = floor(m_atData[i]);
+        return buff;
+    }
+    Vec<T, SIZE> ceil ()
+    {
+        Vec<T, SIZE> buff;
+        for (int i=0; i<SIZE; i++)
+            buff.m_atData[i] = ceil(m_atData[i]);
+        return buff;
+    }
+    
+    T max ()
+    {
+        T retval = m_atData[0];
+
+        // this * this
+        for (int i=0; i<SIZE; i++)
+            if (retval > m_atData[i])
+                retval =  m_atData[i];
+        return retval;
+    }
+    T min ()
+    {
+        T retval = m_atData[0];
+
+        // this * this
+        for (int i=0; i<SIZE; i++)
+            if (retval < m_atData[i])
+                retval =  m_atData[i];
+        return retval;
+    }
+
     T norm ()
     {
         T retval = 0;
diff --git a/lwrserv/program.cpp b/lwrserv/program.cpp
index 0387e0c..7438fd1 100644
--- a/lwrserv/program.cpp
+++ b/lwrserv/program.cpp
@@ -305,87 +305,96 @@ void *threadFriDataExchange(void *arg)
         // PTP Joint Movement
         if (__MSRCMDJNTPOS)
         {
-            double maxVelJntLocal[LBR_MNJ];
-            double maxAccJntLocal[LBR_MNJ];
-            double delta[LBR_MNJ];
-            double sampleTime = (double)0.005;
-            double deltaAbs[LBR_MNJ];
-            double dMaxSpeed[LBR_MNJ];
-            double lMaxSpeed[LBR_MNJ];
-            double dGesamt[LBR_MNJ];
+            Vec<float,LBR_MNJ> maxJointLocalVelocity;
+            Vec<float,LBR_MNJ> maxJointLocalAcceleration;
+            Vec<float,LBR_MNJ> delta;
+            Vec<float,LBR_MNJ> deltaAbs;
+            Vec<float,LBR_MNJ> dMaxSpeed;
+            Vec<float,LBR_MNJ> lMaxSpeed;
+            Vec<float,LBR_MNJ> dGesamt;
+            float sampleTime = 0.005f;
             
-            double currentInk[LBR_MNJ] = {0};
-            double currentInkLast[LBR_MNJ] = {0};
-            double currentDist [LBR_MNJ] = {0};
-            double currentDistLast [LBR_MNJ] = {0};
 
-            Vec<float,LBR_MNJ> maxVelocity = SvrData::getInstance()->getMaxVelocity();
-            Vec<float,LBR_MNJ> maxAccelaration = SvrData::getInstance()->getMaxAcceleration();
+            // get current robot constraints
+            Vec<float,LBR_MNJ> maxVelocityJoint = SvrData::getInstance()->getMaxVelocity();
+            Vec<float,LBR_MNJ> maxAccelarationJoint = SvrData::getInstance()->getMaxAcceleration();
+            Vec<float,LBR_MNJ> messuredJointPos = SvrData::getInstance()->getMessuredJointPos();
+            Vec<float,LBR_MNJ> commandedJointPos = SvrData::getInstance()->getCommandedJointPos();
+
+            float velocity     = SvrData::getInstance()->getRobotVelocity();
+            float accelaration = SvrData::getInstance()->getRobotAcceleration();
 
             // calculate delta positions of movement
-            for ( int i= 0; i < LBR_MNJ; i++)
-            {
-                delta[i] = MSRCMDJNTPOS[i]-(MSRMSRJNTPOS[i]*180/M_PI);
-                deltaAbs[i] = fabs(MSRCMDJNTPOS[i]-(MSRMSRJNTPOS[i]*180/M_PI));
-                maxVelJntLocal[i] = maxVelJnt[i]*sampleTime*(VELOCITY/100.0);
-                maxAccJntLocal[i] = maxAccJnt[i]*sampleTime*(ACCELARATION/100.0);
-            }
+            delta = commandedJointPos - messuredJointPos;
+            deltaAbs = delta.abs();
+            maxJointLocalVelocity = maxVelocityJoint * sampleTime * (velocity/100.0f);
+            maxJointLocalAcceleration = maxAccelarationJoint * sampleTime * (accelaration/100.0f);
             
-            // calculate movement steps
+            // calculate number of movement steps
+            dMaxSpeed = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration);
+            lMaxSpeed = dMaxSpeed.cellmultiply(dMaxSpeed).cellmultiply(maxJointLocalAcceleration) * 0.5f;
+
             for (int j=0; j<LBR_MNJ; j++)
             {
-                dMaxSpeed[j] = maxVelJntLocal[j]/maxAccJntLocal[j];
-                lMaxSpeed[j] = dMaxSpeed[j]*dMaxSpeed[j]*maxAccJntLocal[j]*0.5;
-                if (lMaxSpeed[j] > deltaAbs[j]/(double)2.0)
+                if (lMaxSpeed(j) > deltaAbs(j)/(double)2.0)
                 {
-                    dGesamt[j] = sqrt(deltaAbs[j]/maxAccJntLocal[j])*2.0;
+                    dGesamt(j) = sqrt(deltaAbs(j) / maxJointLocalAcceleration (j))*2.0;
                 } else 
                 {
-                    dGesamt[j] = dMaxSpeed[j]*2 + (deltaAbs[j]-lMaxSpeed[j]*2.0)/maxVelJntLocal[j];
+                    dGesamt(j) = dMaxSpeed(j)*2 + (deltaAbs(j)-lMaxSpeed(j)*2.0)/maxJointLocalVelocity(j);
                 }
             }
-            int maxSteps =ceil(*max_element(dGesamt,(dGesamt+LBR_MNJ-1)));
+            int maxSteps = ceil(dGesamt.max());
 
+            // there are atleast 2 steps otherwise it is no trajectory
             if (maxSteps == 0 || maxSteps == 1)
                 goto end;
 
             for (int j=0; j<LBR_MNJ; j++)
             {
-                dMaxSpeed[j] = ceil(maxSteps/(double)2 - sqrt((maxSteps/(double)2)*(maxSteps/(double)2) - deltaAbs[j]/maxAccJntLocal[j]));
-                double temp = maxSteps/(double)2 - sqrt((maxSteps/(double)2)*(maxSteps/(double)2) - deltaAbs[j]/maxAccJntLocal[j]);             
-                if (dMaxSpeed[j]==0)
+                dMaxSpeed(j) = ceil( maxSteps /2.0f - sqrt((maxSteps/2.0f)*(maxSteps/2.0f) - deltaAbs(j)/maxAccelarationJoint(j)));
+                if (dMaxSpeed(j) == 0.0f)
                 {
-                    maxAccJntLocal[j] = 0;
+                    maxJointLocalAcceleration(j) = 0.0f;
                 } else
                 {
-                    maxAccJntLocal[j] = -deltaAbs[j]/(dMaxSpeed[j]*dMaxSpeed[j]-maxSteps*dMaxSpeed[j]);
+                    maxJointLocalAcceleration(j) = -deltaAbs(j)/(dMaxSpeed(j)*dMaxSpeed(j)-maxSteps*dMaxSpeed(j));
                 }
-                maxVelJntLocal[j] = dMaxSpeed[j]*maxAccJntLocal[j];
+                maxJointLocalVelocity(j) = dMaxSpeed(j)*maxJointLocalAcceleration(j);
             }
 
 
-            for (int i=0;i<maxSteps;i++)
+            // do the trajectory
             {
-                for (int j=0;j<LBR_MNJ;j++)
+                Vec<float,LBR_MNJ> currentInk;
+                Vec<float,LBR_MNJ> currentInkLast;
+                Vec<float,LBR_MNJ> currentDist;
+                Vec<float,LBR_MNJ> currentDistLast;
+                for (int i=0;i<maxSteps;i++)
                 {
-                    if (i+1 <= maxSteps/(double)2)
-                    {
-                        currentInk[j] = min(currentInkLast[j]+maxAccJntLocal[j],maxVelJntLocal[j]);
-                    }else if (i+1 > maxSteps-dMaxSpeed[j])
+                    for (int j=0;j<LBR_MNJ;j++)
                     {
-                        currentInk[j] = max(currentInkLast[j]-maxAccJntLocal[j],(double)0);
-                    }else
-                    {
-                        currentInk[j] = currentInkLast[j];
+                        if (i+1 <= maxSteps/2.0f)
+                        {
+                            currentInk(j) = min(currentInkLast(j)+maxJointLocalAcceleration(j),maxJointLocalVelocity(j));
+                        }else if (i+1 > maxSteps-dMaxSpeed(j))
+                        {
+                            currentInk(j) = max(currentInkLast(j)-maxJointLocalAcceleration(j),0.0f);
+                        }else
+                        {
+                            currentInk(j) = currentInkLast(j);
+                        }
+                        currentDist(j) = currentDistLast(j) + sgn(delta(j))*currentInk(j);
+                        currentInkLast(j) = currentInk(j);
+                        currentDistLast(j) = currentDist(j);
+                        MSRMSRJNTPOS[j]+= sgn(delta(j))*currentInk(j)*(1./180*M_PI);
                     }
-                    currentDist[j] = currentDistLast[j] + sgn(delta[j])*currentInk[j];
-                    currentInkLast[j] = currentInk[j];
-                    currentDistLast[j] = currentDist[j];
-                    MSRMSRJNTPOS[j]+= sgn(delta[j])*currentInk[j]*(1./180*M_PI);
+                    // set new position
+                    friInst.doPositionControl(MSRMSRJNTPOS);
                 }
-                friInst.doPositionControl(MSRMSRJNTPOS);
             }
 end:
+            // mark state to be done
             __MSRCMDJNTPOS = false;
 
         } else if (__CARTMOVE)