init new version without opencl

lwrserv/Makefile

@@ -0,0 +1,82 @@
+################################################################################
+# Contents: makefile with dependency generation                                #
+#           26.05.2015                                                         #
+#------------------------------------------------------------------------------#
+#           makefile for cpp/c bins and libs                                   #
+#           autor Philipp Schoenberger                                         #
+#           ph.schoenberger@googlemail.com                                     #
+################################################################################
+LIBS = pthread
+
+OUT_BINARY = lwrserv
+
+CC = g++
+GLOBAL_FLAGS = -g -o2 -ansi -pedantic -DHAVE_SOCKLEN_T
+CFLAGS   = $(GLOBAL_FLAGS)
+CXXFLAGS = $(GLOBAL_FLAGS)
+CPPFLAGS = $(GLOBAL_FLAGS) 
+LDFLAGS = $(patsubst %,-l%,$(LIBS)) 
+
+BUILD_DIR = build
+
+# Sources
+SRC_C   := $(wildcard *.c)
+SRC_H   := $(wildcard *.h)
+SRC_CPP  := $(wildcard *.cpp)
+SRC_HPP  := $(wildcard *.hpp)
+#SRC_CXX := $(wildcard *.cxx)
+#SRC_CC  := $(wildcard *.C)
+
+# Objects
+OBJS_C = $(patsubst %.c,$(BUILD_DIR)/%.o,$(SRC_C))
+OBJS_CPP = $(patsubst %.cpp,$(BUILD_DIR)/%.o,$(SRC_CPP))
+
+
+OBJS=$(patsubst %.o,%.o,$(OBJS_C) $(OBJS_CPP))
+
+# Dependencies
+DEPS_H = $(patsubst %.h,$(BUILD_DIR)/%.d,$(SRC_H))
+DEPS_HPP = $(patsubst %.hpp,$(BUILD_DIR)/%.d,$(SRC_HPP))
+
+DEPS=$(OBJS:%.o=%.d)
+#DEPS+=$(DEPS_H)
+#DEPS+=$(DEPS_HPP)
+
+.DEFAULT: all
+Q=@
+
+all: $(BUILD_DIR) $(BUILD_DIR)/$(OUT_BINARY)
+
+$(BUILD_DIR):
+	$(Q)echo " [mk] $(BUILD_DIR)"
+	$(Q)mkdir -p $(BUILD_DIR)
+
+clean:
+	$(Q)echo " [rm] $(BUILD_DIR)"
+	$(Q)rm -rf $(BUILD_DIR)
+
+$(BUILD_DIR)/$(OUT_BINARY): $(BUILD_DIR) $(DEPS) $(DEP_LIBS) $(OBJS)
+	$(Q)echo " [ld] $@"
+	$(Q)( $(CC)  $(CFLAGS) $(OBJS) $(LDFLAGS)  -o $(OUT_BINARY)) || exit 1
+
+$(BUILD_DIR)/%.o : %.c $(BUILD_DIR)
+	$(Q)echo " [CC] $@"
+	$(Q)( $(CC) -c $(CFLAGS) $(CPPFLAGS) $< -o $@ ) || exit 1
+
+$(BUILD_DIR)/%.o : %.cpp $(BUILD_DIR)
+	$(Q)echo " [CXX] $@"
+	$(Q)( $(CXX) -c $(CXXFLAGS) $(CPPFLAGS) $< -o $@ ) || exit 1
+
+$(BUILD_DIR)/%.d: $(BUILD_DIR) %.c 
+	$(Q)echo " [DEP] $@"
+	$(Q)touch $@
+	$(Q)echo -n "$@ $(patsubst %.d,%.o,$@):" >$@
+	$(Q)$(CC) -M $(CFLAGS) $(filter %.cpp,$^ ) | sed 's:$*.o\:::' >> $@
+
+$(BUILD_DIR)/%.d: $(BUILD_DIR) %.cpp 
+	$(Q)echo " [DEP] $@"
+	$(Q)touch $@
+	$(Q)echo -n "$@ $(patsubst %.d,%.o,$@):" >$@
+	$(Q)$(CC) -M $(filter-out -g -o0 -o1 -o2 ,$(CPPFLAGS)) $(filter %.cpp,$^) | sed 's:$*.o\:::' >> $@
+
+-include $(DEPS)

lwrserv/Point.h

@@ -0,0 +1,14 @@
+#ifndef POINT_H
+#define POINT_H
+
+class Point {
+public:
+
+   Point (int x=0, int y=0) {
+      this->x = x;
+      this->y = y;
+   }
+
+   int x, y;
+};
+#endif // POINT_H

lwrserv/SocketObject.cpp

@@ -0,0 +1,417 @@
+#include "SocketObject.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <iostream>
+#include <cerrno>
+#include <iomanip>
+
+using namespace std;
+
+#if defined(__msdos__) || defined(WIN32)
+SocketObject::SocketObject(void)
+{
+#ifdef __SOCKET_OBJECT_DEBUG
+    cout << "Windows SocketObject Konstruktor" << endl;
+#endif
+    WSADATA wsaData;
+    WORD wVersionRequested;
+
+    wVersionRequested = MAKEWORD( 2, 0 );
+    _skSocket = INVALID_SOCKET;
+    WSAStartup(wVersionRequested, &wsaData);
+}
+
+SocketObject::SocketObject(SocketObject& sk)
+{
+    WSADATA wsaData;
+    WORD    wVersionRequested;
+
+    wVersionRequested = MAKEWORD( 2, 0 );
+    _skSocket = sk._skSocket;
+    sk._skSocket = INVALID_SOCKET;
+    WSAStartup(wVersionRequested, &wsaData);
+}
+
+SocketObject::~SocketObject(void)
+{
+    Disconnect();
+}
+#endif
+
+
+void SocketObject::setBlockingMode(bool nonblock)
+{
+    int errorCode = 0;
+#if defined(__msdos__) || defined(WIN32)
+    u_long iMode = 1;
+
+    if (!nonblock)
+        iMode=0;
+
+    if (0 != ioctlsocket(_skSocket, FIONBIO, &iMode))
+    {
+        errorCode = WSAGetLastError();
+        cout << "Set blocking mode ("<<iMode<<") failed! Errorcode "<< errorCode;
+    }
+#else
+    //CHANGED: Use the boolean value
+    if (nonblock)
+        fcntl(_skSocket, F_SETFL, O_NONBLOCK);
+#endif
+}
+
+
+int SocketObject::Bind(unsigned short int iPort)
+{
+#if defined(__msdos__) || defined(WIN32)
+
+    struct sockaddr_in      saServerAddress;
+    // TCP-Socket erschaffen
+    _skSocket = socket(AF_INET, SOCK_STREAM, 0);
+    if (_skSocket == INVALID_SOCKET) 
+    {
+#ifdef __SOCKET_OBJECT_DEBUG
+        cout << "SocketObject::Bind(): INVALID SOCKET" << endl;
+#endif
+        return 0;
+    }
+    // serverAdress wählen
+    memset(&saServerAddress, 0, sizeof(sockaddr_in));
+    saServerAddress.sin_family = AF_INET;
+    saServerAddress.sin_addr.s_addr = htonl(INADDR_ANY);
+    saServerAddress.sin_port = htons(iPort);
+
+    if ( bind(_skSocket, (sockaddr*)&saServerAddress, sizeof(sockaddr))== SOCKET_ERROR)
+    {
+        Disconnect();
+        return -1;
+    } else 
+    {
+#ifdef __SOCKET_OBJECT_DEBUG
+        cout << "SocketObject::Bind(): Alles ok" << endl;
+#endif
+        return 0;
+    }
+
+#else
+
+    struct sockaddr_in  saServerAddress;
+
+#ifdef __SOCKET_OBJECT_DEBUG
+    std::cout << "Port: " << iPort << endl;
+#endif
+
+    // Socket erschaffen
+    _skSocket = socket(AF_INET, SOCK_STREAM, 0);            
+    if (_skSocket < 0) 
+    {
+#ifdef __SOCKET_OBJECT_DEBUG
+        std::cout << "Fehler bei Bind - socket()" << std::endl;
+#endif
+        return -1;
+    }
+
+    const int y = 1;
+    /* Socketoptionen fuer schnelles Verbinden setzen*/
+    setsockopt(_skSocket, SOL_SOCKET, SO_REUSEADDR, &y, sizeof(int));
+
+    /* Initialisieren der aockaddr_in - structure */
+    // Server-Adresse mit 0 vorbelegen
+    memset(&saServerAddress, 0, sizeof(struct sockaddr_in));    
+    // Domain
+    saServerAddress.sin_family = AF_INET;               
+    // Addresse
+    saServerAddress.sin_addr.s_addr =  INADDR_ANY;
+    // Port
+    saServerAddress.sin_port = htons( iPort );      
+
+    // Versuch Socket an Port zu binden
+    if (0 > bind(_skSocket, (sockaddr*) &saServerAddress, sizeof(sockaddr)))
+    {   
+        // Socket will nicht mehr lesen und schreiben
+        shutdown(_skSocket,2);          
+#ifdef __SOCKET_OBJECT_DEBUG
+        std::cout << "Fehler bei Bind!" << std::endl;
+#endif
+        return -1;
+    } else 
+    {
+        return 0;
+    }
+#endif
+}
+
+
+int SocketObject::Listen( void )
+{
+    // lauscht bei _skSocket, max Warteschlange = 2
+    return listen(_skSocket, 2);    
+// WIN32 : ,32
+}
+
+
+bool SocketObject::Accept(SocketObject &skAcceptSocket)
+{
+    struct sockaddr_in  saClientAddress;
+    int    iClientSize = sizeof(sockaddr_in);
+
+#if defined(__msdos__) || defined(WIN32)
+
+    skAcceptSocket._skSocket = accept(_skSocket, (struct sockaddr*)&saClientAddress, &iClientSize);
+    return !( skAcceptSocket._skSocket == INVALID_SOCKET);
+
+#else
+
+    skAcceptSocket._skSocket = accept( _skSocket, (sockaddr*) &saClientAddress,(socklen_t*) &iClientSize);
+    return !( skAcceptSocket._skSocket == -1);
+
+#endif
+}
+
+
+bool SocketObject::Connect(const char* szServerAddress, unsigned short int iPort, bool nonblock)
+{
+#if defined(__msdos__) || defined(WIN32)
+
+    struct sockaddr_in server_addr;
+    LPHOSTENT          lpHost;
+    int                error;
+
+    // Open the socket
+    _skSocket = socket(AF_INET, SOCK_STREAM, 0);
+    if (_skSocket == INVALID_SOCKET)
+        return false;
+
+    u_long iMode = 0;
+    if (nonblock)
+        iMode = 1;
+    ioctlsocket(_skSocket, FIONBIO, &iMode);
+
+    memset(&server_addr,0,sizeof(struct sockaddr_in));
+    server_addr.sin_family = AF_INET;
+    server_addr.sin_addr.s_addr = inet_addr(szServerAddress);
+
+    if (server_addr.sin_addr.s_addr == INADDR_NONE)
+    {
+        lpHost = gethostbyname(szServerAddress);
+        if (lpHost != NULL) 
+        {
+            server_addr.sin_addr.s_addr = ((LPIN_ADDR)lpHost->h_addr)->s_addr;
+        }
+        else
+            return false;
+    }
+
+    server_addr.sin_port = htons(iPort);
+    error = connect(_skSocket, (struct sockaddr*)&server_addr, sizeof(sockaddr));
+    if (error == SOCKET_ERROR)
+    {
+        Disconnect();
+        return false;
+    }
+    return true;
+
+#else
+
+    struct sockaddr_in      server_addr;
+    int             status;
+
+    // Socket erschaffen
+    _skSocket = socket(AF_INET, SOCK_STREAM, 0);
+    if (_skSocket < 0)
+        return false;
+
+    if (nonblock)
+        fcntl(_skSocket, F_SETFL, O_NONBLOCK);
+
+    // Initilisieren der sockaddr_in- Struktur
+    memset(&server_addr, 0, sizeof(struct sockaddr_in));
+    server_addr.sin_family = AF_INET;
+    // server-Adresse bergeben
+    server_addr.sin_addr.s_addr = inet_addr(szServerAddress);
+    // std::cout << inet_ntoa (server_addr.sin_addr)<< std::endl;
+    // Port
+    server_addr.sin_port = htons(iPort);
+
+    // Verbindung herstellen
+    status = connect(_skSocket, (struct sockaddr*)&server_addr, sizeof(sockaddr));
+
+    if (status < 0)
+    {
+        shutdown(_skSocket,2);
+        return false;
+    }
+    return true;
+
+#endif
+}
+
+
+bool SocketObject::Connect(const string szServerAddress, unsigned short int iPort, bool nonblock)
+{
+    return Connect((char*) szServerAddress.c_str(), iPort, nonblock);
+}
+
+
+void SocketObject::Disconnect()
+{
+#if defined(__msdos__) || defined(WIN32)
+    if (_skSocket != INVALID_SOCKET)
+    {
+        closesocket(_skSocket);
+        _skSocket = INVALID_SOCKET;
+    }
+#else
+    close(_skSocket);
+#endif
+}
+
+int SocketObject::Recv(char *szBuffer, int iBufferLength, int iFlags)
+{
+    return recv(_skSocket, szBuffer, iBufferLength, iFlags);
+}
+
+int SocketObject::Recv (string& s)
+{
+    char buf [MAXRECV+1];
+    // cout << WSAGetLastError() << " in SocketObject::Recv, vor recv" << endl;
+    s = "";
+    memset(buf, 0, MAXRECV+1);
+    int status = Recv(buf, MAXRECV, 0);
+
+    switch (status) 
+    {
+        case -1:
+#ifdef __SOCKET_OBJECT_DEBUG
+      cout << "status: -1  errno: " << errno << " in SocketObject::Recv" << endl;
+#endif
+            break;
+        case 0:
+            break;
+
+        default:
+            s = buf;
+            // strip the (expected) CRLF
+            if(s.length() == 1) 
+            {
+                if(s[s.length()-1]=='\n' || s[s.length()-1]=='\r')
+                    s.resize(s.length()-1);
+            } else if(s.length() >= 2) 
+            {
+                if(s[s.length()-1]=='\n' && s[s.length()-2]=='\r')
+                    s.resize(s.length()-2);
+                else if(s[s.length()-1]=='\n' || s[s.length()-1]=='\r')
+                    s.resize(s.length()-1);
+            }
+            break;
+    }
+    return status;
+}
+
+
+int SocketObject::receiveLine(char* array, int arraySize)
+{
+    //std::string ret;
+    char r;
+    int pos = 0;
+
+    while (pos<arraySize-2)
+    {
+        switch(recv(_skSocket, &r, 1, 0))
+        {
+            case 0: // not connected anymore;
+                return 0;
+            case -1:
+                return -1;
+        }
+
+        // check for carriage return or line feed (MAC: CR, Linux: LF, Windows: CRLF)
+        if ((r == char(13)) || (r == char(10)))
+        {
+            if (pos > 0)
+            {
+                array[pos]='\0';
+                return pos;
+            }
+        }else
+        {
+            array[pos] = r;
+            pos++;
+        }
+    }
+    return pos;
+}
+
+std::string SocketObject::receiveLine()
+{
+    std::string ret;
+    char r;
+
+    while (1)
+    {
+        //This will not work under windows if the blocking mode is non-blocking
+        // -1 will indicate that there is no value, it is not an error !!!
+        //http://msdn.microsoft.com/en-us/library/ms740121(VS.85).aspx
+        //For linux see:
+        //http://linux.about.com/library/cmd/blcmdl2_recv.htm
+        //If no messages are available at the socket, the receive calls wait for a message to arrive,
+        //unless the socket is nonblocking (see fcntl(2)) in which case the value -1 is returned and
+        // the external variable errno set to EAGAIN.
+        switch(recv(_skSocket, &r, 1, 0))
+        {
+            case 0: // not connected anymore;
+                return SOCKETOBJECT_NOT_CONNECTED;
+            case -1: // error
+                return SOCKETOBJECT_RECV_ERROR;
+        }
+
+        // check for carriage return or line feed (MAC: CR, Linux: LF, Windows: CRLF)
+        if ((r == char(13)) || (r == char(10)))
+        {
+            if (ret.length() > 0)
+                return ret;
+        } else
+        {
+            ret += r;
+        }
+    }
+}
+
+int SocketObject::Send(const char *szBuffer, int iBufferLength, int iFlags) 
+{
+    return send(_skSocket,szBuffer,iBufferLength, iFlags);
+}
+
+
+int SocketObject::Send (const string s)
+{
+    string str = s;
+    // add the (expected) CRLF
+    if(str == "") 
+    {
+        str = "\n\r";
+    } else if(str.size() == 1) 
+    {
+        if(str[0] == '\r') 
+        {
+            str = "\n\r";
+        } else if(str[0] != '\n') 
+        {
+            str += "\n\r";
+        }
+    } else if(str.size() >= 2) 
+    {
+        if(str[str.length()-1] != '\n' || str[str.length()-2]!='\r')
+        {
+            str += "\n\r";
+        }
+    }
+#if defined(__msdos__) || defined(WIN32)
+    return Send((char*) str.c_str(), (int) str.size(), 0);
+#else
+    return Send((char*) str.c_str(), (int) str.size(), (int) MSG_NOSIGNAL);
+#endif
+}
+

lwrserv/SocketObject.h

