add more doxygen

lwrserv/include/BangBangTrajectory.h

@@ -1,12 +1,6 @@
-#ifndef _BANGBANGTRAJCETORY_H_
-#define _BANGBANGTRAJCETORY_H_
-#include "Trajectroy.h"
-#include "Vec.h"
-
 /**
  * @addtogroup trajectory
  * @{
- *
  * @addtogroup BangBang
  * @{
  * @author  Philipp Schoenberger <ph.schoenberger@googlemail.com>
@@ -34,6 +28,10 @@
  * The slowest joint is defining the speed of the other joints.
  * By that all joints start and stop the movement synchronously
  */
+#ifndef _BANGBANGTRAJCETORY_H_
+#define _BANGBANGTRAJCETORY_H_
+#include "Trajectroy.h"
+#include "Vec.h"
 
 /**
  * Class for a Bang Bang Trajectory based on joint angles.
@@ -50,7 +48,7 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
     public:
         /**
          * standard constructor which will setup the class for a 
-         * trajectory from start to end position with a synchron movement
+         * trajectory from start to end position with a synchron movement based on joints
          *
          * @param sampleTimeMs         The discrete time interval for each step within the Trajectory with the unit [ milliseconds ]
          * @param maxJointVelocity     The maximum velocity for each joint with the unit [ degree/ seconds ]

lwrserv/include/FivePolyTrajectory.h

@@ -1,14 +0,0 @@
-#ifndef _FIVEPOLYTRAJECTORY_H_
-#define _FIVEPOLYTRAJECTORY_H_
-#include "Trajectroy.h"
-
-template <unsigned SIZE>
-class FivePolyJointTrajectory: public Trajectory<SIZE>
-{
-};
-template <unsigned SIZE>
-class FivePolyCartTrajectory: public Trajectory<SIZE>
-{
-};
-
-#endif

lwrserv/include/LSPBTrajectory.h

@@ -4,10 +4,63 @@
 #include "Trajectroy.h"
 #include "sgn.h"
 
+/**
+ * @addtogroup trajectory
+ * @{
+ * @addtogroup LSPB
+ * @{
+ * @author  Philipp Schoenberger <ph.schoenberger@googlemail.com>
+ * @version 1.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 LSPB trajectory.
+ * The LSPB trajectory is a trajectory with linear acceleration
+ * phase followed by a constant speed and a de-acceleration phase
+ * The speed curve looks with this like a symmetric Trapez.
+ *
+ * The slowest joint is defining the speed of the other joints.
+ * By that all joints start and stop the movement synchronously
+ */
+
+/**
+ * Class for a Bang Bang Trajectory based on joint angles.
+ * The joint start and stop synchron
+ *
+ * This class is based on the Trajectory joint Template so it 
+ * all possible joint counts 
+ *
+ * @see Trajectory
+ */
 template <unsigned SIZE>
 class LSPBJointTrajectory: public Trajectory<SIZE>
 {
 public:
+    /**
+     * standard constructor which will setup the class for a 
+     * trajectory from start to end position with a synchron movement based on joints
+     *
+     * The trajectory will be of type LSPB (Linear Segment with Parabolic Blend)
+     *
+     * @param sampleTimeMs         The discrete time interval for each step within the Trajectory with the unit [ milliseconds ]
+     * @param maxJointVelocity     The maximum velocity for each joint with the unit [ degree/ seconds ]
+     * @param maxJointAcceleration The maximum acceleration for each joint with the unit [ degree/(seconds^2) ]
+     * @param jointStart           The starting point for each joint in [ degree ]
+     * @param jointEnd             The point which should be reached at the end of the Trajectory [ degree ]
+     */
     LSPBJointTrajectory(
             float sampleTimeMs,
             Vec<float,SIZE> maxJointVelocity,
@@ -36,8 +89,6 @@ public:
         Vec<float,SIZE> maxVeloReachable = (jointMovementAbs.celldivide(maxJointAcceleration)*2.0f).sqrt().cellmultiply(maxJointVelocity);
         maxJointLocalVelocity = (maxVeloReachable/sampleTime).cellmax(maxJointLocalVelocity);
 
-
-
         //      v ^
         //        |
         //  v_bang|      /\  <- bang bang trajectory
@@ -69,60 +120,97 @@ public:
         Vec<float,SIZE> accelerationPhaseTime = maxJointVelocity.celldivide(maxJointAcceleration)/sampleTime;
         Vec<float,SIZE> timeMaxAcceleration = ((jointMovementAbs/2.0f).celldivide(maxJointAcceleration) * 2.0f).sqrt() * 2.0f / sampleTime;
         Vec<float,SIZE> timeMaxVelocity = ((jointMovementAbs/2.0f).celldivide(maxJointVelocity) * 2.0f)*2.0f / sampleTime;
-        Vec<float,SIZE> timeBangBang = timeMaxAcceleration.cellmax(timeMaxVelocity);
 
-        std::cout << timeBangBang << "timeBangBang\n";
+        // calculate the time and steps of the bangbang Trajectory
+        Vec<float,SIZE> timeBangBang = timeMaxAcceleration.cellmax(timeMaxVelocity);
         float steps_bangbang = timeBangBang.ceil().max();
 
-        // calculate maxAcceleration for bangbang
+        // calculate maxAcceleration for a bangbang trajectory
         Vec<float,SIZE> currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(steps_bangbang).celldivide(steps_bangbang)*2.0f;
-        std::cout << "currMaxAcceleration : " << currMaxAcceleration << "\n";
 
+        // use only a percentage of the bangbang max velocity
         float percentLSPB = 0.7f;
         Vec<float,SIZE> timeLSPB = timeBangBang * (1 + 0.25f * (1.0f - percentLSPB));
         float timeBlend = ((timeBangBang /2.0f) * percentLSPB).ceil().max();
-        std::cout << timeBlend << "timeBlend \n";
         
-        std::cout << timeLSPB << "max time \n";
+        // round up and use the maximum of all possible steps 
         Vec<float,SIZE> minStepsPerJoint = timeLSPB.ceil();
-
         this->steps = minStepsPerJoint.max();
         if (this->steps == 0)
             this->steps +=1;
 
+        // allocate an trajectory with this size
         this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) calloc(sizeof(struct Trajectory<SIZE>::trajectoryNode),this->steps);
 
         Vec<float,SIZE> jointLast  = jointStart;
         Vec<float,SIZE> velocityLast(0.0f);
 
 
+        // initialize the accelerated counts to zero
         int count = 0;
+
         for( int i = 0 ; i < this->steps; ++i)
         {
             if (i < timeBlend)
             {
+                // acceleration phase
                 count += 1;
                 this->nodes[i].acceleration = currMaxAcceleration;
             }else if (i < this->steps - timeBlend )
             {
+                // constant speed phase
                 this->nodes[i].acceleration = Vec<float,SIZE>(0.0f);
             }else 
             {
+                // deacceleration phase
                 count -= 1;
                 this->nodes[i].acceleration = currMaxAcceleration* -1.0f;
             }
+
+            // calculate velocity and joint position accordingly to the acceleration
+            // @warning to not add floats over and over again or the error will be huge
             this->nodes[i].velocity = jointMovementSgn.cellmultiply(currMaxAcceleration) * count;
             this->nodes[i].jointPos = jointLast + this->nodes[i].velocity; 
 
+            // check if we already reached end 
+            // This can happen if the steps calculation was
+            // to far off caused by floating point precision
+             
+            float maxdiffLast = (jointEnd - jointLast).abs().max();
+            float maxdiffCurr = (jointEnd - this->nodes[i].jointPos).abs().max();
+            if ( maxdiffLast < maxdiffCurr)
+            {
+                // mark the current index as the last one within this trajectory
+                this->steps = i+1;
+                // set the end position
+                this->nodes[i].jointPos = jointEnd;
+                break;
+            }
+
             jointLast = this->nodes[i].jointPos;
             velocityLast = this->nodes[i].velocity;
         }
     }
 };
 
