diff --git a/lwrserv/.ycm_extra_conf.py b/lwrserv/.ycm_extra_conf.py
new file mode 100644
index 0000000..ee91494
--- /dev/null
+++ b/lwrserv/.ycm_extra_conf.py
@@ -0,0 +1,101 @@
+# Partially stolen from https://bitbucket.org/mblum/libgp/src/2537ea7329ef/.ycm_extra_conf.py
+import os
+import ycm_core
+
+# These are the compilation flags that will be used in case there's no
+# compilation database set (by default, one is not set).
+# CHANGE THIS LIST OF FLAGS. YES, THIS IS THE DROID YOU HAVE BEEN LOOKING FOR.
+flags = [
+    '-Wall',
+    '-Wextra',
+    '-Werror',
+    '-Wc++98-compat',
+    '-Wno-long-long',
+    '-Wno-variadic-macros',
+    '-fexceptions',
+    # THIS IS IMPORTANT! Without a "-std=<something>" flag, clang won't know which
+    # language to use when compiling headers. So it will guess. Badly. So C++
+    # headers will be compiled as C headers. You don't want that so ALWAYS specify
+    # a "-std=<something>".
+    # For a C project, you would set this to something like 'c99' instead of
+    # 'c++11'.
+    '-std=c++11',
+    # ...and the same thing goes for the magic -x option which specifies the
+    # language that the files to be compiled are written in. This is mostly
+    # relevant for c++ headers.
+    # For a C project, you would set this to 'c' instead of 'c++'.
+    '-x', 'c++',
+    # This path will only work on OS X, but extra paths that don't exist are not
+    # harmful
+    '-isystem', '/System/Library/Frameworks/Python.framework/Headers',
+    '-isystem', '/usr/include',
+    '-isystem', '/usr/share/qt4/include',
+    '-isystem', '/usr/local/include',
+    '-I', 'include',
+    '-I', './include',
+    '-I', '.'
+]
+
+# Set this to the absolute path to the folder (NOT the file!) containing the
+# compile_commands.json file to use that instead of 'flags'. See here for
+# more details: http://clang.llvm.org/docs/JSONCompilationDatabase.html
+#
+# Most projects will NOT need to set this to anything; you can just change the
+# 'flags' list of compilation flags. Notice that YCM itself uses that approach.
+compilation_database_folder = ''
+
+if compilation_database_folder:
+  database = ycm_core.CompilationDatabase( compilation_database_folder )
+else:
+  database = None
+
+
+def DirectoryOfThisScript():
+  return os.path.dirname( os.path.abspath( __file__ ) )
+
+
+def MakeRelativePathsInFlagsAbsolute( flags, working_directory ):
+  if not working_directory:
+    return list( flags )
+  new_flags = []
+  make_next_absolute = False
+  path_flags = [ '-isystem', '-I', '-iquote', '--sysroot=' ]
+  for flag in flags:
+    new_flag = flag
+
+    if make_next_absolute:
+      make_next_absolute = False
+      if not flag.startswith( '/' ):
+        new_flag = os.path.join( working_directory, flag )
+
+    for path_flag in path_flags:
+      if flag == path_flag:
+        make_next_absolute = True
+        break
+
+      if flag.startswith( path_flag ):
+        path = flag[ len( path_flag ): ]
+        new_flag = path_flag + os.path.join( working_directory, path )
+        break
+
+    if new_flag:
+      new_flags.append( new_flag )
+  return new_flags
+
+
+def FlagsForFile( filename ):
+  if database:
+    # Bear in mind that compilation_info.compiler_flags_ does NOT return a
+    # python list, but a "list-like" StringVec object
+    compilation_info = database.GetCompilationInfoForFile( filename )
+    final_flags = MakeRelativePathsInFlagsAbsolute(
+      compilation_info.compiler_flags_,
+      compilation_info.compiler_working_dir_ )
+  else:
+    relative_to = DirectoryOfThisScript()
+    final_flags = MakeRelativePathsInFlagsAbsolute( flags, relative_to )
+
+  return {
+    'flags': final_flags,
+    'do_cache': True
+  }
diff --git a/lwrserv/BangBangTrajectroy.cpp b/lwrserv/BangBangTrajectroy.cpp
index 8bf1e05..328b616 100644
--- a/lwrserv/BangBangTrajectroy.cpp
+++ b/lwrserv/BangBangTrajectroy.cpp
@@ -5,14 +5,15 @@
 #include "vec.h"
 #include "SvrData.h"
 