@@ -0,0 +1,172 @@
+/** \class  SocketObject
+*
+*   \brief  Die Klasse SocketObject kapselt die Verwendung von Sockets
+*
+*   Die Klasse SocketObject liefert eine komfortable Benutzungs-Schnittstelle
+*   für Sockets.
+*   Sie bietet Methoden zum Senden und Empfangen von verschiedenen Datentypen,
+*   sowie Methoden zum Verbindungsauf- und abbau.
+*/
+#ifndef _SOCKETOBJECT_H
+#define _SOCKETOBJECT_H
+
+#if (defined(__msdos__) || defined(WIN32))
+#include <winsock.h>
+#else
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+#include <fcntl.h>
+#include <unistd.h>
+#endif
+
+#include <string>
+#include <sstream>
+#include <vector>
+
+
+#define SOCKETOBJECT_NOT_CONNECTED "SOCKETOBJECT_NOT_CONNECTED"
+#define SOCKETOBJECT_RECV_ERROR "SOCKETOBJECT_RECV_ERROR"
+
+class SocketObject {
+
+public:
+
+    static const int MAXRECV = 1024;
+
+
+private:
+
+#if defined(__msdos__) || defined(WIN32)
+    SOCKET  _skSocket;
+#else
+    int _skSocket;
+#endif
+
+
+
+public:
+
+#if defined(__msdos__) || defined(WIN32)
+    //  Konstruktor der Klasse
+    SocketObject();
+
+    // Copy-Konstruktor
+    SocketObject(SocketObject& sk);
+
+    //  Destruktor der Klasse
+    ~SocketObject();
+#endif
+
+    /******     NUR SERVER - METHODEN       ******/
+
+    /** \brief  Bindet Socket an einen Port
+    *
+    *   Die bind()-Methdode gibt dem Socket einen
+    *   lokalen Namen, damit andere Prozesse mit
+    *   ihm kommunizieren k&ouml;nnen.
+    *   Wird <b>nur vom Server</b> benutzt!
+    *
+    *   @param  iPort   Port an den gebunden werden soll
+    *
+    *   @return 0 = ok, SOCKET_ERROR    = Fehler
+    *
+    */
+    int Bind(unsigned short int iPort);
+
+    /** \brief  Wartet auf eigehende Verbindungsanfragen
+    *
+    *   Die Listen()-Methode erwartet eingehende
+    *   Verbindungsanfragen.
+    *   Wird <b>nur vom Server</b> benutzt!
+    *
+    *   @return 0 = ok, SOCKET_ERROR = Fehler
+    *
+    */
+    int Listen();
+
+    /** \brief  Acceptiert eingehende Verbindungsanfrage
+    *
+    *   Die accept()-Methode akzeptiert eine eingehende
+    *   Verbindung und "startet" die Verbindung"
+    *   Wird <b>nur vom Server</b> benutzt!
+    *
+    *   @param   skAcceptSocket  zu akzeptierender Socket
+    */
+    bool Accept(SocketObject &skAcceptSocket);
+
+    /******     SERVER - METHODEN  ENDE     ******/
+
+    /******     NUR CLIENT - METHODEN       ******/
+    /** \brief  Stellt Verbindsanfage an den Server
+    *
+    *   Mittels der connect()-Methode stellt der Client
+    *   eine Verbindungsanfrage an den Server her, der
+    *
+    *
+    *   Wird <b>nur vom Client</b> benutzt!
+    *
+    *   @param  szServerAdrress Adresse des Servers
+    *   @param  iPort   Port des Server
+    */
+    bool Connect(const char *szServerAddress, unsigned short int iPort, bool nonblock);
+
+    /** \brief  Stellt Verbindsanfage an den Server
+    *
+    *   Mittels der connect()-Methode stellt der Client
+    *   eine Verbindungsanfrage an den Server her, der
+    *   Die IP wird hier als String &uuml;bergeben
+    *   Wird <b>nur vom Client</b> benutzt!
+    *
+    *   @param  ServerAdrress Adresse des Servers
+    *   @param  iPort   Port des Server
+    */
+    bool Connect(const std::string ServerAddress, unsigned short int iPort, bool nonblock = false);
+
+    void setBlockingMode(bool nonblock);
+
+    std::string receiveLine();
+    int receiveLine(char* array, int arraySize);
+
+    /******     CLIENT - METHODEN   ENDE    ******/
+    /**
+    *   Die send()-Methode sendet Daten an den verbundenen
+    *   Socket
+    *
+    *   @param szBuffer     zu sendende Nachricht
+    *   @param iBufferLength    L&auml;ge der Nachricht
+    *   @param iFlags       Flags
+    *
+    *   @return Anzahl der gesendeten Bytes
+    */
+    int Send(const char *szBuffer, int iBufferLength, int iFlags);
+
+    /** \brief  Senden eines Strings
+    *
+    *   Die Methode sendet einen String an den Kommunikationspartner
+    *
+    *   @param  toSend  String, der gesendet werden soll
+    *
+    *   @return Anzahl der gesendeten Bytes
+    */
+    int Send(const std::string toSend);
+
+    /** \brief  Empf&auml;ngt Characters
+    *
+    *   @param szBuffer     hier wird die Nachricht reingeschrieben
+    *   @param iBufferLength    max. L&auml;ge der Nachricht
+    *   @param iFlags       Flags
+    *
+    *   @return Anzahl der gesendeten Bytes
+    */
+    int Recv(char *szBuffer, int iBufferLength, int iFlags);
+    int Recv (std::string& s);
+
+    void Disconnect();
+
+
+};
+
+#endif

lwrserv/StringTool.cpp

@@ -0,0 +1,36 @@
+#include "header.h"
+
+void StringTool::String2CmdArg(string s, string & cmd, string & arg)
+{
+    string message = stripWhiteSpace(s);
+    if (message.find_first_of(STRING_WHITESPACES, 1) != string::npos)
+    {
+        cmd = stripWhiteSpace(message.substr(0, message.find_first_of(STRING_WHITESPACES, 1)));
+        arg = stripWhiteSpace(message.substr(message.find_first_of(STRING_WHITESPACES, 1)));
+    } else 
+    {
+        cmd = message;
+        arg = "";
+    }
+    return;
+}
+
+string StringTool::stripWhiteSpace(string s)
+{
+    unsigned int i, j;
+    if (s.length() > 0)
+    {
+        i = s.find_first_not_of(STRING_WHITESPACES);
+        j = s.find_last_not_of(STRING_WHITESPACES);
+
+        if (i == string::npos)
+            i = 0;
+        if (j == string::npos)
+            j = s.length()-1;
+
+        return string(s, i, j-i+1);
+    }
+    return string("");
+}
+
+

lwrserv/StringTool.h

@@ -0,0 +1,13 @@
+#include <string>
+
+#ifndef STRING_TOOL
+#define STRING_TOOL 
+
+class StringTool
+{
+private:
+public:
+    void String2CmdArg(std::string s, std::string & cmd, std::string & arg);
+    std::string stripWhiteSpace(std::string s);
+};
+#endif

lwrserv/SvrHandling.h

@@ -0,0 +1,61 @@
+#include "defines.h"
+#include "StringTool.h"
+#include <string>
+#include "friremote.h"
+
+#ifndef SVR_HANDLING
+#define SVR_HANDLING
+
+// Serve status
+typedef enum {waiting, handshake, accepted, done} svr_state;
+
+class SvrHandling : private StringTool
+{
+private:
+    svr_state c_state;
+    //Handshake
+    bool handshakeAccepted(SocketObject& client);
+    //Handle client
+    void handle(SocketObject& client);
+    //Handling request for current Joint Values
+    void GetPositionJoints(SocketObject& client);
+    //Get Position as POSE Matrix
+    void GetPositionHomRowWise(SocketObject& client);
+    //Get Force/torque values from TCP
+    void GetForceTorqueTcp(SocketObject& client);
+    //Move to given Joint combination
+    void MovePTPJoints(SocketObject& client, std::string& args);
+    //Move to given POSE position
+    void MoveHomRowWiseStatus(SocketObject& client, std::string& args);
+    //Set Velocity
+    void SetSpeed(SocketObject& client, std::string& args);
+    //Set Acceleration
+    void SetAccel(SocketObject& client, std::string& args);
+    //Starting Potential Field Movement Mode
+    void StartPotFieldMode(SocketObject& client, std::string& args);
+    //Stopping Potential Field Movement Mode
+    void StopPotFieldMode(SocketObject& client, std::string& args);
+    //Setting Target Position HomRowWise for Potential Move
+    void SetPos(SocketObject& client, std::string& args);
+    //Setting Target Position as Joints for Potential Move
+    void SetJoints(SocketObject& client, std::string& args);
+    //Cartesian Movement
+    //Move to given POSE position
+    void MoveCartesian(SocketObject& client, std::string& args);
+    //Do Ultrasound
+    void DoUltrasound(SocketObject& client, std::string& args);
+    //Quit
+    void quit(SocketObject& client);
+    //DEBUGGING
+    void debug(SocketObject& client);
+public:
+    //Constructor
+    SvrHandling();
+    //Destructor
+    virtual ~SvrHandling();
+    //Rund server @port
+    void run(int port);
+    //Get current timestamp
+    std::string timestamp();
+};
+#endif

lwrserv/defines.h

@@ -0,0 +1,38 @@
+
+#ifndef __DEFINES_H__
+#define __DEFINES_H__
+
+#define SVR_NAME			"lwrsvr 4.11d"
+#define SVR_DEFAULT_PORT	8000
+#define SVR_HANDSHAKE		"Hello Robot"
+#define SVR_HELLO_MSG		std::string("welcome to ") + SVR_NAME + ("!")
+#define SVR_ACCEPTED		std::string("accepted")
+#define SVR_FAILED			std::string("Failed.")
+#define STRING_WHITESPACES  " \t\r\n"
+#define SVR_TRUE_RSP		"true"
+#define SVR_FALSE_RSP		"false"
+#define M_PI				3.141592654f
+
+
+
+
+//Begin SVR_COMMANDS
+#define CMD_GetPositionJoints			"GPJ"
+#define CMD_GetPositionHomRowWise		"GPHRW"
+#define CMD_GetForceTorqueTcp			"GFT"
+#define CMD_MovePTPJoints				"MPTPJ"
+#define CMD_MoveHomRowWiseStatus		"MHRWS"
+#define CMD_SetSpeed					"SS"
+#define CMD_SetAccel					"SA"
+#define CMD_StartPotFieldMode			"STPF"
+#define CMD_StopPotFieldMode			"SPPF"
+#define CMD_SetPos						"SP"
+#define CMD_SetJoints					"SJ"
+#define CMD_QUIT						"Quit"
+#define CMD_ISKUKA						"IsKukaLWR"
+#define CMD_MoveCartesian				"MC"
+#define CMD_DoUltrasound				"DU"
+//End SVR_COMMANDS
+
+#endif /* __DEFINES_H__ */
+

lwrserv/friComm.h