+/**
+ * Class for a LSPB Trajectory based on homogenous
+ * Cartesian Positions.
+ * The joint start and stop synchron
+ *
+ * This class is based on the Trajectory joint Template so it 
+ * all possible joint counts 
+ *
+ * @TODO implement the Cartesian movement constructor for LSPB trajectories
+ * @see Trajectory
+ */
 template <unsigned SIZE>
 class LSPBCartTrajectory: public Trajectory<SIZE>
 {
 };
 
+/**
+ * @}
+ * @}
+ */
 #endif

lwrserv/include/LinearTrajectory.h

@@ -1,16 +1,69 @@
 #ifndef _LINEAR_TRAJECTORY_H_
 #define _LINEAR_TRAJECTORY_H_
+/**
+ * @addtogroup trajectory
+ * @{
+ * @addtogroup linear
+ * @{
+ * @author  Philipp Schoenberger <ph.schoenberger@googlemail.com>
+ * @version 1.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
+ */
 
 #include "Trajectroy.h"
 #include "stdlib.h"
 #include "Vec.h"
 #include "Mat.h"
 
+
+/**
+ * Class for a Bang Bang Trajectory based on joint angles.
+ * The joint start and stop synchron
+ *
+ * This class is based on the Trajectory joint Template so it 
+ * all possible joint counts 
+ *
+ * @see Trajectory
+ */
 template <unsigned SIZE>
 class LinearJointTrajectory : public Trajectory<SIZE>
 {
     public:
 
+        /**
+         * standard constructor which will setup the class for a 
+         * trajectory from start to end position with a synchron movement based on joints
+         *
+         * The trajectory will be of type linear (linear constant speed)
+         *
+         * @param sampleTimeMs         The discrete time interval for each step within the Trajectory with the unit [ milliseconds ]
+         * @param maxJointVelocity     The maximum velocity for each joint with the unit [ degree/ seconds ]
+         * @param maxJointAcceleration The maximum acceleration for each joint with the unit [ degree/(seconds^2) ] (unused)
+         * @param jointStart           The starting point for each joint in [ degree ]
+         * @param jointEnd             The point which should be reached at the end of the Trajectory [ degree ]
+         */
         LinearJointTrajectory(
                 float sampleTimeMs,
                 Vec<float,SIZE> maxJointVelocity,
@@ -19,12 +72,10 @@ class LinearJointTrajectory : public Trajectory<SIZE>
                 Vec<float,SIZE> jointEnd
                 )
         {
-            std::cout << "linear \n " ;
             float MStoSec = 1000.0f;
             // calculate maximum velocity and acceleration
             Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec);
             Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec);
-            std::cout << "max : " << maxJointLocalVelocity << "\n";
 
             // calculate delta movement
             Vec<float,SIZE> jointMovement = jointEnd - jointStart;
@@ -34,15 +85,14 @@ class LinearJointTrajectory : public Trajectory<SIZE>
 
             // calculate number of movement steps
             Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity);
-            std::cout << "minsteps : " << minStepsPerJoint << "\n";
-            std::cout << "steps : " << minStepsPerJoint.max()  << "\n";
             this->steps = ceil(minStepsPerJoint.max());
-            std::cout << "steps : " << this->steps << "\n";
             if (this->steps == 0)
                 this->steps +=1;
 
+            // allocate an array for the joint movements
             this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) calloc(sizeof(struct Trajectory<SIZE>::trajectoryNode),this->steps);
 
+            // initialize the array of movements
             Vec<float,SIZE> jointLast  = jointStart;
             for( int i = 0 ; i < this->steps; ++i)
             {
@@ -51,19 +101,32 @@ class LinearJointTrajectory : public Trajectory<SIZE>
                 this->nodes[i].velocity = maxJointLocalVelocity; 
                 this->nodes[i].acceleration = Vec<float,SIZE>(0.0f);
             }
-            // last step myth be to wide so cut it to the wanted position
+            // last step might be to wide so cut it to the wanted position
             this->nodes[this->steps-1].jointPos = jointEnd;
-            this->nodes[this->steps-1].velocity = maxJointLocalVelocity; // TODO this is not the truth
+            this->nodes[this->steps-1].velocity = maxJointLocalVelocity;
             this->nodes[this->steps-1].acceleration = Vec<float,SIZE>(0.0f); 
         }
-    private:
-
-    protected:
 };
 
+/**
+ * Class for a linear Trajectory based on homogenous
+ * Cartesian Positions.
+ * The joint start and stop synchron
+ *
+ * This class is based on the Trajectory joint Template so it 
+ * all possible joint counts 
+ *
+ * @TODO implement the Cartesian movement constructor for linear Trajectories
+ * @see Trajectory
+ */
 template <unsigned SIZE>
 class LinearCartTrajectory : public Trajectory<SIZE>
 {
 
 };
+
+/**
+ * @}
+ * @}
+ */
 #endif

lwrserv/include/Mat.h

@@ -3,6 +3,35 @@
 #include <iostream>
 #include <math.h>
 #include "Vec.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;
 
@@ -20,107 +49,167 @@ typedef Mat<float, 4> MatCarthesian;
 
 
 // template square matrix class for SIMPLE data types 
