diff --git a/lwrserv/include/Mat.h b/lwrserv/include/Mat.h
index ce84336..7dbdd72 100644
--- a/lwrserv/include/Mat.h
+++ b/lwrserv/include/Mat.h
@@ -48,7 +48,14 @@ typedef Mat<double, 4> Mat4d;
 typedef Mat<float, 4> MatCarthesian;
 
 
-// template square matrix class for SIMPLE data types 
+/**
+ * Matrix class for class and simple data types
+ * template with type and size of the vector
+ *
+ * @author Philipp Schoenberger <ph.schoenberger@googlemail.com>
+ * @copyright 2012 philipp schoenberger All rights reserved.
+ * @license This project is released under the GNU Public License.
+ */
 template <class T, unsigned SIZE>
 class Mat 
 {
@@ -179,6 +186,7 @@ public:
      * within the array
      *
      * @param scalar the value to add
+     * @return the new calculated matrix
      */
     Mat<T, SIZE> operator + (const T &scalar)
     {
@@ -190,10 +198,11 @@ public:
     }
 
     /**
-     * operator to add a simple type to each entry
+     * operator to subtract a simple type to each entry
      * within the array
      *
-     * @param scalar the value to add
+     * @param scalar the value to subtract
+     * @return the new calculated matrix
      */
     Mat<T, SIZE> operator - (const T &scalar)
     {
@@ -205,10 +214,11 @@ public:
     }
 
     /**
-     * operator to add a simple type to each entry
+     * operator to multiply a simple type to each entry
      * within the array
      *
-     * @param scalar the value to add
+     * @param scalar the value to multiply the matrix cells
+     * @return the new calculated matrix
      */
     Mat<T, SIZE> operator * (const T &scalar)
     {
@@ -219,15 +229,28 @@ public:
         return buf;
     }
 
+    /**
+     * operator to divide a simple type to each entry
+     * within the array
+     *
+     * @param scalar the value to divide the matrix cells
+     * @return the new calculated matrix
+     */
     Mat<T, SIZE> operator / (const T &scalar)
     {
         Mat<T, SIZE> buf;
-        for (unsigned int i=0; i<SIZE; i++) // row i
-            for (unsigned int j=0; j<SIZE; j++) // column j
-                buf(i,j) += m_aatData[i][j] / scalar;
+        for (unsigned int row=0; row<SIZE; row++)
+            for (unsigned int column=0; column<SIZE; column++)
+                buf(row,column) += m_aatData[row][column] / scalar;
         return buf;
     }
 
+    /**
+     * operator to multiply tow different matrices 
+     *
+     * @param mat matrix to multiply 
+     * @return the new calculated matrix
+     */
     Mat<T, SIZE> operator * (const Mat<T, SIZE> &mat)
     {
         Mat<T, SIZE> buf;
@@ -237,11 +260,21 @@ public:
                     buf(i,j) += m_aatData[i][a] * mat(a,j);
         return buf;
     }
+    /**
+     * This functions is used for the assigning operator.
+     * for a simple array for the right hand side.
+     * The array has to be column wise coded.
+     *
+     * @info the template type has to provide the abs function
+     *
+     * @param aatData the right hand side array
+     * @return the new matrix 
+     */
     Mat<T, SIZE> &operator = (const T aatData[SIZE*SIZE]) 
     {
-        for (unsigned int i=0; i<SIZE; i++) // row i
-            for (unsigned int j=0; j<SIZE; j++) // column j
-                m_aatData[i][j] = aatData[j+(i*4)];
+        for (unsigned int row=0; row<SIZE; row++)
+            for (unsigned int column=0; column<SIZE; column++) // column j
+                m_aatData[row][column] = aatData[column+(row*4)];
 
         return (*this); // also an R-value in e.g.
                         // vec1  = vec2  +   (vec2=atData); // parenthesis () needed to evaluate expression vec2=atData
@@ -249,6 +282,14 @@ public:
                         //   "   =   "   + (L-Val = R-Val)
     }
 
+    /**
+     * This functions calculates the matrix containing only the 
+     * absolute values of the current one.
+     *
+     * @info the template type has to provide the abs function
+     *
+     * @return the matrix with absolute values 
+     */
     Mat<T,SIZE> abs()
     {
         Mat<T,SIZE> buf;
@@ -258,12 +299,17 @@ public:
         return buf;
     }
 
+    /**
+     * This functions calculates the determinant of a matrix
+     *
+     * @return the determinant
+     */
     T determinant()
     {
         T buf = 0;
         if (SIZE == 1)
             return m_aatData[0][0];
-        if ( SIZE == 2)
+        if (SIZE == 2)
             return m_aatData[0][0] * m_aatData[1][1] - m_aatData[1][0]*m_aatData[0][1];
 
 
@@ -273,17 +319,27 @@ public:
             multBuff = m_aatData[0][i];
             multBuff *= m_aatData[1][(i+1)%SIZE];
             multBuff *= m_aatData[2][(i+2)%SIZE];
-            std::cout <<"+("<< i << ", " << (i+1)%SIZE << ", " << (i+2)%SIZE << ") ";
+
+            //std::cout <<"+("<< i << ", " << (i+1)%SIZE << ", " << (i+2)%SIZE << ") ";
+
             buf += multBuff;
             multBuff = m_aatData[0][i];
             multBuff *= m_aatData[1][(i+SIZE-1)%SIZE];
             multBuff *= m_aatData[2][(i+SIZE-2)%SIZE];
-            std::cout <<"-("<< i <<", " << (i+SIZE-1)%SIZE << ", " << (i+SIZE-2)%SIZE << ") ";
+
+            //std::cout <<"-("<< i <<", " << (i+SIZE-1)%SIZE << ", " << (i+SIZE-2)%SIZE << ") ";
+
             buf -= multBuff;
         }
         return buf;
     }
 
