add svrData functionality

lwrserv/SvrData.cpp

@@ -1,4 +1,9 @@
+#include <pthread.h>
+#include <errno.h>
+#include <stdio.h>
+#include <string.h>
 #include "SvrData.h"
+
 bool SvrData::instanceFlag = false;
 SvrData* SvrData::single = 0;
 
@@ -26,7 +31,12 @@ SvrData::SvrData(void)
         robot.max.torque[i] = maxTrqJnt[i];
         robot.max.range[i] = maxRangeJnt[i];
     }
+
+    // init mutex for database
+    if (pthread_mutex_init(&dataLock, NULL) != 0)
+        printf("mutex init failed\n");
 }
+
 SvrData::~SvrData()
 {
     if (robot.max.velocity != 0)
@@ -45,6 +55,7 @@ SvrData::~SvrData()
     robot.max.accelaration = 0;
     robot.max.torque = 0;
     robot.max.range = 0;
+    pthread_mutex_destroy(&dataLock);
 }
 
 SvrData* SvrData::getInstance ()
@@ -56,3 +67,165 @@ SvrData* SvrData::getInstance ()
     instanceFlag = true;
     return single;
 }
+
+
+void SvrData::lock()
+{
+    pthread_mutex_lock(&dataLock);
+}
+
+void SvrData::unlock()
+{
+    pthread_mutex_unlock(&dataLock);
+}
+
+int SvrData::getMessuredJointPos(float* data, size_t size)
+{
+    if (data == NULL)
+        return -EINVAL;
+    if (size != sizeof(messured.jointPos))
+        return -EINVAL;
+
+    pthread_mutex_lock(&dataLock);
+    memcpy(data,messured.jointPos,size);
+    pthread_mutex_unlock(&dataLock);
+}
+
+int SvrData::getMessuredCartPos (float* data, size_t size)
+{
+    if (data == NULL)
+        return -EINVAL;
+    if (size != sizeof(messured.cartPos))
+        return -EINVAL;
+
+    pthread_mutex_lock(&dataLock);
+    memcpy(data,messured.cartPos,size);
+    pthread_mutex_unlock(&dataLock);
+}
+
+int SvrData::getMessuredJacobian(float* data, size_t size)
+{
+    if (data == NULL)
+        return -EINVAL;
+    if (size != sizeof(messured.jacobian))
+        return -EINVAL;
+
+    pthread_mutex_lock(&dataLock);
+    memcpy(data,messured.jacobian,size);
+    pthread_mutex_unlock(&dataLock);
+}
+
+int SvrData::getMessuredForceTorque(float* data, size_t size)
+{
+    if (data == NULL)
+        return -EINVAL;
+    if (size != sizeof(messured.forceAndTorque))
+        return -EINVAL;
+
+    pthread_mutex_lock(&dataLock);
+    memcpy(data,messured.forceAndTorque,size);
+    pthread_mutex_unlock(&dataLock);
+}
+
+int SvrData::getCommandedJointPos(float* data, size_t size)
+{
+    if (data == NULL)
+        return -EINVAL;
+    if (size != sizeof(commanded.jointPos))
+        return -EINVAL;
+
+    pthread_mutex_lock(&dataLock);
+    memcpy(data,commanded.jointPos,size);
+    pthread_mutex_unlock(&dataLock);
+}
+
+int SvrData::getCommandedCartPos (float* data, size_t size)
+{
+    if (data == NULL)
+        return -EINVAL;
+    if (size != sizeof(commanded.cartPos))
+        return -EINVAL;
+
+    pthread_mutex_lock(&dataLock);
+    memcpy(data,commanded.cartPos,size);
+    pthread_mutex_unlock(&dataLock);
+}
+
+float SvrData::getMaxTorque(unsigned int pos)
+{
+    float retval = -1.0f;
+    if (pos >= robot.joints)
+        return retval;
+
+    pthread_mutex_lock(&dataLock);
+    retval = robot.max.torque[pos-1];
+    pthread_mutex_unlock(&dataLock);
+}
+
+int   SvrData::getMaxTorque(float* data, size_t size)
+{
+    if (data == NULL)
+        return -EINVAL;
+    if (size != sizeof(float)* robot.joints)
+        return -EINVAL;
+
+    pthread_mutex_lock(&dataLock);
+    memcpy(data,robot.max.torque,size);
+    pthread_mutex_unlock(&dataLock);
+}
+
+float SvrData::getMaxAccelaration(unsigned int pos)
+{
+    float retval = -1.0f;
+    if (pos >= robot.joints)
+        return retval;
+
+    pthread_mutex_lock(&dataLock);
+    retval = robot.max.accelaration[pos-1];
+    pthread_mutex_unlock(&dataLock);
+}
+int   SvrData::getMaxAccelaration(float* data, size_t size)
+{
+    if (data == NULL)
+        return -EINVAL;
+    if (size != sizeof(float)* robot.joints)
+        return -EINVAL;
+
+    pthread_mutex_lock(&dataLock);
+    memcpy(data,robot.max.accelaration,size);
+    pthread_mutex_unlock(&dataLock);
+}
+
+float SvrData::getMaxRange(unsigned int pos)
+{
+    float retval = -1.0f;
+    if (pos >= robot.joints)
+        return retval;
+
+    pthread_mutex_lock(&dataLock);
+    retval = robot.max.range[pos-1];
+    pthread_mutex_unlock(&dataLock);
+}
+
+int   SvrData::getMaxRange(float* data, size_t size)
+{
+    if (data == NULL)
+        return -EINVAL;
+    if (size != sizeof(float)* robot.joints)
+        return -EINVAL;
+
+    pthread_mutex_lock(&dataLock);
+    memcpy(data,robot.max.range,size);
+    pthread_mutex_unlock(&dataLock);
+}
+
+unsigned int SvrData::getJoints()
+{
+    unsigned int retval = 0;
+
+    pthread_mutex_lock(&dataLock);
+    retval = robot.joints;
+    pthread_mutex_unlock(&dataLock);
+    return retval;
+}
+