-template <class T, unsigned SIZE> 
-BangBangJointTrajectroy::BangBangJointTrajectroy(
+template <unsigned SIZE> 
+BangBangJointTrajectroy<SIZE>::BangBangJointTrajectroy(
         unsigned int steps_,
         float totalTime_,
         Vec<float,SIZE> jointMovement
         )
 {
-    Vec< float, SIZE> velocity = jointMovement / steps * 1.5f;
+    Vec< float, SIZE> velocity = jointMovement / steps_ * 1.5f;
+    //TODO
     LinearJointTrajectory(
             steps_,
             totalTime_,
diff --git a/lwrserv/LinearTrajectory.cpp b/lwrserv/LinearTrajectory.cpp
index 529153d..e91bd79 100644
--- a/lwrserv/LinearTrajectory.cpp
+++ b/lwrserv/LinearTrajectory.cpp
@@ -2,23 +2,23 @@
 #include "vec.h"
 #include "stdlib.h"
 
-template <class T, unsigned SIZE> 
-LinearJointTrajectory::LinearJointTrajectory(
+template <unsigned SIZE> 
+LinearJointTrajectory<SIZE>::LinearJointTrajectory(
         unsigned int steps_,
         float totalTime_,
-        Vec<float,SIZE> jointMovement
+        Vec<float,SIZE> jointMovement_
         )
 {
-    Vec< float, SIZE> velocity = jointMovement / steps * 1.5f;
+    Vec< float, SIZE> velocity = jointMovement_ / steps_ * 1.5f;
     LinearJointTrajectory(
             steps_,
             totalTime_,
-            jointMovement,
+            jointMovement_,
             velocity);
 }
 
-template <class T, unsigned SIZE> 
-LinearJointTrajectory::LinearJointTrajectory(
+template <unsigned SIZE> 
+LinearJointTrajectory<SIZE>::LinearJointTrajectory(
         unsigned int steps_,
         float totalTime_,
         Vec<float,SIZE> jointMovement,
@@ -27,48 +27,48 @@ LinearJointTrajectory::LinearJointTrajectory(
 {
     super(steps_, totalTime_, jointMovement);
 
-    if ( velocity > jointMovement/totalTime)
+    if ( velocity > jointMovement/totalTime_)
     {
         std::cerr << "velocity is to small for Trajectory\n";
     }
 
-    if ( velocity < (2.0f*jointMovement)/totalTime)
+    if ( velocity < (2.0f*jointMovement)/totalTime_)
     {
         std::cerr << "velocity is to big for Trajectory\n";
     }
 
-    for (unsigned int currJoint = 0 ; currJoint < joints ; currJoint++)
+    for (unsigned int currJoint = 0 ; currJoint < SIZE; currJoint++)
     {
-        unsigned int timeBlend = (jointMovement(currJoint) + velocity(currJoint)*totalTime)/ velocity(currJoint);
+        unsigned int timeBlend = (jointMovement(currJoint) + velocity(currJoint)*totalTime_)/ velocity(currJoint);
         float acceleration =  velocity(currJoint) / timeBlend;
 
-        for (unsigned int currStep = 0 ; currStep < steps ; ++currStep)
+        for (unsigned int currStep = 0 ; currStep < steps_ ; ++currStep)
         {
-            float currentTime = currStep * totalTime / steps;
+            float currentTime = currStep * totalTime_ / steps_;
             if (currentTime <= timeBlend)
             {
                 // speed up till blend time is reached 
-                nodes[currStep].jointPos     = acceleration/2.0f * currentTime * currentTime;
-                nodes[currStep].velocity     = acceleration * currentTime;
-                nodes[currStep].acceleration = acceleration;
+                this->nodes[currStep].jointPos     = acceleration/2.0f * currentTime * currentTime;
+                this->nodes[currStep].velocity     = acceleration * currentTime;
+                this->nodes[currStep].acceleration = acceleration;
             } else
-            if ( currentTime <= totalTime - timeBlend)
+            if ( currentTime <= totalTime_ - timeBlend)
             {
                 // constant velocity 
-                nodes[currStep].jointPos     = (jointMovement(currJoint) - velocity(currJoint) * totalTime)/2.0f + velocity(currJoint) * currentTime;
-                nodes[currStep].velocity     = velocity(currJoint);
-                nodes[currStep].acceleration = 0.0f;
+                this->nodes[currStep].jointPos     = (jointMovement(currJoint) - velocity(currJoint) * totalTime_)/2.0f + velocity(currJoint) * currentTime;
+                this->nodes[currStep].velocity     = velocity(currJoint);
+                this->nodes[currStep].acceleration = 0.0f;
             }
             else
             {
-                // slow down untill aim is reached 
+                // slow down until aim is reached 
                 // speed up till blend time is reached 
-                nodes[currStep].jointPos     = jointMovement(currJoint) - acceleration/2*totalTime*totalTime + acceleration * totalTime * currentTime - acceleration/2.0f * currentTime *currentTime;
-                nodes[currStep].velocity     = acceleration*timeBlend - acceleration * currentTime ;
-                nodes[currStep].acceleration = -acceleration;
+                this->nodes[currStep].jointPos     = jointMovement(currJoint) - acceleration/2*totalTime_*totalTime_ + acceleration * totalTime_ * currentTime - acceleration/2.0f * currentTime *currentTime;
+                this->nodes[currStep].velocity     = acceleration*timeBlend - acceleration * currentTime ;
+                this->nodes[currStep].acceleration = -acceleration;
             }
-            // calculate  carthesian positions
-            nodes[currStep].cartPos = 0;
+            // calculate Cartesian positions
+            this->nodes[currStep].cartPos = 0;
         }
     }
 }
diff --git a/lwrserv/Makefile b/lwrserv/Makefile
index 58ce0f4..7d91ca1 100644
--- a/lwrserv/Makefile
+++ b/lwrserv/Makefile
@@ -6,14 +6,14 @@
 #           autor Philipp Schoenberger                                         #
 #           ph.schoenberger@googlemail.com                                     #
 ################################################################################
-LIBS = pthread
+LIBS = pthread boost_thread boost_system
 TOP_DIR   = $(PWD)
 TEST_DIR = $(BUILD_DIR)/test
 INCLUDE_DIR = -I . \
               -I ./include 
 
 # comment this out if you want debug compile info
-Q=@
+#Q=@
 
 ifeq ($(MAKECMDGOALS),test)
    GCOV := --coverage
@@ -117,6 +117,7 @@ $(BUILD_DIR)/%.d: $(BUILD_DIR) $(TEST_DIR) %.cpp
 	$(Q)echo " [DEP] $@"
 	$(Q)touch $@
 	$(Q)$(CC) -M $(filter-out -g -o0 -o1 -o2 ,$(CPPFLAGS)) $(filter %.cpp,$^) >> $@
-	#$(Q)echo -n "$@ $(patsubst %.d,%.o,$@): " >$@
+
+#$(Q)echo -n "$@ $(patsubst %.d,%.o,$@): " >$@
 
 -include $(DEPS)
diff --git a/lwrserv/SvrData.cpp b/lwrserv/SvrData.cpp
index aa45ea9..c185d96 100644
--- a/lwrserv/SvrData.cpp
+++ b/lwrserv/SvrData.cpp
@@ -2,7 +2,11 @@
 #include <errno.h>
 #include <stdio.h>
 #include <string.h>
+#include "Trajectroy.h"
 #include "SvrData.h"
+#include "friComm.h"
+#include "friremote.h"
+#include <boost/thread/thread.hpp>
 
 bool SvrData::instanceFlag = false;
 SvrData* SvrData::single = 0;
@@ -18,19 +22,23 @@ SvrData::SvrData(void)
 
     robot.currentVelocity = 20;
     robot.currentAcceleration = 10;
-    robot.joints = LBR_MNJ;
 
-    for (unsigned int i = 0; i < robot.joints; ++i)
+    for (unsigned int i = 0; i < JOINT_NUMBER; ++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];
+        robot.max.velocity(i) = maxVelJnt[i];
+        robot.max.accelaration(i) = maxAccJnt[i];
+        robot.max.torque(i) = maxTrqJnt[i];
+        robot.max.range(i) = maxRangeJnt[i];
     }
 
+    trajectory.sampleTimeMs = 5.0f;
+    trajectory.cancel = false;
+
     // init mutex for database
     if (pthread_mutex_init(&dataLock, NULL) != 0)
         printf("mutex init failed\n");
+
+    this->friInst = new friRemote;
 }
 
 SvrData::~SvrData()
@@ -48,57 +56,83 @@ SvrData* SvrData::getInstance ()
     return single;
 }
 
-
-void SvrData::lock()
+friRemote* SvrData::getFriRemote()
 {
+    friRemote* retval = NULL;
+
     pthread_mutex_lock(&dataLock);
+    retval = this->friInst;
+    pthread_mutex_unlock(&dataLock);
+    return retval;
 }
 
-void SvrData::unlock()
+void SvrData::updateMessurementData(
+        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);
 }
 
-int SvrData::getMessuredJointPos(float* data, size_t size)
+void SvrData::updateMessurement()
 {
-    if (data == NULL)
-        return -EINVAL;
-    if (size != sizeof(messured.jointPos))
-        return -EINVAL;
+    VecJoint newJointPos(0.0f);
+    MatCarthesian newCartPos(0.0f,1.0f);
+    VecTorque newForceAndTorque(0.0f);
+
+    /// do the update using fri
+    /// update current joint positions
+    VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition());
+    VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset());
+    newJointPos = newJointPosComanded + newJointPosOffset;
+
+    /// update current cart position
+    newCartPos = friInst->getMsrCartPosition();
+    
+    /// update current force and torque
+    newForceAndTorque = friInst->getFTTcp();
+
+    updateMessurementData(newJointPos,newCartPos,newForceAndTorque);
+}
 
+
+void SvrData::lock()
+{
     pthread_mutex_lock(&dataLock);
-    memcpy(data,messured.jointPos,size);
+}
+
+void SvrData::unlock()
+{
     pthread_mutex_unlock(&dataLock);
-    return 0;
 }
 
-Vec<float,LBR_MNJ> SvrData::getMessuredJointPos()
+VecJoint SvrData::getMessuredJointPos()
 {
-    Vec<float,LBR_MNJ> buf ;
+    VecJoint buf;
     pthread_mutex_lock(&dataLock);
     buf = messured.jointPos;
     pthread_mutex_unlock(&dataLock);
     return buf;
 }
 
-int SvrData::getMessuredCartPos (float* data, size_t size)
+MatCarthesian SvrData::getMessuredCartPos()
 {
-    if (data == NULL)
-        return -EINVAL;
-    if (size != sizeof(messured.cartPos))
-        return -EINVAL;
-
+    MatCarthesian buf;
     pthread_mutex_lock(&dataLock);
-    memcpy(data,messured.cartPos,size);
+    buf = messured.cartPos;
     pthread_mutex_unlock(&dataLock);
-    return 0 ;
+    return buf;
 }
-
-Mat<float,LBR_MNJ> SvrData::getMessuredCartPos()
+VecTorque SvrData::getMessuredForceTorque()
 {
-    Mat<float,LBR_MNJ> buf ;
+    VecTorque buf;
     pthread_mutex_lock(&dataLock);
-    buf = messured.cartPos;
+    buf = messured.forceAndTorque;
     pthread_mutex_unlock(&dataLock);
     return buf;
 }
@@ -116,226 +150,225 @@ int SvrData::getMessuredJacobian(float* data, size_t size)
     return 0;
 }
 
-int SvrData::getMessuredForceTorque(float* data, size_t size)
+VecJoint SvrData::getCommandedJointPos()
 {
-    if (data == NULL)
-        return -EINVAL;
-    if (size != sizeof(messured.forceAndTorque))
-        return -EINVAL;
-
+    VecJoint buff; 
     pthread_mutex_lock(&dataLock);
-    memcpy(data,messured.forceAndTorque,size);
+    buff = commanded.jointPos;
     pthread_mutex_unlock(&dataLock);
-    return 0;
+    return buff;
 }
-
-int SvrData::getCommandedJointPos(float* data, size_t size)
+void SvrData::setCommandedJointPos(VecJoint newJointPos)
 {
-    if (data == NULL)
-        return -EINVAL;
-    if (size != sizeof(commanded.jointPos))
-        return -EINVAL;
-
     pthread_mutex_lock(&dataLock);
-    memcpy(data,commanded.jointPos,size);
+    commanded.jointPos = newJointPos;
     pthread_mutex_unlock(&dataLock);
-    
-    return 0;
 }
-Vec<float, LBR_MNJ> SvrData::getCommandedJointPos()
+MatCarthesian SvrData::getCommandedCartPos ()
 {
-    Vec<float, LBR_MNJ> buff;
+    MatCarthesian buff; 
     pthread_mutex_lock(&dataLock);
-    buff = commanded.jointPos;
+    buff = commanded.cartPos;
     pthread_mutex_unlock(&dataLock);
     return buff;
 }
-int SvrData::getCommandedCartPos (float* data, size_t size)
+void SvrData::setCommandedCartPos(MatCarthesian newCartPos)
 {
-    if (data == NULL)
-        return -EINVAL;
-    if (size != sizeof(commanded.cartPos))
-        return -EINVAL;
-
     pthread_mutex_lock(&dataLock);
-    memcpy(data,commanded.cartPos,size);
+    commanded.cartPos = newCartPos;
     pthread_mutex_unlock(&dataLock);
-
-    return 0;
 }