+    /**
+     * This functions calculates the norm of the matrix.
+     *
+     * @info the Template type has to provide the sqrt and pow function
+     * @return the normalisation factor
+     */
     Mat<T,SIZE> norm()
     {
         T buf;
@@ -294,14 +350,31 @@ public:
         return buf;
     }
 
+    /**
+     * This functions generates the transposed matrix for the 
+     * current one.
+     * @return the transposed matrix
+     *
+     * @see transpose
+     * @see determinant
+     */
     Mat<T,SIZE> transpose()
     {
         Mat<T, SIZE> buf;
-        for (unsigned int i=0; i<SIZE; i++) // row i
-            for (unsigned int j=0; j<SIZE; j++) // column j
-                buf.m_aatData[i][j] = m_aatData[j][i];
+        for (unsigned int row=0; row<SIZE; row++)
+            for (unsigned int column=0; column<SIZE; column++)
+                buf.m_aatData[row][column] = m_aatData[row][column];
         return buf;
     }
+    /**
+     * This functions generates the inverse matrix for the 
+     * current one. 
+     * This is done via the determinant and the transposed.,
+     * @return the inverted matrix
+     *
+     * @see transpose
+     * @see determinant
+     */
     Mat<T,SIZE> inv()
     {
         T det = determinant();
@@ -316,12 +389,22 @@ public:
 
         return thisTransposed;
     }
+    /**
+     * This functions copies the vales from the matrix to a flat list 
+     * of simple types. The order will be row wise.
+     * @param atData the flat destination array
+     */
     void getData (T atData[SIZE*SIZE]) 
     {
         for (unsigned int row=0; row<SIZE; row++)
             for (unsigned int column=0; column<SIZE; column++)
                 atData[row+column*SIZE] = m_aatData[row][column];
     }
+    /**
+     * This functions copies the vales from a flat list of simple types
+     * to the matrix. The order has to be rowwise.
+     * @param atData the flat source array
+     */
     void setData (const T atData[SIZE*SIZE]) 
     {
         for (unsigned int row=0; row<SIZE; row++)
@@ -330,10 +413,19 @@ public:
     }
 
 private:
+    /// the private container for the matrix cells
     T m_aatData[SIZE][SIZE];
 };
 
 
+/**
+ * This function is used for the standard output stream and converts
+ * the Matrix to a string.
+ *
+ * @param output the outputstream which should be extended by the matrix print
+ * @param mat    the matrix which should be printed to the output stream
+ * @return the changed output stream to be capable of something like "cout<<"test" << matrix;"
+ */
 template <class T, unsigned SIZE>
 static std::ostream& operator<< (std::ostream& output,const Mat<T,SIZE> &mat)
 {
@@ -356,33 +448,47 @@ static std::ostream& operator<< (std::ostream& output,const Mat<T,SIZE> &mat)
     }
     return output;
 }
+
+/**
+ * This function creates a rotation matrix for a x-y-z rotation
+ *
+ * @param x rotation for the x axis in [ degree ]
+ * @param y rotation for the x axis in [ degree ]
+ * @param x rotation for the x axis in [ degree ]
+ * @return a homogeneous cartesian matrix
+ */
 static Mat<float, 4> getRotationMatrix(float x_angle,float y_angle, float z_angle)
 {
     Mat<float,4> tempx,tempy,tempz;
+
+    // create x rotation
     float temp_x[4][4] = { { 1,            0,             0, 0},
                            { 0, cos(x_angle), -sin(x_angle), 0},
                            { 0, sin(x_angle),  cos(x_angle), 0},
                            { 0,            0,             0, 1}
                          };
 
+    // create y rotation
     float temp_y[4][4] = { { cos(y_angle), 0, -sin(y_angle), 0},
                            {            0, 1,             0, 0},
                            { sin(y_angle), 0,  cos(y_angle), 0},
                            {            0, 0,             0, 1}
                          };
+
+    // create z rotation
     float temp_z[4][4] = { { cos(z_angle), -sin(z_angle), 0, 0},
                            { sin(z_angle),  cos(z_angle), 0, 0},
                            {            0,             0, 1, 0},
                            {            0,             0, 0, 1}
                           };
 
+    // combine all rotations to a matrix class
     tempx=temp_x;
     tempy=temp_y;
     tempz=temp_z;
     return tempz*tempy*tempx;
 }
 /**
- * @}
  * @}
  */
 #endif
diff --git a/lwrserv/include/Vec.h b/lwrserv/include/Vec.h
index 4253bf1..02e341f 100644
--- a/lwrserv/include/Vec.h
+++ b/lwrserv/include/Vec.h
@@ -4,6 +4,35 @@
 #include <ostream>
 #include <cmath>
 #include "Mat.h"
+/**
+ * @addtogroup math
+ * @{
+ * @author  Philipp Schoenberger <ph.schoenberger@googlemail.com>
+ * @version 2.0
+ *
+ * @section LICENSE
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program 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 at
+ * https://www.gnu.org/copyleft/gpl.html
+ *
+ * @section DESCRIPTION
+ *
+ * This file contains the Trajectory for a linear trajectory
+ * The linear trajectory only uses the max speed and ignores
+ * the acceleration parameter.
+ * The movement is a strict hard acceleration and deacceration.
+ *
+ * The slowest joint is defining the speed of the other joints.
+ * By that all joints start and stop the movement synchronously
+ */
 
 template<class T, unsigned SIZE> class Vec;
 template <class T, unsigned SIZE> class Mat;