@@ -0,0 +1,504 @@
+/*{{[PH]
+****************************************************************************
+ Project:  FRI
+
+  This material is the exclusive property of KUKA Roboter GmbH 
+  and must be returned to KUKA Roboter GmbH immediately upon 
+  request.  This material and the information illustrated or 
+  contained herein may not be used, reproduced, stored in a 
+  retrieval system, or transmitted in whole or in part in any 
+  way - electronic, mechanical, photocopying, recording, or 
+  otherwise, without the prior written consent of KUKA Roboter GmbH.  
+  
+                 All Rights Reserved
+                 Copyright (C)  2009
+                  KUKA Roboter GmbH
+                  Augsburg, Germany
+  
+[PH]}}
+*/
+
+/*
+{{[FH]
+****************************************************************************
+  friComm.h
+       
+      NOTE: FRI (Fast Research inteface) is subject to radical change
+	  
+      
+[FH]}}
+*/ 
+/** 
+ \author (Andreas Stemmer,DLR)
+ \author (Guenter Schreiber,KUKA)
+  \file
+  \brief   This header file describes the basic communcation structures for
+   the FastRobotInterface
+  
+  
+   - all data is transmitted in Intel byte order (little endian)
+   - structs use 4 byte padding
+   - all floats are IEEE 754 single precision floats with 4 bytes
+   - for the sequence counters in the header, the number following
+     #FRI_UINT16_MAX is assumed to be zero (normal integer overflow)
+   - For Checking, that your compiler setup interprets the binary communication as expceted,
+   use the macros #FRI_CHECK_SIZES_OK, #FRI_PREPARE_CHECK_BYTE_ORDER, #FRI_CHECK_BYTE_ORDER_OK in your code.
+     
+ */
+/*****************************************************************************
+
+ friComm.h
+
+ created: 09/05/26  by  Andreas Stemmer
+
+ $Revision: 1.17 $  $Author: stemmer $    $Date: 2010/02/12 15:15:18 $   
+
+ (C) by Institute of Robotics and Mechatronics, DLR
+
+ Description:
+
+   This header file describes the basic communcation structures for
+   the FastRobotInterface
+
+ Note:
+
+   
+
+ *****************************************************************************/
+
+#ifndef FRICOMM_H
+#define FRICOMM_H
+
+
+#define LBR_MNJ 7				/*!< Number of Managed Joints for Light Weight Robot */
+#define FRI_USER_SIZE 16   		/*!< Number of Userdefined data, which are exchanged to KRL Interpreter  */
+#define FRI_CART_FRM_DIM 12     /*!< Number of data within a 3x4 Cartesian frame - homogenous matrix */
+#define FRI_CART_VEC  6			/*!< Number of data for a Cartesian vector */
+
+
+typedef unsigned char  fri_uint8_t;  /*!< 1 byte unsigned */
+typedef unsigned short fri_uint16_t; /*!< 2 byte unsigned */
+typedef unsigned int   fri_uint32_t; /*!< 4 byte unsigned */
+typedef int            fri_int32_t;  /*!< 4 byte signed */
+typedef float          fri_float_t;  /*!< 4 byte IEEE 754 float */
+#define FRI_UINT16_MAX ((1<<16) - 1) /*!< Biggest UINT16 w.r.t the interface - porting isno fun */
+
+
+/** header data used in every packet in both directions
+ *  
+ *  */
+typedef struct
+{
+    /** sequence counter (increasing with every packet) of the sender
+      Used for detecting packet loss of UDP Transfer,
+      put in your code s.th. like
+     * fri_uint16_t seqCount;
+     * seqCount++;
+     * cmd.head.sendSeqCount=seqCount;
+     */
+    fri_uint16_t sendSeqCount;
+
+    /** reflected sequence counter (mirrored from last received packet)
+       Used for detecting packet loss of UDP transfer and latency statistics 
+       \sa tFriIntfStatistics
+     * remote side: put s.th. like
+     * cmd.head.reflSeqCount=msr.head.sendSeqCount;
+      */
+    fri_uint16_t reflSeqCount;
+
+    /** overall packet size 
+       Remote side:  FRI_CMD_DATA_SIZE
+      */
+    fri_uint16_t packetSize;
+
+    /** unique datagram id to detect version problems etc.
+        Remote side: FRI_DATAGRAM_ID_CMD
+      */
+    fri_uint16_t datagramId;
+} tFriHeader;
+#define FRI_HEADER_SIZE 8
+
+
+
+
+/** data for direct interaction with KRL */
+typedef struct
+{
+    /** 16 float values (corresponds to $FRI_TO_REA[] and $FRI_FRM_REA[]) */
+    fri_float_t realData[FRI_USER_SIZE];
+
+    /** 16 int values (corresponds to $FRI_TO_INT[] and $FRI_FRM_INT[]) */
+    fri_int32_t intData[FRI_USER_SIZE];
+
+    /** 16 bool values, stored in one int */
+    fri_uint16_t boolData;
+
+    /** manual padding to a multiple of 4 bytes */
+    fri_uint16_t fill;
+} tFriKrlData;
+#define FRI_KRL_DATA_SIZE 132
+
+
+
+
+/** reported communication statistics (average rating over the last
+    100 packets) */
+typedef struct
+{
+    /** avg. answer rate (should be close to 1) */
+    fri_float_t answerRate;
+
+    /** avg. latency in seconds (should be as small as possible) */
+    fri_float_t latency;
+
+    /** avg. jitter in seconds (should be as small as possible) */
+    fri_float_t jitter;
+
+    /** avg. missing answer packets (should be as small as possible) */
+    fri_float_t missRate;
+
+    /** absolute missing answer packets */
+    fri_uint32_t missCounter;
+} tFriIntfStatistics;
+#define FRI_INTF_STATISTICS_SIZE 20
+
+
+
+
+/** feedback about the interface itself */
+typedef struct
+{
+    /** current system time in seconds */
+    fri_float_t timestamp;
+
+    /** state of interface (monitor or command FRI_STATE) */
+    fri_uint16_t state;
+
+    /** quality of communication (FRI_QUALITY) */
+    fri_uint16_t quality;
+
+    /** configured sample time for sent measurment packets */
+    fri_float_t desiredMsrSampleTime;
+    
+    /** configured sample time for received command packets */
+    fri_float_t desiredCmdSampleTime;
+
+    /** configured safety limits */
+    fri_float_t safetyLimits;
+
+    /** communication statistics */
+    tFriIntfStatistics stat;
+} tFriIntfState;
+#define FRI_INTF_STATE_SIZE (20 + FRI_INTF_STATISTICS_SIZE)
+
+
+
+
+/** these are the states that are reported */
+typedef enum
+{
+	FRI_STATE_INVALID =-1, /**< State invalid e.g. not initialized */
+    FRI_STATE_OFF = 0,  /**< internal state only (no active interface) */
+    FRI_STATE_MON = 1,  /**< FRI is in monitor mode */
+    FRI_STATE_CMD = 2   /**< FRI is in command mode */
+} FRI_STATE;
+
+
+
+
+/** quality of the connection is classified in four steps */
+typedef enum
+{
+	FRI_QUALITY_INVALID =-1, /**<  not allowed, since improber initialized */
+    FRI_QUALITY_UNACCEPTABLE = 0, /**< commanding is not allowed */
+    FRI_QUALITY_BAD = 1,          /**< commanding is allowed */
+    FRI_QUALITY_OK = 2,           /**< commanding is allowed */
+    FRI_QUALITY_PERFECT = 3       /**< commanding is allowed */
+} FRI_QUALITY;
+
+
+
+
+/** currently active controller */
+typedef enum
+{
+    /** only joint position can be commanded */
+    FRI_CTRL_POSITION = 1,
+    /** joint/cart positions, joint/cart stiffness, joint/cart damping
+        and additional TCP F/T can be commanded */
+    FRI_CTRL_CART_IMP = 2,
+    /** joint positions, stiffness and damping and additional joint
+        torques can be commanded */
+    FRI_CTRL_JNT_IMP = 3,
+    /** for all the other modes... */
+    FRI_CTRL_OTHER = 0
+} FRI_CTRL;
+
+
+
+
+/** feedback about the robot */
+typedef struct
+{
+    /** power state of drives (bitfield) */
+    fri_uint16_t power;
+
+    /** selected control strategy (FRI_CTRL) */
+    fri_uint16_t control;
+
+    /** drive errors (leads to power off) */
+    fri_uint16_t error;
+
+    /** drive warnings */
+    fri_uint16_t warning;
+
+    /** temperature of drives */
+    fri_float_t temperature[LBR_MNJ];
+} tFriRobotState;
+#define FRI_ROBOT_STATE_SIZE 36
+
+
+
+
+/** current robot data */
+typedef struct
+{
+    /** measured joint angles (in rad) 
+     * KRL: $AXIS_ACT_MSR
+     * */
+    fri_float_t msrJntPos[LBR_MNJ];
+
+    /** measured Cartesian frame (in m)
+     * KRL: $POS_ACT_MSR
+     * Reference: Base and tool are specified by $stiffness.base, $stiffness.tool 
+     * */
+    fri_float_t msrCartPos[FRI_CART_FRM_DIM];
+
+    /** commanded joint angle (in rad, before FRI)
+     * KRL: $AXIS_ACT_CMD 
+     * */
+    fri_float_t cmdJntPos[LBR_MNJ];
+
+    /** commanded joint offset (in rad, due to FRI) */
+    fri_float_t cmdJntPosFriOffset[LBR_MNJ];
+
+    /** commanded Cartesian frame (in m, before FRI) 
+     * KRL: $POS_ACT_CMD
+     * Reference: Base and tool are specified by $stiffness.base, $stiffness.tool 
+     * */
+    fri_float_t cmdCartPos[FRI_CART_FRM_DIM];
+
+    /** commanded Cartesian frame (in m, due to FRI) */
+    fri_float_t cmdCartPosFriOffset[FRI_CART_FRM_DIM];
+
+    /** measured joint torque (in Nm)
+     * KRL $TORQUE_AXIS_ACT
+     *  */
+    fri_float_t msrJntTrq[LBR_MNJ];
+
+    /** estimated external torque (in Nm) 
+     * KRL: $TORQUE_AXIS_EST
+     * */
+    fri_float_t estExtJntTrq[LBR_MNJ];
+
+    /** estimated TCP force/torque (N, Nm) 
+      KRL: $TORQUE_TCP_EST
+     - reference frame: $STIFFNESS.TASKFRAME
+     - Layout: Fx, Fy, Fz, Tz, Ty, Tx  
+     */
+    fri_float_t estExtTcpFT[FRI_CART_VEC];
+
+    /** Jacobian matrix  
+     * reference frame: $STIFFNESS.TASKFRAME
+     * row major (#FRI_CART_VEC x #LBR_MNJ)
+     * You should be able to cast it directly into a C-matrix with the layout of
+       J[FRI_CART_VEC][LBR_MNJ]
+     *  Or copy it like 
+      for ( int i = 0; i < FRI_CART_VEC; i++) 
+        for ( int j = 0; j < LBR_MNJ; j++) 
+			J[i][j] = jacobian[i*LBR_MNJ+j]
+     * Interpretation of the colums [ _X_ _Y_ _Z_ _RZ_ _RY_ _RX_]
+     * The Jacobian is generated by "geometric" reasoning with a corresponding physical
+      interpretation of instantaneous angular velocity
+      */
+    fri_float_t jacobian[FRI_CART_VEC*LBR_MNJ];
+
+    /** mass matrix */
+    fri_float_t massMatrix[LBR_MNJ*LBR_MNJ];
+
+    /** gravity (in m/s^2) */
+    fri_float_t gravity[LBR_MNJ];
+} tFriRobotData;
+#define FRI_ROBOT_DATA_SIZE (175*4) /**< Binary size of #tFriRobotData */
+
+
+
+/** Identification constant for the structure #tFriMsrData 
+    Note: This datagram id will be abused for versioning as well */
+#define FRI_DATAGRAM_ID_MSR 0x2006
+/** Data that is sent from the KRC to the external controller. 
+    The structure is identified by #FRI_DATAGRAM_ID_MSR
+  */
+typedef struct
+{
+    /** the header */
+    tFriHeader head;
+
+    /** data from KRL */
+    tFriKrlData krl;
+
+    /** state of interface */
+    tFriIntfState intf;
+
+    /** robot state */
+    tFriRobotState robot;
+    
+    /** robot data */
+    tFriRobotData data;
+} tFriMsrData;
+#define FRI_MSR_DATA_SIZE (FRI_HEADER_SIZE + FRI_KRL_DATA_SIZE +    \
+                           FRI_INTF_STATE_SIZE + FRI_ROBOT_STATE_SIZE + \
+                           FRI_ROBOT_DATA_SIZE)
+
+
+
+
+/** The bitfield is subject to change, any use of hardcoded constants
+    is NOT RECOMMENDED! Use the provided defines instead! */
+#define FRI_CMD_JNTPOS    0x0001
+// Currently not supported #define FRI_CMD_JNTVEL    0x0002
+#define FRI_CMD_JNTTRQ    0x0004
+#define FRI_CMD_JNTSTIFF  0x0010
+#define FRI_CMD_JNTDAMP   0x0020
+
+#define FRI_CMD_CARTPOS   0x0100
+//Currently not supported #define FRI_CMD_CARTVEL   0x0200
+#define FRI_CMD_TCPFT     0x0400
+#define FRI_CMD_CARTSTIFF 0x1000
+#define FRI_CMD_CARTDAMP  0x2000
+
+
+
+
+/** new robot commands */
+typedef struct
+{
+    /** bitfield which selects which commanded values are relevant */
+    fri_uint32_t cmdFlags;
+
+    /** commanded joint angle (in rad) 
+      #FRI_CMD_JNTPOS
+     */
+    fri_float_t jntPos[LBR_MNJ];
+
+    /** commanded Cartesian frame (in m) 
+      #FRI_CMD_CARTPOS
+      */
+    fri_float_t cartPos[FRI_CART_FRM_DIM];
+
+    /** commanded additional joint torque (in Nm) 
+      #FRI_CMD_JNTTRQ*/
+    fri_float_t addJntTrq[LBR_MNJ];
+
+    /** commanded additional TCP force/torque (N, Nm) 
+      #FRI_CMD_TCPFT
+     * reference frame: $STIFFNESS.TASKFRAM 
+     * Layout [Fx, Fy, Fz, Tz, Ty, Tx]*/
+    fri_float_t addTcpFT[FRI_CART_VEC];
+
+    /** joint stiffness (Nm/rad) 
+      #FRI_CMD_JNTSTIFF */
+    fri_float_t jntStiffness[LBR_MNJ];
+
+    /** joint damping (normalized) 
+      #FRI_CMD_JNTDAMP*/
+    fri_float_t jntDamping[LBR_MNJ];
+    
+    /** Cartesian stiffness (N/m, Nm/rad)
+      * #FRI_CMD_CARTSTIFF 
+      * Layout [Cx, Cy, Cz, Ca(Rz), Cb(Ry), Cc(Rx)]*/
+    fri_float_t cartStiffness[FRI_CART_VEC];
+
+    /** Cartesian damping (normalized) 
+      * #FRI_CMD_CARTDAMP
+      * Layout [Dx, Dy, Dz, Da(Rz), Db(Ry), Dc(Rx)]*/
+    fri_float_t cartDamping[FRI_CART_VEC];
+} tFriRobotCommand;
+#define FRI_ROBOT_COMMAND_SIZE (59*4)
+
+
+
+/** Symbolic define to identify #tFriCmdData 
+  Note: This datagram id will be abused for versioning as well */
+#define FRI_DATAGRAM_ID_CMD 0x1005
+/** The commanding structure, which is sent to the KRC Side.
+	Identified by #FRI_DATAGRAM_ID_CMD */
+typedef struct
+{
+    /** the header */
+    tFriHeader head;
+
+    /** data to KRL */
+    tFriKrlData krl;
+
+    /** robot commands */
+    tFriRobotCommand cmd;
+} tFriCmdData;
+#define FRI_CMD_DATA_SIZE (FRI_HEADER_SIZE + FRI_KRL_DATA_SIZE + FRI_ROBOT_COMMAND_SIZE)
+
+
+
+
+/** a convenience macro for the user to check if padding on the used
+    platform is as expected: if (!FRI_CHECK_SIZES_OK) ... */
+#define FRI_CHECK_SIZES_OK \
+    ((sizeof(tFriHeader)         == FRI_HEADER_SIZE) && \
+     (sizeof(tFriKrlData)        == FRI_KRL_DATA_SIZE) && \
+     (sizeof(tFriIntfStatistics) == FRI_INTF_STATISTICS_SIZE) && \
+     (sizeof(tFriIntfState)      == FRI_INTF_STATE_SIZE) && \
+     (sizeof(tFriRobotState)     == FRI_ROBOT_STATE_SIZE) && \
+     (sizeof(tFriRobotData)      == FRI_ROBOT_DATA_SIZE) && \
+     (sizeof(tFriRobotCommand)   == FRI_ROBOT_COMMAND_SIZE) && \
+     (sizeof(tFriMsrData)        == FRI_MSR_DATA_SIZE) && \
+     (sizeof(tFriCmdData)        == FRI_CMD_DATA_SIZE))
+
+
+
+
+/** convenience macros for the user to check if byte order and float
+    representation on the used platform is as expected:
+    {
+        FRI_PREPARE_CHECK_BYTE_ORDER
+        if (!FRI_CHECK_BYTE_ORDER_OK) ...
+    }*/
+#define FRI_PREPARE_CHECK_BYTE_ORDER \
+    union {fri_uint32_t a; fri_uint8_t b[4]; fri_float_t c;} _friTestByteOrder;   \
+    _friTestByteOrder.a = 0x40490FDB;
+	/** convenience macros for the user to check if byte order and float
+		representation on the used platform is as expected:
+		{
+			FRI_PREPARE_CHECK_BYTE_ORDER
+			if (!FRI_CHECK_BYTE_ORDER_OK) ...
+		}*/	
+#define FRI_CHECK_BYTE_ORDER_OK \
+    ((_friTestByteOrder.a == 0x40490FDB) &&\
+     (_friTestByteOrder.b[0] == 0xDB) &&\
+     (_friTestByteOrder.b[1] == 0x0F) &&\
+     (_friTestByteOrder.b[2] == 0x49) &&\
+     (_friTestByteOrder.b[3] == 0x40) &&\
+     (_friTestByteOrder.c > 3.141592) &&\
+     (_friTestByteOrder.c < 3.141593))
+
+
+/**! Versioning Major Version */   
+#define FRI_MAJOR_VERSION 1
+/**! Versioning Minor sub version for minor fixes */   
+#define FRI_SUB_VERSION 0
+#define EXPAND_CONCAT(X1,X2,X3,X4,X5,X6,X7) X1 ## X2 ## X3 ## X4 ## X5 ## X6 ## X7
+#define CONCAT(name1,name2,name3,name4) EXPAND_CONCAT(#name1,".",#name2,".",#name3,".",#name4)
+
+/**! The Versionstring with all significant information */   
+#define FRI_VERSION_STRING CONCAT(FRI_MAJOR_VERSION, FRI_SUB_VERSION,FRI_DATAGRAM_ID_CMD,FRI_DATAGRAM_ID_MSR)
+//"." #F "." #FRI_DATAGRAM_ID_MSR
+
+#endif  /* FRICOMM_H */

lwrserv/friremote.cpp

@@ -0,0 +1,288 @@
+/*{{[PH]
+****************************************************************************
+ Project:  FRI
+
+  This material is the exclusive property of KUKA Roboter GmbH 
+  and must be returned to KUKA Roboter GmbH immediately upon 
+  request.  This material and the information illustrated or 
+  contained herein may not be used, reproduced, stored in a 
+  retrieval system, or transmitted in whole or in part in any 
+  way - electronic, mechanical, photocopying, recording, or 
+  otherwise, without the prior written consent of KUKA Roboter GmbH.  
+  
+                 All Rights Reserved
+                 Copyright (C)  2009
+                  KUKA Roboter GmbH
+                  Augsburg, Germany
+  
+[PH]}}
+*/
+
+/*
+{{[FH]
+****************************************************************************
+  friRemote.cpp
+       
+      NOTE: This sample, as the corresponding FRI (Fast Research inteface) is subject to radical change
+	  
+      
+[FH]}}
+*/ 
+/**
+	\addtogroup friRemoteLib
+	\brief Library for FRI (FastResearchInterface) 
+*/
+/* @{ */
+
+/** *************************************************************************
+ \author (Guenter Schreiber)
+\file
+\brief  FRI Remote class encapsulating UDP package handshakes
+ *                                                                         *
+ ***************************************************************************/
+#include "friremote.h"
+
+#include <iostream>
+#include <stdlib.h>
+
+friRemote::friRemote(int port,char *hintToRemoteHost):remote(port,hintToRemoteHost)
+{
+	//std::cout << __FUNCTION__ << " " <<port <<std::endl;
+	//std::cout << "FRI Version " << FRI_MAJOR_VERSION << "." << FRI_SUB_VERSION << "." <<FRI_DATAGRAM_ID_CMD << "." <<FRI_DATAGRAM_ID_MSR << std::endl;
+	memset((void*)(& msr),0x0,sizeof(msr));
+	memset((void*)(& cmd), 0x0,sizeof(cmd));
+	{
+		// do checks, whether the interface - and the host meets the requirements
+		FRI_PREPARE_CHECK_BYTE_ORDER;
+		if (!FRI_CHECK_BYTE_ORDER_OK) 
+		{
+			std::cerr << __FUNCTION__<<"Byte order on your system is not appropriate - expect deep trouble" << std::endl;
+		}
+		if (!FRI_CHECK_SIZES_OK)
+		{
+			std::cout << __FUNCTION__<<"Sizes of datastructures not appropriate - expect even deeper trouble" << std::endl;
+		}
+	}
+}
+
+friRemote::~friRemote()
+{
+	std::cout << __FUNCTION__ << " bye for now "  << std::endl;
+}
+
+int friRemote::doReceiveData()
+{
+	return remote.Recv(&msr);
+}
+
+
+/** Data Exchanger -- normally update within access routine implicitely ... */
+int friRemote::doSendData()
+{
+  // received at least something 
+   seqCount++;
+   cmd.head.sendSeqCount = seqCount;
+   cmd.head.reflSeqCount = msr.head.sendSeqCount;
+   cmd.head.datagramId = FRI_DATAGRAM_ID_CMD;
+   cmd.head.packetSize = sizeof(tFriCmdData);
+    int rc=remote.Send(&cmd);
+  return rc;
+}
+
+/** send commands based on last msr datagram and 
+    receives a new one
+*/
+int friRemote::doDataExchange()
+{
+	doSendData();
+	return doReceiveData();
+}
+
+/** automatically do data exchange, if not otherwise specified */
+int friRemote::doPositionControl(float newJntPosition[LBR_MNJ], bool flagDataExchange)
+{
+	// Helper, if not properly initialized or the like...
+
+	cmd.cmd.cmdFlags=FRI_CMD_JNTPOS;
+	// Note:: If the interface is not in Command mode,
+	// The commands have to be "mirrored" to get in sync
+	// Note:: If the interface is not in Command mode,
+	// The commands have to be "mirrored" to get in sync
+	if ((getState() != FRI_STATE_CMD) || (!isPowerOn()))
+	{
+		for (int i = 0; i < LBR_MNJ; i++)
+		{
+			cmd.cmd.jntPos[i]=msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i];
+		}
+	}
+	else
+	{
+		// compute new values here ...
+		for (int i = 0; i < LBR_MNJ; i++)
+		{
+			cmd.cmd.jntPos[i]=newJntPosition[i];
+		}
+	}
+
+	if (flagDataExchange)
+	{
+		return doDataExchange();
+	}
+	return 1;
+}
+
+
+/** automatically do data exchange, if not otherwise specified 
+ if flagDataExchange is set to false, call doDataExchange() 
+ or doReceiveData()/doSendData() on your own
+ IN: newJntPosition   - joint positions
+	 newJntStiff      - joint stiffness (Spring factor)
+	 newJntDamp       - joint damping   (Damping factor)
+	 newJntAddTorque  - additional torque 
+       
+ Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the 
+	   value is ignored -> the respective  cmd.cmd.cmdFlags field is set properly
+ Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !!
+ */
+int friRemote::doJntImpedanceControl(const float newJntPosition[LBR_MNJ], 
+										const float newJntStiff[LBR_MNJ], 
+										const float newJntDamp[LBR_MNJ], 
+										const float newJntAddTorque[LBR_MNJ],
+										bool flagDataExchange)
+
+{
+	// Helper, if not properly initialized or the like...
+	cmd.cmd.cmdFlags=0;
+	if (newJntPosition)
+	{
+		cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS;
+		// Note:: If the interface is not in Command mode,
+		// The commands have to be "mirrored" to get in sync
+		if ((getState() != FRI_STATE_CMD) || (!isPowerOn()))
+		{
+			for (int i = 0; i < LBR_MNJ; i++)
+			{
+				cmd.cmd.jntPos[i]=msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i];
+			}
+		}
+		else
+		{
+			// compute new values here ...
+			for (int i = 0; i < LBR_MNJ; i++)
+				cmd.cmd.jntPos[i]=newJntPosition[i];
+		}
+	}
+	if (newJntStiff)
+	{
+		cmd.cmd.cmdFlags|=FRI_CMD_JNTSTIFF;
+		for (int i = 0; i < LBR_MNJ; i++)
+			cmd.cmd.jntStiffness[i]=newJntStiff[i];
+	}
+	if (newJntDamp)
+	{
+		cmd.cmd.cmdFlags|=FRI_CMD_JNTDAMP;
+		for (int i = 0; i < LBR_MNJ; i++)
+			cmd.cmd.jntDamping[i]=newJntDamp[i];
+	}
+	if (newJntAddTorque)
+	{
+		cmd.cmd.cmdFlags|=FRI_CMD_JNTTRQ;
+		for (int i = 0; i < LBR_MNJ; i++)
+			cmd.cmd.addJntTrq[i]=newJntAddTorque[i];
+	}
+
+	if (flagDataExchange)
+	{
+		return doDataExchange();
+	}
+	return 1;
+}
+
+	/** automatically do data exchange, if not otherwise specified 
+	if flagDataExchange is set to false, call doDataExchange() 
+	or doReceiveData()/doSendData() on your own
+	IN: newJntPosition   - joint positions
+	newJntStiff      - joint stiffness (Spring factor)
+	newJntDamp       - joint damping   (Damping factor)
+	newJntAddTorque  - additional torque 
+
+	Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the 
+	value is ignored -> the respective  cmd.cmd.cmdFlags field is set properly
+	Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !!
+	*/
+int friRemote::doCartesianImpedanceControl(const float newCartPosition[FRI_CART_FRM_DIM], 
+										   const float newCartStiff[FRI_CART_VEC], 
+										   const float newCartDamp[FRI_CART_VEC], 
+										   const float newAddTcpFT[FRI_CART_VEC],
+										   const float newJntNullspace[LBR_MNJ],  
+										   bool flagDataExchange)
+{
+
+		// Helper, if not properly initialized or the like...
+	cmd.cmd.cmdFlags=0;
+	if ( newCartPosition )
+	{
+		cmd.cmd.cmdFlags|=FRI_CMD_CARTPOS;
+		for ( int i = 0; i < FRI_CART_FRM_DIM; i++)
+		{
+			cmd.cmd.cartPos[i]=newCartPosition[i];
+
+		}
+	}
+	if ( newCartStiff)
+	{
+		cmd.cmd.cmdFlags|=FRI_CMD_CARTSTIFF;
+		for ( int i = 0; i < FRI_CART_VEC; i++)
+		{
+			cmd.cmd.cartStiffness[i]=newCartStiff[i];
+
+		}
+
+	}
+	if ( newCartDamp)
+	{
+		cmd.cmd.cmdFlags|=FRI_CMD_CARTDAMP;
+			;
+		for ( int i = 0; i < FRI_CART_VEC; i++)
+		{
+			cmd.cmd.cartDamping[i]=newCartDamp[i];
+
+		}
+	}
+	if ( newAddTcpFT)
+	{
+		cmd.cmd.cmdFlags|=FRI_CMD_TCPFT;
+			;
+		for ( int i = 0; i < FRI_CART_VEC; i++)
+		{
+			cmd.cmd.addTcpFT[i]=newAddTcpFT[i];
+
+		}
+	}
+
+	if (newJntNullspace)
+	{
+		cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS;
+		// Note:: If the interface is not in Command mode,
+		// The commands have to be "mirrored" to get in sync
+		
+			// compute new values here ...
+			for (int i = 0; i < LBR_MNJ; i++)
+				cmd.cmd.jntPos[i]=newJntNullspace[i];		
+	}
+
+
+if (flagDataExchange)
+	{
+		return doDataExchange();
+	}
+	return 1;
+}
+
+
+
+
+
+
+/* @} */
+