-Mat<float, LBR_MNJ> SvrData::getCommandedCartPos ()
+
+
+VecJoint SvrData::getMaxTorque()
 {
-    Mat<float, LBR_MNJ> buff;
+    VecJoint buff;
     pthread_mutex_lock(&dataLock);
-    buff = commanded.cartPos;
+    buff = robot.max.torque;
     pthread_mutex_unlock(&dataLock);
     return buff;
 }
 
-float SvrData::getMaxTorque(unsigned int pos)
+VecJoint SvrData::getMaxAcceleration()
 {
-    float retval = -1.0f;
-    if (pos >= robot.joints)
-        return retval;
-
+    VecJoint buff;
     pthread_mutex_lock(&dataLock);
-    retval = robot.max.torque[pos-1];
+    buff = robot.max.accelaration;
     pthread_mutex_unlock(&dataLock);
-    return retval;
+    return buff;
 }
 
-int   SvrData::getMaxTorque(float* data, size_t size)
+VecJoint SvrData::getMaxVelocity()
 {
-    if (data == NULL)
-        return -EINVAL;
-    if (size != sizeof(float)* robot.joints)
-        return -EINVAL;
+    VecJoint buff;
 
     pthread_mutex_lock(&dataLock);
-    memcpy(data,robot.max.torque,size);
+    buff = robot.max.velocity;
     pthread_mutex_unlock(&dataLock);
-    return 0;
+    return buff;
 }
-
-float SvrData::getMaxAcceleration(unsigned int pos)
+VecJoint SvrData::getMaxRange()
 {
-    float retval = -1.0f;
-    if (pos >= robot.joints)
-        return retval;
+    VecJoint buff;
 
     pthread_mutex_lock(&dataLock);
-    retval = robot.max.accelaration[pos-1];
+    buff = robot.max.range;
     pthread_mutex_unlock(&dataLock);
-    return retval;
+    return buff;
 }
-int   SvrData::getMaxAcceleration(float* data, size_t size)
+bool SvrData::checkJointRange(VecJoint vectorToCheck)
 {
-    if (data == NULL)
-        return -EINVAL;
-    if (size != sizeof(float)* robot.joints)
-        return -EINVAL;
+    bool retval = false;
 
     pthread_mutex_lock(&dataLock);
-    memcpy(data,robot.max.accelaration,size);
+    Vec<float,LBR_MNJ> maxJointRange;
+    maxJointRange = robot.max.range;
     pthread_mutex_unlock(&dataLock);
-    return 0;
+    
+    for (int i = 0; i<JOINT_NUMBER; i++)
+    {
+        if (abs(vectorToCheck(i)) > abs(maxJointRange(i)))
+        {
+            // join angle is too big
+            return false;
+        }
+    }
+    // no join angle is too big 
+    return false;
 }
