remove obsolete code of ultrasound

lwrserv/SvrHandling.cpp

@@ -2,33 +2,11 @@
 #include <ctime>
 #include "global.h"
 #include "mat.h"
-#include <boost/lambda/lambda.hpp>
 #include "StringTool.h"
 #include <iostream>
 #include <iterator>
 #include <algorithm>
 
-Mat4f getProbeTrans(){
-    Mat4f Transf;
-    Transf(0,0) = 0.72994228;
-    Transf(0,1) = 0.01910900;
-    Transf(0,2) = 0.67421692;
-    Transf(0,3) = 32.00235858/1000.0;
-
-    Transf(1,0) = 0.00313707;
-    Transf(1,1) = 0.98508639;
-    Transf(1,2) = -0.02537338;
-    Transf(1,3) = -9.29382467/1000.0;
-
-    Transf(2,0) = -0.67612835;
-    Transf(2,1) = 0.02308023;
-    Transf(2,2) = 0.72720430;
-    Transf(2,3) = 152.32390224/1000.0;
-    Transf(3,3) = 1.0;
-
-    return Transf;
-}
-
 void mattoabc(float M[3][4], float &a, float &b, float &c)
 {
     float norm;
@@ -962,11 +940,8 @@ void SvrHandling::MoveCartesian(SocketObject& client, string& args)
     //Calculating end-effector position for target "TRobot"
     Mat4f Tsurface;
     Tsurface = vecToMat(temp);
-    Mat4f TransfUS = getProbeTrans();
-    Mat4f InvTransf;
-    InvTransf = TransfUS.inv();
     Mat4f TRobot;
-    TRobot = Tsurface*InvTransf;
+    TRobot = Tsurface;
     
     //Calculating end-effector position for approach position "ApRobot"
     Mat4f ApRobot;
@@ -975,7 +950,7 @@ void SvrHandling::MoveCartesian(SocketObject& client, string& args)
     // 100mm approach position
     TransZ = getTranslation(0,0,-0.1);
     ApSurface = Tsurface * TransZ;
-    ApRobot = ApSurface * InvTransf;
+    ApRobot = ApSurface ;
     float* tempVec = NULL;
     tempVec = matToVec(TRobot);
     for (int i=0; i<12; i++)
@@ -990,77 +965,6 @@ void SvrHandling::MoveCartesian(SocketObject& client, string& args)
 
 
 
-}
-
-void SvrHandling::DoUltrasound(SocketObject& client, string& args)
-{
-    __SVR_CURRENT_JOB = true;
-    string buf;
-    float temp[15];
-    stringstream ss(args);
-    stringstream ss2f;
-    vector<string> tokens;
-    int i = 0;
-
-    while (ss >> buf)
-    {
-        if (i>15-1)
-        {
-            client.Send(SVR_FALSE_RSP);
-            return;
-        }
-        tokens.push_back(buf);
-        ss2f.flush();
-        ss2f.clear();
-        ss2f << buf;
-        ss2f >> temp[i];
-        i++;
-    }
-    if (i<15-1)
-    {
-        client.Send(SVR_FALSE_RSP);
-        return ;
-    }
-    
-    float arg[3];
-    for (int i=0;i<3;i++)
-    {
-        arg[i] = temp[12+i];
-    }
-
-    //Calculating end-effector position for target "TRobot"
-    Mat4f Tsurface;
-    Tsurface = vecToMat(temp);
-    Mat4f TransfUS = getProbeTrans();
-    Mat4f InvTransf;
-    InvTransf = TransfUS.inv();
-    Mat4f TRobot;
-    //10cm tiefer als Sollposition
-    Mat4f TransZ; 
-    TransZ = getTranslation(0.0f ,0.0f,0.1f);
-    TRobot = Tsurface*TransZ*InvTransf;
-    //TRobot = Tsurface*InvTransf;
-    //Calculating end-effector position for approach position "ApRobot"
-    Mat4f ApRobot;
-    Mat4f ApSurface;
-    /*Mat TransZ;*/ 
-    
-    // 100mm approach position
-    TransZ = getTranslation(0,0,-0.1);
-    ApSurface = Tsurface * TransZ;
-    ApRobot = ApSurface * InvTransf;
-    float* tempVec = NULL;
-    tempVec = matToVec(TRobot);
-
-    for (int i=0; i<12; i++)
-        USTarget[i] = tempVec[i];
-    tempVec = matToVec(ApRobot);
-    for (int i=0; i<12; i++)
-        USApproach[i] = tempVec[i];
-    
-    client.Send(SVR_TRUE_RSP);
-    __DOUS2 = true;
-    
 }
 
 void SvrHandling::quit(SocketObject& client)