phschoen
/
lwr_server
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
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.
37 Commits
1 Branch
0 Tags
31 MiB
 
 
 
 
 
 
Tree: 01d2110c9a
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '01d2110c9a'
${ noResults }
lwr_server/lwrserv/SvrHandling.cpp

1023 lines
25 KiB
Raw Blame History

#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)
{
client.Send(SVR_HELP_MSG);
}
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)
{
cout << M << "\n\r" << endl;
}
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()
{
}
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))
{
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);
//SvrData::getInstance().lock();
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 == "debug")
{
debug(client);
} else if (cmd == "")
{
continue;
} else if (cmd == CMD_HELP)
{
printUsage(client);
} 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);
}
//SvrData::getInstance().unlock();
}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);
// 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 SvrHandling::GetPositionHomRowWise(SocketObject& client)
{
string out = "";
stringstream ss (stringstream::in | stringstream::out);
// get current Cartesianpositions from database
MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos();
// assemble command feadback 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 SvrHandling::GetForceTorqueTcp(SocketObject& client)
{
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 SvrHandling::MovePTPJoints(SocketObject& client, string& args)
{
//__SVR_CURRENT_JOB = true;
string buf;
stringstream ss(args);
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);
//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 SvrHandling::MoveHomRowWiseStatus(SocketObject& client, string& args)
{
string buf;
double temp[15];
stringstream ss(args);
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 arg[3];
for (int i=0;i<3;i++)
{
arg[i] = temp[12+i];
}
//Backward Calculation
VecJoint newJointPos = kukaBackwardCalc(temp, arg);
//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 SvrHandling::SetSpeed(SocketObject& client, string& args)
{
string buf;
float newVelocityPercentage;
int argc = 0;
stringstream ss(args);
stringstream ss2f;
vector<string> tokens;
// no speed argument
if (args == "")
{
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 SvrHandling::SetAccel(SocketObject& client, string& args)
{
string buf;
float newAccelerationPercentage;
stringstream ss(args);
stringstream ss2f;
int argc = 0;
// need at least one argument
if (args == "")
{
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 SvrHandling::StartPotFieldMode(SocketObject& client, string& args)
{
#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 SvrHandling::StopPotFieldMode(SocketObject& client, string& args)
{
#if 0
__POTFIELDMODE = false;
while (1)
{
if (!__MSRSTOPPOTFIELD)
{
break;
}
}
#endif
client.Send(SVR_TRUE_RSP);
}
void SvrHandling::SetPos(SocketObject& client, string& args)
{
string buf;
double temp[15];
stringstream ss(args);
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 arg[3];
for (int i=0;i<3;i++)
{
arg[i] = temp[12+i];
}
//Backward Calculation of the position
VecJoint CalcJoints = kukaBackwardCalc(temp, arg);
//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 SvrHandling::SetJoints(SocketObject& client, string& args)
{
string buf;
VecJoint newJointPos(0.0f);
stringstream ss(args);
stringstream ss2f;
int i = 0;
// convert to Joint vector
while (ss >> buf)
{
// to many input arguments or Joints
if (i>=JOINT_NUMBER)
{
client.Send(SVR_INVALID_ARGUMENT_COUNT);
return;
}
// convert string to float
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))
{
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 SvrHandling::MoveCartesian(SocketObject& client, string& args)
{
string buf;
float temp[15];
stringstream ss(args);
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 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 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 SvrHandling::quit(SocketObject& client)
{
client.Send(SVR_TRUE_RSP);
client.Disconnect();
}
void SvrHandling::debug(SocketObject& client)
{
//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);
//}
}