-Vec<float,LBR_MNJ>   SvrData::getMaxAcceleration()
-{   
-    Vec<float,LBR_MNJ> buff;
+
+int SvrData::setRobotVelocity(float newVelocity)
+{
+    unsigned int retval = 0;
+
     pthread_mutex_lock(&dataLock);
-    buff = robot.max.accelaration;
+    robot.currentVelocity = newVelocity;
     pthread_mutex_unlock(&dataLock);
-    return buff;
+    return retval;
 }
-float SvrData::getMaxVelocity(unsigned int pos)
+float SvrData::getRobotVelocity()
 {
-    float retval = -1.0f;
-    if (pos >= robot.joints)
-        return retval;
+    float retval = 0;
 
     pthread_mutex_lock(&dataLock);
-    retval = robot.max.velocity[pos-1];
+    retval = robot.currentVelocity;
     pthread_mutex_unlock(&dataLock);
     return retval;
 }
-int   SvrData::getMaxVelocity(float* data, size_t size)
+int SvrData::setRobotAcceleration(float newAcceleration)
 {
-    if (data == NULL)
-        return -EINVAL;
-    if (size != sizeof(float)* robot.joints)
-        return -EINVAL;
+    unsigned int retval = 0;
 
     pthread_mutex_lock(&dataLock);
-    memcpy(data,robot.max.velocity,size);
+    robot.currentAcceleration = newAcceleration;
     pthread_mutex_unlock(&dataLock);
-    return 0;
+    return retval;
 }
-Vec<float,LBR_MNJ>   SvrData::getMaxVelocity()
-{   
-    Vec<float,LBR_MNJ> buff;
+float SvrData::getRobotAcceleration()
+{
+    float retval = 0;
+
     pthread_mutex_lock(&dataLock);
-    buff = robot.max.velocity;
+    retval = robot.currentAcceleration;
     pthread_mutex_unlock(&dataLock);
-    return buff;
+    return retval;
 }
-
-float SvrData::getMaxRange(unsigned int pos)
+unsigned int SvrData::getJoints()
 {
-    float retval = -1.0f;
-    if (pos >= robot.joints)
-        return retval;
+    unsigned int retval = 0;
 
     pthread_mutex_lock(&dataLock);
-    retval = robot.max.range[pos-1];
+    retval = JOINT_NUMBER;
     pthread_mutex_unlock(&dataLock);
     return retval;
 }
 
-int   SvrData::getMaxRange(float* data, size_t size)
+/// Trajectory function
+float SvrData::getSampleTimeMs()
 {
-    if (data == NULL)
-        return -EINVAL;
-    if (size != sizeof(float)* robot.joints)
-        return -EINVAL;
+    float retval = 0;
 
     pthread_mutex_lock(&dataLock);
-    memcpy(data,robot.max.range,size);
+    retval = trajectory.sampleTimeMs;
     pthread_mutex_unlock(&dataLock);
-    return 0;
+    return retval;
 }
-Vec<float, LBR_MNJ> SvrData::getMaxRange()
+int   SvrData::setSampleTimeMs(float newSampleTimeMs)
 {
-    Vec<float, LBR_MNJ> retval ;
+    int retval = 0;
+    
+    // sample time can not be negative or 0
+    if (newSampleTimeMs <= 0.0f)
+        return -1;
+
     pthread_mutex_lock(&dataLock);
-    retval = robot.max.range;
+    trajectory.sampleTimeMs = newSampleTimeMs;
     pthread_mutex_unlock(&dataLock);
     return retval;
 }
 
-int SvrData::setRobotVelocity(float newVelocity)
+class Trajectory<JOINT_NUMBER>*  SvrData::popTrajectory()
 {
-    unsigned int retval = 0;
-
+    class Trajectory<JOINT_NUMBER>* retval = NULL;
     pthread_mutex_lock(&dataLock);
-    robot.currentVelocity = newVelocity;
+    retval = trajectory.list.front();
     pthread_mutex_unlock(&dataLock);
     return retval;
 }
-float SvrData::getRobotVelocity()
+int SvrData::pushTrajectory(class Trajectory<JOINT_NUMBER>* newTrajectory)
 {
-    float retval = 0;
+    if (newTrajectory == NULL)
+        return -1;
 
     pthread_mutex_lock(&dataLock);
-    retval = robot.currentVelocity;
+    trajectory.list.push(newTrajectory);
     pthread_mutex_unlock(&dataLock);
-    return retval;
+    return 0;
 }
-int SvrData::setRobotAcceleration(float newAcceleration)
+bool SvrData::getTrajectoryCancel()
 {
-    unsigned int retval = 0;
-
+    bool retval = false;
     pthread_mutex_lock(&dataLock);
-    robot.currentAcceleration = newAcceleration;
+    retval = trajectory.cancel;
     pthread_mutex_unlock(&dataLock);
     return retval;
 }
-float SvrData::getRobotAcceleration()
-{
-    float retval = 0;
 
+bool SvrData::cancelCurrentTrajectory()
+{
     pthread_mutex_lock(&dataLock);
-    retval = robot.currentAcceleration;
+    // set cancel state 
+    trajectory.cancel = true;
+    
+    // clear trajectory list
+    while(!trajectory.list.empty())
+    {
+        class Trajectory<JOINT_NUMBER>* currentTrajectory = trajectory.list.front();
+        if(currentTrajectory != NULL)
+            delete currentTrajectory;
+        trajectory.list.pop();
+    }
+
     pthread_mutex_unlock(&dataLock);
-    return retval;
+
+    // wait for stop signaled by calling cancel done
+    bool run = true;
+    while (run)
+    {
+        pthread_mutex_lock(&dataLock);
+        if (trajectory.cancel == false)
+        {
+            run = false;
+        }
+        pthread_mutex_unlock(&dataLock);
+        boost::this_thread::sleep( boost::posix_time::milliseconds(trajectory.sampleTimeMs) );
+    }
+    return true;
 }
-unsigned int SvrData::getJoints()
+void SvrData::trajectoryCancelDone()
 {
-    unsigned int retval = 0;
-
     pthread_mutex_lock(&dataLock);
-    retval = robot.joints;
+    trajectory.cancel = false;
     pthread_mutex_unlock(&dataLock);
-    return retval;
 }
 
diff --git a/lwrserv/SvrHandling.cpp b/lwrserv/SvrHandling.cpp
index dfa4ad2..44d77d1 100755
--- a/lwrserv/SvrHandling.cpp
+++ b/lwrserv/SvrHandling.cpp
@@ -9,6 +9,13 @@
 
 #include "SvrHandling.h"
 #include "SvrData.h"
+#include "Trajectroy.h"
+
+void printUsage(SocketObject& client)
+{
+    client.Send(SVR_HELP_MSG);
+
+}
 
 void mattoabc(float M[3][4], float &a, float &b, float &c)
 {
@@ -265,21 +272,6 @@ double* kukaBackwardCalc(double M_[12], float arg[3])
     return Joints;
 }
 
-bool checkJntRange(double Joints[7])
-{
-    Vec<float,LBR_MNJ> maxJointRange = SvrData::getInstance()->getMaxRange();
-    for (int i = 0; i<LBR_MNJ; i++)
-    {
-        if (abs(Joints[i]) > abs(maxJointRange(i)))
-        {
-            // join angle is too big
-            return false;
-        }
-    }
-    // no join angle is too big 
-    return false;
-}
-
 Mat4f vecToMat(float vec[12])
 {
     Mat4f result;
@@ -445,6 +437,9 @@ void SvrHandling::handle(SocketObject& client)
             } else if (cmd == "") 
             {
                 continue;
+            } else if (cmd == CMD_HELP)
+            {
+                printUsage(client);
             } else if (cmd == CMD_QUIT || cmd == CMD_QUIT1 || cmd == CMD_QUIT2)
             {
                 quit(client);
@@ -485,14 +480,13 @@ void SvrHandling::GetPositionJoints(SocketObject& client)
     string out = "";
     stringstream ss (stringstream::in | stringstream::out);
 
-    float pos[LBR_MNJ];
-    if( 0 != SvrData::getInstance()->getMessuredJointPos(pos,sizeof(pos)))
-        client.Send("ERROR could not get joint pos");
+    VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos();
 
-    for (int i=0; i< LBR_MNJ; i++)
+    // disassemble command
+    for (int i=0; i< JOINT_NUMBER; i++)
     {
         ss.str("");
-        ss << (pos[i]*180/M_PI);
+        ss << (jointPos(i)*180/M_PI);
         out += ss.str() + " ";
     }
     client.Send(out);
@@ -503,14 +497,15 @@ void SvrHandling::GetPositionHomRowWise(SocketObject& client)
     string out = "";
     stringstream ss (stringstream::in | stringstream::out);
 
-    float pos[FRI_CART_FRM_DIM];
-    if( 0 != SvrData::getInstance()->getMessuredCartPos(pos,sizeof(pos)))
-        client.Send("ERROR could not get cart pos");
+    MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos();
 
     for (int i=0; i< FRI_CART_FRM_DIM; i++)
     {
+        int row = i / 4;
+        int column = i % 4;
+
         ss.str("");
-        ss << (pos[i]);
+        ss << (cartPos(row,column));
         out += ss.str() + " ";
     }
     client.Send(out);
@@ -521,13 +516,11 @@ void SvrHandling::GetForceTorqueTcp(SocketObject& client)
     string out = "";
     stringstream ss (stringstream::in | stringstream::out);
 
-    float torque[FRI_CART_VEC];
-    if( 0 != SvrData::getInstance()->getMessuredForceTorque(torque,sizeof(torque)))
-        client.Send("ERROR could not get force and torque");
+    VecTorque torque = SvrData::getInstance()->getMessuredForceTorque();
     for (int i=0; i< FRI_CART_VEC; i++)
     {
         ss.str("");
-        ss << torque[i];
+        ss << torque(i);
         out += ss.str() + " ";
     }
     client.Send(out);
@@ -535,44 +528,65 @@ void SvrHandling::GetForceTorqueTcp(SocketObject& client)
 
 void SvrHandling::MovePTPJoints(SocketObject& client, string& args)
 {
-    __SVR_CURRENT_JOB = true;
+    //__SVR_CURRENT_JOB = true;
     string buf;
-    float temp[LBR_MNJ];
+
     stringstream ss(args);
     stringstream ss2f;
-    vector<string> tokens;
+
+    // convert string to joint vector
+    VecJoint newJointPos(0.0f);
     int i = 0;
     while (ss >> buf)
     {
-        if (i>LBR_MNJ-1)
+        // to many joint values
+        if (i>=JOINT_NUMBER)
         {
-            client.Send(SVR_FALSE_RSP);
+            client.Send(SVR_INVALID_ARGUMENT_COUNT);
             return;
         }
-        tokens.push_back(buf);
         ss2f.flush();
         ss2f.clear();
         ss2f << buf;
-        ss2f >> temp[i];
+        ss2f >> newJointPos(i);
         i++;
     }
-    if (i<LBR_MNJ-1)
+    // to less joint values
+    if (i!=JOINT_NUMBER)
     {
-        client.Send(SVR_FALSE_RSP);
+        client.Send(SVR_INVALID_ARGUMENT_COUNT);
         return ;
     }
-    for (int i=0 ;i<LBR_MNJ; i++)
-    {
-        MSRCMDJNTPOS[i]=temp[i];
-    }
 
-    //Check for Joint Range
-    if (!checkJntRange(MSRCMDJNTPOS))
+
+    //check for joint range
+    if (!SvrData::getInstance()->checkJointRange(newJointPos))
     {
         string out = "Error: Joints out of Range!";
-        client.Send(out);
+        client.Send(SVR_OUT_OF_RANGE);
         return;
     }
+    //set new target for next trajectory
+    VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos();
+    class Trajectory<JOINT_NUMBER>* newTrajectory;
+    
+    // calculate number of movement steps
+    dMaxSpeed = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration);
+    lMaxSpeed = dMaxSpeed.cellmultiply(dMaxSpeed).cellmultiply(maxJointLocalAcceleration) * 0.5f;
+
+            for (int j=0; j<JOINT_NUMBER; j++)
+            {
+                if (lMaxSpeed(j) > deltaAbs(j)/(double)2.0)
+                {
+                    dGesamt(j) = sqrt(deltaAbs(j) / maxJointLocalAcceleration (j))*2.0;
+                } else 
+                {
+                    dGesamt(j) = dMaxSpeed(j)*2 + (deltaAbs(j)-lMaxSpeed(j)*2.0)/maxJointLocalVelocity(j);
+                }
+            }
+            int maxSteps = ceil(dGesamt.max());
+
+    newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(steps, totalTime, newJointPos);
 
     __MSRCMDJNTPOS = true;
 
@@ -915,7 +929,6 @@ void SvrHandling::MoveCartesian(SocketObject& client, string& args)
     float temp[15];
     stringstream ss(args);
     stringstream ss2f;
-    vector<string> tokens;
     int i = 0;
 
     while (ss >> buf)
@@ -925,7 +938,6 @@ void SvrHandling::MoveCartesian(SocketObject& client, string& args)
             client.Send(SVR_FALSE_RSP);
             return;
         }
-        tokens.push_back(buf);
         ss2f.flush();
         ss2f.clear();
         ss2f << buf;
diff --git a/lwrserv/SvrHandling.h b/lwrserv/SvrHandling.h
index 3ca792b..dee6382 100755
--- a/lwrserv/SvrHandling.h
+++ b/lwrserv/SvrHandling.h
@@ -15,6 +15,10 @@
 #define SVR_FAILED			std::string("Failed.")
 #define SVR_TRUE_RSP		"true"
 #define SVR_FALSE_RSP		"false"
+// server error messages 
+#define SVR_INVALID_ARGUMENT_COUNT  "The argument count is not correct."
+#define SVR_OUT_OF_RANGE            "At least one argument is out of Range."
+#define SVR_HELP_MSG                "GPJ - GetPositionJoints \n"
 
 //Begin SVR_COMMANDS
 #define CMD_GetPositionJoints			"GPJ"
@@ -31,6 +35,7 @@
 #define CMD_QUIT						"Quit"
 #define CMD_QUIT1						"quit"
 #define CMD_QUIT2						"exit"
+#define CMD_HELP                        "help"
 #define CMD_ISKUKA						"IsKukaLWR"
 #define CMD_MoveCartesian				"MC"
 //End SVR_COMMANDS
diff --git a/lwrserv/Trajectroy.cpp b/lwrserv/Trajectroy.cpp
index 3dcedb4..e837ad5 100644
--- a/lwrserv/Trajectroy.cpp
+++ b/lwrserv/Trajectroy.cpp
@@ -2,15 +2,18 @@
 #include "Trajectroy.h"
 #include "vec.h"
 
-template <class T, unsigned SIZE> 
-Trajectory::Trajectory(
+template <unsigned SIZE> 
+Trajectory<SIZE>::Trajectory(
         unsigned int steps_,
         float totalTime_,
         Vec<float,SIZE> jointMovement
         )
 {
+    movementType = MovementJointBased;
+
     steps = steps_;
-    joints = SIZE;
+    currentStep = 0;
+
     totalTime = totalTime_;
 
     nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps_);
@@ -19,7 +22,164 @@ Trajectory::Trajectory(
     (void) jointMovement;
 }
 
-Trajectory::~Trajectory()
+template <unsigned SIZE> 
+Trajectory<SIZE>::Trajectory(
+        unsigned int steps_,
+        float totalTime_,
+        Mat<float,4> cartStart,
+        Mat<float,4> cartEnd
+        )
+{
+    movementType = MovementCartBased;
+
+    steps = steps_;
+    currentStep = 0;
+
+    totalTime = totalTime_;
+
+    nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps_);
+
+    // unused
+    (void) cartStart;
+    (void) cartEnd;
+}
+
+template <unsigned SIZE> 
+Trajectory<SIZE>::~Trajectory()
 {
     free(nodes);
 }
+
+
+template <unsigned SIZE> 
+unsigned int Trajectory<SIZE>::getSteps()
+{
+    return steps;
+}
+
+template <unsigned SIZE> 
+unsigned int Trajectory<SIZE>::getRemainingSteps()
+{
+    if (steps >= currentStep )
+        return steps - currentStep;
+    else 
+        return 0;
+}
+
+template <unsigned SIZE> 
+unsigned int Trajectory<SIZE>::getCurrentStep()
+{
+    return currentStep;
+}
+
+template <unsigned SIZE> 
+enum MovementType Trajectory<SIZE>::getMovementType()
+{
+    return movementType;
+}
+
+template <unsigned SIZE> 
+Vec<float,SIZE> Trajectory<SIZE>::getNextJointPos ()
+{
+    Vec<float,SIZE> retval(0.0f);
+    unsigned int pos = currentStep;
+    if (pos >= steps)
+    {
+        pos = steps;
+    }
+
+    if (nodes != NULL && movementType == MovementJointBased)
+    {
+        retval = nodes[pos].jointPos;
+    }
+
+    // increment step
+    currentStep ++;
+
+    return retval;
+}
+template <unsigned SIZE> 
+Mat<float,4> Trajectory<SIZE>::getNextCartPos ()
+{
+    Mat<float,4> retval(0.0f,1.0f);
+    unsigned int pos = currentStep;
+    if (pos >= steps)
+    {
+        pos = steps;
+    }
+
+    if (nodes != NULL && movementType == MovementCartBased)
+    {
+        retval = nodes[pos].cartPos;
+    }
+
+    // increment step
+    currentStep ++;
+
+    return retval;
+}
+
+template <unsigned SIZE> 
+Vec<float,SIZE> Trajectory<SIZE>::getJointPos (unsigned int step)
+{
+    Vec<float,SIZE> retval(0.0f);
+    if (step >= steps)
+    {
+        return retval;
+    }
+
+    if (nodes != NULL && movementType == MovementJointBased)
+    {
+        retval = nodes[step].cartPos;
+    }
+
+    return retval;
+}
+template <unsigned SIZE> 
+Mat<float,4> Trajectory<SIZE>::getCartPos (unsigned int step)
+{
+    Mat<float,4> retval(0.0f, 1.0f);
+    if (step >= steps)
+    {
+        return retval;
+    }
+
+    if (nodes != NULL && movementType == MovementCartBased)
+    {
+        retval = nodes[step].cartPos;
+    }
+
+    return retval;
+}
+template <unsigned SIZE> 
+Vec<float,SIZE> Trajectory<SIZE>::getJointVelocity    (unsigned int step)
+{
+    Vec<float,SIZE> retval(0.0f);
+    if (step >= steps)
+    {
+        return retval;
+    }
+
+    if (nodes != NULL && movementType == MovementCartBased)
+    {
+        retval = nodes[step].velocity;
+    }
+
+    return retval;
+}
+template <unsigned SIZE> 
+Vec<float,SIZE> Trajectory<SIZE>::getJointAcceleration(unsigned int step)
+{
+    Vec<float,SIZE> retval(0.0f);
+    if (step >= steps)
+    {
+        return retval;
+    }
+
+    if (nodes != NULL && movementType == MovementCartBased)
+    {
+        retval = nodes[step].acceleration;
+    }
+
+    return retval;
+}
diff --git a/lwrserv/global.cpp b/lwrserv/global.cpp
index 31c618e..9ca5f04 100644
--- a/lwrserv/global.cpp
+++ b/lwrserv/global.cpp
@@ -15,6 +15,7 @@ bool __DOUS2 = false;
 
 int globi = 0;
 
+#if 0
 float MSRMSRJNTPOS[7];
 double MSRCMDJNTPOS[7];
 double MSRMSRCARTPOS[12];
@@ -22,14 +23,14 @@ float MSRCMDCARTPOS[12];
 double MSRMSRJACOBIAN[42];
 double MSRMSRFTTCP[6];
 double MSRCMDPOSE[3][4];
-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 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};
+#endif
 
 
 
diff --git a/lwrserv/include/BangBangTrajectroy.h b/lwrserv/include/BangBangTrajectroy.h
index 6746a5d..a6eb9b0 100644
--- a/lwrserv/include/BangBangTrajectroy.h
+++ b/lwrserv/include/BangBangTrajectroy.h
@@ -3,11 +3,10 @@
 #include "Trajectroy.h"
 #include "vec.h"
 
-class Trajectory;
-class BangBangJointTrajectroy : public Trajectory
+template <unsigned SIZE> 
+class BangBangJointTrajectroy : public Trajectory<SIZE>
 {
     public:
-        template <class T, unsigned SIZE> 
         BangBangJointTrajectroy(
                 unsigned int steps_,
                 float totalTime_,
@@ -19,7 +18,8 @@ class BangBangJointTrajectroy : public Trajectory
     protected:
 
 };
-class BangBangCartTrajectory: public Trajectory
+template <unsigned SIZE> 
+class BangBangCartTrajectory: public Trajectory<SIZE>
 {
 };
 
diff --git a/lwrserv/include/LinearTrajectory.h b/lwrserv/include/LinearTrajectory.h
index 02c8b3e..879ba3c 100644
--- a/lwrserv/include/LinearTrajectory.h
+++ b/lwrserv/include/LinearTrajectory.h
@@ -2,27 +2,27 @@
 #define _LINPOLYTRAJECTORY_H_
 #include "Trajectroy.h"
 
-class LinearJointTrajectory: public Trajectory
+template <unsigned SIZE>
+class LinearJointTrajectory: public Trajectory<SIZE>
 {
     public:
 
-        template <class T, unsigned SIZE>
         LinearJointTrajectory(
                 unsigned int steps_,
                 float totalTime_,
-                Vec<float,SIZE> jointMovement);
-        template <class T, unsigned SIZE>
+                Vec<float,SIZE> jointMovement_);
         LinearJointTrajectory(
                 unsigned int steps_,
                 float totalTime_,
-                Vec<float,SIZE> jointMovement,
-                Vec<float,SIZE> velocity);
+                Vec<float,SIZE> jointMovement_,
+                Vec<float,SIZE> velocity_);
         ~LinearJointTrajectory();
     private:
 
     protected:
 };
-class LinearCartTrajectory: public Trajectory
+template <unsigned SIZE>
+class LinearCartTrajectory: public Trajectory<SIZE>
 {
 };
 
diff --git a/lwrserv/include/SvrData.h b/lwrserv/include/SvrData.h
index 40115e1..0890914 100644
--- a/lwrserv/include/SvrData.h
+++ b/lwrserv/include/SvrData.h
@@ -4,39 +4,52 @@
 #include <new>
 #include <iostream>
 #include <pthread.h>
+#include <queue>
 
 #include "Singleton.h"
+#include "Trajectroy.h"
 #include "vec.h"
 #include "friComm.h"
+#include "friremote.h"
 
+#define JOINT_NUMBER (LBR_MNJ)
+typedef Vec<float, JOINT_NUMBER> VecJoint;
+typedef Vec<float, FRI_CART_VEC> VecTorque;
+typedef Mat<float, 4> MatCarthesian;
 
 class SvrData: public Singleton
 {
 private:
     struct {
-        float jointPos[LBR_MNJ];
-        float cartPos[FRI_CART_FRM_DIM];
-        float jacobian[FRI_CART_VEC * LBR_MNJ];
-        float forceAndTorque[FRI_CART_VEC];
+        VecJoint jointPos;
+        MatCarthesian cartPos;
+        VecTorque forceAndTorque;
+        float jacobian[FRI_CART_VEC * JOINT_NUMBER];
     } messured;
     struct {
-        float jointPos[LBR_MNJ];
-        float cartPos[FRI_CART_FRM_DIM];
-        float pose[3][4]; //TODO what is this for
+        VecJoint jointPos;
+        MatCarthesian cartPos;
     } commanded;
     
     struct {
         struct {
-            float velocity[LBR_MNJ];
-            float accelaration[LBR_MNJ];
-            float torque[LBR_MNJ];
-            float range[LBR_MNJ];
+            VecJoint velocity;
+            VecJoint accelaration;
+            VecJoint torque;
+            VecJoint range;
         } max;
-        unsigned int joints;
         float currentVelocity;
         float currentAcceleration;
     } robot;
 
+    struct {
+        float sampleTimeMs;
+        bool cancel;
+        std::queue< Trajectory<JOINT_NUMBER>* > list;
+    }trajectory;
+
+    friRemote* friInst;
+
     pthread_mutex_t dataLock;
 
     /// private constructor, because the database is a singleton
@@ -44,46 +57,45 @@ private:
 
     static bool instanceFlag;
     static SvrData* single;
+    
+    
+    void updateMessurementData(
+        VecJoint newJointPos,
+        MatCarthesian newCartPos,
+        VecTorque newForceAndTorque
+        );
 
 public:
     ~SvrData(void);
     static SvrData* getInstance();
+    
+    friRemote* getFriRemote();
+
+    void updateMessurement();
 
     void lock();
     void unlock();
 
-    int getMessuredJointPos(float* data, size_t size);
-    Vec<float,LBR_MNJ> getMessuredJointPos();
-
-    int getMessuredCartPos (float* data, size_t size);
-    Mat<float,LBR_MNJ> getMessuredCartPos();
+    VecJoint getMeasuredJointPos();
+    MatCarthesian getMeasuredCartPos();
 
     int getMessuredJacobian(float* data, size_t size);
+    VecTorque getMessuredForceTorque();
+    VecJoint getMessuredJointPos();
+    MatCarthesian getMessuredCartPos();
 
-    int getMessuredForceTorque(float* data, size_t size);
-    Vec<float,LBR_MNJ> getMessuredForceTorque();
-
-    int getCommandedJointPos(float* data, size_t size);
-    Vec<float,LBR_MNJ> getCommandedJointPos();
-
-    int getCommandedCartPos (float* data, size_t size);
-    Mat<float,LBR_MNJ> getCommandedCartPos();
-
-    float getMaxTorque(unsigned int pos);
-    int   getMaxTorque(float* data, size_t size);
-    Vec<float,LBR_MNJ> getMaxTorque();
-
-    float getMaxVelocity(unsigned int pos);
-    int   getMaxVelocity(float* data, size_t size);
-    Vec<float,LBR_MNJ> getMaxVelocity();
-
-    float getMaxAcceleration(unsigned int pos);
-    int   getMaxAcceleration(float* data, size_t size);
-    Vec<float,LBR_MNJ> getMaxAcceleration();
-
-    float getMaxRange(unsigned int pos);
-    int   getMaxRange(float* data, size_t size);
-    Vec<float,LBR_MNJ> getMaxRange();
+    VecJoint getCommandedJointPos();
+    void     setCommandedJointPos(VecJoint newJointPos);
+    
+    MatCarthesian getCommandedCartPos();
+    void     setCommandedCartPos(MatCarthesian newCartPos);
+    
+    VecJoint getMaxTorque();
+    VecJoint getMaxVelocity();
+    VecJoint getMaxAcceleration();
+    VecJoint getMaxRange();
+    
+    bool     checkJointRange(VecJoint vectorToCheck);
 
     float getRobotVelocity();
     int   setRobotVelocity(float newVelocity);
@@ -92,5 +104,15 @@ public:
     int   setRobotAcceleration(float newVelocity);
 
     unsigned int getJoints();
+
+    // trajectory functions
+    float getSampleTimeMs();
+    int   setSampleTimeMs(float newSampleTime);
+
+    bool getTrajectoryCancel();
+    class Trajectory<JOINT_NUMBER>*  popTrajectory();
+    int  pushTrajectory(class Trajectory<JOINT_NUMBER>* newFrajectory);
+    bool cancelCurrentTrajectory();
+    void trajectoryCancelDone();
 };
 #endif 
diff --git a/lwrserv/include/Trajectroy.h b/lwrserv/include/Trajectroy.h
index ae6672d..0a0bbfe 100644
--- a/lwrserv/include/Trajectroy.h
+++ b/lwrserv/include/Trajectroy.h
@@ -2,43 +2,74 @@
 #define _TRAJECTORY_H_
 #include "vec.h"
 
+// all types of trajectories
+enum TrajectoryType {
+    TrajectoryDefault     = 0,
+    TrajectoryJointLinear = 0,
+    TrajectoryCartLinear,
+
+    TrajectoryJointBangBang,
+    TrajectoryCartBangBang,
+    
+    TrajectoryJointFivePoly,
+    TrajectoryCartFivePoly,
+    
+    TrajectoryJointLSPB,
+    TrajectoryCartLSPB
+};
+enum MovementType
+{
+    MovementJointBased= 0,
+    MovementCartBased
+};
+
+template <unsigned SIZE>
 class Trajectory
 {
     public:
-        template <class T, unsigned SIZE> 
         Trajectory (
             unsigned int steps_,
             float totalTime_,
             Vec<float,SIZE> jointMovement
             );
+        Trajectory(
+            unsigned int steps_,
+            float totalTime_,
+            Mat<float,4> cartStart,
+            Mat<float,4> cartEnd
+        );
         ~Trajectory();
 
         unsigned int getSteps();
-        unsigned int getJoints();
-
-        /**
-         *  
-         */
-        template <class T, unsigned SIZE> Vec<float,SIZE> getJointPos    (unsigned int step);
-        template <class T, unsigned SIZE> Vec<float,SIZE> getCartPos     (unsigned int step);
-        template <class T, unsigned SIZE> Vec<float,SIZE> getVelocity    (unsigned int step);
-        template <class T, unsigned SIZE> Vec<float,SIZE> getAcceleration(unsigned int step);
+        unsigned int getRemainingSteps();
+        unsigned int getCurrentStep();
+
+        enum MovementType getMovementType();
+
+        Vec<float,SIZE> getNextJointPos    ();
+        Mat<float,4>    getNextCartPos     ();
+
+        Vec<float,SIZE> getJointPos         (unsigned int step);
+        Mat<float,4>    getCartPos          (unsigned int step);
+        Vec<float,SIZE> getJointVelocity    (unsigned int step);
+        Vec<float,SIZE> getJointAcceleration(unsigned int step);
         
         struct trajectoryNode
         {
-            float jointPos;
-            float cartPos;
-            float velocity;
-            float acceleration;
+            Vec<float,SIZE> jointPos;
+            Mat<float,4>*    cartPos;
+            Vec<float,SIZE> velocity;
+            Vec<float,SIZE> acceleration;
         };
+
     protected:
         static const unsigned int defaultSteps = 100;
 
         unsigned int steps;
+        unsigned int currentStep;
         float totalTime;
-        unsigned int joints;
         struct trajectoryNode* nodes;
-
+        enum MovementType movementType;
 };
 
 #endif
diff --git a/lwrserv/include/mat.h b/lwrserv/include/mat.h
index 8f91c7d..c7f89fc 100644
--- a/lwrserv/include/mat.h
+++ b/lwrserv/include/mat.h
@@ -16,6 +16,7 @@ typedef Mat<double, 2> Mat2d;
 typedef Mat<double, 3> Mat3d;
 typedef Mat<double, 4> Mat4d;
 
+
 // template square matrix class for SIMPLE data types 
 template <class T, unsigned SIZE> class Mat 
 {
@@ -103,8 +104,8 @@ public:
     Mat<T, SIZE> operator + (const T &scalar)
     {
         Mat<T, SIZE> buf;
-        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
-            for (unsigned int j=0; j<SIZE; j++) // Spalte j
+        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;
         return buf;
     }
@@ -112,8 +113,8 @@ public:
     Mat<T, SIZE> operator - (const T &scalar)
     {
         Mat<T, SIZE> buf;
-        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
-            for (unsigned int j=0; j<SIZE; j++) // Spalte j
+        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;
         return buf;
     }
@@ -121,17 +122,17 @@ public:
     Mat<T, SIZE> operator * (const T &scalar)
     {
         Mat<T, SIZE> buf;
-        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
-            for (unsigned int j=0; j<SIZE; j++) // Spalte 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;
     }
 
     Mat<T, SIZE> operator / (const T &scalar)
     {
         Mat<T, SIZE> buf;
-        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
-            for (unsigned int j=0; j<SIZE; j++) // Spalte j
+        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;
         return buf;
     }
@@ -139,16 +140,16 @@ public:
     Mat<T, SIZE> operator * (const Mat<T, SIZE> &mat)
     {
         Mat<T, SIZE> buf;
-        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
-            for (unsigned int j=0; j<SIZE; j++) // Spalte j
+        for (unsigned int i=0; i<SIZE; i++) // row i
+            for (unsigned int j=0; j<SIZE; j++) // column j
                 for (unsigned int a=0; a<SIZE; a++) // a
                     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
+        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)];
 
         return (*this); // also an R-value in e.g.
@@ -161,8 +162,8 @@ public:
     Mat<T, SIZE> operator * (const Vec<T, SIZE> &vec)
     {
         Vec<T, SIZE> buf;
-        for (unsigned int j=0; j<SIZE; j++) // SPALTE j
-            for (unsigned int i=0; i<SIZE; i++) // ZEILE i
+        for (unsigned int j=0; j<SIZE; j++) // column j
+            for (unsigned int i=0; i<SIZE; i++) // row i
                 buf(i) += m_aatData[i][j]*vec(j);
         return buf;
     }
@@ -171,8 +172,8 @@ public:
     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
+        for (unsigned int j=0; j<SIZE; j++) // column j
+            for (unsigned int i=0; i<SIZE; i++) // row i
                 buf.m_aatData[i][j] = abs(m_aatData[i][j]);
         return buf;
     }