lwrserv/SvrHandling.cpp

@@ -7,6 +7,9 @@
 #include <iterator>
 #include <algorithm>
 
+#include "SvrHandling.h"
+#include "SvrData.h"
+
 void mattoabc(float M[3][4], float &a, float &b, float &c)
 {
     float norm;
@@ -31,19 +34,7 @@ void mattoabc(float M[3][4], float &a, float &b, float &c)
 
 void debugMat(Mat4f M)
 {
-#if 0 
-    for (int i=0;i<M.rows;i++)
-    {
-        for (int j=0;j<M.cols;j++)
-        {
-            cout << M(i,j) << " ";
-        }
-        cout << "\n\r";
-    }
-    cout << "\n\r\n\r";
-#else
     cout << M << "\n\r" << endl;
-#endif
 }
 
 double* kukaBackwardCalc(double M_[12], float arg[3])
@@ -408,6 +399,8 @@ void SvrHandling::handle(SocketObject& client)
             cout<<message;
             StringTool::String2CmdArg((const string) message, cmd, arg);
 
+            //SvrData::getInstance().lock();
+
             if (cmd == CMD_GetPositionJoints)
             {
                 GetPositionJoints(client);
@@ -462,6 +455,7 @@ void SvrHandling::handle(SocketObject& client)
                 message = "Error: Unknown command!";
                 client.Send(message);
             }
+            //SvrData::getInstance().unlock();
         }else 
         {
             cout << timestamp() + "Connection to client lost..\n";
@@ -489,10 +483,15 @@ void SvrHandling::GetPositionJoints(SocketObject& client)
 {
     string out = "";
     stringstream ss (stringstream::in | stringstream::out);
+
+    float pos[LBR_MNJ];
+    if( 0 != SvrData::getInstance()->getMessuredJointPos(pos,sizeof(pos)))
+        client.Send("ERROR could not get joint pos");
+
     for (int i=0; i< LBR_MNJ; i++)
     {
         ss.str("");
-        ss << (MSRMSRJNTPOS[i]*180/M_PI);
+        ss << (pos[i]*180/M_PI);
         out += ss.str() + " ";
     }
     client.Send(out);
@@ -502,10 +501,15 @@ void SvrHandling::GetPositionHomRowWise(SocketObject& client)
 {
     string out = "";
     stringstream ss (stringstream::in | stringstream::out);
-    for (int i=0; i< 12; i++)
+
+    float pos[FRI_CART_FRM_DIM];
+    if( 0 != SvrData::getInstance()->getMessuredCartPos(pos,sizeof(pos)))
+        client.Send("ERROR could not get cart pos");
+
+    for (int i=0; i< FRI_CART_FRM_DIM; i++)
     {
         ss.str("");
-        ss << (MSRMSRCARTPOS[i]);
+        ss << (pos[i]);
         out += ss.str() + " ";
     }
     client.Send(out);
@@ -515,10 +519,14 @@ void SvrHandling::GetForceTorqueTcp(SocketObject& client)
 {
     string out = "";
     stringstream ss (stringstream::in | stringstream::out);
-    for (int i=0; i< 6; i++)
+
+    float torque[FRI_CART_VEC];
+    if( 0 != SvrData::getInstance()->getMessuredForceTorque(torque,sizeof(torque)))
+        client.Send("ERROR could not get force and torque");
+    for (int i=0; i< FRI_CART_VEC; i++)
     {
         ss.str("");
-        ss << (MSRMSRFTTCP[i]);
+        ss << torque[i];
         out += ss.str() + " ";
     }
     client.Send(out);
@@ -576,7 +584,6 @@ void SvrHandling::MovePTPJoints(SocketObject& client, string& args)
         }
     }
     client.Send(SVR_TRUE_RSP);
-
 }
 
 void SvrHandling::MoveHomRowWiseStatus(SocketObject& client, string& args)
@@ -958,10 +965,6 @@ void SvrHandling::MoveCartesian(SocketObject& client, string& args)
     
     client.Send(SVR_TRUE_RSP);
     __DOUS2 = true;
-
-
-
-
 }
 
 void SvrHandling::quit(SocketObject& client)