-template <class T, unsigned SIZE> class Mat 
+template <class T, unsigned SIZE>
+class Mat 
 {
 public:
 
+    /**
+     * standard constructor which is initialising all vales to zero
+     */
     Mat<T, SIZE> ()
     {
-        for (unsigned int j=0; j<SIZE; j++)
+        for (unsigned int x=0; x<SIZE; x++)
         {
-            for (unsigned int i=0; i<SIZE; i++)
+            for (unsigned int y=0; y<SIZE; y++)
             {
-                m_aatData[i][j] = T(0);
+                m_aatData[x][y] = T(0);
             }
         }
     }
 
+    /**
+     * default deconstructor which has to do nothing
+     */
     ~Mat<T, SIZE> ()
     {
         // nothing to do here ...
     }
 
-    Mat<T, SIZE> (const T aatData[SIZE][SIZE])
+    /**
+     * copy constructor for flat arrays of the simple type 
+     *
+     * @param srcData source data for the new matrix
+     */
+    Mat<T, SIZE> (const T srcData[SIZE][SIZE])
     {
-        for (unsigned int j=0; j<SIZE; j++)
+        for (unsigned int x=0; x<SIZE; x++)
         {
-            for (unsigned int i=0; i<SIZE; i++)
+            for (unsigned int y=0; y<SIZE; y++)
             {
-                m_aatData[i][j] = aatData[i][j];
+                m_aatData[x][y] = srcData[x][y];
             }
         }
     }
-    Mat<T, SIZE> (const T init_val,const T eye_val)
+
+    /**
+     * constructor for an identity matrix from left top to right bottom
+     *
+     * @param initVal the value in each field beside the diagonal entries
+     * @param eyeVal  the value on each filed exactly on the diagonal
+     */
+    Mat<T, SIZE> (const T initVal,const T eyeVal)
     {
-        for (unsigned int j=0; j<SIZE; j++)
+        for (unsigned int x=0; x<SIZE; x++)
         {
-            for (unsigned int i=0; i<SIZE; i++)
+            for (unsigned int y=0; y<SIZE; y++)
             {
-                if (j == i )
-                    m_aatData[i][j] = eye_val;
+                if (x == y)
+                    m_aatData[x][y] = eyeVal;
                 else
-                    m_aatData[i][j] = init_val;
+                    m_aatData[x][y] = initVal;
             }
         }
     }
-    Mat<T, SIZE> (const T init_val)
+
+    /**
+     * constructor for an matrix with a defined value in each cell
+     *
+     * @param initVal the value for each field
+     */
+    Mat<T, SIZE> (const T initVal)
     {
-        for (unsigned int j=0; j<SIZE; j++)
+        for (unsigned int x=0; x<SIZE; x++)
         {
-            for (unsigned int i=0; i<SIZE; i++)
+            for (unsigned int y=0; y<SIZE; y++)
             {
-                m_aatData[i][j] = init_val;
+                m_aatData[x][y] = initVal;
             }
         }
     }
 
-    Mat<T, SIZE> (const Mat<T, SIZE> &mat)
+    /**
+     * copy constructor for matrix-matrix copy
+     *
+     * @param srcMat the value for each field
+     */
+    Mat<T, SIZE> (const Mat<T, SIZE> &srcMat)
     {
-        for (unsigned int j=0; j<SIZE; j++)
+        for (unsigned int x=0; x<SIZE; x++)
         {
-            for (unsigned int i=0; i<SIZE; i++)
+            for (unsigned int y=0; y<SIZE; y++)
             {
-                m_aatData[i][j] = mat.m_aatData[i][j];
+                m_aatData[x][y] = srcMat.m_aatData[x][y];
             }
         }
     }
 
-    T &operator () (unsigned i, unsigned j)
+    /**
+     * operator for cell access and assignment
+     *
+     * @param x x axis position within the matrix
+     * @param y y axis position within the matrix
+     */
+    T &operator () (unsigned x, unsigned y)
     {
-        if (i>=SIZE)
-            i = SIZE-1;
-        if (j>=SIZE)
-            j = SIZE-1;
-        return m_aatData[i][j];
+        if (y>=SIZE)
+            y = SIZE-1;
+        if (x>=SIZE)
+            x = SIZE-1;
+        return m_aatData[x][y];
     }
 
-    T operator () (unsigned i, unsigned j) const
+    /**
+     * operator for cell access without writing to it
+     *
+     * @param x x axis position within the matrix
+     * @param y y axis position within the matrix
+     */
+    T operator () (unsigned x, unsigned y) const
     {
-        if (i>=SIZE)
-            i = SIZE-1;
-        if (j>=SIZE)
-            j = SIZE-1;
-        return m_aatData[i][j];
+        if (x>=SIZE)
+            x = SIZE-1;
+        if (y>=SIZE)
+            y = SIZE-1;
+        return m_aatData[x][y];
     }
 
+    /**
+     * operator to add a simple type to each entry
+     * within the array
+     *
+     * @param scalar the value to add
+     */
     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 x=0; x<SIZE; x++) // row
+            for (unsigned int y=0; y<SIZE; y++) // column
+                buf(x,y) += m_aatData[x][y] + scalar;
         return buf;
     }
 
+    /**
+     * operator to add a simple type to each entry
+     * within the array
+     *
+     * @param scalar the value to add
+     */
     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 x=0; x<SIZE; x++) // row
+            for (unsigned int y=0; y<SIZE; y++) // column
+                buf(x,y) += m_aatData[x][y] - scalar;
         return buf;
     }
 
+    /**
+     * operator to add a simple type to each entry
+     * within the array
+     *
+     * @param scalar the value to add
+     */
     Mat<T, SIZE> operator * (const T &scalar)
     {
         Mat<T, SIZE> buf;
@@ -303,4 +392,8 @@ static Mat<float, 4> getRotationMatrix(float x_angle,float y_angle, float z_angl
     tempz=temp_z;
     return tempz*tempy*tempx;
 }
+/**
+ * @}
+ * @}
+ */
 #endif

lwrserv/include/Robot.h

@@ -13,27 +13,122 @@ typedef Vec<float, FRI_CART_VEC> VecTorque;
 class Robot
 {
     public:
+        /// number of joints for the default robot
         static const unsigned int joints = LBR_MNJ;
 
+        unsigned int joints;
+        /// typdef for a vector witch can hold a value for each joint
         typedef Vec<float,joints> VecJoint;
 
-        static int      getJointCount(void)
+    protected:
+        VecJoint jointRangeMin;
+        VecJoint jointRangeMax;
+        VecJoint jointVelocity;
+        VecJoint jointAcceleration;
+        VecJoint jointTorque;
+
+    public:
+
+        /**
+         * default constructor for initialising the robot constraints
+         */
+        Robot()
+        {
+            this->joints = LBR_MNJ;
+            this->jointRangeMin = VecJoint(-180.0f);
+            this->jointRangeMax = VecJoint(+180.0f);
+            this->jointVelocity = VecJoint(+100.0f);
+            this->jointAcceleration = VecJoint(+2.0f);
+            this->jointTorque = VecJoint(10.0f);
+        }
+
+        /**
+         * Returns the joint count of the robot.
+         *
+         * @retval joint count
+         */
+        int getJointCount(void)
+        {
+            return joints;
+        }
+
+        /**
+         * Returns the minimum range for each joint of the robot
+         *
+         * @retval vector containing minimum joint angles [ degree ]
+         */
+        VecJoint getJointRangeMin(void)
+        {
+            return VecJoint(this->jointRangeMin);
+        }
+
+        /**
+         * Returns the maximum range for each joint of the robot
+         *
+         * @retval vector containing maximum joint angles [ degree ]
+         */
+        VecJoint getJointRangeMax(void)
+        {
+            return VecJoint(this->jointRangeMax);
+        }
+
+        /**
+         * Checks if the angles provided by the function parameter
+         * does match the valid range of the robot.
+         *
+         * @retval true if all angles are matching the range or false otherwise
+         */
+        bool validateJointRange(VecJoint jointAngles)
         {
+            for(unsigned int i = 0 ; i < joints ; ++i)
+            {
+                // angle to big
+                if (jointAngles(i) > this->jointRangeMax(i))
+                    return false;
+                
+                // angle to small
+                if (jointAngles(i) < this->jointRangeMin(i))
+                    return false;
+            }
+            return true;
         }
-        static VecJoint getJointRange(void)
+
+        /**
+         * Returns the maximum velocity for each joint of the robot
+         *
+         * @retval vector containing maximum velocity for each joint [ degree/seconds ]
+         */
+        VecJoint getJointVelocity(void)
         {
-            return VecJoint(180.0f);
+            return VecJoint(this->jointVelocity);
         }
-        static VecJoint getJointVelocity(void)
+
+        /**
+         * Returns the maximum acceleration for each joint of the robot
+         *
+         * @retval vector containing maximum acceleration for each joint [ degree/seconds^2 ]
+         */
+        VecJoint getJointAcceleration(void)
         {
-            return VecJoint(200.0f);
+            return VecJoint(this->jointAcceleration);
         }
-        static VecJoint getJointAcceleration(void)
+
+        /**
+         * Returns the maximum torque for each joint of the robot
+         *
+         * @retval vector containing maximum torque for each joint [ Nm ]
+         */
+        VecJoint getJointTorqueMax(void)
         {
-            return VecJoint(10.0f);
+            return VecJoint(this->jointTorque);
         }
 
-        static Vec<float, 4> getDhParameter(unsigned int joint)
+        /**
+         * Returns the david  hadenberg parameter for the requested joint
+         *
+         * @retval vector containing the 4 parameter
+         */
+        Vec<float, 4> getDhParameter(unsigned int joint)
         {
             // unused parameter
             (void) joint;
@@ -47,8 +142,38 @@ class Robot
             bool j1os;
         };
 
-        static VecJoint      backwardCalc(MatCarthesian, struct RobotConfig config );
-        static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config );
+        /**
+         * Calculates the joint angles for a given Position and arm configuration
+         *
+         * @param M      homogenious cartesian position
+         * @param config the robot arm configuration ( elbow etc.)
+         * @return the joint angles for the requested position
+         */
+        VecJoint backwardCalc(MatCarthesian M, struct RobotConfig config )
+        {
+            // unused
+            (void) M;
+            (void) config;
+
+            return VecJoint(0.0f);
+        }
+
+        /**
+         * Calculates the homogenious cartesian position for a given angle 
+         * and robot arm configuration
+         *
+         * @param M      homogenious cartesian position
+         * @param config the robot arm configuration ( elbow etc.)
+         * @return the joint angles for the requested position
+         */
+        MatCarthesian forwardCalc(VecJoint j, struct RobotConfig config )
+        {
+            // unused
+            (void) j;
+            (void) config;
+
+            return MatCarthesian(0.0f,1.0f);
+        }
 };
 
 #endif