@@ -186,7 +187,7 @@ public:
             return m_aatData[0][0] * m_aatData[1][1] - m_aatData[1][0]*m_aatData[0][1];
 
 
-        for (unsigned int i=0; i<SIZE; i++) // SPALTE j
+        for (unsigned int i=0; i<SIZE; i++) // column j
         {
             T multBuff;
             multBuff = m_aatData[0][i];
@@ -206,9 +207,9 @@ public:
     Mat<T,SIZE> norm()
     {
         T buf;
-        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
-            for (unsigned int j=0; j<SIZE; j++) // Spalte j
-                buf +=  pow(abs(m_aatData[i][j]),2);
+        for (unsigned int row=0; row < SIZE; row++)
+            for (unsigned int column=0; column < SIZE; column++)
+                buf +=  pow(abs(m_aatData[row][column]),2);
         sqrt(buf);
         return buf;
     }
@@ -216,8 +217,8 @@ public:
     Mat<T,SIZE> transpose()
     {
         Mat<T, SIZE> buf;
-        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
-            for (unsigned int j=0; j<SIZE; j++) // Spalte j
+        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];
         return buf;
     }
@@ -235,6 +236,18 @@ public:
 
         return thisTransposed;
     }
+    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];
+    }
+    void setData (const T atData[SIZE*SIZE]) 
+    {
+        for (unsigned int row=0; row<SIZE; row++)
+            for (unsigned int column=0; column<SIZE; column++)
+                m_aatData[row][column] = atData[row+column*SIZE];
+    }
 
 private:
     T m_aatData[SIZE][SIZE];