lwrserv/include/Singleton.h

@@ -1,7 +1,7 @@
 class Singleton
 {
    public:
-       Singleton* getInstance(void);
+       static Singleton* getInstance(void);
        Singleton() {};
        ~Singleton() {};
    private:

lwrserv/include/SvrData.h

@@ -1,4 +1,3 @@
-
 #ifndef _SVRDATA_H_
 #define _SVRDATA_H_
 
@@ -38,7 +37,7 @@ private:
         unsigned int joints;
     } robot;
 
-    pthread_mutex_t mutexsum;
+    pthread_mutex_t dataLock;
 
     /// private constructor, because the database is a singleton
     SvrData();
@@ -48,7 +47,18 @@ private:
 
 public:
     ~SvrData(void);
-    SvrData* getInstance();
+    static SvrData* getInstance();
+
+    void lock();
+    void unlock();
+
+    int getMessuredJointPos(float* data, size_t size);
+    int getMessuredCartPos (float* data, size_t size);
+    int getMessuredJacobian(float* data, size_t size);
+    int getMessuredForceTorque(float* data, size_t size);
+
+    int getCommandedJointPos(float* data, size_t size);
+    int getCommandedCartPos (float* data, size_t size);
 
     float getMaxTorque(unsigned int pos);
     int   getMaxTorque(float* data, size_t size);

lwrserv/include/mat.h

@@ -70,6 +70,39 @@ public:
     // CMatrix<T, SIZE>operator - (const Matrix<T, SIZE> &mat)
     // ...
 
+    Mat<T, SIZE> operator + (const T &scalar)
+    {
+        Mat<T, SIZE> buf;
+        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
+            for (unsigned int j=0; j<SIZE; j++) // Spalte j
+                buf(i,j) += m_aatData[i][j] + scalar;
+        return buf;
+    }
+    Mat<T, SIZE> operator - (const T &scalar)
+    {
+        Mat<T, SIZE> buf;
+        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
+            for (unsigned int j=0; j<SIZE; j++) // Spalte j
+                buf(i,j) += m_aatData[i][j] - scalar;
+        return buf;
+    }
+    Mat<T, SIZE> operator * (const T &scalar)
+    {
+        Mat<T, SIZE> buf;
+        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
+            for (unsigned int j=0; j<SIZE; j++) // Spalte j
+                buf(i,j) += m_aatData[i][j] * scalar;
+        return buf;
+    }
+    Mat<T, SIZE> operator / (const T &scalar)
+    {
+        Mat<T, SIZE> buf;
+        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
+            for (unsigned int j=0; j<SIZE; j++) // Spalte j
+                buf(i,j) += m_aatData[i][j] / scalar;
+        return buf;
+    }
+
     Mat<T, SIZE> operator * (const Mat<T, SIZE> &mat)
     {
         Mat<T, SIZE> buf;
@@ -80,7 +113,8 @@ public:
         return buf;
     }
 
-    Vec<T, SIZE> operator * (const Vec<T, SIZE> &vec)
+#if 0
+    Mat<T, SIZE> operator * (const Vec<T, SIZE> &vec)
     {
         Vec<T, SIZE> buf;
         for (unsigned int j=0; j<SIZE; j++) // SPALTE j
@@ -88,6 +122,45 @@ public:
                 buf(i) += m_aatData[i][j]*vec(j);
         return buf;
     }
+#endif
+
+    Mat<T,SIZE> determinant()
+    {
+        T buf;
+
+        for (unsigned int j=0; j<SIZE; j++) // SPALTE j
+        {
+            unsigned int i = j;
+            T multBuff;
+            multBuff = m_aatData[i][j];
+            multBuff *= m_aatData[(i+1)%SIZE][(i+1)%SIZE];
+            multBuff *= m_aatData[(i+2)%SIZE][(i+2)%SIZE];
+            buf += multBuff;
+            multBuff = m_aatData[i][j];
+            multBuff *= m_aatData[(i-1)%SIZE][(i-1)%SIZE];
+            multBuff *= m_aatData[(i-2)%SIZE][(i-2)%SIZE];
+            buf -= multBuff;
+        }
+    }
+
+    Mat<T,SIZE> norm()
+    {
+        T buf;
+        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
+            for (unsigned int j=0; j<SIZE; j++) // Spalte j
+                buf +=  pow(abs(m_aatData[i][j]),2);
+        sqrt(buf);
+        return buf;
+    }
+
+    Mat<T,SIZE> transpose()
+    {
+        Mat<T, SIZE> buf;
+        for (unsigned int i=0; i<SIZE; i++) // ZEILE i
+            for (unsigned int j=0; j<SIZE; j++) // Spalte j
+                buf(i,j) += m_aatData[j][i];
+    }
+
     Mat<float, 4> inv()
     {
         Mat<float,4> invOut;