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.
1096 lines
28 KiB
1096 lines
28 KiB
#include "header.h"
|
|
#include <ctime>
|
|
#include "global.h"
|
|
#include "mat.h"
|
|
#include "StringTool.h"
|
|
#include <iostream>
|
|
#include <iterator>
|
|
#include <algorithm>
|
|
|
|
#include "SvrHandling.h"
|
|
#include "SvrData.h"
|
|
#include "Trajectroy.h"
|
|
#include "LinearTrajectory.h"
|
|
#include <boost/thread/thread.hpp>
|
|
|
|
void printUsage(SocketObject& client, std::string& arg)
|
|
{
|
|
(void) arg;
|
|
for (unsigned int i = 0 ; i < commandCount; ++i)
|
|
{
|
|
if(commands[i].printHelp != NULL)
|
|
commands[i].printHelp();
|
|
else
|
|
client.Send(commands[i].aberration+"\t-\t"+commands[i].longVersion);
|
|
}
|
|
}
|
|
|
|
VecJoint kukaBackwardCalc(double M_[12], float arg[3])
|
|
{
|
|
VecJoint Joints(0.0f);
|
|
//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;
|
|
}
|
|
|
|
Mat4f vecToMat(float vec[12])
|
|
{
|
|
Mat4f result;
|
|
for (int i=0; i<3; i++) // ZEILE
|
|
{
|
|
for (int j=0; j<4; j++) // SPALTE
|
|
{
|
|
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()
|
|
{
|
|
// add each command which is possible
|
|
commands = new ClientCommand[20];
|
|
|
|
int i = 0;
|
|
commands[i].aberration = "GPJ";
|
|
commands[i].longVersion = "GetPositionJoints";
|
|
commands[i].processCommand = &getPositionJoints;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "GPHRW";
|
|
commands[i].longVersion = "GetPositionHomRowWise";
|
|
commands[i].processCommand = &getPositionHomRowWise;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "GFT";
|
|
commands[i].longVersion = "GetForceTorqueTcp";
|
|
commands[i].processCommand = &getForceTorqueTcp;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "MPTPJ";
|
|
commands[i].longVersion = "MovePTPJoints";
|
|
commands[i].processCommand = &movePTPJoints;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "MHRWS";
|
|
commands[i].longVersion = "MoveHomRowWiseStatus";
|
|
commands[i].processCommand = &moveHomRowWiseStatus;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "SS";
|
|
commands[i].longVersion = "SetSpeed";
|
|
commands[i].processCommand = &setSpeed;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "SA";
|
|
commands[i].longVersion = "SetAccel";
|
|
commands[i].processCommand = &setAccel;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "STPF";
|
|
commands[i].longVersion = "StartPotFieldMode";
|
|
commands[i].processCommand = &startPotFieldMode;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "SPPF";
|
|
commands[i].longVersion = "StopPotFieldMode";
|
|
commands[i].processCommand = &stopPotFieldMode;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "SP";
|
|
commands[i].longVersion = "SetPos";
|
|
commands[i].processCommand = &setPos;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "SJ";
|
|
commands[i].longVersion = "SetJoints";
|
|
commands[i].processCommand = &setJoints;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "Q";
|
|
commands[i].longVersion = "Quit";
|
|
commands[i].processCommand = &quit;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "?";
|
|
commands[i].longVersion = "Help";
|
|
commands[i].processCommand = &printUsage;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "ISKUKA";
|
|
commands[i].longVersion = "IsKukaLWR";
|
|
commands[i].processCommand = NULL;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "MC";
|
|
commands[i].longVersion = "MoveCartesian";
|
|
commands[i].processCommand = &moveCartesian;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "GT";
|
|
commands[i].longVersion = "GetTrajectoryType";
|
|
commands[i].processCommand = &getTrajectoryType;
|
|
commands[i].printHelp = NULL;
|
|
i+=1;
|
|
commands[i].aberration = "ST";
|
|
commands[i].longVersion = "SetTrajectoryType";
|
|
commands[i].processCommand = &setTrajectoryType;
|
|
commands[i].printHelp = NULL;
|
|
|
|
commandCount = i;
|
|
}
|
|
|
|
SvrHandling::~SvrHandling()
|
|
{
|
|
}
|
|
|
|
void SvrHandling::run()
|
|
{
|
|
this->run(SVR_DEFAULT_PORT);
|
|
}
|
|
|
|
void SvrHandling::run(int port)
|
|
{
|
|
cout << timestamp() + "Starting " << SVR_NAME << " on port "<< port <<"\n";
|
|
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))
|
|
{
|
|
clientCommandLoop(*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::clientCommandLoop(SocketObject& client)
|
|
{
|
|
string message, cmd, arg;
|
|
while (c_state == accepted)
|
|
{
|
|
if (client.Recv(message) > 0)
|
|
{
|
|
cout<<message;
|
|
StringTool::String2CmdArg((const string) message, cmd, arg);
|
|
|
|
// ignore blank lines
|
|
if (cmd == "")
|
|
continue;
|
|
|
|
// check command table for equivalence
|
|
bool match = false;
|
|
for (unsigned int i = 0 ; i < commandCount ; ++ i)
|
|
{
|
|
if (cmd == commands[i].longVersion || cmd == commands[i].aberration)
|
|
{
|
|
if (commands[i].processCommand != NULL)
|
|
{
|
|
commands[i].processCommand(client,arg);
|
|
}else
|
|
{
|
|
client.Send(SVR_UNIMPLEMENTED_COMMAND);
|
|
}
|
|
match = true;
|
|
match = true;
|
|
break;
|
|
}
|
|
}
|
|
// command not found
|
|
if (match == false)
|
|
{
|
|
client.Send(SVR_UNKNOWN_COMMAND);
|
|
printUsage(client, arg);
|
|
}
|
|
|
|
}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 getPositionJoints(SocketObject& client, std::string& arg)
|
|
{
|
|
// unused
|
|
(void) arg;
|
|
|
|
string out = "";
|
|
stringstream ss (stringstream::in | stringstream::out);
|
|
|
|
// get current joint positions from database
|
|
VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos();
|
|
|
|
// assemble command feadback
|
|
for (int i=0; i< JOINT_NUMBER; i++)
|
|
{
|
|
ss.str("");
|
|
ss << (jointPos(i)*180/M_PI);
|
|
out += ss.str() + " ";
|
|
}
|
|
|
|
// send msg to client
|
|
client.Send(out);
|
|
}
|
|
|
|
void getPositionHomRowWise(SocketObject& client, std::string& arg)
|
|
{
|
|
// unused
|
|
(void) arg;
|
|
|
|
string out = "";
|
|
stringstream ss (stringstream::in | stringstream::out);
|
|
|
|
// get current Cartesian positions from database
|
|
MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos();
|
|
|
|
// assemble command feedback row wise
|
|
for (int i=0; i< FRI_CART_FRM_DIM; i++)
|
|
{
|
|
int row = i % 4;
|
|
int column = i / 4;
|
|
|
|
ss.str("");
|
|
ss << (cartPos(row,column));
|
|
out += ss.str() + " ";
|
|
}
|
|
|
|
// send msg to client
|
|
client.Send(out);
|
|
}
|
|
|
|
void getForceTorqueTcp(SocketObject& client, std::string& arg)
|
|
{
|
|
// unused
|
|
(void) arg;
|
|
|
|
string out = "";
|
|
stringstream ss (stringstream::in | stringstream::out);
|
|
|
|
VecTorque torque = SvrData::getInstance()->getMessuredForceTorque();
|
|
for (int i=0; i< FRI_CART_VEC; i++)
|
|
{
|
|
ss.str("");
|
|
ss << torque(i);
|
|
out += ss.str() + " ";
|
|
}
|
|
client.Send(out);
|
|
}
|
|
|
|
void movePTPJoints(SocketObject& client, string& arg)
|
|
{
|
|
string buf;
|
|
|
|
stringstream ss(arg);
|
|
stringstream ss2f;
|
|
|
|
// convert string to joint vector
|
|
VecJoint newJointPos(0.0f);
|
|
int i = 0;
|
|
while (ss >> buf)
|
|
{
|
|
// to many joint values
|
|
if (i>=JOINT_NUMBER)
|
|
{
|
|
client.Send(SVR_INVALID_ARGUMENT_COUNT);
|
|
return;
|
|
}
|
|
ss2f.flush();
|
|
ss2f.clear();
|
|
ss2f << buf;
|
|
ss2f >> newJointPos(i);
|
|
i++;
|
|
}
|
|
|
|
// to less joint values
|
|
if (i!=JOINT_NUMBER)
|
|
{
|
|
client.Send(SVR_INVALID_ARGUMENT_COUNT);
|
|
return ;
|
|
}
|
|
|
|
|
|
//check for joint range
|
|
if (!SvrData::getInstance()->checkJointRange(newJointPos))
|
|
{
|
|
string out = "Error: Joints out of Range!";
|
|
client.Send(SVR_OUT_OF_RANGE);
|
|
return;
|
|
}
|
|
//set new target for next trajectory
|
|
VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos();
|
|
class Trajectory<JOINT_NUMBER>* newTrajectory;
|
|
|
|
float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
|
|
VecJoint maxJointVelocity = SvrData::getInstance()->getMaxVelocity();
|
|
VecJoint maxJointAcceleration = SvrData::getInstance()->getMaxAcceleration();
|
|
|
|
newTrajectory = (class Trajectory<JOINT_NUMBER>*) new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos);
|
|
|
|
// add trajectory to queue for sender thread
|
|
SvrData::getInstance()->pushTrajectory(newTrajectory);
|
|
|
|
SvrData::getInstance()->setCommandedJointPos(newJointPos);
|
|
|
|
//Wait to end of movement
|
|
while (true)
|
|
{
|
|
if (SvrData::getInstance()->getTrajectoryDone() == true)
|
|
break;
|
|
boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) );
|
|
}
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
|
|
void moveHomRowWiseStatus(SocketObject& client, string& arg)
|
|
{
|
|
string buf;
|
|
double temp[15];
|
|
stringstream ss(arg);
|
|
stringstream ss2f;
|
|
int argc = 0;
|
|
|
|
// convert position parameters from string to float
|
|
while (ss >> buf)
|
|
{
|
|
// only need 15 parameters not more
|
|
if (argc>15-1)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
|
|
ss2f.flush();
|
|
ss2f.clear();
|
|
ss2f << buf;
|
|
ss2f >> temp[argc];
|
|
argc++;
|
|
}
|
|
|
|
// check for exactly 15 parameters
|
|
if (argc != 15)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return ;
|
|
}
|
|
|
|
// first part is homegenous position koordinates
|
|
MatCarthesian newCartPos(0.0f, 1.0f);
|
|
for (int i=0 ;i<3; i++)
|
|
{
|
|
for (int j=0;j<4;j++)
|
|
{
|
|
newCartPos(i,j)=temp[i*4+j];
|
|
}
|
|
}
|
|
|
|
// extract elbow flip and hand parameter
|
|
float configurationParameter[3];
|
|
for (int i=0;i<3;i++)
|
|
{
|
|
configurationParameter[i] = temp[12+i];
|
|
}
|
|
|
|
//Backward Calculation
|
|
VecJoint newJointPos = kukaBackwardCalc(temp, configurationParameter);
|
|
|
|
|
|
//Check for Joint Range
|
|
if (!SvrData::getInstance()->checkJointRange(newJointPos))
|
|
{
|
|
string out = "Error: Joints out of Range!";
|
|
client.Send(out);
|
|
return;
|
|
}
|
|
|
|
// calculate trajectory
|
|
VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos();
|
|
float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
|
|
VecJoint maxJointVelocity = SvrData::getInstance()->getMaxVelocity();
|
|
VecJoint maxJointAcceleration = SvrData::getInstance()->getMaxAcceleration();
|
|
|
|
class Trajectory<JOINT_NUMBER>* newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos);
|
|
|
|
// set new commanded position for next trajectory
|
|
SvrData::getInstance()->setCommandedJointPos(newJointPos);
|
|
|
|
// add new trajectory to position
|
|
SvrData::getInstance()->pushTrajectory(newTrajectory);
|
|
|
|
//Wait to end of movement
|
|
while (true)
|
|
{
|
|
if (SvrData::getInstance()->getTrajectoryDone() == true)
|
|
break;
|
|
boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) );
|
|
}
|
|
|
|
// send a positive client feedback
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
|
|
void setSpeed(SocketObject& client, string& arg)
|
|
{
|
|
string buf;
|
|
float newVelocityPercentage;
|
|
int argc = 0;
|
|
stringstream ss(arg);
|
|
stringstream ss2f;
|
|
vector<string> tokens;
|
|
|
|
// no speed argument
|
|
if (arg == "")
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
|
|
// convert speed argument to float
|
|
while (ss >> buf)
|
|
{
|
|
// only need one speed argument
|
|
if (argc>0)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
ss2f.flush();
|
|
ss2f.clear();
|
|
ss2f << buf;
|
|
ss2f >> newVelocityPercentage;
|
|
argc++;
|
|
}
|
|
|
|
// check for a valid range for the new Velocity
|
|
if ((newVelocityPercentage <= 0.0f)||(newVelocityPercentage >100.0f))
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
|
|
// save new velocity into the database
|
|
SvrData::getInstance()->setRobotVelocityPercentage(newVelocityPercentage);
|
|
|
|
// signal a positive feedback to the client side
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
|
|
void setAccel(SocketObject& client, string& arg)
|
|
{
|
|
string buf;
|
|
float newAccelerationPercentage;
|
|
stringstream ss(arg);
|
|
stringstream ss2f;
|
|
int argc = 0;
|
|
|
|
// need at least one argument
|
|
if (arg == "")
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
|
|
// convert acceleration string into float
|
|
while (ss >> buf)
|
|
{
|
|
// only one argument is necessary
|
|
if (argc>0)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
ss2f.flush();
|
|
ss2f.clear();
|
|
ss2f << buf;
|
|
ss2f >> newAccelerationPercentage;
|
|
argc++;
|
|
}
|
|
|
|
// check for a valid rang for the robot acceleration
|
|
if ((newAccelerationPercentage < 1.0f)||(newAccelerationPercentage >100.0f))
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
|
|
// save new acceleration
|
|
SvrData::getInstance()->setRobotAccelerationPercentage(newAccelerationPercentage);
|
|
|
|
// signal a positive client feedback
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
|
|
void startPotFieldMode(SocketObject& client, string& arg)
|
|
{
|
|
// unused
|
|
(void) arg;
|
|
#if 0
|
|
__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;
|
|
}
|
|
}
|
|
#endif
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
|
|
void stopPotFieldMode(SocketObject& client, string& arg)
|
|
{
|
|
// unused
|
|
(void) arg;
|
|
#if 0
|
|
__POTFIELDMODE = false;
|
|
|
|
while (1)
|
|
{
|
|
if (!__MSRSTOPPOTFIELD)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
#endif
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
|
|
void setPos(SocketObject& client, string& arg)
|
|
{
|
|
string buf;
|
|
double temp[15];
|
|
stringstream ss(arg);
|
|
stringstream ss2f;
|
|
int argc = 0;
|
|
|
|
// convert position arguments from string to float
|
|
while (ss >> buf)
|
|
{
|
|
// only 15 parameters are necessary
|
|
if (argc>=15)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
|
|
ss2f.flush();
|
|
ss2f.clear();
|
|
ss2f << buf;
|
|
ss2f >> temp[argc];
|
|
argc++;
|
|
}
|
|
|
|
// check for exactly 15 parameters
|
|
if (argc!=15)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return ;
|
|
}
|
|
|
|
|
|
// the position is saved in the first parameter part
|
|
MatCarthesian newCartPos(0.0f,1.0f);
|
|
for (int i=0 ;i<3; i++)
|
|
{
|
|
for (int j=0;j<4;j++)
|
|
{
|
|
newCartPos(i,j)=temp[i*4+j];
|
|
}
|
|
}
|
|
|
|
// extract elbow flip and hand parameter
|
|
float configurationParameter[3];
|
|
for (int i=0;i<3;i++)
|
|
{
|
|
configurationParameter[i] = temp[12+i];
|
|
}
|
|
|
|
//Backward Calculation of the position
|
|
VecJoint CalcJoints = kukaBackwardCalc(temp, configurationParameter);
|
|
|
|
//Check for a valid joint range
|
|
if (!SvrData::getInstance()->checkJointRange(CalcJoints))
|
|
{
|
|
client.Send(SVR_OUT_OF_RANGE);
|
|
return;
|
|
}
|
|
|
|
// calculate new trajectory for next position
|
|
|
|
// set new commanded Position for next Trajectory
|
|
SvrData::getInstance()->setCommandedJointPos(CalcJoints);
|
|
|
|
//Wait to end of movement
|
|
|
|
// send positive client feedback
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
|
|
void setJoints(SocketObject& client, string& arg)
|
|
{
|
|
string buf;
|
|
VecJoint newJointPos(0.0f);
|
|
stringstream ss(arg);
|
|
stringstream ss2f;
|
|
int argc = 0;
|
|
|
|
// convert to Joint vector
|
|
while (ss >> buf)
|
|
{
|
|
// to many input arguments or Joints
|
|
if (argc>=JOINT_NUMBER)
|
|
{
|
|
client.Send(SVR_INVALID_ARGUMENT_COUNT);
|
|
return;
|
|
}
|
|
|
|
// convert string to float
|
|
ss2f.flush();
|
|
ss2f.clear();
|
|
ss2f << buf;
|
|
ss2f >> newJointPos(argc);
|
|
argc++;
|
|
}
|
|
|
|
// to less joint values
|
|
if (argc != JOINT_NUMBER)
|
|
{
|
|
client.Send(SVR_INVALID_ARGUMENT_COUNT);
|
|
return;
|
|
}
|
|
|
|
//Check for Joint Range
|
|
if (!SvrData::getInstance()->checkJointRange(newJointPos))
|
|
{
|
|
client.Send(SVR_OUT_OF_RANGE);
|
|
return;
|
|
}
|
|
|
|
// calculate trajectory from last commanded joint position
|
|
VecJoint currentComandedJoint = SvrData::getInstance()->getCommandedJointPos();
|
|
|
|
// set new trajectory
|
|
|
|
// Set new target Position and also calculate Cartesian position
|
|
SvrData::getInstance()->setCommandedJointPos(newJointPos);
|
|
// TODO calculate and calculate position in Cartesian coordinates
|
|
|
|
// wait till position is reached
|
|
|
|
// return positive response to client
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
|
|
void moveCartesian(SocketObject& client, string& arg)
|
|
{
|
|
string buf;
|
|
float temp[15];
|
|
stringstream ss(arg);
|
|
stringstream ss2f;
|
|
int argc = 0;
|
|
|
|
// convert Cartesian coordinates from string into float
|
|
while (ss >> buf)
|
|
{
|
|
// only need maximum 16 coordinates for a homogeneous matrix
|
|
if (argc>=15)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return;
|
|
}
|
|
ss2f.flush();
|
|
ss2f.clear();
|
|
ss2f << buf;
|
|
ss2f >> temp[argc];
|
|
argc++;
|
|
}
|
|
|
|
// check for exactly 15 coordinates
|
|
if (argc != 15)
|
|
{
|
|
client.Send(SVR_FALSE_RSP);
|
|
return ;
|
|
}
|
|
|
|
float configurationParameter[3];
|
|
for (int i=0;i<3;i++)
|
|
{
|
|
configurationParameter[i] = temp[12+i];
|
|
}
|
|
|
|
//Calculating end-effector position for target "TRobot"
|
|
Mat4f Tsurface;
|
|
Tsurface = vecToMat(temp);
|
|
Mat4f TRobot;
|
|
TRobot = Tsurface;
|
|
|
|
//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 ;
|
|
float* tempVec = NULL;
|
|
tempVec = matToVec(TRobot);
|
|
//tempVec = matToVec(ApRobot);
|
|
|
|
// send a positive client feedback
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
|
|
void quit(SocketObject& client , std::string& arg)
|
|
{
|
|
//unused
|
|
(void) arg;
|
|
|
|
client.Send(SVR_TRUE_RSP);
|
|
client.Disconnect();
|
|
}
|
|
|
|
void setTrajectoryType(SocketObject& client, std::string& arg)
|
|
{
|
|
string buf;
|
|
enum TrajectoryType newType = TrajectoryDefault;
|
|
|
|
newType = toEnum(arg);
|
|
SvrData::getInstance()->setTrajectoryType(newType);
|
|
cout << "new trajectory type : "<< toString(newType) << "\n";
|
|
|
|
client.Send(SVR_TRUE_RSP);
|
|
}
|
|
void getTrajectoryType(SocketObject& client, std::string& arg)
|
|
{
|
|
enum TrajectoryType currType = SvrData::getInstance()->getTrajectoryType();
|
|
cout << "curr trajectory type : "<< toString(currType) << "\n";
|
|
|
|
client.Send("current trajectory: " + toString(currType));
|
|
}
|
|
|
|
void debug(SocketObject& client, std::string& arg)
|
|
{
|
|
|
|
//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);
|
|
//}
|
|
}
|