diff --git a/lwrserv/include/vec.h b/lwrserv/include/vec.h
index a1b09b3..1b2204a 100644
--- a/lwrserv/include/vec.h
+++ b/lwrserv/include/vec.h
@@ -55,7 +55,7 @@ public:
             m_atData[i] = atData[i];
     }
 
-    void getData (const T atData[SIZE]) 
+    void getData (T atData[SIZE]) 
     {
         for (unsigned int i=0; i<SIZE; i++)
             atData[i] = m_atData[i];
diff --git a/lwrserv/program.cpp b/lwrserv/program.cpp
index 8a046be..08defe6 100644
--- a/lwrserv/program.cpp
+++ b/lwrserv/program.cpp
@@ -4,8 +4,10 @@
 #include <error.h>
 #include <errno.h>
 #include <math.h>
+#include <Trajectroy.h>
 
 #include "SvrData.h"
+#include <boost/thread/thread.hpp>
 
 void printMat(Mat4f M)
 {
@@ -18,22 +20,22 @@ float* abctomat(float a, float b, float c)
     float ca = cos(c);
     float sa = sin(c);
 
-    rx(0,0) = 1;
+    rx(0,0) = 1.0f;
     rx(1,1) = ca; 
     rx(1,2) = -sa;
     rx(2,1) = sa; 
     rx(2,2) = ca; 
-    rx(3,3) = 1;
+    rx(3,3) = 1.0f;
 
     Mat4f ry;
     float cb = cos(b);
     float sb = sin(b);
     ry(0,0) = cb;
     ry(0,2) = sb;
-    ry(1,1) = 1;
+    ry(1,1) = 1.0f;
     ry(2,0) = -sb;
     ry(2,2) = cb; 
-    ry(3,3) = 1;
+    ry(3,3) = 1.0f;
 
     Mat4f rz;
     float cc = cos(a);
@@ -42,8 +44,8 @@ float* abctomat(float a, float b, float c)
     rz(0,1) = -sc;
     rz(1,0) = sc;
     rz(1,1) = cc;
-    rz(2,2) = 1; 
-    rz(3,3) = 1;
+    rz(2,2) = 1.0f;
+    rz(3,3) = 1.0f;
 
     
     Mat4f result;
@@ -96,78 +98,6 @@ float* mattoabc(float M[12])
     return abc;
 }
 
-void doRalf() 
-{
-    // Global definitions
-    float lowStiff[6]  = {  0.01f,   0.01f,   0.01f,  0.01f,  0.01f,  0.01f};
-    float highStiff[6] = {1000.0f, 1000.0f, 1000.0f,  10.0f,  10.0f,  10.0f};
-    float slowDamp[6]  = { 0.007f,  0.007f,  0.007f, 0.007f, 0.007f, 0.007f};
-    float fastDamp[6]  = {   0.7f,    0.7f,    0.7f,   0.7f,   0.7f,   0.7f};
-
-    float pos1[12] = {
-                        1.0f, 0.0f, 0.0f, 500.0f,
-                        0.0f, 1.0f, 0.0f,   0.0f,
-                        0.0f, 0.0f, 1.0f, 500.0f
-                     };
-    float pos2[12] = {
-                        1.0f, 0.0f, 0.0f , 500.0f,
-                        0.0f, 1.0f, 0.0f ,   0.0f,
-                        0.0f, 0.0f ,1.0f , 500.0f
-                     };
-
-    friRemote friInst;
-    double timeCounter=0;
-    friInst.doDataExchange();
-
-    float* cartPos;
-    while(true)
-    {
-        // Time
-        timeCounter+=friInst.getSampleTime();
-
-        friInst.doReceiveData();
-
-        // Get all variables from FRI
-        float* jntPos = friInst.getMsrMsrJntPosition();
-        float* jacobian = friInst.getJacobian();
-        float* ftTcp = friInst.getFTTcp();
-
-        if (timeCounter < 20)
-            cartPos = friInst.getMsrCartPosition();
-        //friInst.doDataExchange();
-        //friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, true);
-#if 0
-        if (timeCounter>10 && timeCounter<10) 
-        {
-            friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, false);
-        } else 
-        {
-            friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, false);
-        }
-#endif
-        float npos[12];
-        memcpy(npos,cartPos,12);
-        friInst.doCartesianImpedanceControl(npos, highStiff, fastDamp, NULL, NULL, false);
-        
-#if 0
-        if (timeCounter<20) 
-        {
-            friInst.doCartesianImpedanceControl(Test, highStiff, fastDamp, NULL, NULL, true);
-        } else if (timeCounter<30) 
-        {
-            friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, true);
-        } else if (timeCounter<40) 
-        {
-            friInst.doCartesianImpedanceControl(cartPos , highStiff, fastDamp, NULL, NULL, true);
-        } else 
-        {
-            friInst.doDataExchange();
-        }
-#endif
-        friInst.doSendData();
-    }
-}
-
 Mat4f vecToMat2(float vec[12])
 {
     Mat4f result;
@@ -178,7 +108,7 @@ Mat4f vecToMat2(float vec[12])
             result(i,j) = (float)vec[i*4+j];
         }
     }