lwrserv/friremote.h

@@ -0,0 +1,194 @@
+/* {{[PH]
+****************************************************************************
+ Project:  FRI
+
+  This material is the exclusive property of KUKA Roboter GmbH 
+  and must be returned to KUKA Roboter GmbH immediately upon 
+  request.  This material and the information illustrated or 
+  contained herein may not be used, reproduced, stored in a 
+  retrieval system, or transmitted in whole or in part in any 
+  way - electronic, mechanical, photocopying, recording, or 
+  otherwise, without the prior written consent of KUKA Roboter GmbH.  
+  
+                 All Rights Reserved
+                 Copyright (C)  2009
+                  KUKA Roboter GmbH
+                  Augsburg, Germany
+  
+[PH]}}
+*/
+
+/*
+{{[FH]
+****************************************************************************
+  friFirstApp.cpp
+       
+      NOTE: This sample, as the corresponding FRI (Fast Research inteface) is subject to radical change
+	  
+      
+[FH]}}
+*/ 
+/**
+	\defgroup friRemoteLib
+	\brief Library for FRI (FastResearchInterface) 
+*/
+/* @{ */
+
+/** 
+ \author (Guenter Schreiber)
+\file
+\brief Remote class for handshaking/dealing with udp datastructures
+
+  This code is most important to understand the concepts behind data handshake
+*/
+#ifndef FRIFRIREMOTE_H
+#define FRIFRIREMOTE_H
+
+
+#include "friudp.h"
+
+
+
+
+/**
+	@author Günter Schreiber <guenter@jacobus>
+*/
+
+class friRemote
+{
+public:
+	friRemote(int port = FRI_DEFAULT_SERVER_PORT, char * hintToRemoteHost=NULL);
+	~friRemote();
+
+	/** Data Exchanger -- normally update within access routine implicitely 
+	send commands based on last datagram and after waits on new measurement
+	calls doSendData() and doReceiveData();
+	... */
+	int doDataExchange();
+	/** Receives data while calling friUdp::Recv()
+	The call will block..
+	*/
+	int doReceiveData();
+	/** Sends the data */
+	int doSendData();
+		/* @{ */
+	/** automatically do data exchange, if not otherwise specified 
+	if flagDataExchange is set to false, call doDataExchange() 
+	or doReceiveData()/doSendData() on your own
+	*/
+	int doPositionControl(float newJntPosition[LBR_MNJ], bool flagDataExchange=true);
+
+	/** automatically do data exchange, if not otherwise specified 
+	if flagDataExchange is set to false, call doDataExchange() 
+	or doReceiveData()/doSendData() on your own
+	IN: newJntPosition   - joint positions
+	newJntStiff      - joint stiffness (Spring factor)
+	newJntDamp       - joint damping   (Damping factor)
+	newJntAddTorque  - additional torque 
+
+	Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the 
+	value is ignored -> the respective  cmd.cmd.cmdFlags field is set properly
+	Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !!
+	*/
+	int doJntImpedanceControl(const float newJntPosition[LBR_MNJ], const float newJntStiff[LBR_MNJ] = NULL, const float newJntDamp[LBR_MNJ]=NULL, const float newJntAddTorque[LBR_MNJ]=NULL,bool flagDataExchange=true);
+
+
+	/** automatically do data exchange, if not otherwise specified 
+	if flagDataExchange is set to false, call doDataExchange() 
+	or doReceiveData()/doSendData() on your own
+	IN: newJntPosition   - joint positions
+	newJntStiff      - joint stiffness (Spring factor)
+	newJntDamp       - joint damping   (Damping factor)
+	newJntAddTorque  - additional torque 
+
+	Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the 
+	value is ignored -> the respective  cmd.cmd.cmdFlags field is set properly
+	Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !!
+	*/
+	int doCartesianImpedanceControl(const float newCartPosition[FRI_CART_FRM_DIM], 
+		const float newCartStiff[FRI_CART_VEC]=NULL, 
+		const float newCartDamp[FRI_CART_VEC]=NULL, 
+		const float newAddTcpFT[FRI_CART_VEC]=NULL,
+		const float newJntNullspace[LBR_MNJ]=NULL,  bool flagDataExchange=true);
+		/* @} */
+
+	/** measured Cartesian frame (in m)
+	KRL: $POS_ACT_MSR
+	Reference: Base and tool are specified by $stiffness.base, $stiffness.tool 
+	*/
+	float * getMsrCartPosition() { return msr.data.msrCartPos; }
+	/** commanded Cartesian frame (in m, before FRI) 
+	KRL: $POS_ACT_CMD
+	Reference: Base and tool are specified by $stiffness.base, $stiffness.tool 
+	*/
+	float * getMsrCmdCartPosition() { return msr.data.cmdCartPos; }
+
+	/** commanded Cartesian frame (in m, due to FRI) */
+	float * getMsrCmdCartPosFriOffset() { return msr.data.cmdCartPosFriOffset; }
+
+	/** Access to inner buffers for further manipulation */
+	tFriMsrData & getMsrBuf() { return msr;}
+	tFriCmdData & getCmdBuf() { return cmd;}
+	/* @{ */
+	/** interpretational access routines */
+	FRI_STATE getState()      { return (FRI_STATE)msr.intf.state;   }
+	FRI_QUALITY getQuality()  { return (FRI_QUALITY)msr.intf.quality; }
+	FRI_CTRL getCurrentControlScheme (){ return (FRI_CTRL)msr.robot.control; }
+
+	bool isPowerOn() { return msr.robot.power!=0; }
+	/* @} */
+
+	/* @{ */
+	/** Important value for superposition - and during poweroff stages, to become command mode */
+	float * getMsrCmdJntPosition() { return msr.data.cmdJntPos; }
+	/** returns the offset, which is commanded by FRI Remote side 
+	*  Complete desired position inside LBR Kernel is the sum of cmdJntPos and cmdJntPosFriOffset */
+	float * getMsrCmdJntPositionOffset() { return msr.data.cmdJntPosFriOffset; }
+
+	void getCurrentCmdJntPosition( float jntVec[LBR_MNJ] ) { for ( int i = 0; i < LBR_MNJ; i++) jntVec[i]= msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i];}
+	/** Current measured jnt position of the robot */
+	float * getMsrMsrJntPosition() { return msr.data.msrJntPos; }
+	float * getMsrEstExtJntTrq() { return msr.data.estExtJntTrq; }
+	float * getMsrJntTrq() { return msr.data.msrJntTrq; }
+		/* @} */
+
+	float getSampleTime() { return msr.intf.desiredCmdSampleTime; }
+	int getSequenceCount() { return seqCount; }
+
+	/* @{ */
+	/** KRL Interaction -- Corresponds to $FRI_TO_REA */
+	float getFrmKRLReal(int index) { return msr.krl.realData[index]; }
+	/** KRL Interaction -- Corresponds to $FRI_FRM_REA */
+	void  setToKRLReal(int index, float val) { cmd.krl.realData[index]=val; }
+		/** KRL Interaction -- Corresponds to $FRI_TO_INT */
+	int   getFrmKRLInt(int index) { return msr.krl.intData[index]; }
+		/** KRL Interaction -- Corresponds to $FRI_FRM_INT */
+	void  setToKRLInt(int index, int val) { cmd.krl.intData[index]=val; }
+		/** KRL Interaction -- Corresponds to $FRI_TO_BOOL */
+	bool  getFrmKRLBool(int index) { return ((msr.krl.boolData & (1<<index)) != 0);}
+		/** KRL Interaction -- Corresponds to $FRI_FRM_BOOL */
+	fri_uint16_t getFrmKRLBool() { return msr.krl.boolData; }
+		/** KRL Interaction -- Corresponds to $FRI_FRM_BOOL */
+	void  setToKRLBool(int index, bool val) 
+	{ if ( val) { cmd.krl.boolData |= (1<<index);} 
+	else {cmd.krl.boolData &= (~(1<<index));}}
+	void setToKRLBool(fri_uint16_t val) {   cmd.krl.boolData = val; }
+	/* @} */
+	/** Get Jacobian Matrix
+	*/
+	float * getJacobian() { return msr.data.jacobian; }
+	float * getFTTcp() {return msr.data.estExtTcpFT;}
+
+protected:
+	tFriMsrData msr;
+	tFriCmdData cmd;
+private:
+	friUdp remote;
+	int seqCount;
+};
+
+
+
+
+#endif
+/* @} */

lwrserv/friudp.cpp