@@ -113,7 +142,7 @@ public:
 
     /**
      * assignment operator between vectors
-     * also capabile of muilti asignments 
+     * also capable of multi assignments 
      * e.g: 
      *
      * vec1    =       vec2      = vec3;
@@ -139,6 +168,17 @@ public:
         return (*this); 
     }
 
+    /**
+     * assignment operator between simple types
+     * also capable of multi assignments 
+     * e.g: 
+     *
+     * vec1    =       vec2      = vec3;
+     * L-Value = R-Value/L-Value = R-Value
+     *
+     * @param atData the source data array which should be copied
+     * @retval returns the current 
+     */
     Vec<T, SIZE> &operator = (const T atData[SIZE]) 
     {
         for (unsigned int i=0; i<SIZE; i++) // not me, so L-Value action: copy data
@@ -150,10 +190,16 @@ public:
                         //   "   =   "   + (L-Val = R-Val)
     }
 
-    // usage of operator:
-    // vec(i) = var;    // 0 <= i <= SIZE-1
-    // var = vec(i);
-    // vec1(i) = vec2(j);
+    /**
+     * This functions requests the cell with a defined index
+     * usage of operator:
+     * vec(i) = var;    // 0 <= i <= SIZE-1
+     * var = vec(i);
+     * vec1(i) = vec2(j);
+     *
+     * @param i the index of the vector
+     * @retval returns the cell with the provided index
+     */
     T &operator () (unsigned i) 
     {
         if (i>=SIZE)
@@ -161,6 +207,16 @@ public:
         return m_atData[i];         // ... so we can safely return a reference
     }
 
+    /**
+     * This functions requests the cell with a defined index
+     * usage of operator:
+     * vec(i) = var;    // 0 <= i <= SIZE-1
+     * var = vec(i);
+     * vec1(i) = vec2(j);
+     *
+     * @param i the index of the vector
+     * @retval returns the cell with the provided index
+     */
     T operator () (unsigned i) const 
     {
         if (i>=SIZE)
@@ -168,12 +224,25 @@ public:
         return m_atData[i];
     }
 
+    /**
+     * operator to adds the vector by 
+     * an other vector and reasign it.
+     *
+     * @param vec the vector to add to the current instance
+     */
     void operator += (const Vec<T, SIZE> &vec) 
     {
         for (int i=0; i<SIZE; i++)
             m_atData[i] += vec.m_atData[i];
     }
 
+    /**
+     * operator to adds the vector by 
+     * an other vector.
+     *
+     * @param vec the vector to add to the current instance
+     * @return the calculated vector
+     */
     Vec<T, SIZE> operator + (const Vec<T, SIZE> &vec) 
     {
         Vec<T, SIZE> buf (m_atData);
@@ -182,14 +251,25 @@ public:
         return buf;
     }
 
+    /**
+     * operator to subtract the vector by 
+     * an other vector.and reasign it
+     *
+     * @param vec the vector to subtract to the current instance
+     */
     void operator -= (const Vec<T, SIZE> &vec) 
     {
         for (unsigned int i=0; i<SIZE; i++)
             m_atData[i] -= vec.m_atData[i];
     }
 
-    // binary -
-    // vec1 - vec2 i.e. (*this) - vec
+    /**
+     * operator to subtract the vector by 
+     * an other vector.
+     *
+     * @param vec the vector to subtract to the current instance
+     * @return the calculated vector
+     */
     Vec<T, SIZE> operator - (const Vec<T, SIZE> &vec) 
     {
         Vec<T, SIZE> buf (m_atData);
@@ -198,8 +278,12 @@ public:
         return buf;
     }
 
-    // unary -
-    // -vec i.e. -(*this)
+    /**
+     * operator to calculate the negative vector
+     * of the current one
+     *
+     * @return the negative vector
+     */
     Vec<T, SIZE> operator - () 
     {
         T atBuffer[SIZE];
@@ -208,6 +292,13 @@ public:
         return Vec<T, SIZE> (atBuffer);
     }
 
+    /**
+     * operator to multiply the vector by 
+     * an other vector.
+     *
+     * @param vec the vector to multiply to the current instance
+     * @return the vector product
+     */
     T operator * (const Vec<T, SIZE> & vec) 
     {
         T dp = T(0);
@@ -216,12 +307,25 @@ public:
         return dp;
     }
 