lwrserv/include/SvrData.h

@@ -30,12 +30,7 @@ private:
     } commanded;
     
     struct {
-        struct {
-            Robot::VecJoint velocity;
-            Robot::VecJoint accelaration;
-            Robot::VecJoint torque;
-            Robot::VecJoint range;
-        } max;
+        Robot* robotClass;
         float currentVelocityPercentage;
         float currentAccelerationPercentage;
     } robot;
@@ -60,7 +55,7 @@ private:
     static SvrData* single;
     
     
-    void updateMessurementData(
+    void updateMeasurementData(
         Robot::VecJoint newJointPos,
         MatCarthesian newCartPos,
         VecTorque newForceAndTorque
@@ -72,10 +67,7 @@ public:
     
     friRemote* getFriRemote();
 
-    void updateMessurement();
-
-    void lock();
-    void unlock();
+    void updateMeasurement();
 
     Robot::VecJoint getMeasuredJointPos();
     MatCarthesian getMeasuredCartPos();

lwrserv/include/commandsHelp.h

@@ -33,7 +33,7 @@ void stopPotFieldModeHelp (SocketObject& client);
 void setTrajectoryTypeHelp (SocketObject& client);
 // get the current trajectory type 
 void getTrajectoryTypeHelp (SocketObject& client);
-//Quit
+// Quit
 void quitHelp (SocketObject& client);
 // check if we use a kuka
 void isKukaLwrHelp (SocketObject& client);

lwrserv/include/friComm.h

@@ -23,7 +23,7 @@
 ****************************************************************************
   friComm.h
        
-      NOTE: FRI (Fast Research inteface) is subject to radical change
+      NOTE: FRI (Fast Research interface) is subject to radical change
 	  
       
 [FH]}}
@@ -32,7 +32,7 @@
  \author (Andreas Stemmer,DLR)
  \author (Guenter Schreiber,KUKA)
   \file
-  \brief   This header file describes the basic communcation structures for
+  \brief   This header file describes the basic communication structures for
    the FastRobotInterface
   
   

lwrserv/include/lwr4.h

@@ -5,20 +5,79 @@
 #include "friComm.h"
 #include "Robot.h"
 
-
+/**
+ * @addtogroup robot
+ * @{
+ * @addtogroup lwr4
+ * @{
+ * @author  Philipp Schoenberger <ph.schoenberger@googlemail.com>
+ * @version 1.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 calculation functions for the lwr4 robot
+ *
+ * The slowest joint is defining the speed of the other joints.
+ * By that all joints start and stop the movement synchronously
+ */
+
+/**
+ * Class for a LWR4 robot
+ * The joint start and stop synchron
+ *
+ * This class is based on the Robot class
+ *
+ * @see Robot
+ */
 class LWR4 : public Robot
 {
-    public :
-        static Robot::VecJoint getJointRange(void);
-        static Robot::VecJoint getJointVelocity(void);
-        static Robot::VecJoint getJointAcceleration(void);
+    // the LWR4 robot has 7 Joints
+    // use the macro from fri remote
+public :
+    unsigned int joints;
 
-        static Vec<float, 4>   getDhParameter(unsigned int joint);
+    LWR4();
 
-        static Robot::VecJoint backwardCalc(MatCarthesian pos, struct RobotConfig config );
-        static MatCarthesian   forwardCalc(Robot::VecJoint, struct RobotConfig config );
+    LWR4::VecJoint backwardCalc(MatCarthesian pos, struct RobotConfig config );
+    MatCarthesian   forwardCalc(Robot::VecJoint, struct RobotConfig config );
 };
 
+LWR4::LWR4()
+{
+    joints = LBR_MNJ;
+    float jointRangeMax_[]     = { 170.0f, 120.0f, 170.0f, 120.0f, 170.0f, 120.0f, 170.0f};
+    float jointRangeMin_[]     = {-170.0f,-120.0f,-170.0f,-120.0f,-170.0f,-120.0f,-170.0f};
+    float jointVelocity_[]     = { 110.0f, 110.0f, 128.0f, 128.0f, 204.0f, 184.0f, 184.0f};
+    float jointAcceleration_[] = {   1.0f,   1.0f,   1.0f,   1.0f,   2.0f,   1.0f,   1.0f};
+    float jointTorque_[]       = { 176.0f, 176.0f, 100.0f, 100.0f, 100.0f,  38.0f,  38.0f};
+
+    this->jointRangeMin     = VecJoint(jointRangeMin_);
+    this->jointRangeMax     = VecJoint(jointRangeMax_);
+    this->jointVelocity     = VecJoint(jointVelocity_);
+    this->jointAcceleration = VecJoint(jointAcceleration_);
+    this->jointTorque       = VecJoint(jointTorque_);
+}
+
+/**
+ * Calculates an vector for the joint angles for a given carthesian Position.
+ * 
+ * @param M homogenious matrix for the Position
+ * @param config the robot configuration like elbow etc.
+ * @return the angles if the position could be reached.
+ */
 Robot::VecJoint LWR4::backwardCalc(MatCarthesian M , struct RobotConfig config )
 {
     Robot::VecJoint Joints(0.0f);

lwrserv/src/SvrData.cpp

@@ -11,54 +11,83 @@
 #include "Mat.h"
 #include "Vec.h"
 #include "Robot.h"
+#include "lwr4.h"
 
 bool SvrData::instanceFlag = false;
 SvrData* SvrData::single = 0;
 
+/**
+ * private Default Constructor which is initializing everything to the default parameters.
+ *
+ * By default 10% of the Maximum Velocity and Acceleration is used.
+ * Also the default robot is the LWR4 and a sample time of 5 ms
+ *
+ * This is a private constructor so only getInstance can call this 
+ * Constructor once and keep this class a singleton.
+ *
+ * @see getInstance
+ */
 SvrData::SvrData(void)
 {
-    // these are the default values for the robot 
-    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};
-
+    // only use 10 percent of the maximum velocity by default
     robot.currentVelocityPercentage = 10;
     robot.currentAccelerationPercentage = 10;
 
-    for (unsigned int i = 0; i < Robot::joints; ++i)
-    {
-        robot.max.velocity(i) = maxVelJnt[i];
-        robot.max.accelaration(i) = maxAccJnt[i];
-        robot.max.torque(i) = maxTrqJnt[i];
-        robot.max.range(i) = maxRangeJnt[i];
-    }
+    // use the lwr4 robot by default
+    robot.robotClass = new LWR4();
 
+    // use 5ms interval for each step
     trajectory.sampleTimeMs = 5.0f;
+
+    // do not cancel current trajectory (there also should not be any at this point)
     trajectory.cancel = false;
 
     // init mutex for database
     if (pthread_mutex_init(&dataLock, NULL) != 0)
-        printf("mutex init failed\n");
+        printf("mutex initializing failed\n");
 
+    // connect to the fri remote interface
     this->friInst = new friRemote();
 }
 