@@ -0,0 +1,543 @@
+/*{{[PH]
+****************************************************************************
+ Project:  FRI
+
+  This material is the exclusive property of KUKA Roboter GmbH 
+  and must be returned to KUKA Roboter GmbH immediately upon 
+  request.  This material and the information illustrated or 
+  contained herein may not be used, reproduced, stored in a 
+  retrieval system, or transmitted in whole or in part in any 
+  way - electronic, mechanical, photocopying, recording, or 
+  otherwise, without the prior written consent of KUKA Roboter GmbH.  
+  
+                 All Rights Reserved
+                 Copyright (C)  2009
+                  KUKA Roboter GmbH
+                  Augsburg, Germany
+  
+[PH]}}
+*/
+
+/*
+{{[FH]
+****************************************************************************
+  friUdp.cpp
+       
+      NOTE: This sample, as the corresponding FRI (Fast Research inteface) is subject to radical change
+      
+      
+[FH]}}
+*/ 
+/**
+    \addtogroup friRemoteLib
+    \brief Library for FRI (FastResearchInterface) 
+*/
+/* @{ */
+
+/** *************************************************************************
+ \author (Guenter Schreiber)
+\file
+\brief Implementation of a UDP socket
+ *                                                                         *
+ ***************************************************************************/
+#include "friudp.h"
+
+#ifdef WIN32
+#include "SocketObject.h"
+//#pragma comment(lib, "ws2_32.lib") 
+#endif // WIN32
+
+
+friUdp::friUdp(int port, char * remoteHost) : serverPort(port)
+{
+    /* check struct sizes */
+    if (!FRI_CHECK_SIZES_OK)
+    {
+        printf("data structure size error!\n");
+        exit(1);
+    }
+
+    m_timestamp=0;
+    // Make shure, that e.g. simulink uses no stupid standard definition - e.g. 0
+    if ( serverPort < 10 )
+    {
+        serverPort=FRI_DEFAULT_SERVER_PORT; 
+    }
+
+#ifdef WIN32
+    StartWinsock();
+#endif
+    Init(remoteHost);
+}
+
+
+friUdp::~friUdp()
+{
+  Close();
+}
+
+#ifdef WIN32
+int friUdp::StartWinsock(void)
+{
+    WSADATA WSAData;
+    return WSAStartup(MAKEWORD(2,0), &WSAData);
+}
+#endif// WIN32
+
+void friUdp::Init(char * remoteHost)
+{
+    struct sockaddr_in servAddr;
+    m_timestamp = 0;
+    memset(&servAddr, 0, sizeof(servAddr));
+    memset(&krcAddr, 0, sizeof(krcAddr));
+
+    /* socket creation */
+    udpSock = socket(PF_INET, SOCK_DGRAM, 0);
+    if (udpSock < 0)
+    {
+        printf("opening socket failed!\n");
+        exit(1);
+    }
+
+#ifdef HAVE_TIME_STAMP_RECEIVE
+    {
+        int temp = 1;
+        if (setsockopt(udpSock, SOL_SOCKET, SO_TIMESTAMP, &temp, sizeof(int)) < 0)
+        {
+            printf("failed to enable receive time stamps\n");
+            exit(1);
+        }
+    }
+#endif
+
+    /* bind local server port */
+    servAddr.sin_family = AF_INET;
+    servAddr.sin_addr.s_addr = htonl(INADDR_ANY);
+    servAddr.sin_port = htons(serverPort);
+
+    if (bind(udpSock, (struct sockaddr *) &servAddr, sizeof(servAddr)) < 0)
+    {
+        printf("binding port number %d failed!\n", serverPort);
+        Close();
+        exit(1);
+    }
+
+    // if the remote host is specified, 
+    // preinitialize the socket properly
+    if ( remoteHost ) 
+    {
+        printf("preinitialized remote host to %s\n",remoteHost);
+        krcAddr.sin_addr.s_addr = inet_addr(remoteHost);
+        krcAddr.sin_family = AF_INET;
+        krcAddr.sin_port=htons(serverPort);
+    }
+
+
+#ifdef HAVE_GETHOSTNAME
+    /* get IP(s) and port number and display them (just for
+     convenience, so debugging friOpen is easier) */
+    {
+        char hostname[100];
+        struct hostent * host;
+        int i;
+
+        gethostname(hostname, sizeof(hostname));
+        host = gethostbyname(hostname);
+        for (i=0; host->h_addr_list[i]!=0; i++)
+        {
+            struct in_addr addr;
+            memcpy(&addr, host->h_addr_list[i], sizeof(addr));
+            //printf("helloIP %s - Port %d\n", inet_ntoa(addr), serverPort);
+        }
+    }
+#endif // 
+
+
+}
+
+/* reveive one packet from KRC (blocking!) */
+int friUdp::Recv(tFriMsrData *packet)
+{
+    if (udpSock >= 0)
+    {
+        int received;
+        struct timeval ts;
+        
+        received = RecvPacket(udpSock, packet, &ts, &krcAddr);
+
+        if (received == sizeof(tFriMsrData))
+        {
+#ifdef HAVE_TIME_STAMP_RECEIVE
+
+            /* FIXME: need another #ifdef for VxWorks */
+#ifdef QNX
+            struct timespec ts;
+            clock_gettime(CLOCK_REALTIME, &ts);
+            m_timestamp = (double)ts.tv_sec + (double)ts.tv_nsec/1.0e9;
+#else
+            m_timestamp = (double)ts.tv_sec + (double)ts.tv_usec/1.0e6;
+#endif // QNX
+#endif // HAVE_TIME_STAMP
+            return 0;
+        }
+        else
+        {
+            printf("received something, but wrong size %d (expected %d)...\n",received,(int) sizeof(tFriMsrData));
+            fflush(stdout);
+        }
+    }
+    memset(packet, 0, sizeof(tFriMsrData));
+    return -1;
+}
+
+
+
+/* send one answer packet to KRC */
+int friUdp::Send(tFriCmdData *data)
+{
+    krcAddr.sin_family = AF_INET;
+#ifdef KRC_IP_ADDRESS
+    krcAddr.sin_addr.s_addr = inet_addr(KRC_IP_ADDRESS);
+#endif
+#ifdef KRC_RECEIVE_PORT
+    krcAddr.sin_port = htons(KRC_RECEIVE_PORT);
+#endif
+
+    if ((udpSock >= 0) && (ntohs(krcAddr.sin_port) != 0))
+    {
+        int sent;
+        sent = sendto(udpSock, (char *) data, sizeof(tFriCmdData), 0,
+                      (struct sockaddr *)&krcAddr, sizeof(krcAddr));
+        if (sent == sizeof(tFriCmdData))
+        {
+            return 0;
+        }
+    }
+    return -1;
+}
+
+
+
+
+/* close the socket */
+void friUdp::Close(void)
+{
+    if (udpSock >= 0)
+    {
+#ifdef WIN32
+        closesocket(udpSock); 
+        WSACleanup(); 
+#else
+        close(udpSock);
+#endif
+    }
+    udpSock = -1;
+}
+
+
+#ifdef HAVE_TIME_STAMP_RECEIVE
+// Socket option SO_TIMESTAMP is supported 
+/* receive with timestamp  */
+int friUdp::RecvPacket(int fd, tFriMsrData* p, struct timeval* ts, struct sockaddr_in* client) 
+{
+    struct msghdr msg;
+    struct iovec vec[1];
+    union {
+        struct cmsghdr cm;
+        char control[20];
+    } cmsg_un;
+    struct cmsghdr *cmsg;
+    struct timeval *tv = NULL;
+    int n;
+        
+    vec[0].iov_base = p;
+    vec[0].iov_len = sizeof(*p);
+        
+    memset(&msg, 0, sizeof(msg));
+    memset(&cmsg_un, 0, sizeof(cmsg_un));
+        
+    msg.msg_name = (caddr_t)client;
+    if(client)
+        msg.msg_namelen = sizeof(*client);
+    else
+        msg.msg_namelen = 0;
+
+    msg.msg_iov = vec;
+    msg.msg_iovlen = 1;
+    msg.msg_control = cmsg_un.control;
+    msg.msg_controllen = sizeof(cmsg_un.control);
+    msg.msg_flags = 0;
+        
+    n = recvmsg(fd, &msg, 0); // MSG_DONTWAIT
+    if(n < 0)
+    {
+        perror("recvmsg");
+        return -1;
+    }
+    if(msg.msg_flags & MSG_TRUNC)
+    {
+        printf("received truncated message\n");
+        return -1;
+    }
+    if(!ts)
+        return n;
+
+    /* get time stamp of packet */
+    if(msg.msg_flags & MSG_CTRUNC)
+    {
+        printf("received truncated ancillary data\n");
+        return -1;
+    }
+    if(msg.msg_controllen < sizeof(cmsg_un.control))
+    {
+        printf("received short ancillary data (%d/%d)\n", msg.msg_controllen, (int)sizeof(cmsg_un.control));
+        return -1;
+    }
+    for(cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg))
+    {
+        if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMP)
+            tv = (struct timeval *)CMSG_DATA(cmsg);
+    }
+    if(tv)
+    {
+        ts->tv_sec = tv->tv_sec;
+        ts->tv_usec = tv->tv_usec;
+    }
+    return n;
+}
+
+#else
+/* receive with timestamp  */
+int friUdp::RecvPacket(int udpSock, tFriMsrData* data, struct timeval* ts, struct sockaddr_in* client) 
+{
+
+    if (udpSock >= 0)
+    {
+        /** HAVE_SOCKLEN_T
+            Yes - unbelieavble: There are differences in standard calling parameters (types) to recvfrom 
+            Windows winsock, VxWorks and QNX use int
+            newer Posix (most Linuxes) use socklen_t
+         */
+
+#ifdef HAVE_SOCKLEN_T
+        socklen_t sockAddrSize;
+#else
+        int sockAddrSize;
+#endif
+        int received;
+
+        sockAddrSize = sizeof(struct sockaddr_in);
+
+        received = recvfrom(udpSock, (char *) data, sizeof(tFriMsrData), 0,
+                            (struct sockaddr *)&krcAddr, &sockAddrSize);
+
+        return received;
+    }
+    return -1;
+}
+
+#endif // HAVE_TIME_STAMP_RECEIVE
+
+
+#ifdef VXWORKS //USE_BERKELEY_PACKAGE_FILTER_VXWORKS
+#define DEBUG_BPF_READ
+
+#include "vxworks.h" 
+#include "bpfDrv.h" 
+#include "ioLib.h"
+#include <logLib.h> 
+#include <sys/ioctl.h>
+//#include "drv/netif/smNetLib.h"
+#include <wrn/coreip/net/ethernet.h> 
+#include <wrn/coreip/net/if.h>
+#include <wrn/coreip/netinet/ip.h>
+#include <wrn/coreip/netinet/udp.h>
+
+#ifdef DEBUG_BPF_READ
+#include <iostream>
+#include "friremote.h"
+
+#endif
+ 
+
+/*
+ * Packet filter program...
+ *
+ * XXX: Changes to the filter program may require changes to the
+ * constant offsets used in if_register_send to patch the BPF program!
+ */
+struct bpf_insn friUpdSock_bpf_filter[] =
+{
+    /* Make sure this is an IP packet... */
+    BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 12),
+    BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ETHERTYPE_IP, 0, 8),
+
+    /* Make sure it's a UDP packet... */
+    BPF_STMT(BPF_LD + BPF_B + BPF_ABS, 23),
+    BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, IPPROTO_UDP, 0, 6),
+
+    /* Make sure this isn't a fragment... */
+    BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 20),
+    BPF_JUMP(BPF_JMP + BPF_JSET + BPF_K, 0x1fff, 4, 0),
+
+    /* Get the IP header length... */
+    BPF_STMT(BPF_LDX + BPF_B + BPF_MSH, 14),
+
+    /* Make sure it's to the right port... */
+    BPF_STMT(BPF_LD + BPF_H + BPF_IND, 16),
+    BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 67, 0, 1),      /* patch */
+
+    /* If we passed all the tests, ask for the whole packet. */
+    BPF_STMT(BPF_RET+BPF_K, (u_int)-1),
+
+    /* Otherwise, drop it. */
+    BPF_STMT(BPF_RET+BPF_K, 0),
+};
+
+int friUpdSock_bpf_filter_len = sizeof(friUpdSock_bpf_filter) / sizeof(struct bpf_insn);
+struct bpf_program  mybpf;
+
+
+void testBPF1(int socketPort, char * devName) 
+{
+    int bpffd = 0; 
+    struct bpf_hdr * buf = NULL; 
+    char * pbuffer = NULL; 
+    int buflen; 
+    int len,i; 
+    ///int j=10; 
+    char dev[8] = "gei0"; 
+    struct ifreq ifr; 
+    int trueValue=1; 
+    int Rcvlen; 
+
+    if ( socketPort <= 10)
+        socketPort = 12345;
+
+    if ( devName != NULL )
+    {
+        strncpy(dev,devName,8);
+        dev[8]=0;
+    }
+    mybpf.bf_len = friUpdSock_bpf_filter_len; 
+    mybpf.bf_insns = friUpdSock_bpf_filter; 
+
+    /* Patch the server port into the BPF program...
+     *
+     * XXX: changes to filter program may require changes to the
+     * insn number(s) used below!
+     */
+    friUpdSock_bpf_filter[8].k = socketPort;
+
+    bpfDrv();
+
+    if ( bpfDevCreate("/dev/bpf",2,4096) == ERROR)
+    {
+        printf("bpfDevCreate failed \n");
+        return;
+    }
+    bpffd = open( "/dev/bpf0",0,0);
+    if ( bpffd <= 0)
+    {
+        printf("open /dev/bpf0 failed\n");
+        return;
+    }
+
+    memset(&ifr, sizeof(struct ifreq), 0); 
+    strncpy(ifr.ifr_name, dev, sizeof(ifr.ifr_name)); 
+
+#define IOCTRL_CAST_THIRD_ARG (int)
+
+    if (ioctl(bpffd,BIOCIMMEDIATE, IOCTRL_CAST_THIRD_ARG &trueValue) < 0) 
+    {
+        printf("Error BIOCIMMEDIATE \n");
+    }
+
+    if (ioctl(bpffd,(BIOCSETF),IOCTRL_CAST_THIRD_ARG (caddr_t)(&mybpf)) < 0) 
+    {
+        perror("Error BIOCSETF \n");
+        goto errorMark;
+    }
+
+    if (ioctl(bpffd,(BIOCSETIF),IOCTRL_CAST_THIRD_ARG (caddr_t)&ifr) < 0) 
+    {
+        printf("ERROR BIOCSETIF %s \n",dev);
+        goto errorMark;
+    }
+
+    if (ioctl(bpffd,BIOCGBLEN, IOCTRL_CAST_THIRD_ARG &buflen) < 0) 
+    {
+        printf("Error BIOCGBLEN \n");
+    }
+
+    if (buflen > 4096)
+        buflen=4096;
+
+    buf = (struct bpf_hdr *)malloc(buflen); 
+    //bzero(buf,buflen);  
+    memset(buf,0x0,buflen);
+
+    while ((len = read(bpffd,(char *)buf,buflen)) != 0) 
+    {
+        // im bpf header steht noch ein Timestamp -- waere gut fuer Timing thematik
+        //
+
+        //  Empfangene Rohdaten ohne bpf Header 
+        pbuffer = (char *)buf + buf->bh_hdrlen; 
+
+        // Empfangene Rohdatenlaenge ohne bpf Header
+        Rcvlen = len - (buf->bh_hdrlen); 
+        //
+        // Wie trennt man nun die "Nutzdaten" von den Verwaltungsdaten??
+        //
+
+        struct ip * iph = (struct ip *) ((char *) buf + buf->bh_hdrlen + sizeof(struct ether_header));
+        struct udphdr * udph = (struct udphdr *) ((char *) iph + sizeof(struct ip));
+        char * userData = ((char *) udph) + sizeof( struct udphdr); 
+
+        tFriCmdData * cmd = ( tFriCmdData * ) userData;
+#ifdef DEBUG_BPF_READ
+        printf("recvLen %d\n",Rcvlen);
+        printf("IP SRC:\t\t%s\n", inet_ntoa(iph->ip_src));
+        printf("IP DST:\t\t%s\n", inet_ntoa(iph->ip_dst));
+        printf("UDP SRC:\t%u\n", ntohs(udph->uh_sport));
+        printf("UDP DST:\t%u\n", ntohs(udph->uh_dport));
+        printf("Len #:\t\t%u\n", ntohs(udph->uh_ulen));
+#endif
+
+        logMsg("BUF LEN = 0x%x\n",Rcvlen,0,0,0,0,0); 
+
+        std::cout << (*cmd) << std::endl;
+        for (i=0;i< ntohs(udph->uh_ulen);i++) 
+        {
+            printf(" %2x", userData[i]);
+            if ((i % 10) == 9)
+            {
+                printf("\n");//,0,0,0,0,0,0);
+            }
+        }
+        printf("\nFull packet \n");
+        for (i=0; i < Rcvlen; i++) 
+        {
+            printf(" %2x(%4d)", pbuffer[i],pbuffer[i]);
+            if ((i % 10) == 9)
+            {
+                printf("\n");//,0,0,0,0,0,0);
+            }
+       }
+    }
+errorMark:
+    printf("leaving %s\n",__PRETTY_FUNCTION__);
+    if ( buf != NULL )
+        free(buf);
+    if ( bpffd > 0 )
+        close (bpffd);
+    bpfDevDelete("/dev/bpf");
+}
+#endif
+
+
+/*****************************************************************************
+ $Log: $
+ *****************************************************************************/
+/* @} */

lwrserv/friudp.h

@@ -0,0 +1,257 @@
+/*{{[PH]
+****************************************************************************
+ Project:  FRI
+
+  This material is the exclusive property of KUKA Roboter GmbH 
+  and must be returned to KUKA Roboter GmbH immediately upon 
+  request.  This material and the information illustrated or 
+  contained herein may not be used, reproduced, stored in a 
+  retrieval system, or transmitted in whole or in part in any 
+  way - electronic, mechanical, photocopying, recording, or 
+  otherwise, without the prior written consent of KUKA Roboter GmbH.  
+  
+                 All Rights Reserved
+                 Copyright (C)  2009
+                  KUKA Roboter GmbH
+                  Augsburg, Germany
+  
+[PH]}}
+*/
+
+/*
+{{[FH]
+****************************************************************************
+  friUdp.h
+       
+      NOTE: This sample, as the corresponding FRI (Fast Research inteface) is subject to radical change
+      
+      
+[FH]}}
+*/ 
+/**
+    \addtogroup friRemoteLib
+    \brief Library for FRI (FastResearchInterface) 
+*/
+/* @{ */
+/** *************************************************************************
+ \author (Guenter Schreiber)
+\file
+\brief header for udp Communications
+ *                                                                         *
+ ***************************************************************************/
+#ifndef FRIFRIUDP_H
+#define FRIFRIUDP_H
+
+#ifdef VXWORKS
+#else
+#define HAVE_GETHOSTNAME
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#ifdef _MSC_VER
+#include "SocketObject.h"
+#else
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <netdb.h>
+#include <string.h>
+#ifndef _WRS_KERNEL
+#include <sys/time.h>
+#endif
+#include <time.h>
+#endif
+#include "friComm.h"
+
+#include <iostream>
+
+#ifdef VXWORKS // VxWorks Kernel
+
+#include <sockLib.h>
+#endif
+
+
+#define FRI_DEFAULT_SERVER_PORT 49938
+
+#ifdef QNX
+#define HAVE_TIME_STAMP_RECEIVE
+#endif
+
+
+#ifdef HAVE_TIME_STAMP_RECEIVE
+/** Receive Timestamp -- mechanism works under 
+QNX, Linux?
+
+Not under Windows - and under VxWorks
+*/
+
+typedef struct
+{
+    tFriMsrData load;
+    /* add a received-timestamp */
+    double timestamp;
+} tFriMsrPacket;
+
+#endif
+
+/**
+FRI Remote Sample Implementation
+
+    @author Günter Schreiber <guenter@jacobus>
+*/
+class friUdp
+{
+public:
+    friUdp(int port=FRI_DEFAULT_SERVER_PORT, char *remoteHost = NULL);
+    ~friUdp();
+
+protected:
+    /// Note: Remote host need not to be specified - if NULL, wait for the 
+    /// incoming packages
+    void Init(char * remoteHost=NULL);
+    void Close(void);
+#ifdef WIN32
+    int StartWinsock(void);
+#endif
+public:
+    int Send(tFriCmdData *data);
+    int Recv(tFriMsrData *packet);
+private:
+    int RecvPacket(int fd, tFriMsrData* p, struct timeval* ts, struct sockaddr_in* client);
+    int udpSock ;
+    int serverPort;
+    struct sockaddr_in krcAddr;
+    /// if timestamp on receive is available, last received value can be inquired here
+    double m_timestamp;
+public:
+    /// This feature will be available only for systems, which support 
+    /// SO_TIMESTAMP in the socket options, e.g. qnx
+    double  getLastTimestamp() { return m_timestamp; }
+};
+
+
+
+
+inline std::ostream & operator<<(std::ostream &out , tFriHeader & head)
+{
+  out << "sendSeqCount " << head.sendSeqCount << "\n";
+  out << "reflSeqCount " << head.reflSeqCount << "\n";
+  out << "packetSize   " << head.packetSize   << "\n";
+  out << "datagramId   " << std::hex  << head.datagramId << std::dec  ;
+  switch (head.datagramId )
+  {
+  case FRI_DATAGRAM_ID_CMD:
+      out << " FRI_DATAGRAM_ID_CMD \n" ;
+      break;
+  case FRI_DATAGRAM_ID_MSR:
+      out << " FRI_DATAGRAM_ID_MSR \n" ;
+      break;
+  default:
+      out <<" Unkown \n";
+  }
+  return out;
+}
+inline std::ostream & operator<<(std::ostream &out , tFriKrlData& krl)
+{
+    out << "krl_real ";
+  for ( int i = 0; i < FRI_USER_SIZE; i++)
+  out << " " << krl.realData[i];
+  out << "\n";
+    out << "krl_int ";
+  for ( int i = 0; i < FRI_USER_SIZE; i++)
+  out << " " << krl.intData[i];
+  out << "\n";
+    out << "krl_bool ";
+  out << std::hex << krl.boolData << std::dec << "\n";
+  return out;
+}
+
+inline std::ostream & operator<<(std::ostream &out , tFriIntfStatistics & stat)
+{
+  out << "answerRate  " << stat.answerRate << "\n";
+  out << "latency     " << stat.latency << "\n";
+  out << "jitter      " << stat.jitter << "\n";
+  out << "missRate    " << stat.missRate << "\n";
+  out << "missCounter " << stat.missCounter << "\n";
+  return out;
+}
+
+inline std::ostream & operator<<(std::ostream &out , tFriIntfState & state)
+{
+  out << "timestamp " << state.timestamp<< "\n";
+  out << "state     " << state.state<< "\n";
+  out << "quality   " << state.quality << "\n";
+  out << "desiredMsrSampleTime " << state.desiredMsrSampleTime << "\n";
+  out << "desiredCmdSampleTiintfme " << state.desiredCmdSampleTime  << "\n";
+  out << "safetyLimits " << state.safetyLimits << "\n";
+  out << "statistics " << state.stat << "\n";
+return out;
+}
+
+inline std::ostream & operator<< (std::ostream & out, tFriRobotState & robot)
+{
+  out << "power   " << robot.power<< "\n";
+  out << "control " << robot.control << "\n";
+  out << "error   " << robot.error << "\n";
+  out << "warning " << robot.warning << "\n";
+  out << "temperature " ;
+  for (int i = 0; i < LBR_MNJ; i++)
+    out << robot.temperature[i] << " " ;
+  out << "\n";
+  return out;
+
+}
+
+#define WRITE_JNT_VEC(field) \
+  out << ""#field; \
+  for ( int i  = 0; i < LBR_MNJ; i++) \
+  out << " " << robot.field[i]; \
+  out << "\n";
+inline std::ostream & operator<<(std::ostream &out, tFriRobotData & robot)
+{
+  WRITE_JNT_VEC(msrJntPos);
+  WRITE_JNT_VEC(cmdJntPos);
+  WRITE_JNT_VEC(msrJntTrq);
+  WRITE_JNT_VEC(estExtJntTrq);
+    return out;
+
+}
+
+inline std::ostream & operator<<(std::ostream &out, tFriRobotCommand & robot)
+{
+  out << std::hex << robot.cmdFlags << std::dec << "\n";
+  WRITE_JNT_VEC(jntPos);
+    return out;
+
+}
+
+
+inline std::ostream & operator<<(std::ostream &out, tFriMsrData & msr)
+{
+  out << "head " << msr.head;
+  out << "krl " << msr.krl;
+  out << "intf " << msr.intf;
+  out << "robot " << msr.robot;
+  out << "data " << msr.data;
+  return out;
+}
+
+
+
+
+inline std::ostream & operator<<(std::ostream &out, tFriCmdData & cmd)
+{
+  out << "head " << cmd.head;
+  out << "krl " << cmd.krl;
+  out << "cmd " << cmd.cmd;
+    return out;
+}
+
+
+
+
+#endif
+/* @} */