-    result(3,3)=(float)1;
+    result(3,3)=1.0f;
     return result;
 }
 
@@ -248,57 +178,161 @@ float* quattovec(float quat[4])
     return vec;
 }
 
-
-void *threadFriDataExchange(void *arg)
+void *threadRobotMovement(void *arg)
 {
-    // unused 
+    // unused parameters 
     (void) arg;
 
-    //doRalf();
+    Trajectory<JOINT_NUMBER>* currentTrajectory = NULL;
+    enum MovementType currentMovementType  = MovementJointBased;
 
-    friRemote friInst;
+    VecJoint      currentJointPos(0.0f);
+    MatCarthesian currentCartPos(0.0f,1.0f);
 
-    while (1)
+    while(1)
     {
-        //#######################################
-        // Communication loop
+        // get current sample time cause it could be changed
+        float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
+        friRemote* friInst = SvrData::getInstance()->getFriRemote();
 
-        // get current joint positions
-        for ( int i= 0; i < LBR_MNJ; i++)
+        // check if we have to cancel current trajectory
+        bool cancel = SvrData::getInstance()->getTrajectoryCancel();
+        if (cancel)
         {
-            MSRMSRJNTPOS[i] = friInst.getMsrCmdJntPosition()[i] + friInst.getMsrCmdJntPositionOffset()[i];
-            //MSRMSRJNTPOS[i] = friInst.getMsrMsrJntPosition()[i];
+            // free the current trajectory 
+            if ( currentTrajectory != NULL)
+            {
+                delete currentTrajectory;
+                currentTrajectory = NULL;
+            }
+            // signal waiting thread the cancellation
+            SvrData::getInstance()->trajectoryCancelDone();
+            goto end;
         }
 
-        // get current jacobian
-        float* friJacobian = friInst.getJacobian();
-        if ( friJacobian == NULL)
+        // check for new trajectory
+        if (currentTrajectory == NULL)
         {
-            fprintf(stderr,"Failed: could not get jacobian\n");
-            break;
+            // get next trajectory from svrData 
+            currentTrajectory = SvrData::getInstance()->popTrajectory();
+
+            // no new trajectory 
+            if(currentTrajectory == NULL)
+            {
+                // stay on current position
+                goto end;
+            }
         }
-        for (int i = 0; i < FRI_CART_VEC*LBR_MNJ ; i++)
+
+        // get next commanded trajectory type
+        switch (currentTrajectory->getMovementType())
         {
-            MSRMSRJACOBIAN[i] = friJacobian[i];
+            case MovementCartBased:
+            {
+                currentCartPos = currentTrajectory->getNextCartPos();
+                currentMovementType = MovementCartBased;
+            }
+            break;
+            case MovementJointBased:
+            {
+                currentJointPos = currentTrajectory->getNextJointPos();
+                currentMovementType = MovementJointBased;
+            }
+            default:
+            {
+                // invalid trajectory skip it
+                delete currentTrajectory;
+                currentTrajectory = NULL;
+                goto end;
+            }
+            break;
         }
 
-        // get current messured carthesian position
-        float* friMsrCartPosition = friInst.getMsrCartPosition();
-        if ( friMsrCartPosition == NULL)
+end:
+        // TODO stop timer for last sent message 
+        
+        // time we used to come to this point caused by the calculation and lock time
+        // TODO calculate this time
+        float calculationTimeMs = 0.0f;
+        
+        // wait only the remaining time 
+        float waitingTimeMs = sampleTimeMs - calculationTimeMs;
+
+        // sleep the calculated waiting time
+        boost::this_thread::sleep( boost::posix_time::milliseconds(waitingTimeMs) );
+        
+
+        switch (currentMovementType)
         {
-            fprintf(stderr,"Failed: could not get Messurement in Carthesian Position\n");
-            break;
+            case MovementJointBased:
+                {
+                    // send the new joint values
+                    float newJointPosToSend[JOINT_NUMBER] = {0.0f};
+                    currentJointPos.getData(newJointPosToSend);
+                    friInst->doPositionControl(newJointPosToSend);
+                }
+                break;
+            case MovementCartBased:
+                {
+#if 0
+                    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};
+                    float currentCartPosA[12] = {0.0f};
+                    columncurrentCartPos.getData(newCartPosToSend);
+                    friInst->doCartesianImpedanceControl(currentCartPosA,Stiff, Damp, NULL, NULL, true);
+#endif
+                }
+                break;
         }
-        for ( int i =0; i < FRI_CART_FRM_DIM; i++)
+
+        // start timer for the last sent msg
+        // TODO 
+        
+        // check if current trajectory is over 
+        if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0)
         {
-            MSRMSRCARTPOS[i] = friMsrCartPosition[i];
+            // invalid trajectory skip it
+            delete currentTrajectory;
+            currentTrajectory = NULL;
         }