+/** 
+ * Destructor for cleaning up the memory
+ */
 SvrData::~SvrData()
 {
     pthread_mutex_destroy(&dataLock);
+    delete robot.robotClass;
 }
 
+/**
+ * Returns always the same reference to the SvrData.
+ * For the first call it will call the private Constructor otherwise
+ * it will return the same reference like the first time.
+ * 
+ * @return A pointer to the SvrData singleton class
+ */
+
 SvrData* SvrData::getInstance ()
 {
+    // check if the instance was already initialized
     if (instanceFlag)
         return single;
 
+    // first time so we have to create a new instance of SvrData
     single = new SvrData();
+
+    // mark the success on initializing to not allocate a second 
+    // SvrData reference
     instanceFlag = true;
+
     return single;
 }
 
+/**
+ * Returns the same reference to the FRI remote interface
+ * 
+ * @return a pointer to the FRI class
+ */
 friRemote* SvrData::getFriRemote()
 {
     friRemote* retval = NULL;
@@ -66,35 +95,54 @@ friRemote* SvrData::getFriRemote()
     pthread_mutex_lock(&dataLock);
     retval = this->friInst;
     pthread_mutex_unlock(&dataLock);
+
     return retval;
 }
 
-void SvrData::updateMessurementData(
+/**
+ * This function updates the measured Units directly
+ * via function parameters
+ * 
+ * @param newJointPos new measured Unit for the joint angles
+ * @param newCartPos  new measured homogeneous cartesian positions
+ * @param newForceAndTorque new measured force and torque for the end effector
+ *
+ * @see updateMeasurement
+ */
+void SvrData::updateMeasurementData(
         Robot::VecJoint newJointPos,
         MatCarthesian newCartPos,
         VecTorque newForceAndTorque
         )
 {
     pthread_mutex_lock(&dataLock);
+
     this->messured.jointPos = newJointPos;
     this->messured.cartPos  = newCartPos;
     this->messured.forceAndTorque = newForceAndTorque;
+
     pthread_mutex_unlock(&dataLock);
 }
 
-void SvrData::updateMessurement()
+/**
+ * This function updates the measured Units directly
+ * via the FRI remote instance and saves them into the database.
+ *
+ * @see updateMeasurementData
+ */
+void SvrData::updateMeasurement()
 {
     Robot::VecJoint newJointPos(0.0f);
     MatCarthesian newCartPos(0.0f,1.0f);
     VecTorque newForceAndTorque(0.0f);
 
-    /// do the update using fri
+    /// do the update using the FRI Interface
     /// update current joint positions
-    friInst->getState();
-	FRI_STATE state = friInst->getState();
     Robot::VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition());
     Robot::VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset());
     newJointPos = newJointPosComanded + newJointPosOffset;
+
+    // convert the joints from radian to degree
     newJointPos = newJointPos * ( 180.0f / M_PI);
 
     /// update current cart position
@@ -102,48 +150,87 @@ void SvrData::updateMessurement()
 
     /// update current force and torque
     newForceAndTorque = friInst->getFTTcp();
-    std::cout << newJointPos << " current newJointPos \n" << state << "\n";
-
-    updateMessurementData(newJointPos,newCartPos,newForceAndTorque);
-}
-
 
-void SvrData::lock()
-{
-    pthread_mutex_lock(&dataLock);
+    /// save the new values to the database
+    updateMeasurementData(newJointPos,newCartPos,newForceAndTorque);
 }
 
-void SvrData::unlock()
-{
-    pthread_mutex_unlock(&dataLock);
-}
-
-Robot::VecJoint SvrData::getMessuredJointPos()
+/**
+ * This function returns the current measurement of the joints
+ * within the database.
+ * This value will be updated if updateMeasurement was called
+ * which is done by the robot server thread.
+ *
+ * @see updateMeasurement
+ * @see threadRobotMovement
+ */
+Robot::VecJoint SvrData::getMeasuredJointPos()
 {
     Robot::VecJoint buf;
+
     pthread_mutex_lock(&dataLock);
     buf = messured.jointPos;
     pthread_mutex_unlock(&dataLock);
+
     return buf;
 }
 
+/**
+ * This function returns the current measurement homogeneous 
+ * cartesian positions within the database.
+ *
+ * This value will be updated if updateMeasurement was called
+ * which is done by the robot server thread.
+ *
+ * @see updateMeasurement
+ * @see threadRobotMovement
+ */
 MatCarthesian SvrData::getMessuredCartPos()
 {
     MatCarthesian buf;
+
     pthread_mutex_lock(&dataLock);
     buf = messured.cartPos;
     pthread_mutex_unlock(&dataLock);
+
     return buf;
 }
+
+/**
+ * This function returns the current measurement force and 
+ * torque within the database.
+ *
+ * This value will be updated if updateMeasurement was called
+ * which is done by the robot server thread.
+ *
+ * @see updateMeasurement
+ * @see threadRobotMovement
+ */
 VecTorque SvrData::getMessuredForceTorque()
 {
     VecTorque buf;
+
     pthread_mutex_lock(&dataLock);
     buf = messured.forceAndTorque;
     pthread_mutex_unlock(&dataLock);
+
     return buf;
 }
 
+/**
+ * This function returns the current Jacobian matrix.
+ *
+ * This value will be updated if updateMeasurement was called
+ * which is done by the robot server thread.
+ *
+ * @param data pointer for the matrix for the jacobian matrix
+ * @param size the size of the given data pointer to prevent segmentation faults
+ *
+ * @return 0 on success and the negative error code if an error occurred.
+ *
+ * @see updateMeasurement
+ * @see threadRobotMovement
+ */
 int SvrData::getMessuredJacobian(float* data, size_t size)
 {
     if (data == NULL)
@@ -154,31 +241,67 @@ int SvrData::getMessuredJacobian(float* data, size_t size)
     pthread_mutex_lock(&dataLock);
     memcpy(data,messured.jacobian,size);
     pthread_mutex_unlock(&dataLock);
+
     return 0;
 }
 
+/**
+ * This function returns the current commanded joint positions
+ *
+ * @return Vector for the current commanded joint values in unit [degree]
+ *
+ * @see setCommandedJointPos
+ */
 Robot::VecJoint SvrData::getCommandedJointPos()
 {
     Robot::VecJoint buff; 
+
     pthread_mutex_lock(&dataLock);
     buff = commanded.jointPos;
     pthread_mutex_unlock(&dataLock);
+
     return buff;
 }
+
+/**
+ * This function returns the current commanded 
+ *
+ * @return Vector for the current commanded joint values in unit [degree]
+ *
+ * @see getCommandedJointPos
+ */
 void SvrData::setCommandedJointPos(Robot::VecJoint newJointPos)
 {
     pthread_mutex_lock(&dataLock);
     commanded.jointPos = newJointPos;
     pthread_mutex_unlock(&dataLock);
 }
+
+/**
+ * This function returns the current commanded position
+ *
+ * @return Matrix for the current commanded cartesian homogeneous values
+ *
+ * @see setCommandedCartPos
+ */
 MatCarthesian SvrData::getCommandedCartPos ()
 {
     MatCarthesian buff;
+
     pthread_mutex_lock(&dataLock);
     buff = commanded.cartPos;
     pthread_mutex_unlock(&dataLock);
+
     return buff;
 }