lwrserv/global.cpp

@@ -0,0 +1,36 @@
+bool __SVR_CURRENT_JOB = false;
+bool __MSRMSRJNTPOS = false;
+bool __MSRCMDJNTPOS = false;
+bool __MSRCMDPOSE = false;
+bool __SETVELOCITY = false;
+bool __SETACCEL = false;
+bool __DEBUG = false;
+bool __POTFIELDMODE = false;
+bool __MSRSTARTPOTFIELD = false;
+bool __MSRSTOPPOTFIELD = false;
+bool __MSRSETPOS = false;
+bool __CARTMOVE = false;
+bool __DOUS = false;
+bool __DOUS2 = false;
+
+int globi = 0;
+
+float MSRMSRJNTPOS[7];
+double MSRCMDJNTPOS[7];
+double MSRMSRCARTPOS[12];
+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};
+
+
+
+

lwrserv/global.h

@@ -0,0 +1,32 @@
+extern bool __SVR_CURRENT_JOB;
+extern bool __MSRMSRJNTPOS;
+extern bool __MSRCMDJNTPOS;
+extern bool __MSRCMDPOSE;
+extern bool __SETVELOCITY;
+extern bool __SETACCEL;
+extern bool __DEBUG;
+extern bool __POTFIELDMODE;
+extern bool __MSRSTARTPOTFIELD;
+extern bool __MSRSTOPPOTFIELD;
+extern bool __MSRSETPOS;
+extern bool __CARTMOVE;
+extern bool __DOUS;
+extern bool __DOUS2;
+
+extern int globi;
+
+extern float MSRMSRJNTPOS[7];
+extern double MSRMSRCARTPOS[12];
+extern float MSRCMDCARTPOS[12];
+extern double MSRMSRJACOBIAN[42];
+extern double MSRMSRFTTCP[6];
+extern double MSRCMDJNTPOS[7];
+extern double MSRCMDPOSE[3][4];
+extern float USTarget[12];
+extern float USApproach[12];
+extern double VELOCITY;
+extern double ACCELARATION;
+extern double maxVelJnt[LBR_MNJ];
+extern double maxAccJnt[LBR_MNJ];
+extern double maxTrqJnt[LBR_MNJ];
+extern double maxRangeJnt[LBR_MNJ];

lwrserv/header.h

@@ -0,0 +1,24 @@
+#include <iostream>
+#include <sstream>
+#include "defines.h"
+#include "SocketObject.h"
+#include "SvrHandling.h"
+#include "StringTool.h"
+#include <string>
+#include "pthread.h"
+#include "friremote.h"
+#include "math.h"
+#include <cmath>
+#include "mat.h"
+#include "vec.h"
+//#include "opencv2\opencv.hpp"
+//#pragma comment (lib, "opencv_core248d.lib")
+
+
+//debug
+#include <algorithm>
+//end debug
+
+using namespace std;
+//using namespace cv;
+

lwrserv/mat.h

@@ -0,0 +1,354 @@
+#ifndef _MAT_H_
+#define _MAT_H_
+#include <iostream>
+#include "vec.h"
+
+template <class T, unsigned SIZE> class CVector;
+
+// template square matrix class for SIMPLE data types 
+template <class T, unsigned SIZE> class Mat 
+{
+public:
+
+    Mat<T, SIZE> ()
+    {
+        for (unsigned int j=0; j<SIZE; j++)
+        {
+            for (unsigned int i=0; i<SIZE; i++)
+            {
+                m_aatData[i][j] = T(0);
+            }
+        }
+    }
+
+    ~Mat<T, SIZE> ()
+    {
+        // nothing to do here ...
+    }
+
+    Mat<T, SIZE> (const T aatData[SIZE][SIZE])
+    {
+        for (unsigned int j=0; j<SIZE; j++)
+        {
+            for (unsigned int i=0; i<SIZE; i++)
+            {
+                m_aatData[i][j] = aatData[i][j];
+            }
+        }
+    }
+
+    Mat<T, SIZE> (const Mat<T, SIZE> &mat)
+    {
+        for (unsigned int j=0; j<SIZE; j++)
+        {
+            for (unsigned int i=0; i<SIZE; i++)
+            {
+                m_aatData[i][j] = mat.m_aatData[i][j];
+            }
+        }
+    }
+
+    T &operator () (unsigned i, unsigned j)
+    {
+        if (i>=SIZE)
+            i = SIZE-1;
+        if (j>=SIZE)
+            j = SIZE-1;
+        return m_aatData[i][j];
+    }
+
+    T operator () (unsigned i, unsigned j) const
+    {
+        if (i>=SIZE)
+            i = SIZE-1;
+        if (j>=SIZE)
+            j = SIZE-1;
+        return m_aatData[i][j];
+    }
+
+    // CMatrix<T, SIZE>operator + (const Matrix<T, SIZE> &mat)
+    // CMatrix<T, SIZE>operator - (const Matrix<T, SIZE> &mat)
+    // ...
+
+    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 a=0; a<SIZE; a++) // a
+                    buf(i,j) += m_aatData[i][a] * mat(a,j);
+        return buf;
+    }
+
+    Vec<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
+                buf(i) += m_aatData[i][j]*vec(j);
+        return buf;
+    }
+    Mat<float, 4> inv()
+    {
+        Mat<float,4> invOut;
+
+        long indxc[4], indxr[4], ipiv[4];
+        long i, icol, irow, j, ir, ic;
+        long big, dum, pivinv,temp, bb;
+
+        ipiv[0] = -1;
+        ipiv[1] = -1;
+        ipiv[2] = -1;
+        ipiv[3] = -1;
+
+        invOut = *this;
+
+
+        for (i =0; i < 4 ; i++) {
+            big =0.0f;
+            for (j =0; j < 4 ; j++) {
+                if (ipiv[j] != 0) {
+                    if (ipiv[0] == - 1) {
+                        if ((bb = (float) fabs(invOut(j,0))) > big) {
+                            big = bb;
+                            irow = j;
+                            icol = 0;
+                            }
+                    } else if (ipiv[0] > 0) {
+                        goto end;
+                    }
+
+                    if (ipiv[1] == - 1) {
+                        if ((bb = (float) fabs(invOut(j,1))) > big) {
+                            big = bb;
+                            irow = j;
+                            icol = 1;
+                        }
+                    } else if (ipiv[1] > 0) {
+                        goto end;
+                    }
+
+                    if (ipiv[2] == - 1) {
+                        if ((bb = (float) fabs(invOut(j,2))) > big) {
+                            big = bb;
+                            irow = j;
+                            icol = 2;
+                        }
+                    } else if (ipiv[2] > 0) {
+                        goto end;
+                    }
+
+                    if (ipiv[3] == - 1) {
+                        if ((bb = (float) fabs(invOut(j,3))) > big) {
+                            big = bb;
+                            irow = j;
+                            icol = 3;
+                        }
+                    } else if (ipiv[3] > 0) {
+                        goto end;
+                    }
+                }
+            }
+            ++(ipiv[icol]);
+
+            if (irow != icol) {
+                temp = invOut(irow,0);
+                invOut(irow, 0) = invOut(icol, 0);
+                invOut(icol, 0) = temp;
+
+                temp = invOut(irow,1);
+                invOut(irow, 1) = invOut(icol, 1);
+                invOut(icol,1) = temp;
+
+                temp = invOut(irow,2);
+                invOut(irow,2) = invOut(icol,2);
+                invOut(icol,2) = temp;
+
+                temp = invOut(irow,3);
+                invOut(irow,3) = invOut(icol,3);
+                invOut(icol,3) = temp;
+            }
+
+            indxr[i] = irow;
+            indxc[i] = icol;
+
+            if (invOut(icol,icol) ==0.0) {
+                goto end;
+            }
+
+            pivinv =1.0f / invOut(icol,icol);
+            invOut(icol,icol) =1.0f;
+            invOut(icol,0) *= pivinv;
+            invOut(icol,1) *= pivinv;
+            invOut(icol,2) *= pivinv;
+            invOut(icol,3) *= pivinv;
+
+            if (icol != 0) {
+                dum = invOut(0,icol);
+                invOut(0,icol) =0.0f;
+                invOut(0,0) -= invOut(icol,0) * dum;
+                invOut(0,1) -= invOut(icol,1) * dum;
+                invOut(0,2) -= invOut(icol,2) * dum;
+                invOut(0,3) -= invOut(icol,3) * dum;
+            }
+
+            if (icol != 1) {
+                dum = invOut(1,icol);
+                invOut(1,icol) =0.0f;
+                invOut(1,0) -= invOut(icol,0) * dum;
+                invOut(1,1) -= invOut(icol,1) * dum;
+                invOut(1,2) -= invOut(icol,2) * dum;
+                invOut(1,3) -= invOut(icol,3) * dum;
+            }
+
+            if (icol != 2) {
+                dum = invOut(2,icol);
+                invOut(2,icol) =0.0f;
+                invOut(2,0) -= invOut(icol,0) * dum;
+                invOut(2,1) -= invOut(icol,1) * dum;
+                invOut(2,2) -= invOut(icol,2) * dum;
+                invOut(2,3) -= invOut(icol,3) * dum;
+            }
+
+            if (icol != 3) {
+                dum = invOut(3,icol);
+                invOut(3,icol) =0.0f;
+                invOut(3,0) -= invOut(icol,0) * dum;
+                invOut(3,1) -= invOut(icol,1) * dum;
+                invOut(3,2) -= invOut(icol,2) * dum;
+                invOut(3,3) -= invOut(icol,3) * dum;
+            }
+        }
+        if (indxr[3] != indxc[3]) {
+            ir = indxr[3];
+            ic = indxc[3];
+            temp = invOut(0,ir);
+            invOut(0,ir) = invOut(0,ic);
+            invOut(0,ic) = temp;
+            temp = invOut(1,ir);
+            invOut(1,ir) = invOut(1,ic);
+            invOut(1,ic) = temp;
+            temp = invOut(2,ir);
+            invOut(2,ir) = invOut(2,ic);
+            invOut(2,ic) = temp;
+            temp = invOut(3,ir);
+            invOut(3,ir) = invOut(3,ic);
+            invOut(3,ic) = temp;
+        }
+        if (indxr[2] != indxc[2]) {
+            ir = indxr[2];
+            ic = indxc[2];
+
+            temp = invOut(0,ir);
+            invOut(0,ir) = invOut(0,ic);
+            invOut(0,ic) = temp;
+
+            temp = invOut(1,ir);
+            invOut(1,ir) = invOut(1,ic);
+            invOut(1,ic) = temp;
+
+            temp = invOut(2,ir);
+            invOut(2,ir) = invOut(2,ic);
+            invOut(2,ic) = temp;
+
+            temp = invOut(3,ir);
+            invOut(3,ir) = invOut(3,ic);
+            invOut(3,ic) = temp;
+        }
+        if (indxr[1] != indxc[1]) {
+            ir = indxr[1];
+            ic = indxc[1];
+            temp = invOut(0,ir);
+            invOut(0,ir) = invOut(0,ic);
+            invOut(0,ic) = temp;
+            temp = invOut(1,ir);
+            invOut(1,ir) = invOut(1,ic);
+            invOut(1,ic) = temp;
+            temp = invOut(2,ir);
+            invOut(2,ir) = invOut(2,ic);
+            invOut(2,ic) = temp;
+            temp = invOut(3,ir);
+            invOut(3,ir) = invOut(3,ic);
+            invOut(3,ic) = temp;
+        }
+        if (indxr[0] != indxc[0]) {
+            ir = indxr[0];
+            ic = indxc[0];
+            temp = invOut(0,ir);
+            invOut(0,ir) = invOut(0,ic);
+            invOut(0,ic) = temp;
+            temp = invOut(1,ir);
+            invOut(1,ir) = invOut(1,ic);
+            invOut(1,ic) = temp;
+            temp = invOut(2,ir);
+            invOut(2,ir) = invOut(2,ic);
+            invOut(2,ic) = temp;
+            temp = invOut(3,ir);
+            invOut(3,ir) = invOut(3,ic);
+            invOut(3,ic) = temp;
+        }
+end:
+        return invOut;
+    }
+
+private:
+    T m_aatData[SIZE][SIZE];
+};
+// some common vector classes (abbr. names)
+typedef Mat<float, 2> Mat2f;
+typedef Mat<float, 3> Mat3f;
+typedef Mat<float, 4> Mat4f;
+
+typedef Mat<double, 2> Mat2d;
+typedef Mat<double, 3> Mat3d;
+typedef Mat<double, 4> Mat4d;
+
+
+template <class T, unsigned SIZE>
+static std::ostream& operator<< (std::ostream& output,const Mat<T,SIZE> &mat)
+{
+    output << "( ";
+    for(unsigned int j =0; j< SIZE; ++j)
+    {
+        output << "( ";
+        for(unsigned int i =0; i< SIZE; ++i)
+        {
+            output << mat(j,i);
+            if (i<4-1)
+                output << " , ";
+            else
+                output << " )";
+        }
+        if (j<SIZE-1)
+            output << " ) ; ";
+        else
+            output << " ) )";
+    }
+    return output;
+}
+static Mat<float, 4> getRotationMatrix(float x_angle,float y_angle, float z_angle)
+{
+    Mat<float,4> tempx,tempy,tempz;
+    float temp_x[4][4] = { { 1,            0,             0, 0},
+                           { 0, cos(x_angle), -sin(x_angle), 0},
+                           { 0, sin(x_angle),  cos(x_angle), 0},
+                           { 0,            0,             0, 1}
+                         };
+
+    float temp_y[4][4] = { { cos(y_angle), 0, -sin(y_angle), 0},
+                           {            0, 1,             0, 0},
+                           { sin(y_angle), 0,  cos(y_angle), 0},
+                           {            0, 0,             0, 1}
+                         };
+    float temp_z[4][4] = { { cos(z_angle), -sin(z_angle), 0, 0},
+                           { sin(z_angle),  cos(z_angle), 0, 0},
+                           {            0,             0, 1, 0},
+                           {            0,             0, 0, 1}
+                          };
+
+    tempx=temp_x;
+    tempy=temp_y;
+    tempz=temp_z;
+    return tempz*tempy*tempx;
+}
+#endif