+    }
+}
+
+#if 0
+void *threadFriDataExchange(void *arg)
+{
+    // unused 
+    (void) arg;
+
+    friRemote* friInst = SvrData::getInstance()->getFriRemote();
+
+    while (1)
+    {
+        //#######################################
+        // Communication loop
+
+        // update current joint positions
+        SvrData::getInstance()->updateMessurement();
+
+        // get current joint position
+        VecJoint currentJointPos = SvrData::getInstance()->getMessuredJointPos();
+
+        MatCarthesian currentCartPost = SvrData::getInstance()->getMessuredCartPos();
 
         // get current force and torque
-        float* friFTTcp =  friInst.getFTTcp();
-        for ( int i = 0; i < FRI_CART_VEC; i++)
+        VecJoint currentForceAndTorque = SvrData::getInstance()->getMessuredForceTorque();
+
+        // get current jacobian
+        // TODO use svr data
+        float* friJacobian = friInst->getJacobian();
+        if ( friJacobian == NULL)
         {
-            MSRMSRFTTCP[i] = friFTTcp[i];
+            fprintf(stderr,"Failed: could not get jacobian\n");
+            break;
+        }
+        for (int i = 0; i < FRI_CART_VEC*LBR_MNJ ; i++)
+        {
+            MSRMSRJACOBIAN[i] = friJacobian[i];
         }
 
         //#########################################################
@@ -312,8 +346,7 @@ void *threadFriDataExchange(void *arg)
             Vec<float,LBR_MNJ> dMaxSpeed;
             Vec<float,LBR_MNJ> lMaxSpeed;
             Vec<float,LBR_MNJ> dGesamt;
-            float sampleTime = 0.005f;
-            
+            float sampleTime = SvrData::getInstance()->getSampleTimeMs();
 
             // get current robot constraints
             Vec<float,LBR_MNJ> maxVelocityJoint = SvrData::getInstance()->getMaxVelocity();
@@ -329,7 +362,7 @@ void *threadFriDataExchange(void *arg)
             deltaAbs = delta.abs();
             maxJointLocalVelocity = maxVelocityJoint * sampleTime * (velocity/100.0f);
             maxJointLocalAcceleration = maxAccelarationJoint * sampleTime * (accelaration/100.0f);
-            
+
             // calculate number of movement steps
             dMaxSpeed = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration);
             lMaxSpeed = dMaxSpeed.cellmultiply(dMaxSpeed).cellmultiply(maxJointLocalAcceleration) * 0.5f;
@@ -364,13 +397,13 @@ void *threadFriDataExchange(void *arg)
                 maxJointLocalVelocity(j) = dMaxSpeed(j)*maxJointLocalAcceleration(j);
             }
 
-
             // do the trajectory
             {
                 Vec<float,LBR_MNJ> currentInk;
                 Vec<float,LBR_MNJ> currentInkLast;
                 Vec<float,LBR_MNJ> currentDist;
                 Vec<float,LBR_MNJ> currentDistLast;
+
                 // bang bang
                 for (int i=0;i<maxSteps;i++)
                 {
@@ -389,10 +422,10 @@ void *threadFriDataExchange(void *arg)
                         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);
+                        MSRMSRJNTPOS[j] += sgn(delta(j))*currentInk(j)*(1./180*M_PI);
                     }
                     // set new position
-                    friInst.doPositionControl(MSRMSRJNTPOS);
+                    friInst->doPositionControl(MSRMSRJNTPOS);
                 }
             }
 end:
@@ -424,8 +457,8 @@ end:
             {
                 for ( int i =0; i < FRI_CART_FRM_DIM; i++)
                 {
-                    Pos[i] = friInst.getMsrCartPosition()[i];
-                    MSRMSRCARTPOS[i] = friInst.getMsrCartPosition()[i];
+                    Pos[i] = friInst->getMsrCartPosition()[i];
+                    MSRMSRCARTPOS[i] = friInst->getMsrCartPosition()[i];
                     deltaCart[i] = MSRCMDCARTPOS[i]-Pos[i];
                 }
                 sum = 0;
@@ -484,24 +517,25 @@ end:
                 {
                     Pos[i]+= deltaCart[i]*0.02;
                 }
-                friInst.doCartesianImpedanceControl(Pos, Stiff,Damp,NULL,NULL, true);
+                friInst->doCartesianImpedanceControl(Pos, Stiff,Damp,NULL,NULL, true);
 
             }
         } 
         //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};
         //float Test[12] = {MSRMSRCARTPOS[0],MSRMSRCARTPOS[1],MSRMSRCARTPOS[2],MSRMSRCARTPOS[3],MSRMSRCARTPOS[4],MSRMSRCARTPOS[5],MSRMSRCARTPOS[6],MSRMSRCARTPOS[7],MSRMSRCARTPOS[8],MSRMSRCARTPOS[9],MSRMSRCARTPOS[10],MSRMSRCARTPOS[11]};
-        //friInst.doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
+        //friInst->doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
 
         // innerhlab 5ms
-        friInst.doPositionControl(MSRMSRJNTPOS);
+        friInst->doPositionControl(MSRMSRJNTPOS);
 
         //i = 0;
         //}
-        //friInst.doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
+        //friInst->doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
     }
     return NULL;
 }
+#endif
 
 int main()
 {
@@ -509,7 +543,7 @@ int main()
     //Setting pthread for FRI interface
     pthread_t fri_t;
     //Start fri_thread
-    err = pthread_create(&fri_t,NULL,&threadFriDataExchange,NULL);
+    err = pthread_create(&fri_t,NULL,&threadRobotMovement,NULL);
     if (err > 0 )
     {
         cerr << "Failed: could not create thread\n" << endl;