+
+/**
+ * This function sets the current commanded position
+ *
+ * @return Matrix for the current commanded cartesian homogeneous values
+ *
+ * @see getCommandedCartPos
+ */
 void SvrData::setCommandedCartPos(MatCarthesian newCartPos)
 {
     pthread_mutex_lock(&dataLock);
@@ -187,40 +310,96 @@ void SvrData::setCommandedCartPos(MatCarthesian newCartPos)
 }
 
 
+/**
+ * This function returns the current maximum force and torque per joint
+ * for the configured Robot.
+ *
+ * @return Vector containing the Torque in the unit [ Nm ]
+ */
 Robot::VecJoint SvrData::getMaxTorque()
 {
-    Robot::VecJoint buff;
+    Robot::VecJoint retval;
+
     pthread_mutex_lock(&dataLock);
-    buff = robot.max.torque;
+    retval = robot.robotClass->getJointCount();
     pthread_mutex_unlock(&dataLock);
-    return buff;
+    return retval;
 }
 
+/**
+ * This function returns the current maximum acceleration per joint
+ * for the configured Robot.
+ *
+ * @return Vector containing the maximum acceleration [ degree/ seconds^2 ]
+ */
 Robot::VecJoint SvrData::getMaxAcceleration()
 {
     Robot::VecJoint retval(0.0f);
+
     pthread_mutex_lock(&dataLock);
-    retval = robot.max.accelaration;
+    retval = robot.robotClass->getJointAcceleration();
     pthread_mutex_unlock(&dataLock);
+
     return retval;
 }
 
+/**
+ * This function returns the current maximum velocity per joint
+ * for the configured Robot.
+ *
+ * @return Vector containing the maximum velocity [ degree/ seconds ]
+ */
 Robot::VecJoint SvrData::getMaxVelocity()
 {
     Robot::VecJoint retval(0.0f);
+
     pthread_mutex_lock(&dataLock);
-    retval = robot.max.velocity;
+    retval = robot.robotClass->getJointVelocity();
     pthread_mutex_unlock(&dataLock);
+
     return retval;
 }
+
+/**
+ * This function returns the current maximum range per joint
+ * for the configured Robot.
+ *
+ * @return Vector containing the maximum angle for the joints [ degree ]
+ */
 Robot::VecJoint SvrData::getMaxRange()
 {
     Robot::VecJoint retval(0.0f);
+
     pthread_mutex_lock(&dataLock);
-    retval = robot.max.range;
+    retval = robot.robotClass->getJointRangeMax();
     pthread_mutex_unlock(&dataLock);
+
     return retval;
 }
+
+/**
+ * This function returns the current minimum range per joint
+ * for the configured Robot.
+ *
+ * @return Vector containing the minimum angle for the joints [ degree ]
+ */
+Robot::VecJoint SvrData::getMinRange()
+{
+    Robot::VecJoint retval(0.0f);
+
+    pthread_mutex_lock(&dataLock);
+    retval = robot.robotClass->getJointRangeMin();
+    pthread_mutex_unlock(&dataLock);
+
+    return retval;
+}
+
+/**
+ * This function returns the current minimum range per joint
+ * for the configured Robot.
+ *
+ * @return Vector containing the minimum angle for the joints [ degree ]
+ */
 bool SvrData::checkJointRange(Robot::VecJoint vectorToCheck)
 {
     // save temporal the max range for short time lock
@@ -230,7 +409,7 @@ bool SvrData::checkJointRange(Robot::VecJoint vectorToCheck)
     pthread_mutex_unlock(&dataLock);
     
     // check if the value is in plus or minus range 
-    for (int i = 0; i<Robot::joints; i++)
+    for (int i = 0; i< robot.robotClass->joints; i++)
     {
         if (abs(vectorToCheck(i)) > abs(maxJointRange(i)))
         {
@@ -264,7 +443,7 @@ Robot::VecJoint SvrData::getRobotVelocity()
 
     pthread_mutex_lock(&dataLock);
     percent = robot.currentVelocityPercentage / 100.0f;
-    retval = robot.max.velocity * percent;
+    retval = robot.robotClass->getJointVelocity() * percent;
     pthread_mutex_unlock(&dataLock);
     return retval;
 }
@@ -275,6 +454,7 @@ float SvrData::getRobotVelocityPercentage()
     pthread_mutex_lock(&dataLock);
     retval = robot.currentVelocityPercentage;
     pthread_mutex_unlock(&dataLock);
+
     return retval;
 }
 int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage)
@@ -290,6 +470,7 @@ int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage)
     pthread_mutex_lock(&dataLock);
     robot.currentAccelerationPercentage = newAccelerationPercentage;
     pthread_mutex_unlock(&dataLock);
+
     return retval;
 }
 Robot::VecJoint SvrData::getRobotAcceleration()
@@ -299,8 +480,9 @@ Robot::VecJoint SvrData::getRobotAcceleration()
 
     pthread_mutex_lock(&dataLock);
     percent = robot.currentAccelerationPercentage / 100.0f;
-    retval  = robot.max.accelaration * percent;
+    retval  = robot.robotClass->getJointAcceleration() * percent;
     pthread_mutex_unlock(&dataLock);
+
     return retval;
 }
 float SvrData::getRobotAccelerationPercentage()
@@ -310,6 +492,7 @@ float SvrData::getRobotAccelerationPercentage()
     pthread_mutex_lock(&dataLock);
     retval = robot.currentAccelerationPercentage;
     pthread_mutex_unlock(&dataLock);
+
     return retval;
 }
 unsigned int SvrData::getJoints()
@@ -319,6 +502,7 @@ unsigned int SvrData::getJoints()
     pthread_mutex_lock(&dataLock);
     retval = Robot::joints;
     pthread_mutex_unlock(&dataLock);
+
     return retval;
 }
 

lwrserv/src/SvrHandling.cpp