lwrserv/program.cpp

@@ -0,0 +1,520 @@
+#include "header.h"
+#include "global.h"
+#include "sgn.h"
+#include <error.h>
+#include <errno.h>
+
+void printMat(Mat4f M)
+{
+#if 0
+    for (int i=0;i<M.rows;i++)
+    {
+        for (int j=0;j<M.cols;j++)
+        {
+            cout << M.at<float>(i,j) << " ";
+        }
+        cout << "\n\r";
+    }
+    cout << "\n\r\n\r";
+#else
+    cout << M << endl;
+#endif 
+}
+
+float* abctomat(float a, float b, float c)
+{
+    Mat4f rx;
+    float ca = cos(c);
+    float sa = sin(c);
+
+    rx(0,0) = 1;
+    rx(1,1) = ca; 
+    rx(1,2) = -sa;
+    rx(2,1) = sa; 
+    rx(2,2) = ca; 
+    rx(3,3) = 1;
+
+    Mat4f ry;
+    float cb = cos(b);
+    float sb = sin(b);
+    ry(0,0) = cb;
+    ry(0,2) = sb;
+    ry(1,1) = 1;
+    ry(2,0) = -sb;
+    ry(2,2) = cb; 
+    ry(3,3) = 1;
+
+    Mat4f rz;
+    float cc = cos(a);
+    float sc = sin(a);
+    rz(0,0) = cc;
+    rz(0,1) = -sc;
+    rz(1,0) = sc;
+    rz(1,1) = cc;
+    rz(2,2) = 1; 
+    rz(3,3) = 1;
+    
+    Mat4f result;
+    Mat4f temp;
+    temp   = rz * ry;
+    result = temp * rx;
+
+#ifdef __DEBUG__
+    printMat(rx);
+    printMat(ry);
+    printMat(rz);
+    printMat(result);
+#endif
+
+    float *res = new float[12];
+    //TODO simple converter for mat
+    for (int j=0;j<3;j++)
+    {
+        for (int k=0;k<4;k++)
+        {
+            res[j*4+k] = result(j,k);
+        }
+    }
+    return res;
+}
+
+float* mattoabc(float M[12])
+{
+    float norm;
+    float sa;
+    float ca;
+    float *abc = new float[3];
+
+    norm = sqrt((M[0]*M[0])+(M[4]*M[4]));
+
+    if (norm>1e-5)
+    {
+        sa = M[4]/norm;
+        ca = M[0]/norm;
+        abc[0]  = atan2(sa,ca);
+    }
+    else 
+    {
+        sa = 0;
+        ca = 1;
+        abc[0] = 0;
+    }
+    abc[1] = atan2(-M[8],ca*M[0]+sa*M[4]);
+    abc[2] = atan2(sa*M[2]-ca*M[6],-sa*M[1]+ca*M[5]);
+    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;
+    for (int i=0; i<3; i++)
+    {
+        for (int j=0; j<4; j++)
+        {
+            result(i,j) = (float)vec[i*4+j];
+        }
+    }
+    result(3,3)=(float)1;
+    return result;
+}
+
+float* matToVec2(Mat4f mat)
+{
+    float* vec = new float[12];
+    for (int j=0;j<3;j++)
+    {
+        for (int k=0;k<4;k++)
+        {
+            vec[j*4+k] = mat(j,k);
+        }
+    }
+    return vec;
+}
+
+float* vectoquat(float vec[12])
+{
+    float *quat = new float[4];
+    float diag[3];
+    int v; int w; int u;
+    diag[0] = vec[0];
+    diag[1] = vec[5];
+    diag[2] = vec[10];
+    float u_ = *std::max_element(diag,diag+2);
+
+    if (u_==diag[0])
+    {
+        u = 1;
+        v = 2;
+        w = 3;
+    } else if (u_==diag[1])
+    {
+        u = 2;
+        v = 3;
+        w = 1;
+    } else
+    {
+        u = 3;
+        v = 1;
+        w = 2;
+    }
+    float r = sqrt(1+vec[(u-1)*4+(u-1)] - vec[(v-1)*4+(v-1)] - vec[(w-1)*4+(w-1)]);
+    quat[0] = (vec[(w-1)*4+(v-1)] - vec[(v-1)*4+(w-1)]) / (2*r);
+    quat[u] = r/2;
+    quat[v] = (vec[(u-1)*4+(v-1)] + vec[(v-1)*4+(u-1)]) / (2*r);
+    quat[w] = (vec[(w-1)*4+(u-1)] + vec[(u-1)*4+(w-1)]) / (2*r);
+
+    return quat;
+}
+
+float* quattovec(float quat[4])
+{
+    float *vec = new float[12];
+    vec[0] = quat[0]*quat[0]+quat[1]*quat[1]-quat[2]*quat[2]-quat[3]*quat[3];
+    vec[4] = 2*(quat[1]*quat[2]+quat[0]*quat[3]);
+    vec[8] = 2*(quat[1]*quat[3]-quat[0]*quat[2]);
+    
+    vec[1] = 2*(quat[1]*quat[2]-quat[0]*quat[3]);
+    vec[5] = quat[0]*quat[0]-quat[1]*quat[1]+quat[2]*quat[2]-quat[3]*quat[3];
+    vec[9] = 2*(quat[2]*quat[3]+quat[0]*quat[1]);
+    
+    vec[2] = 2*(quat[1]*quat[3]+quat[0]*quat[2]);
+    vec[6] = 2*(quat[2]*quat[3]-quat[0]*quat[1]);
+    vec[10] = quat[0]*quat[0]-quat[1]*quat[1]-quat[2]*quat[2]+quat[3]*quat[3];
+
+    return vec;
+}
+
+
+void *threadFriDataExchange(void *arg)
+{
+    //doRalf();
+
+    friRemote friInst;
+
+    while (1)
+    {
+        //#######################################
+        // Communication loop
+
+        // get current joint positions 
+        for ( int i= 0; i < LBR_MNJ; i++)
+        {
+            MSRMSRJNTPOS[i] = friInst.getMsrCmdJntPosition()[i] + friInst.getMsrCmdJntPositionOffset()[i];
+            //MSRMSRJNTPOS[i] = friInst.getMsrMsrJntPosition()[i];
+        }
+
+        // get current jacobian
+        float* friJacobian = friInst.getJacobian();
+        if ( friJacobian == NULL)
+        {
+            fprintf(stderr,"Failed: could not get jacobian\n");
+            break;
+        }
+        for (int i = 0; i < FRI_CART_VEC*LBR_MNJ ; i++)
+        {
+            MSRMSRJACOBIAN[i] = friJacobian[i];
+        }
+
+        // get current messured carthesian position
+        float* friMsrCartPosition = friInst.getMsrCartPosition();
+        if ( friMsrCartPosition == NULL)
+        {
+            fprintf(stderr,"Failed: could not get Messurement in Carthesian Position\n");
+            break;
+        }
+        for ( int i =0; i < FRI_CART_FRM_DIM; i++)
+        {
+            MSRMSRCARTPOS[i] = friMsrCartPosition[i];
+        }
+
+        // get current force and torque
+        float* friFTTcp =  friInst.getFTTcp();
+        for ( int i = 0; i < FRI_CART_VEC; i++)
+        {
+            MSRMSRFTTCP[i] = friFTTcp[i];
+        }
+
+        //#########################################################
+        // PTP Joint Movement
+        if (__MSRCMDJNTPOS)
+        {
+            double maxVelJntLocal[LBR_MNJ];
+            double maxAccJntLocal[LBR_MNJ];
+            double delta[LBR_MNJ];
+            double sampleTime = (double)0.005;
+            double deltaAbs[LBR_MNJ];
+            double dMaxSpeed[LBR_MNJ];
+            double lMaxSpeed[LBR_MNJ];
+            double dGesamt[LBR_MNJ];
+            
+            double currentInk[LBR_MNJ] = {0};
+            double currentInkLast[LBR_MNJ] = {0};
+            double currentDist [LBR_MNJ] = {0};
+            double currentDistLast [LBR_MNJ] = {0};
+
+            // calculate delta positions of movement
+            for ( int i= 0; i < LBR_MNJ; i++)
+            {
+                delta[i] = MSRCMDJNTPOS[i]-(MSRMSRJNTPOS[i]*180/M_PI);
+                deltaAbs[i] = fabs(MSRCMDJNTPOS[i]-(MSRMSRJNTPOS[i]*180/M_PI));
+                maxVelJntLocal[i] = maxVelJnt[i]*sampleTime*(VELOCITY/100.0);
+                maxAccJntLocal[i] = maxAccJnt[i]*sampleTime*(ACCELARATION/100.0);
+            }
+            
+            // calculate movement steps
+            for (int j=0; j<LBR_MNJ; j++)
+            {
+                dMaxSpeed[j] = maxVelJntLocal[j]/maxAccJntLocal[j];
+                lMaxSpeed[j] = dMaxSpeed[j]*dMaxSpeed[j]*maxAccJntLocal[j]*0.5;
+                if (lMaxSpeed[j] > deltaAbs[j]/(double)2.0)
+                {
+                    dGesamt[j] = sqrt(deltaAbs[j]/maxAccJntLocal[j])*2.0;
+                } else 
+                {
+                    dGesamt[j] = dMaxSpeed[j]*2 + (deltaAbs[j]-lMaxSpeed[j]*2.0)/maxVelJntLocal[j];
+                }
+            }
+            int maxSteps =ceil(*max_element(dGesamt,(dGesamt+LBR_MNJ-1)));
+
+            if (maxSteps == 0 || maxSteps == 1)
+                goto end;
+
+            for (int j=0; j<LBR_MNJ; j++)
+            {
+                dMaxSpeed[j] = ceil(maxSteps/(double)2 - sqrt((maxSteps/(double)2)*(maxSteps/(double)2) - deltaAbs[j]/maxAccJntLocal[j]));
+                double temp = maxSteps/(double)2 - sqrt((maxSteps/(double)2)*(maxSteps/(double)2) - deltaAbs[j]/maxAccJntLocal[j]);             
+                if (dMaxSpeed[j]==0)
+                {
+                    maxAccJntLocal[j] = 0;
+                } else
+                {
+                    maxAccJntLocal[j] = -deltaAbs[j]/(dMaxSpeed[j]*dMaxSpeed[j]-maxSteps*dMaxSpeed[j]);
+                }
+                maxVelJntLocal[j] = dMaxSpeed[j]*maxAccJntLocal[j];
+            }
+
+
+            for (int i=0;i<maxSteps;i++)
+            {
+                for (int j=0;j<LBR_MNJ;j++)
+                {
+                    if (i+1 <= maxSteps/(double)2)
+                    {
+                        currentInk[j] = min(currentInkLast[j]+maxAccJntLocal[j],maxVelJntLocal[j]);
+                    }else if (i+1 > maxSteps-dMaxSpeed[j])
+                    {
+                        currentInk[j] = max(currentInkLast[j]-maxAccJntLocal[j],(double)0);
+                    }else
+                    {
+                        currentInk[j] = currentInkLast[j];
+                    }
+                    currentDist[j] = currentDistLast[j] + sgn(delta[j])*currentInk[j];
+                    currentInkLast[j] = currentInk[j];
+                    currentDistLast[j] = currentDist[j];
+                    MSRMSRJNTPOS[j]+= sgn(delta[j])*currentInk[j]*(1./180*M_PI);
+                }
+                friInst.doPositionControl(MSRMSRJNTPOS);
+            }
+end:
+            __MSRCMDJNTPOS = false;
+
+        } else if (__CARTMOVE)
+        {
+            //##########################################################
+            //Cartesian Movement
+
+            const float Stiff[6] = {1000.0, 1000.0, 1000.0, 150.0, 150.0, 150.0};
+            const float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7};
+            float th = 0.000;
+            float deltaCart[12];
+            float Pos[12];
+            float dist;
+            float rot;
+            float sum;
+            float b;
+            float c;
+            float t_fact = 1;
+            float t_outerT = 0.007;
+            float t_innerT = 0.000;
+            float t_outerR = 0.007;
+            float t_innerR = 0.000;
+
+            while (true)
+            {
+                for ( int i =0; i < FRI_CART_FRM_DIM; i++)
+                {
+                    Pos[i] = friInst.getMsrCartPosition()[i];
+                    MSRMSRCARTPOS[i] = friInst.getMsrCartPosition()[i];
+                    deltaCart[i] = MSRCMDCARTPOS[i]-Pos[i];
+                }
+                sum = 0;
+                for (int i=0; i<3 ; i++)
+                {
+                    sum = sum + (deltaCart[(i*4)+3]*deltaCart[(i*4)+3]);
+                }
+                dist = sqrt(sum);
+                sum = 0;
+                for (int i=0;i<12;i++)
+                {
+                    if (i != 3 && i != 7 && i != 11)
+                    {
+                        sum = sum + deltaCart[i]*deltaCart[i];
+                    }
+                }
+                rot = sqrt(sum);
+                b = t_fact*max((float)0.0,(float)min((float)1.0,(t_outerT-dist+(t_innerT-t_outerT))*(1/(t_innerT-t_outerT))));
+                c = t_fact*max((float)0.0,(float)min((float)1.0,(float)(t_outerR-rot+(t_innerR-t_outerR))*(float)(1/(t_innerR-t_outerR))));
+
+                if (dist < th)
+                {
+                    for (int i=0; i<3; i++)
+                    {
+                        deltaCart[(i*4)+3] = 0;
+                    }
+                }else 
+                {
+                    for (int i=0; i<3; i++)
+                    {
+                        deltaCart[(i*4)+3] = (deltaCart[(i*4)+3] / dist)*b;
+                    }
+                }
+
+                // check offset to point 
+                if (rot < th)
+                {
+                    for (int i=0;i<12;i++)
+                    {
+                        if (i != 3 && i != 7 && i != 11)
+                        {
+                            deltaCart[i] = 0;
+                        }
+                    }
+                }else 
+                {
+                    for (int i=0;i<12;i++)
+                    {
+                        if (i != 3 && i != 7 && i != 11)
+                        {
+                            deltaCart[i] = (deltaCart[i] / rot)*c;
+                        }
+                    }
+                }
+                for (int i=0;i<12;i++)
+                {
+                    Pos[i]+= deltaCart[i]*0.02;
+                }
+                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.doPositionControl(MSRMSRJNTPOS);
+
+        //i = 0;
+        //}
+        //friInst.doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
+    }
+    return NULL;
+}
+
+int main()
+{
+    int err = 0;
+    //Setting pthread for FRI interface
+    pthread_t fri_t;
+    //Start fri_thread
+    err = pthread_create(&fri_t,NULL,&threadFriDataExchange,NULL);
+    if (err > 0 )
+    {
+        cerr << "Failed: could not create thread\n" << endl;
+        return err;
+    }
+
+
+    //Start client handling
+    SvrHandling *svr = new SvrHandling();
+    if (svr == NULL)
+    {
+        cerr << "Failed: could not create server \n" << endl;
+        return -ENOMEM;
+    }
+    svr->run(SVR_DEFAULT_PORT);
+
+    return 0;
+}

lwrserv/pthread/sched.h

@@ -0,0 +1,183 @@
+/*
+ * Module: sched.h
+ *
+ * Purpose:
+ *      Provides an implementation of POSIX realtime extensions
+ *      as defined in 
+ *
+ *              POSIX 1003.1b-1993      (POSIX.1b)
+ *
+ * --------------------------------------------------------------------------
+ *
+ *      Pthreads-win32 - POSIX Threads Library for Win32
+ *      Copyright(C) 1998 John E. Bossom
+ *      Copyright(C) 1999,2005 Pthreads-win32 contributors
+ * 
+ *      Contact Email: rpj@callisto.canberra.edu.au
+ * 
+ *      The current list of contributors is contained
+ *      in the file CONTRIBUTORS included with the source
+ *      code distribution. The list can also be seen at the
+ *      following World Wide Web location:
+ *      http://sources.redhat.com/pthreads-win32/contributors.html
+ * 
+ *      This library is free software; you can redistribute it and/or
+ *      modify it under the terms of the GNU Lesser General Public
+ *      License as published by the Free Software Foundation; either
+ *      version 2 of the License, or (at your option) any later version.
+ * 
+ *      This library 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
+ *      Lesser General Public License for more details.
+ * 
+ *      You should have received a copy of the GNU Lesser General Public
+ *      License along with this library in the file COPYING.LIB;
+ *      if not, write to the Free Software Foundation, Inc.,
+ *      59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+#if !defined(_SCHED_H)
+#define _SCHED_H
+
+#undef PTW32_SCHED_LEVEL
+
+#if defined(_POSIX_SOURCE)
+#define PTW32_SCHED_LEVEL 0
+/* Early POSIX */
+#endif
+
+#if defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 199309
+#undef PTW32_SCHED_LEVEL
+#define PTW32_SCHED_LEVEL 1
+/* Include 1b, 1c and 1d */
+#endif
+
+#if defined(INCLUDE_NP)
+#undef PTW32_SCHED_LEVEL
+#define PTW32_SCHED_LEVEL 2
+/* Include Non-Portable extensions */
+#endif
+
+#define PTW32_SCHED_LEVEL_MAX 3
+
+#if ( defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 200112 )  || !defined(PTW32_SCHED_LEVEL)
+#define PTW32_SCHED_LEVEL PTW32_SCHED_LEVEL_MAX
+/* Include everything */
+#endif
+
+
+#if defined(__GNUC__) && !defined(__declspec)
+# error Please upgrade your GNU compiler to one that supports __declspec.
+#endif
+
+/*
+ * When building the library, you should define PTW32_BUILD so that
+ * the variables/functions are exported correctly. When using the library,
+ * do NOT define PTW32_BUILD, and then the variables/functions will
+ * be imported correctly.
+ */
+#if !defined(PTW32_STATIC_LIB)
+#  if defined(PTW32_BUILD)
+#    define PTW32_DLLPORT __declspec (dllexport)
+#  else
+#    define PTW32_DLLPORT __declspec (dllimport)
+#  endif
+#else
+#  define PTW32_DLLPORT
+#endif
+
+/*
+ * This is a duplicate of what is in the autoconf config.h,
+ * which is only used when building the pthread-win32 libraries.
+ */
+
+#if !defined(PTW32_CONFIG_H)
+#  if defined(WINCE)
+#    define NEED_ERRNO
+#    define NEED_SEM
+#  endif
+#  if defined(__MINGW64__)
+#    define HAVE_STRUCT_TIMESPEC
+#    define HAVE_MODE_T
+#  elif defined(_UWIN) || defined(__MINGW32__)
+#    define HAVE_MODE_T
+#  endif
+#endif
+
+/*
+ *
+ */
+
+#if PTW32_SCHED_LEVEL >= PTW32_SCHED_LEVEL_MAX
+#if defined(NEED_ERRNO)
+#include "need_errno.h"
+#else
+#include <errno.h>
+#endif
+#endif /* PTW32_SCHED_LEVEL >= PTW32_SCHED_LEVEL_MAX */
+
+#if (defined(__MINGW64__) || defined(__MINGW32__)) || defined(_UWIN)
+# if PTW32_SCHED_LEVEL >= PTW32_SCHED_LEVEL_MAX
+/* For pid_t */
+#  include <sys/types.h>
+/* Required by Unix 98 */
+#  include <time.h>
+# else
+   typedef int pid_t;
+# endif
+#else
+ typedef int pid_t;
+#endif
+
+/* Thread scheduling policies */
+
+enum {
+  SCHED_OTHER = 0,
+  SCHED_FIFO,
+  SCHED_RR,
+  SCHED_MIN   = SCHED_OTHER,
+  SCHED_MAX   = SCHED_RR
+};
+
+struct sched_param {
+  int sched_priority;
+};
+
+#if defined(__cplusplus)
+extern "C"
+{
+#endif                          /* __cplusplus */
+
+PTW32_DLLPORT int __cdecl sched_yield (void);
+
+PTW32_DLLPORT int __cdecl sched_get_priority_min (int policy);
+
+PTW32_DLLPORT int __cdecl sched_get_priority_max (int policy);
+
+PTW32_DLLPORT int __cdecl sched_setscheduler (pid_t pid, int policy);
+
+PTW32_DLLPORT int __cdecl sched_getscheduler (pid_t pid);
+
+/*
+ * Note that this macro returns ENOTSUP rather than
+ * ENOSYS as might be expected. However, returning ENOSYS
+ * should mean that sched_get_priority_{min,max} are
+ * not implemented as well as sched_rr_get_interval.
+ * This is not the case, since we just don't support
+ * round-robin scheduling. Therefore I have chosen to
+ * return the same value as sched_setscheduler when
+ * SCHED_RR is passed to it.
+ */
+#define sched_rr_get_interval(_pid, _interval) \
+  ( errno = ENOTSUP, (int) -1 )
+
+
+#if defined(__cplusplus)
+}                               /* End of extern "C" */
+#endif                          /* __cplusplus */
+
+#undef PTW32_SCHED_LEVEL
+#undef PTW32_SCHED_LEVEL_MAX
+
+#endif                          /* !_SCHED_H */
+

lwrserv/pthread/semaphore.h

@@ -0,0 +1,169 @@
+/*
+ * Module: semaphore.h
+ *
+ * Purpose:
+ *	Semaphores aren't actually part of the PThreads standard.
+ *	They are defined by the POSIX Standard:
+ *
+ *		POSIX 1003.1b-1993	(POSIX.1b)
+ *
+ * --------------------------------------------------------------------------
+ *
+ *      Pthreads-win32 - POSIX Threads Library for Win32
+ *      Copyright(C) 1998 John E. Bossom
+ *      Copyright(C) 1999,2005 Pthreads-win32 contributors
+ * 
+ *      Contact Email: rpj@callisto.canberra.edu.au
+ * 
+ *      The current list of contributors is contained
+ *      in the file CONTRIBUTORS included with the source
+ *      code distribution. The list can also be seen at the
+ *      following World Wide Web location:
+ *      http://sources.redhat.com/pthreads-win32/contributors.html
+ * 
+ *      This library is free software; you can redistribute it and/or
+ *      modify it under the terms of the GNU Lesser General Public
+ *      License as published by the Free Software Foundation; either
+ *      version 2 of the License, or (at your option) any later version.
+ * 
+ *      This library 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
+ *      Lesser General Public License for more details.
+ * 
+ *      You should have received a copy of the GNU Lesser General Public
+ *      License along with this library in the file COPYING.LIB;
+ *      if not, write to the Free Software Foundation, Inc.,
+ *      59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+#if !defined( SEMAPHORE_H )
+#define SEMAPHORE_H
+
+#undef PTW32_SEMAPHORE_LEVEL
+
+#if defined(_POSIX_SOURCE)
+#define PTW32_SEMAPHORE_LEVEL 0
+/* Early POSIX */
+#endif
+
+#if defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 199309
+#undef PTW32_SEMAPHORE_LEVEL
+#define PTW32_SEMAPHORE_LEVEL 1
+/* Include 1b, 1c and 1d */
+#endif
+
+#if defined(INCLUDE_NP)
+#undef PTW32_SEMAPHORE_LEVEL
+#define PTW32_SEMAPHORE_LEVEL 2
+/* Include Non-Portable extensions */
+#endif
+
+#define PTW32_SEMAPHORE_LEVEL_MAX 3
+
+#if !defined(PTW32_SEMAPHORE_LEVEL)
+#define PTW32_SEMAPHORE_LEVEL PTW32_SEMAPHORE_LEVEL_MAX
+/* Include everything */
+#endif
+
+#if defined(__GNUC__) && ! defined (__declspec)
+# error Please upgrade your GNU compiler to one that supports __declspec.
+#endif
+
+/*
+ * When building the library, you should define PTW32_BUILD so that
+ * the variables/functions are exported correctly. When using the library,
+ * do NOT define PTW32_BUILD, and then the variables/functions will
+ * be imported correctly.
+ */
+#if !defined(PTW32_STATIC_LIB)
+#  if defined(PTW32_BUILD)
+#    define PTW32_DLLPORT __declspec (dllexport)
+#  else
+#    define PTW32_DLLPORT __declspec (dllimport)
+#  endif
+#else
+#  define PTW32_DLLPORT
+#endif
+
+/*
+ * This is a duplicate of what is in the autoconf config.h,
+ * which is only used when building the pthread-win32 libraries.
+ */
+
+#if !defined(PTW32_CONFIG_H)
+#  if defined(WINCE)
+#    define NEED_ERRNO
+#    define NEED_SEM
+#  endif
+#  if defined(__MINGW64__)
+#    define HAVE_STRUCT_TIMESPEC
+#    define HAVE_MODE_T
+#  elif defined(_UWIN) || defined(__MINGW32__)
+#    define HAVE_MODE_T
+#  endif
+#endif
+
+/*
+ *
+ */
+
+#if PTW32_SEMAPHORE_LEVEL >= PTW32_SEMAPHORE_LEVEL_MAX
+#if defined(NEED_ERRNO)
+#include "need_errno.h"
+#else
+#include <errno.h>
+#endif
+#endif /* PTW32_SEMAPHORE_LEVEL >= PTW32_SEMAPHORE_LEVEL_MAX */
+
+#define _POSIX_SEMAPHORES
+
+#if defined(__cplusplus)
+extern "C"
+{
+#endif				/* __cplusplus */
+
+#if !defined(HAVE_MODE_T)
+typedef unsigned int mode_t;
+#endif
+
+
+typedef struct sem_t_ * sem_t;
+
+PTW32_DLLPORT int __cdecl sem_init (sem_t * sem,
+			    int pshared,
+			    unsigned int value);
+
+PTW32_DLLPORT int __cdecl sem_destroy (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_trywait (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_wait (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_timedwait (sem_t * sem,
+				 const struct timespec * abstime);
+
+PTW32_DLLPORT int __cdecl sem_post (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_post_multiple (sem_t * sem,
+				     int count);
+
+PTW32_DLLPORT int __cdecl sem_open (const char * name,
+			    int oflag,
+			    mode_t mode,
+			    unsigned int value);
+
+PTW32_DLLPORT int __cdecl sem_close (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_unlink (const char * name);
+
+PTW32_DLLPORT int __cdecl sem_getvalue (sem_t * sem,
+				int * sval);
+
+#if defined(__cplusplus)
+}				/* End of extern "C" */
+#endif				/* __cplusplus */
+
+#undef PTW32_SEMAPHORE_LEVEL
+#undef PTW32_SEMAPHORE_LEVEL_MAX
+
+#endif				/* !SEMAPHORE_H */

lwrserv/resource.h

@@ -0,0 +1,14 @@
+//{{NO_DEPENDENCIES}}
+// Microsoft Visual C++ generated include file.
+// Used by lwrserv.rc
+
+// Nächste Standardwerte für neue Objekte
+// 
+#ifdef APSTUDIO_INVOKED
+#ifndef APSTUDIO_READONLY_SYMBOLS
+#define _APS_NEXT_RESOURCE_VALUE        101
+#define _APS_NEXT_COMMAND_VALUE         40001
+#define _APS_NEXT_CONTROL_VALUE         1001
+#define _APS_NEXT_SYMED_VALUE           101
+#endif
+#endif

lwrserv/sgn.h

@@ -0,0 +1,13 @@
+#ifndef __SGN_H__
+#define __SGN_H__
+inline float sgn(float x) 
+{ 
+    if ( x == 0 ) 
+        return 0.0f; 
+
+    if ( x > 0)
+        return 1.0f;
+    else
+        return -1.0f; 
+}
+#endif

lwrserv/vec.h

@@ -0,0 +1,234 @@
+#ifndef _VEC_H_
+#define _VEC_H_
+#include <math.h>
+#include "mat.h"
+#include <ostream>
+
+template<class T, unsigned SIZE> class Mat;
+template<class T, unsigned SIZE> class Vec;
+
+// Vector class for SIMPLE data types
+template <class T, unsigned SIZE> class Vec 
+{
+public:
+
+    // standard constructor
+    Vec<T,SIZE> ()  
+    {   // initialize all elements with zero
+        for (unsigned int i=0; i<SIZE; i++)
+            m_atData[i] = T(0);
+    }
+
+    // construction with data array
+    Vec<T, SIZE> (const T atData[SIZE]) 
+    {
+        for (unsigned int i=0; i<SIZE; i++)
+            m_atData[i] = atData[i];
+    }
+
+    // copy constructor
+    Vec<T, SIZE> (const Vec<T, SIZE> &vec) 
+    {
+        if (this==&vec)
+            return; // nothing to do, it's me
+        for (unsigned int i=0; i<SIZE; i++)
+            m_atData[i] = vec.m_atData[i];
+    }
+
+    // destructor
+    ~Vec () 
+    {   // nothing to do here ...
+    }
+
+    void setData (const T atData[SIZE]) 
+    {
+        for (unsigned int i=0; i<SIZE; i++)
+            m_atData[i] = atData[i];
+    }
+
+    void getData (const T atData[SIZE]) 
+    {
+        for (unsigned int i=0; i<SIZE; i++)
+            atData[i] = m_atData[i];
+    }
+
+    unsigned getDimension () 
+    {
+        return SIZE;
+    }
+
+
+    Vec<T, SIZE> &operator = (const Vec<T, SIZE> &vec) 
+    {
+        if (this==&vec)
+            return (*this); // ok, it's me, so no L-value action
+        for (unsigned int i=0; i<SIZE; i++)     // not me, so L-Value action: copy data
+            m_atData[i] = vec.m_atData[i];
+
+        return (*this); // also an R-value in e.g
+                        // vec1    =       vec2      = vec3;
+                        // L-Value = R-Value/L-Value = R-Value
+    }
+
+    Vec<T, SIZE> &operator = (const T atData[SIZE]) 
+    {
+        for (unsigned int i=0; i<SIZE; i++) // not me, so L-Value action: copy data
+            m_atData[i] = atData[i];
+
+        return (*this); // also an R-value in e.g.
+                        // vec1  = vec2  +   (vec2=atData); // parenthesis () needed to evaluate expression vec2=atData
+                        // L-Val = R-Val +       R-Val
+                        //   "   =   "   + (L-Val = R-Val)
+    }
+
+    // usage of operator:
+    // vec(i) = var;    // 0 <= i <= SIZE-1
+    // var = vec(i);
+    // vec1(i) = vec2(j);
+    T &operator () (unsigned i) 
+    {
+        if (i>=SIZE)
+            i = SIZE-1; // !!! operator clips index ...
+        return m_atData[i];         // ... so we can safely return a reference
+    }
+
+    T operator () (unsigned i) const 
+    {
+        if (i>=SIZE)
+            i = SIZE-1;
+        return m_atData[i];
+    }
+
+    void operator += (const Vec<T, SIZE> &vec) 
+    {
+        for (int i=0; i<SIZE; i++)
+            m_atData[i] += vec.m_atData[i];
+    }
+
+    Vec<T, SIZE> operator + (const Vec<T, SIZE> &vec) 
+    {
+        Vec<T, SIZE> buf (m_atData);
+        for (unsigned int i=0; i<SIZE; i++)
+            buf.m_atData[i] += vec.m_atData[i];
+        return buf;
+    }
+
+    void operator -= (const Vec<T, SIZE> &vec) 
+    {
+        for (unsigned int i=0; i<SIZE; i++)
+            m_atData[i] -= vec.m_atData[i];
+    }
+
+    // binary -
+    // vec1 - vec2 i.e. (*this) - vec
+    Vec<T, SIZE> operator - (const Vec<T, SIZE> &vec) 
+    {
+        Vec<T, SIZE> buf (m_atData);
+        for (unsigned int i=0; i<SIZE; i++)
+            buf.m_atData[i] -= vec.m_atData[i];
+        return buf;
+    }
+
+    // unary -
+    // -vec i.e. -(*this)
+    Vec<T, SIZE> operator - () 
+    {
+        T atBuffer[SIZE];
+        for (int i=0; i<SIZE; i++)
+            atBuffer[i] = -m_atData[i];
+        return Vec<T, SIZE> (atBuffer);
+    }
+
+    T operator * (const Vec<T, SIZE> & vec) 
+    {
+        T dp = T(0);
+        for (int i=0; i<SIZE; i++)
+            dp += m_atData[i]*vec.m_atData[i];
+        return dp;
+    }
+
+    void operator *= (T tScale) 
+    {
+        for (unsigned int i=0; i<SIZE; i++)
+            m_atData[i] *= tScale;
+    }
+
+    Vec<T, SIZE> operator * (T tScale) 
+    {
+        T atBuffer[SIZE];
+        for (unsigned int i=0; i<SIZE; i++)
+            atBuffer[i] = m_atData[i]*tScale;
+        return Vec<T, SIZE> (atBuffer);
+    }
+
+    Vec<T, SIZE> operator * (const Mat<T, SIZE> &mat)
+    {
+        Vec<T, SIZE> vec;
+        for (unsigned int j=0; j<SIZE; j++)
+            for (unsigned int i=0; i<SIZE; i++)
+               vec(j) += m_atData[i]*mat(i,j);
+        return vec;
+    }
+
+    T norm ()
+    {
+        T retval = 0;
+
+        // this * this
+        for (int i=0; i<SIZE; i++)
+            retval += m_atData[i] * m_atData[i];
+        sqrt(retval);
+        return retval;
+    }
+
+    Vec<T, SIZE> x(const Vec<T, SIZE> &vec)
+    {
+        T temp[SIZE];
+        temp[0] = m_atData[1]*vec(2) - m_atData[2]*vec(1);
+        temp[1] = m_atData[2]*vec(0) - m_atData[0]*vec(2);
+        temp[2] = m_atData[0]*vec(1) - m_atData[1]*vec(0);
+        temp[3] = vec(3);
+        return Vec<T, SIZE> (temp);
+    }
+
+private:
+
+    T m_atData[SIZE];
+
+}; // class Vec
+
+
+// some common vector classes (abbr. names)
+typedef Vec<float, 2> Vec2f;
+typedef Vec<float, 3> Vec3f;
+typedef Vec<float, 4> Vec4f;
+
+typedef Vec<double, 2> Vec2d;
+typedef Vec<double, 3> Vec3d;
+typedef Vec<double, 4> Vec4d;
+
+typedef Vec<int, 2> Vec2i;
+typedef Vec<int, 3> Vec3i;
+typedef Vec<int, 4> Vec4i;
+
+typedef Vec<bool, 2> Vec2b;
+typedef Vec<bool, 3> Vec3b;
+typedef Vec<bool, 4> Vec4b;
+
+
+template <class T, unsigned SIZE>
+static std::ostream& operator<< (std::ostream& output,const Vec<T,SIZE> &vec)
+{
+    output << "( ";
+    for(unsigned int i = 0 ; i< SIZE; ++i)
+    {
+        output << vec(i);
+        if (i<SIZE-1)
+            output << " , ";
+        else
+            output << " )";
+    }
+    return output;
+}
+
+#endif