+    /**
+     * operator to multiply the vector by the simple type
+     * provided and reasign it.
+     *
+     * @param tscale the simple type
+     */
     void operator *= (T tScale) 
     {
         for (unsigned int i=0; i<SIZE; i++)
             m_atData[i] *= tScale;
     }
 
+    /**
+     * operator to multiply the vector by the simple type
+     * provided 
+     *
+     * @param tscale the simple type
+     * @return the new calculated matrix
+     */
     Vec<T, SIZE> operator * (T tScale) 
     {
         Vec<T, SIZE> vec;
@@ -229,6 +333,14 @@ public:
             vec(i) = m_atData[i]*tScale;
         return vec;
     }
+
+    /**
+     * operator to divide the vector by the simple type
+     * provided 
+     *
+     * @param tscale the simple type
+     * @return the new calculated matrix
+     */
     Vec<T, SIZE> operator / (T tScale) 
     {
         Vec<T, SIZE> vec;
@@ -237,6 +349,15 @@ public:
         return vec;
     }
 
+    /**
+     * This functions returns from the current and 
+     * the provided vector the vector division
+     * for each cell separately
+     *
+     * @info the operator/ function has to be implemented for the template type
+     *
+     * @return the vector containing the cell wise divisor
+     */
     Vec<T, SIZE> operator * (const Mat<T, SIZE> &mat)
     {
         Vec<T, SIZE> vec;
@@ -246,6 +367,16 @@ public:
         return vec;
     }
 
+    /**
+     * This functions returns from the current and 
+     * the provided vector the vector division
+     * for each cell separately
+     *
+     * @info the operator/ function has to be implemented for the template type
+     *
+     * @pram vec the vector to divide the current vector
+     * @return the vector containing the cell wise divisor
+     */
     Vec<T, SIZE> celldivide (const Vec<T, SIZE> & vec) 
     {
         Vec<T, SIZE> buff;
@@ -253,6 +384,17 @@ public:
             buff.m_atData[i] = m_atData[i]/vec.m_atData[i];
         return buff;
     }
+
+    /**
+     * This functions returns from the current and 
+     * the provided vector the vector multiplication 
+     * for each cell separately
+     *
+     * @info the operator* function has to be implemented for the template type
+     *
+     * @pram vec the vector to multiply the current vector
+     * @return the vector containing the cell wise product
+     */
     Vec<T, SIZE> cellmultiply (const Vec<T, SIZE> & vec) 
     {
         Vec<T, SIZE> buff;
@@ -260,6 +402,15 @@ public:
             buff.m_atData[i] = m_atData[i]*vec.m_atData[i];
         return buff;
     }
+
+    /**
+     * This functions returns from the current and 
+     * the provided vector the minimum for each cell
+     *
+     * @info the min function has to be implemented for the template type
+     *
+     * @return the vector containing the minimum cells in each dimension
+     */
     Vec<T, SIZE> cellmin(const Vec<T, SIZE> & vec) 
     {
         Vec<T, SIZE> buff;
@@ -267,6 +418,15 @@ public:
             buff.m_atData[i] = std::min(m_atData[i],vec.m_atData[i]);
         return buff;
     }
+
+    /**
+     * This functions returns from the current and 
+     * the provided vector the maximum for each cell
+     *
+     * @info the max function has to be implemented for the template type
+     *
+     * @return the vector containing the maximum cells in each dimension
+     */
     Vec<T, SIZE> cellmax(const Vec<T, SIZE> & vec) 
     {
         Vec<T, SIZE> buff;
@@ -275,6 +435,14 @@ public:
         return buff;
     }
 
+    /**
+     * This functions calculates the absolute value
+     * for each cell.
+     *
+     * @info the abs function has to be implemented for the template type
+     *
+     * @return the vector containing the absolute values
+     */
     Vec<T, SIZE> abs ()
     {
         Vec<T, SIZE> buff;
@@ -283,6 +451,14 @@ public:
         return buff;
     }
     