@@ -21,6 +21,7 @@
  */
 void printUsage(SocketObject& client,  std::string& cmd)
 {
+    // search if the cmd command was specified
     for (unsigned int i = 0 ; i < commandCount; ++i)
     {
         if (cmd == commands[i].longVersion || cmd == commands[i].aberration)
@@ -38,7 +39,7 @@ void printUsage(SocketObject& client,  std::string& cmd)
 
     // no command was specified.
     // print only short versions of the commands
-    client.Send("List of Commands with aberration and long form\n For detailed help try \"help <commandname>\"\n");
+    client.Send("List of Commands with aberration and long form\nFor detailed help try \"help <commandname>\"\n");
     client.Send("short version \t-\t long version\n");
     for (unsigned int i = 0 ; i < commandCount; ++i)
     {
@@ -50,7 +51,10 @@ void printUsage(SocketObject& client,  std::string& cmd)
 
 /**
  * Normal Constructor
- * Initalizes the command array with long/short string version and function pointers for each command.
+ * Initalizes the command array with long/short string version
+ * for the commands.
+ * Also sets function pointers for processing 
+ * and printing the detailed help information for each command
  */
 SvrHandling::SvrHandling()
 {
@@ -148,27 +152,36 @@ void * SvrHandling::threadRobotMovement(void *arg)
     // get current robot position
     int tries = 2;
 
+    // try to connect to the robot 
+    // there is a bug in the FRI so we have to try it multiple times
     for (int i= 0 ; i < tries ; ++i)
     {
+        // update the measured Data
         if (REAL_ROBOT)
-            SvrData::getInstance()->updateMessurement();
-        currentJointPos = SvrData::getInstance()->getMeasuredJointPos();
+            SvrData::getInstance()->updateMeasurement();
 
+        // query the current position of the robot
+        currentJointPos = SvrData::getInstance()->getMeasuredJointPos();
         SvrData::getInstance()->setCommandedJointPos(currentJointPos);
 
+        // move robot to current measured position to stop any kind of movement
         float newJointPosToSend[Robot::joints] = {0.0f};
         currentJointPos.getData(newJointPosToSend);
         if (REAL_ROBOT)
             friInst->doPositionControl(newJointPosToSend);
     }
-    std::cout << "init position is " << currentJointPos << "\n";
 
+    std::cout << "starting position is " << currentJointPos << "\n";
+
+    // start the client command processing loop
     while(true)
     {
+        // update all measurements in each tic
         if (REAL_ROBOT)
-            SvrData::getInstance()->updateMessurement();
+            SvrData::getInstance()->updateMeasurement();
 
         bool positionChanged = false;
+
         // check if we have to cancel current trajectory
         bool cancel = SvrData::getInstance()->getTrajectoryCancel();
         if (cancel)
@@ -184,13 +197,13 @@ void * SvrHandling::threadRobotMovement(void *arg)
             goto end;
         }
 
-        // check for new trajectory
+        // check if we currently have a trajectory
         if (currentTrajectory == NULL)
         {
-            // get next trajectory from svrData 
+            // try to get next trajectory from svrData 
             currentTrajectory = SvrData::getInstance()->popTrajectory();
 
-            // no new trajectory 
+            // check if it was a valid trajectory 
             if(currentTrajectory == NULL)
             {
                 // mark that we reached the end of the trajectory
@@ -199,7 +212,7 @@ void * SvrHandling::threadRobotMovement(void *arg)
             }
             else
             {
-                std::cout <<"new Trajectory with " << currentTrajectory->getSteps()<<"\n";
+                std::cout <<"new Trajectory with " << currentTrajectory->getSteps()<<" steps\n";
             }
         }
 
@@ -210,6 +223,7 @@ void * SvrHandling::threadRobotMovement(void *arg)
             {
                 case MovementCartBased:
                 {
+                    // use new carthesian position coming from the trajectory
                     currentCartPos = currentTrajectory->getNextCartPos();
                     positionChanged = true;
                     currentMovementType = MovementCartBased;
@@ -217,6 +231,7 @@ void * SvrHandling::threadRobotMovement(void *arg)
                 break;
                 case MovementJointBased:
                 {
+                    // use new joint angles coming from the trajectory
                     currentJointPos = currentTrajectory->getNextJointPos();
                     positionChanged = true;
                     currentMovementType = MovementJointBased;
@@ -224,7 +239,7 @@ void * SvrHandling::threadRobotMovement(void *arg)
                 break;
                 default:
                 {
-                    // invalid trajectory skip it
+                    // invalid trajectory delete and skip it
                     delete currentTrajectory;
                     currentTrajectory = NULL;
                     goto end;
@@ -234,43 +249,64 @@ void * SvrHandling::threadRobotMovement(void *arg)
         }
 
 end:
+        // actually do the movement to the new position or to the 
+        // current one if there was no new position.
+        // This has to be send to robot to not decrease quality of
+        // the signal
         switch (currentMovementType)
         {
             case MovementJointBased:
                 {
-                    // send the new joint values
+                    // convert joint values to float array
                     float newJointPosToSend[Robot::joints] = {0.0f};
                     Robot::VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI);
                     newJointPosToSendRad.getData(newJointPosToSend);
+
+                    // send the new joint values to the robot
                     if (REAL_ROBOT)
                         friInst->doPositionControl(newJointPosToSend);
                 }
                 break;
             case MovementCartBased:
                 {
+                    // use default stiffness and damping
                     float Stiff[6] = {1000.0, 1000.0, 1000.0, 10.0, 10.0, 10.0};
                     float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7};
+
+                    // convert cartesian position to an array of floats
                     float newCartPosToSend[12] = {0.0f};
                     currentCartPos.getData(newCartPosToSend);
+
+                    // send the new cartesian values to the robot
                     if (REAL_ROBOT)
                         friInst->doCartesianImpedanceControl(newCartPosToSend,Stiff, Damp, NULL, NULL, true);
                 }
                 break;
         }
 
-        // start timer for the last sent msg
-        // TODO 
+        // start timer for the last sent message
+        // TODO measure the time for updating the Position, so do not update the
+        // positions faster than needed ( FRI Remote does actually not wait for sample time like suggested before)
 
-        // check if current trajectory is over 
+        // check if current trajectory has reached the last step
         if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0)
         {
             std::cout << " Trajectory finished \n ";
 
+            // each trajectory gets a new number starting from 1
             static int trajcetorycount = 0;
             trajcetorycount +=1;
 
-            currentTrajectory->saveJointToFile(std::string("trajectory/a.csv"));
-            // invalid trajectory skip it
+            // assemble the filename
+            std::string filename = "";
+            std::stringstream ss;
+            ss << "trajectory/trajectory_"<< trajcetorycount << ".csv";
+            filename = ss.str();
+
+
+            currentTrajectory->saveJointToFile(filename);
+
+            // free the current trajectory
             delete currentTrajectory;
             currentTrajectory = NULL;
         }
@@ -289,7 +325,7 @@ SvrHandling::~SvrHandling()
 /**
  * Starts the server thread with default port.
  *
- * @info will never return and should run in a thread
+ * @info will never return. So it should run in a separate thread.
  */
 void SvrHandling::run()
 {
@@ -306,10 +342,10 @@ void SvrHandling::run(int port)
 {
     std::cout << timestamp() + "Starting " << SVR_NAME << " on port "<< port <<"\n";
 
-    // only abourt loop if state done is reached
+    // only abort loop if state done is reached
     while (c_state != done)
     {
-        // create new socket and bind it to the speciffic port
+        // create new socket and bind it to the specific port
         SocketObject *socket = new SocketObject;
         if (socket->Bind(port) <0)
         {

lwrserv/test/main.cpp

@@ -1,4 +1,29 @@
 #ifdef UNIT_TEST
+/**
+ * @addtogroup testsuite
+ * @{
+ * @file
+ * @author  Philipp Schoenberger <ph.schoenberger@googlemail.com>
+ * @version 1.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 functions to process the commands
+ * comming from the client interface
+ */
 
 #include "CppUTest/CommandLineTestRunner.h"
 #include "CppUTest/TestHarness.h"
@@ -6,11 +31,18 @@
 #include "CppUTest/TestTestingFixture.h"
 #include "CppUTest/PlatformSpecificFunctions.h"
 
-
-
+/**
+ * main function for the test suite
+ *
+ * only start the cpputest
+ */
 int main(int ac, char** av)
 {
    return CommandLineTestRunner::RunAllTests(ac, av);
 }
 
+/**
+ * @}
+ */
+
 #endif

lwrserv/test/test_mat.cpp

@@ -1,3 +1,33 @@
+/**
+ * @addtogroup testsuite
+ * @{
+ * @addtogroup matrixtests
+ * @{
+ * @author  Philipp Schoenberger <ph.schoenberger@googlemail.com>
+ * @version 1.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 bang bang trajectory.
+ * The bang bang trajectory is a trajectory with linear acceleration
+ * phase followed by a direct de-acceleration phase
+ *
+ * The slowest joint is defining the speed of the other joints.
+ * By that all joints start and stop the movement synchronously
+ */
 #include "CppUTest/TestHarness.h"
 #include "CppUTest/TestRegistry.h"
 #include "CppUTest/TestOutput.h"
@@ -10,10 +40,18 @@ int testMat [TESTSIZE][TESTSIZE];
 int testVec [TESTSIZE];
 float tolerance = 0.00001f;
 
+/**
+ * testgroup for the matrix class
+ */
 TEST_GROUP(Matrix)
 {
+    /**
+     * setup for for all test suite
+     * This Function is called before every Test case
+     */
     void setup()
     {
+        // initializing the testmatrix 
         int i = 1;
         int j = 1;
         for (int x = 0; x <TESTSIZE ; x++)
@@ -26,13 +64,22 @@ TEST_GROUP(Matrix)
         }
     }
 
+    /**
+     * exit for for all test suite
+     * This Function is called after every Test case
+     */
     void teardown()
     {
     }
 };
 
+/**
+ * Test the matrix for invert an non identity matrix 
+ * with a x rotation
+ */
 TEST(Matrix, vectorInvRotX)
 {
+    // setup the test matrix to an identity matrix
     Mat<float, TESTSIZE> a;
     for(int x = 0 ; x < TESTSIZE ; ++x)
     {
@@ -45,15 +92,16 @@ TEST(Matrix, vectorInvRotX)
         }
     }
 
+    // set the rotation matrix to 45 degree in z
     a(0,0) = 1.0f;
     a(1,1) = cos(45.0f *M_PI/180.0f);
     a(1,2) = -sin(45.0f *M_PI/180.0f);
     a(2,2) = cos(45.0f *M_PI/180.0f);
     a(2,1) = sin(45.0f *M_PI/180.0f);
 
-
-
     float deg = 45.0f*M_PI/180.0f;
+
+    // check the matrix after setting it up
     DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
     DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
     DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
@@ -74,8 +122,10 @@ TEST(Matrix, vectorInvRotX)
     DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
     DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
 
+    // invert the matrix 
     a = a.inv();
 
+    // check for the negative angle for a z rotation
     deg = -45.0f*M_PI/180.0f;
     DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
     DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
@@ -98,8 +148,13 @@ TEST(Matrix, vectorInvRotX)
     DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
 }
 
+/**
+ * Test the matrix for invert an non identity matrix
+ * with a y rotation
+ */
 TEST(Matrix, vectorInvRotY)
 {
+    // setup the test matrix to an identity matrix
     Mat<float, TESTSIZE> a(0.0f);
     
     float deg = 45.0f*M_PI/180.0f;
@@ -136,12 +191,17 @@ TEST(Matrix, vectorInvRotY)
     DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
 }
 
+/**
+ * Test the matrix for invert an identity matrix
+ */
 TEST(Matrix, matrixInvEye)
 {
     Mat<float, TESTSIZE> a(0.0f , 1.0f);
     
     // do the invert
     a = a.inv();
+
+    // check if the matrix is still an identity matrix
     DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
     DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
     DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
@@ -162,6 +222,11 @@ TEST(Matrix, matrixInvEye)
     DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
     DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
 }
+
+
+/**
+ * Test if the matrix constructor for identity matrix is working
+ */
 TEST(Matrix, matrixEye)
 {
     Mat<float, TESTSIZE> a(0.0f , 1.0f);
@@ -186,6 +251,11 @@ TEST(Matrix, matrixEye)
     DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
     DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
 }
+
+/**
+ * Test if the determinant function is working with an identity matrix
+ * This should always return an 1
+ */
 TEST(Matrix, matrixDeterminantEye)
 {
     //eye matrix
@@ -193,6 +263,12 @@ TEST(Matrix, matrixDeterminantEye)
 
     DOUBLES_EQUAL(1.0f,a.determinant(),tolerance);
 }
+
+/**
+ * Test if the determinant function is working with an identity matrix
+ * with only 3 dimensions.
+ * This should always return an 1
+ */
 TEST(Matrix, matrixDeterminantEye3)
 {
     //eye matrix
@@ -200,6 +276,11 @@ TEST(Matrix, matrixDeterminantEye3)
 
     DOUBLES_EQUAL(1.0f,a.determinant(),tolerance);
 }
+
+/**
+ * Test if the determinant function is working with an identity matrix
+ * with 1 dimension should always return the first cell
+ */
 TEST(Matrix, matrixDeterminantSimple)
 {
     Mat<float, 1> a;
@@ -207,6 +288,11 @@ TEST(Matrix, matrixDeterminantSimple)
 
     DOUBLES_EQUAL(50.0f,a.determinant(), tolerance);
 }
+
+/**
+ * Test if the determinant function is working with an identity matrix
+ * with 2 dimension should always return the multiplication of the diagonal
+ */
 TEST(Matrix, matrixDeterminantSimple2)
 {
     Mat<float, 2> a;
@@ -215,6 +301,11 @@ TEST(Matrix, matrixDeterminantSimple2)
     DOUBLES_EQUAL(50.0f,a.determinant(), tolerance);
 }
 
+/**
+ * Test if the determinant function is working with an complete non zero matrix
+ * with 2 dimension should always return the multiplication of the diagonal
+ * and negative counter diagonal multiplication
+ */
 TEST(Matrix, matrixDeterminantSimple3)
 {
     Mat<float, 2> a;
@@ -225,6 +316,11 @@ TEST(Matrix, matrixDeterminantSimple3)
     DOUBLES_EQUAL(0.0f,a.determinant(), tolerance);
 }
 
+/**
+ * Test if the determinant function is working with an complete non zero matrix
+ * with 2 dimension should always return the multiplication of the diagonal
+ * and negative counter diagonal multiplication
+ */
 TEST(Matrix, matrixDeterminantSimple4)
 {
     Mat<float, 2> a;
@@ -235,20 +331,16 @@ TEST(Matrix, matrixDeterminantSimple4)
     DOUBLES_EQUAL(75.0f,a.determinant(), tolerance);
 }
 
+/**
+ * Test if the transpose function is working with an identity matrix 
+ * with 4 dimension should always be the same again 
+ */
 TEST(Matrix, matrixTransposeEye)
 {
-    Mat<float, TESTSIZE> a;
-    for(int x = 0 ; x < TESTSIZE ; ++x)
-    {
-        for (int y = 0; y <TESTSIZE ; ++y)
-        {
-            float val = 0.0f;
-            if (x == y )
-                val = 1.0f;
-            a(x,y) = val;
-        }
-    }
+    //eye matrix
+    Mat<float, TESTSIZE> a(0.0f,1.0f);
     
+    // check if the identity matrix is correctly created
     DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
     DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
     DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
@@ -269,9 +361,10 @@ TEST(Matrix, matrixTransposeEye)
     DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
     DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
 
+    // invert the identity matrix
     a = a.inv();
 
-    // should stille be the same
+    // should still be the same
     DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
     DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
     DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
@@ -292,12 +385,17 @@ TEST(Matrix, matrixTransposeEye)
     DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
     DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
 }
+
+/**
+ * Test if the transpose function is working with an identity matrix 
+ * with 4 dimension should always be the same again 
+ */
 TEST(Matrix, vectorMultiply)
 {
     Mat<int, TESTSIZE> a = testMat;
     Vec<int, TESTSIZE> v = testVec;
 
-    //Vec<int, TESTSIZE> b = a * v;
+    Vec<int, TESTSIZE> b = a * v;
     for (int x = 0; x <TESTSIZE ; x++)
     {
         int val = 0;
@@ -305,7 +403,7 @@ TEST(Matrix, vectorMultiply)
         {
             val += testMat[x][y] * testVec[x];
         }
-        //CHECK_EQUAL(val,b(x));
+        CHECK_EQUAL(val,b(x));
     }
 }
 

lwrserv/trajectory/start.m

@@ -1,7 +1,7 @@
 clear all
 close all
 
-filename = 'a.csv';
+filename = 'trajectory_1.csv';
 M = csvread(filename);
 
 sampletime = 0.005