You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1089 lines
24 KiB
1089 lines
24 KiB
#include "header.h"
|
|
#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;
|
|
float sa;
|
|
float ca;
|
|
|
|
norm = sqrt((M[0][0]*M[0][0])+(M[1][0]*M[1][0]));
|
|
|
|
if (norm>1e-5)
|
|
{
|
|
sa = M[1][0]/norm;
|
|
ca = M[0][0]/norm;
|
|
a = atan2(sa,ca);
|
|
} else {
|
|
sa = 0;
|
|
ca = 1;
|
|
a = 0;
|
|
}
|
|
b = atan2(-M[2][0],ca*M[0][0]+sa*M[1][0]);
|
|
c = atan2(sa*M[0][2]-ca*M[1][2],-sa*M[0][1]+ca*M[1][1]);
|
|
}
|
|
|
|
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])
|
|
{
|
|
|
|
double* Joints = new double[7];
|
|
//DH-Parameter
|
|
float alp[7] = {-M_PI/2.0f, M_PI/2.0f, -M_PI/2.0f, M_PI/2.0f, -M_PI/2.0f, M_PI/2.0f, 0.0f};
|
|
float d[7] = {310.4f/1000.0f, 0.0f, 400.1f/1000.0f, 0.0f, 390.0f/1000.0f, 0.0f, 78.0f/1000.0f};
|
|
//Parameter
|
|
int ELBOW = arg[0];
|
|
int FLIP = arg[1];
|
|
float J1os = arg[2]/180.0*M_PI;
|
|
|
|
//Calculating M06
|
|
Mat4f R67;
|
|
int i = 6;
|
|
|
|
R67(0,0) = cos(0);
|
|
R67(0,1) = -sin(0)*cos(alp[i]);
|
|
R67(0,2) = sin(0)*sin(alp[i]);
|
|
R67(0,3) = 0;
|
|
|
|
R67(1,0) = sin(0);
|
|
R67(1,1) = cos(0)*cos(alp[i]);
|
|
R67(1,2) = -cos(0)*sin(alp[i]);
|
|
R67(1,3) = 0;
|
|
|
|
R67(2,0) = 0;
|
|
R67(2,1) = sin(alp[i]);
|
|
R67(2,2) = cos(alp[i]);
|
|
R67(2,3) = d[i];
|
|
|
|
R67(3,0) = 0;
|
|
R67(3,1) = 0;
|
|
R67(3,2) = 0;
|
|
R67(3,3) = 1;
|
|
|
|
Mat4f M;
|
|
for (int i=0;i<3;i++)
|
|
{
|
|
for (int j=0;j<4;j++)
|
|
{
|
|
M(i,j) = (float)M_[i*4+j];
|
|
}
|
|
}
|
|
M(3,3)=(float)1;
|
|
Mat4f M06;
|
|
Mat4f Inv;
|
|
Inv = R67.inv();
|
|
M06=M*Inv;
|
|
|
|
//Joint 1 including given offset
|
|
Joints[0] = atan2(M06(1,3),M06(0,3))+J1os;
|
|
|
|
//Calculating M16
|
|
Mat4f R01;
|
|
i = 0;
|
|
R01(0,0) = cos(Joints[i]);
|
|
R01(0,1) = -sin(Joints[i])*cos(alp[i]);
|
|
R01(0,2) = sin(Joints[i])*sin(alp[i]);
|
|
R01(0,3) = 0;
|
|
|
|
R01(1,0) = sin(Joints[i]);
|
|
R01(1,1) = cos(Joints[i])*cos(alp[i]);
|
|
R01(1,2) = -cos(Joints[i])*sin(alp[i]);
|
|
R01(1,3) = 0;
|
|
|
|
R01(2,0) = 0;
|
|
R01(2,1) = sin(alp[i]);
|
|
R01(2,2) = cos(alp[i]);
|
|
R01(2,3) = d[i];
|
|
|
|
R01(3,0) = 0;
|
|
R01(3,1) = 0;
|
|
R01(3,2) = 0;
|
|
R01(3,3) = 1;
|
|
|
|
Inv = R01.inv();
|
|
Mat4f M16;
|
|
M16 = Inv*M06;
|
|
|
|
//Joint 2
|
|
float m14 = M16(0,3);
|
|
float m24 = M16(1,3);
|
|
float m34 = M16(2,3);
|
|
float d3 = d[2];
|
|
float d5 = d[4];
|
|
|
|
float arg1 = (0.5*(ELBOW*m24*(-ELBOW*m34*m34*m24-ELBOW*m24*d3*d3+ELBOW*m24*d5*d5-ELBOW*m24*m14*m14
|
|
-ELBOW*m24*m24*m24+sqrt(-2*m24*m24*m34*m34*m14*m14-1*m14*m14*d3*d3*d3*d3
|
|
+2*m14*m14*m14*m14*d5*d5+2*m14*m14*m14*m14*d3*d3-1*m14*m14*d5*d5*d5*d5-1*m14*m14*m14*m14*m14*m14
|
|
-2*m24*m24*m14*m14*m14*m14-1*m24*m24*m24*m24*m14*m14-2*m14*m14*m14*m14*m34*m34-1*m34*m34*m34*m34*m14*m14
|
|
+2*m24*m24*d3*d3*m14*m14-2*m14*m14*d3*d3*m34*m34+2*m14*m14*d5*d5*m34*m34
|
|
+2*m14*m14*d5*d5*d3*d3+2*m24*m24*d5*d5*m14*m14))/(m24*m24+m14*m14)
|
|
+m34*m34+d3*d3-1*d5*d5+m14*m14+m24*m24))/(d3*m14);
|
|
|
|
float arg2 = ELBOW*(0.5*(-ELBOW*m34*m34*m24-ELBOW*m24*d3*d3+ELBOW*m24*d5*d5-ELBOW*m24*m14*m14-ELBOW*m24*m24*m24
|
|
+sqrt(-2*m24*m24*m34*m34*m14*m14-1*m14*m14*d3*d3*d3*d3+2*m14*m14*m14*m14*d5*d5
|
|
+2*m14*m14*m14*m14*d3*d3-1*m14*m14*d5*d5*d5*d5-1*m14*m14*m14*m14*m14*m14-2*m24*m24*m14*m14*m14*m14-1*m24*m24*m24*m24
|
|
*m14*m14-2*m14*m14*m14*m14*m34*m34-1*m34*m34*m34*m34*m14*m14+2*m24*m24*d3*d3*m14*m14
|
|
-2*m14*m14*d3*d3*m34*m34+2*m14*m14*d5*d5*m34*m34+2*m14*m14*d5*d5*d3*d3
|
|
+2*m24*m24*d5*d5*m14*m14)))/((m24*m24+m14*m14)*d3);
|
|
|
|
Joints[1] = atan2(arg1,arg2);
|
|
|
|
//Calculating M26
|
|
Mat4f R12;
|
|
i = 1;
|
|
|
|
R12(0,0) = cos(Joints[i]);
|
|
R12(0,1) = -sin(Joints[i])*cos(alp[i]);
|
|
R12(0,2) = sin(Joints[i])*sin(alp[i]);
|
|
R12(0,3) = 0;
|
|
|
|
R12(1,0) = sin(Joints[i]);
|
|
R12(1,1) = cos(Joints[i])*cos(alp[i]);
|
|
R12(1,2) = -cos(Joints[i])*sin(alp[i]);
|
|
R12(1,3) = 0;
|
|
|
|
R12(2,0) = 0;
|
|
R12(2,1) = sin(alp[i]);
|
|
R12(2,2) = cos(alp[i]);
|
|
R12(2,3) = d[i];
|
|
|
|
R12(3,0) = 0;
|
|
R12(3,1) = 0;
|
|
R12(3,2) = 0;
|
|
R12(3,3) = 1;
|
|
|
|
Mat4f R02;
|
|
R02 = R01*R12;
|
|
Inv = R02.inv();
|
|
Mat4f M26;
|
|
M26 = Inv*M06;
|
|
|
|
//Joint 3
|
|
Joints[2] = atan2(ELBOW*M26(1,3),ELBOW*M26(0,3));
|
|
|
|
//Calculating M36
|
|
Mat4f R23;
|
|
i = 2;
|
|
|
|
R23(0,0) = cos(Joints[i]);
|
|
R23(0,1) = -sin(Joints[i])*cos(alp[i]);
|
|
R23(0,2) = sin(Joints[i])*sin(alp[i]);
|
|
R23(0,3) = 0;
|
|
|
|
R23(1,0) = sin(Joints[i]);
|
|
R23(1,1) = cos(Joints[i])*cos(alp[i]);
|
|
R23(1,2) = -cos(Joints[i])*sin(alp[i]);
|
|
R23(1,3) = 0;
|
|
|
|
R23(2,0) = 0;
|
|
R23(2,1) = sin(alp[i]);
|
|
R23(2,2) = cos(alp[i]);
|
|
R23(2,3) = d[i];
|
|
|
|
R23(3,0) = 0;
|
|
R23(3,1) = 0;
|
|
R23(3,2) = 0;
|
|
R23(3,3) = 1;
|
|
|
|
Mat4f R03;
|
|
R03 = R01*R12*R23;
|
|
Inv = R03.inv();
|
|
Mat4f M36;
|
|
M36 = Inv*M06;
|
|
|
|
//Joint 4
|
|
Joints[3] = atan2(M36(0,3),-M36(1,3));
|
|
|
|
//Calculating M47
|
|
Mat4f R34;
|
|
i = 3;
|
|
R34(0,0) = cos(Joints[i]);
|
|
R34(0,1) = -sin(Joints[i])*cos(alp[i]);
|
|
R34(0,2) = sin(Joints[i])*sin(alp[i]);
|
|
R34(0,3) = 0;
|
|
|
|
R34(1,0) = sin(Joints[i]);
|
|
R34(1,1) = cos(Joints[i])*cos(alp[i]);
|
|
R34(1,2) = -cos(Joints[i])*sin(alp[i]);
|
|
R34(1,3) = 0;
|
|
|
|
R34(2,0) = 0;
|
|
R34(2,1) = sin(alp[i]);
|
|
R34(2,2) = cos(alp[i]);
|
|
R34(2,3) = d[i];
|
|
|
|
R34(3,0) = 0;
|
|
R34(3,1) = 0;
|
|
R34(3,2) = 0;
|
|
R34(3,3) = 1;
|
|
|
|
Mat4f R04;
|
|
R04 = R01*R12*R23*R34;
|
|
Inv = R04.inv();
|
|
Mat4f M47;
|
|
M47 = Inv*M;
|
|
|
|
//Joint 5
|
|
float th = 0.0001;
|
|
if (abs(M47(1,2))<th && abs(M47(0,2))<th)
|
|
Joints[4] = 0;
|
|
else
|
|
Joints[4] = atan2(FLIP*M47(1,2),FLIP*M47(0,2));
|
|
|
|
//Joint 6
|
|
Joints[5] = atan2(FLIP*sqrt(1-M47(2,2)*M47(2,2)),M47(2,2));
|
|
|
|
//Joint 7
|
|
if (abs(M47(2,1))<th && abs(M47(2,0))<th)
|
|
Joints[6] = 0;
|
|
else
|
|
Joints[6] = atan2(FLIP*M47(2,1),FLIP*-M47(2,0));
|
|
|
|
for (int i = 0; i<7; i++)
|
|
{
|
|
Joints[i] = Joints[i]*180.0/M_PI;
|
|
}
|
|
|
|
//Kuka Joints
|
|
//Joints[3] = -Joints[3];
|
|
Joints[1] = -Joints[1];
|
|
Joints[5] = -Joints[5];
|
|
|
|
return Joints;
|
|
}
|
|
|
|
bool checkJntRange(double Joints[7])
|
|
{
|
|
for (int i = 0; i<LBR_MNJ; i++)
|
|
{
|
|
if (abs(Joints[i])>abs(maxRangeJnt[i]))
|
|
{
|
|
// join angle is too big
|
|
return false;
|
|
}
|
|
}
|
|
// no join angle is too big
|
|
return false;
|
|
}
|
|
|
|
Mat4f vecToMat(float vec[12])
|
|
{
|
|
Mat4f result;
|
|
for (int i=0; i<3; i++)
|
|
{
|
|
for (int j=0; j<4; j++)
|
|
{
|
|
result(i,j) = vec[i*4+j];
|
|
}
|
|
}
|
|
result(3,3)= 1.0f;
|
|
return result;
|
|
}
|
|
|
|
float* matToVec(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;
|
|
}
|
|
|
|
Mat4f getTranslation(double tx, double ty, double tz)
|
|
{
|
|
Mat4f transl;
|
|
transl(0,0) = 1;
|
|
transl(1,1) = 1;
|
|
transl(2,2) = 1;
|
|
|
|
transl(0,3) = tx;
|
|
transl(1,3) = ty;
|
|
transl(2,3) = tz;
|
|
transl(3,3) = 1;
|
|
return transl;
|
|
}
|
|
|
|
SvrHandling::SvrHandling()
|
|
{
|
|
}
|
|
|
|
SvrHandling::~SvrHandling()
|
|
{
|
|
}
|
|
|
|
void SvrHandling::run()
|
|
{
|
|
this->run(SVR_DEFAULT_PORT);
|
|
}
|
|
|
|
void SvrHandling::run(int port)
|
|
{
|
|
cout << timestamp() + "Starting " << SVR_NAME << "\n";
|
|
port = SVR_DEFAULT_PORT;
|
|
while (c_state !=done)
|
|
{
|
|
SocketObject *socket = new SocketObject;
|
|
if (socket->Bind(port) <0)
|
|
{
|
|
exit (1);
|
|
}
|
|
socket->Listen();
|
|
while(1)
|
|
{
|
|
SocketObject *client = new SocketObject;
|
|
if (socket->Accept(*client))
|
|
{
|
|
cout << timestamp() + "Client accepted!\n";
|
|
if (handshakeAccepted(*client))
|
|
{
|
|
handle(*client);
|
|
}
|
|
client->Disconnect();
|
|
cout << timestamp() + "Client disconnected.\n";
|
|
}
|
|
else
|
|
{
|
|
cout << timestamp() + "Client bin error.\n";
|
|
}
|
|
delete client;
|
|
}
|
|
delete socket;
|
|
}
|
|
}
|
|
|
|
bool SvrHandling::handshakeAccepted(SocketObject& client)
|
|
{
|
|
this->c_state = handshake;
|
|
string message = SVR_HELLO_MSG;
|
|
client.Send(message);
|
|
client.Recv(message);
|
|
if (message.find(SVR_HANDSHAKE,0) != string::npos)
|
|
{
|
|
c_state = accepted;
|
|
message = SVR_ACCEPTED;
|
|
}
|
|
else
|
|
{
|
|
c_state = waiting;
|
|
message = SVR_FAILED;
|
|
cout << timestamp() + "Handshake failed. " << "Invalid recv msg \'" << message <<"\'";
|
|
}
|
|
client.Send(message);
|
|
return(c_state == accepted);
|
|
}
|
|
|
|
void SvrHandling::handle(SocketObject& client)
|
|
{
|
|
string message, cmd, arg;
|
|
while (c_state == accepted)
|
|
{
|
|
if (client.Recv(message) > 0)
|
|
{
|
|
cout<<message;
|
|
StringTool::String2CmdArg((const string) message, cmd, arg);
|
|
|
|
if (cmd == CMD_GetPositionJoints)
|
|
{
|
|
GetPositionJoints(client);
|
|
} else if (cmd == CMD_GetPositionHomRowWise)
|
|
{
|
|
GetPositionHomRowWise(client);
|
|
} else if (cmd == CMD_GetForceTorqueTcp)
|
|
{
|
|
GetForceTorqueTcp(client);
|
|
} else if (cmd == CMD_MovePTPJoints)
|
|
{
|
|
MovePTPJoints(client, arg);
|
|
} else if (cmd == CMD_MoveHomRowWiseStatus)
|
|
{
|
|
MoveHomRowWiseStatus(client, arg);
|
|
// TODO: MoveMinChangeHomRowWiseStatus
|
|
} else if (cmd == CMD_SetSpeed)
|
|
{
|
|
SetSpeed(client, arg);
|
|
} else if (cmd == CMD_SetAccel)
|
|
{
|
|
SetAccel(client, arg);
|
|
}else if (cmd == CMD_StartPotFieldMode)
|
|
{
|
|
StartPotFieldMode(client, arg);
|
|
}else if (cmd == CMD_StopPotFieldMode)
|
|
{
|
|
StopPotFieldMode(client, arg);
|
|
}else if (cmd == CMD_SetPos)
|
|
{
|
|
SetPos(client, arg);
|
|
}else if (cmd == CMD_SetJoints)
|
|
{
|
|
SetJoints(client, arg);
|
|
}else if (cmd == CMD_MoveCartesian)
|
|
{
|
|
MoveCartesian(client, arg);
|
|
}else if (cmd == CMD_DoUltrasound)
|
|
{
|
|
DoUltrasound(client, arg);
|
|
} else if (cmd == "debug")
|
|
{
|
|
debug(client);
|
|
} else if (cmd == "")
|
|
{
|
|
continue;
|
|
} else if (cmd == CMD_QUIT || cmd == CMD_QUIT1 || cmd == CMD_QUIT2)
|
|
{
|
|
quit(client);
|
|
} else if (cmd == CMD_ISKUKA)
|
|
{
|
|
client.Send(SVR_TRUE_RSP);
|
|
} else
|
|
{
|
|
message = "Error: Unknown command!";
|
|
client.Send(message);
|
|
}
|
|
}else
|
|
{
|
|
cout << timestamp() + "Connection to client lost..\n";
|
|
c_state = waiting;
|
|
}
|
|
}
|
|
}
|
|
|
|
string SvrHandling::timestamp()
|
|
{
|
|
time_t rawtime;
|
|
struct tm * timeinfo;
|
|
char buffer [80];
|
|
|
|
time ( &rawtime );
|
|
timeinfo = localtime ( &rawtime );
|
|
|
|
strftime (buffer,80,"%Y-%m-%d-%H-%M-%S",timeinfo);
|
|
string temp = ": ";
|
|
string output = buffer + temp;
|
|
return output;
|
|
}
|
|
|
|
void SvrHandling::GetPositionJoints(SocketObject& client)
|
|
{
|
|
string out = "";
|
|
stringstream ss (stringstream::in | stringstream::out);
|
|
for (int i=0; i< LBR_MNJ; i++)
|
|
{
|
|
ss.str("");
|
|
ss << (MSRMSRJNTPOS[i]*180/M_PI);
|
|
out += ss.str() + " ";
|
|
}
|
|
client.Send(out);
|
|
}
|
|
|
|
void SvrHandling::GetPositionHomRowWise(SocketObject& client)
|
|
{
|
|
string out = "";
|
|
stringstream ss (stringstream::in | stringstream::out);
|
|
for (int i=0; i< 12; i++)
|
|
{
|
|
ss.str("");
|
|
ss << (MSRMSRCARTPOS[i]);
|
|
out += ss.str() + " ";
|
|
}
|
|
client.Send(out);
|
|
}
|
|
|
|
void SvrHandling::GetForceTorqueTcp(SocketObject& client)
|
|
{
|
|
string out = "";
|
|
stringstream ss (stringstream::in | stringstream::out);
|
|
for (int i=0; i< 6; i++)
|
|
{
|
|
ss.str("");
|
|
ss << (MSRMSRFTTCP[i]);
|
|
out += ss.str() + " ";
|
|
}
|
|
client.Send(out);
|
|
}
|
|
|
|
void SvrHandling::MovePTPJoints(SocketObject& client, string& args)
|
|
{
|
|
__SVR_CURRENT_JOB = true;
|
|
string buf;
|
|
float temp[LBR_MNJ];
|
|
stringstream ss(args);
|
|
stringstream ss2f;
|
|
vector<string> tokens;
|
|
int i = 0;
|
|
while (ss >> buf)
|
|
{
|
|
if (i>LBR_MNJ-1)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
tokens.push_back(buf);
|
|
ss2f.flush();
|
|
ss2f.clear();
|
|
ss2f << buf;
|
|
ss2f >> temp[i];
|
|
i++;
|
|
}
|
|
if (i<LBR_MNJ-1)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return ;
|
|
}
|
|
for (int i=0 ;i<LBR_MNJ; i++)
|
|
{
|
|
MSRCMDJNTPOS[i]=temp[i];
|
|
}
|
|
|
|
//Check for Joint Range
|
|
if (!checkJntRange(MSRCMDJNTPOS))
|
|
{
|
|
string out = "Error: Joints out of Range!";
|
|
client.Send(out);
|
|
return;
|
|
}
|
|
|
|
__MSRCMDJNTPOS = true;
|
|
|
|
//Wait to end of movement
|
|
while (1)
|
|
{
|
|
if (!__MSRCMDJNTPOS)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
client.Send(SVR_TRUE_RSP);
|
|
|
|
}
|
|
|
|
void SvrHandling::MoveHomRowWiseStatus(SocketObject& client, string& args)
|
|
{
|
|
__SVR_CURRENT_JOB = true;
|
|
string buf;
|
|
double 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 ;
|
|
}
|
|
for (int i=0 ;i<3; i++)
|
|
{
|
|
for (int j=0;j<4;j++)
|
|
{
|
|
MSRCMDPOSE[i][j]=temp[i*4+j];
|
|
}
|
|
}
|
|
float arg[3];
|
|
for (int i=0;i<3;i++)
|
|
{
|
|
arg[i] = temp[12+i];
|
|
}
|
|
|
|
//Backward Calculation
|
|
|
|
double* CalcJoints=NULL;
|
|
CalcJoints = kukaBackwardCalc(temp, arg);
|
|
|
|
//Check for Joint Range
|
|
if (!checkJntRange(CalcJoints))
|
|
{
|
|
string out = "Error: Joints out of Range!";
|
|
client.Send(out);
|
|
return;
|
|
}
|
|
|
|
// Jmove
|
|
for (int i=0 ;i<LBR_MNJ; i++)
|
|
{
|
|
MSRCMDJNTPOS[i]=CalcJoints[i];
|
|
}
|
|
__MSRCMDJNTPOS = true;
|
|
|
|
//Wait to end of movement
|
|
while (1)
|
|
{
|
|
if (!__MSRCMDJNTPOS)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
client.Send(SVR_TRUE_RSP);
|
|
|
|
}
|
|
|
|
void SvrHandling::SetSpeed(SocketObject& client, string& args)
|
|
{
|
|
__SETVELOCITY = true;
|
|
__SVR_CURRENT_JOB = true;
|
|
string buf;
|
|
float temp[1];
|
|
stringstream ss(args);
|
|
stringstream ss2f;
|
|
vector<string> tokens;
|
|
int i = 0;
|
|
if (args == "")
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
while (ss >> buf)
|
|
{
|
|
if (i>0)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
tokens.push_back(buf);
|
|
ss2f.flush();
|
|
ss2f.clear();
|
|
ss2f << buf;
|
|
ss2f >> temp[i];
|
|
i++;
|
|
}
|
|
if ((temp[0] < 1)||(temp[0]>1000))
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
VELOCITY = temp[0];
|
|
client.Send(SVR_TRUE_RSP);
|
|
__SVR_CURRENT_JOB = false;
|
|
}
|
|
|
|
void SvrHandling::SetAccel(SocketObject& client, string& args)
|
|
{
|
|
__SETACCEL = true;
|
|
__SVR_CURRENT_JOB = true;
|
|
string buf;
|
|
float temp[1];
|
|
stringstream ss(args);
|
|
stringstream ss2f;
|
|
vector<string> tokens;
|
|
int i = 0;
|
|
|
|
if (args == "")
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
while (ss >> buf)
|
|
{
|
|
if (i>0)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
tokens.push_back(buf);
|
|
ss2f.flush();
|
|
ss2f.clear();
|
|
ss2f << buf;
|
|
ss2f >> temp[i];
|
|
i++;
|
|
}
|
|
if ((temp[0] < 1)||(temp[0]>100))
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
ACCELARATION = temp[0];
|
|
client.Send(SVR_TRUE_RSP);
|
|
__SVR_CURRENT_JOB = false;
|
|
}
|
|
|
|
void SvrHandling::StartPotFieldMode(SocketObject& client, string& args)
|
|
{
|
|
__MSRSTARTPOTFIELD = false;
|
|
__MSRSTOPPOTFIELD = false;
|
|
//Set current Joint Vals
|
|
for (int i=0; i<LBR_MNJ; i++)
|
|
{
|
|
MSRCMDJNTPOS[i] = MSRMSRJNTPOS[i]*180/M_PI;
|
|
}
|
|
__POTFIELDMODE = true;
|
|
while (1)
|
|
{
|
|
if (!__MSRSTARTPOTFIELD)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
|
|
void SvrHandling::StopPotFieldMode(SocketObject& client, string& args)
|
|
{
|
|
__POTFIELDMODE = false;
|
|
|
|
while (1)
|
|
{
|
|
if (!__MSRSTOPPOTFIELD)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
|
|
void SvrHandling::SetPos(SocketObject& client, string& args){
|
|
|
|
string buf;
|
|
double 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 ;
|
|
}
|
|
for (int i=0 ;i<3; i++)
|
|
{
|
|
for (int j=0;j<4;j++)
|
|
{
|
|
MSRCMDPOSE[i][j]=temp[i*4+j];
|
|
}
|
|
}
|
|
float arg[3];
|
|
for (int i=0;i<3;i++)
|
|
{
|
|
arg[i] = temp[12+i];
|
|
}
|
|
|
|
//Backward Calculation
|
|
|
|
double* CalcJoints=NULL;
|
|
CalcJoints = kukaBackwardCalc(temp, arg);
|
|
|
|
//Check for Joint Range
|
|
if (!checkJntRange(CalcJoints))
|
|
{
|
|
string out = "Error: Joints out of Range!";
|
|
client.Send(out);
|
|
return;
|
|
}
|
|
|
|
// Set global Position
|
|
__MSRSETPOS = true;
|
|
globi = 1;
|
|
for (int i=0 ;i<LBR_MNJ; i++)
|
|
{
|
|
MSRCMDJNTPOS[i]=CalcJoints[i];
|
|
}
|
|
__MSRSETPOS = false;
|
|
sleep(5);
|
|
globi = 0;
|
|
//__MSRCMDJNTPOS = true;
|
|
|
|
////Wait to end of movement
|
|
//while (1)
|
|
//{
|
|
// if (!__MSRCMDJNTPOS)
|
|
// {
|
|
// break;
|
|
// }
|
|
//}
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
|
|
void SvrHandling::SetJoints(SocketObject& client, string& args)
|
|
{
|
|
string buf;
|
|
double temp[15];
|
|
stringstream ss(args);
|
|
stringstream ss2f;
|
|
vector<string> tokens;
|
|
int i = 0;
|
|
|
|
while (ss >> buf)
|
|
{
|
|
if (i>LBR_MNJ-1)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
tokens.push_back(buf);
|
|
ss2f.flush();
|
|
ss2f.clear();
|
|
ss2f << buf;
|
|
ss2f >> temp[i];
|
|
i++;
|
|
}
|
|
if (i<LBR_MNJ-1)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
|
|
//Check for Joint Range
|
|
if (!checkJntRange(temp))
|
|
{
|
|
string out = "Error: Joints out of Range!";
|
|
client.Send(out);
|
|
return;
|
|
}
|
|
|
|
// Set global Position
|
|
__MSRSETPOS = true;
|
|
globi = 1;
|
|
for (int i=0 ;i<LBR_MNJ; i++)
|
|
{
|
|
MSRCMDJNTPOS[i]=temp[i];
|
|
}
|
|
__MSRSETPOS = false;
|
|
sleep(5);
|
|
globi = 0;
|
|
//__MSRCMDJNTPOS = true;
|
|
|
|
////Wait to end of movement
|
|
//while (1){
|
|
// if (!__MSRCMDJNTPOS){
|
|
// break;}
|
|
//}
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
|
|
void SvrHandling::MoveCartesian(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;
|
|
TRobot = Tsurface*InvTransf;
|
|
|
|
//Calculating end-effector position for approach position "ApRobot"
|
|
Mat4f ApRobot;
|
|
Mat4f ApSurface;
|
|
Mat4f 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::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)
|
|
{
|
|
client.Send(SVR_TRUE_RSP);
|
|
client.Disconnect();
|
|
}
|
|
|
|
void SvrHandling::debug(SocketObject& client)
|
|
{
|
|
|
|
__DEBUG = true;
|
|
|
|
//check = true;
|
|
//float temp[7];
|
|
//while (1){
|
|
// friInst.setToKRLInt(15,1);
|
|
// friInst.doSendData();
|
|
// //client.Send(friInst.getFrmKRLInt(2));
|
|
// for ( int i= 0; i < LBR_MNJ; i++)
|
|
//{
|
|
// temp[i]=friInst.getMsrMsrJntPosition()[i];
|
|
//}
|
|
//friInst.doPositionControl(temp);
|
|
//}
|
|
}
|