+    /**
+     * This functions calculates the square root
+     * for each cell.
+     *
+     * @info the sqrt function has to be implemented for the template type
+     *
+     * @return the vector containing the square root values
+     */
     Vec<T, SIZE> sqrt()
     {
         Vec<T, SIZE> buff;
@@ -290,6 +466,15 @@ public:
             buff.m_atData[i] = std::sqrt(m_atData[i]);
         return buff;
     }
+
+    /**
+     * This functions calculates the floor
+     * for each cell.
+     *
+     * @info the floor function has to be implemented for the template type
+     *
+     * @return the vector containing the floor values
+     */
     Vec<T, SIZE> floor ()
     {
         Vec<T, SIZE> buff;
@@ -297,6 +482,15 @@ public:
             buff.m_atData[i] = floor(m_atData[i]);
         return buff;
     }
+    
+    /**
+     * This functions calculates the ceil
+     * for each cell.
+     *
+     * @info the ceil function has to be implemented for the template type
+     *
+     * @return the vector containing the ceil values
+     */
     Vec<T, SIZE> ceil ()
     {
         Vec<T, SIZE> buff;
@@ -304,6 +498,14 @@ public:
             buff.m_atData[i] = std::ceil(m_atData[i]);
         return buff;
     }
+
+    /**
+     * This functions is here for the float simple type
+     * to calculate the sgn
+     *
+     * @param x the float value to check
+     * @return the sign of the provided float
+     */
     float sgn(float x) 
     { 
         if ( x == 0 ) 
@@ -314,6 +516,14 @@ public:
         else
             return -1.0f; 
     }
+
+    /**
+     * This functions returns the vector containing the 
+     * sign for each cell.
+     *
+     * @info the Template type has to provide the sgn function
+     * @return the vector with all signs
+     */
     Vec<T, SIZE> sgn ()
     {
         Vec<T, SIZE> buff;
@@ -322,6 +532,12 @@ public:
         return buff;
     }
     
+    /**
+     * This functions returns the maximum cell of the vector.
+     *
+     * @info the Template type has to provide the operator>
+     * @return the maximum cell
+     */
     T max ()
     {
         T retval = m_atData[0];
@@ -332,6 +548,13 @@ public:
                 retval =  m_atData[i];
         return retval;
     }
+    
+    /**
+     * This functions returns the minimum cell of the vector.
+     *
+     * @info the Template type has to provide the operator>
+     * @return the minimum cell
+     */
     T min ()
     {
         T retval = m_atData[0];
@@ -343,6 +566,12 @@ public:
         return retval;
     }
 
+    /**
+     * This functions calculates the norm of the vector
+     *
+     * @info the Template type has to provide the sqrt function
+     * @return the normalisation factor
+     */
     T norm ()
     {
         T retval = 0;
@@ -354,6 +583,13 @@ public:
         return retval;
     }
 
+
+    /**
+     * This functions calculates the cross product of 2 vectors.
+     * 
+     * @param vec the 2th vector for the cross product
+     * @return the cross product
+     */
     Vec<T, SIZE> x(const Vec<T, SIZE> &vec)
     {
         T temp[SIZE];
@@ -389,6 +625,14 @@ typedef Vec<bool, 3> Vec3b;
 typedef Vec<bool, 4> Vec4b;
 
 
+/**
+ * This function is used for the standard output stream and converts
+ * the vector to a string.
+ *
+ * @param output the outputstream which should be extended by the vector
+ * @param vec    the vector which should be printed to the output stream
+ * @return the changed output stream to be capable of something like "cout<<"test" << matrix;"
+ */
 template <class T, unsigned SIZE>
 static std::ostream& operator<< (std::ostream& output,const Vec<T,SIZE> &vec)
 {
@@ -404,4 +648,7 @@ static std::ostream& operator<< (std::ostream& output,const Vec<T,SIZE> &vec)
     return output;
 }
 
+/**
+ * @}
+ */
 #endif