|
@ -267,9 +267,10 @@ double* kukaBackwardCalc(double M_[12], float arg[3]) |
|
|
|
|
|
|
|
|
bool checkJntRange(double Joints[7]) |
|
|
bool checkJntRange(double Joints[7]) |
|
|
{ |
|
|
{ |
|
|
|
|
|
Vec<float,LBR_MNJ> maxJointRange = SvrData::getInstance()->getMaxRange(); |
|
|
for (int i = 0; i<LBR_MNJ; i++) |
|
|
for (int i = 0; i<LBR_MNJ; i++) |
|
|
{ |
|
|
{ |
|
|
if (abs(Joints[i])>abs(maxRangeJnt[i])) |
|
|
|
|
|
|
|
|
if (abs(Joints[i]) > abs(maxJointRange(i))) |
|
|
{ |
|
|
{ |
|
|
// join angle is too big
|
|
|
// join angle is too big
|
|
|
return false; |
|
|
return false; |
|
@ -282,9 +283,9 @@ bool checkJntRange(double Joints[7]) |
|
|
Mat4f vecToMat(float vec[12]) |
|
|
Mat4f vecToMat(float vec[12]) |
|
|
{ |
|
|
{ |
|
|
Mat4f result; |
|
|
Mat4f result; |
|
|
for (int i=0; i<3; i++) |
|
|
|
|
|
|
|
|
for (int i=0; i<3; i++) // ZEILE
|
|
|
{ |
|
|
{ |
|
|
for (int j=0; j<4; j++) |
|
|
|
|
|
|
|
|
for (int j=0; j<4; j++) // SPALTE
|
|
|
{ |
|
|
{ |
|
|
result(i,j) = vec[i*4+j]; |
|
|
result(i,j) = vec[i*4+j]; |
|
|
} |
|
|
} |
|
@ -663,8 +664,9 @@ void SvrHandling::SetSpeed(SocketObject& client, string& args) |
|
|
{ |
|
|
{ |
|
|
__SETVELOCITY = true; |
|
|
__SETVELOCITY = true; |
|
|
__SVR_CURRENT_JOB = true; |
|
|
__SVR_CURRENT_JOB = true; |
|
|
|
|
|
//SvrData::getInstance()->setState(SVR_STATE_SET_VELOCITY);
|
|
|
string buf; |
|
|
string buf; |
|
|
float temp[1]; |
|
|
|
|
|
|
|
|
float newVelocity; |
|
|
stringstream ss(args); |
|
|
stringstream ss(args); |
|
|
stringstream ss2f; |
|
|
stringstream ss2f; |
|
|
vector<string> tokens; |
|
|
vector<string> tokens; |
|
@ -685,16 +687,17 @@ void SvrHandling::SetSpeed(SocketObject& client, string& args) |
|
|
ss2f.flush(); |
|
|
ss2f.flush(); |
|
|
ss2f.clear(); |
|
|
ss2f.clear(); |
|
|
ss2f << buf; |
|
|
ss2f << buf; |
|
|
ss2f >> temp[i]; |
|
|
|
|
|
|
|
|
ss2f >> newVelocity; |
|
|
i++; |
|
|
i++; |
|
|
} |
|
|
} |
|
|
if ((temp[0] < 1)||(temp[0]>1000)) |
|
|
|
|
|
|
|
|
if ((newVelocity < 1.0f)||(newVelocity >1000.0)) |
|
|
{ |
|
|
{ |
|
|
client.Send(SVR_FALSE_RSP); |
|
|
client.Send(SVR_FALSE_RSP); |
|
|
return; |
|
|
return; |
|
|
} |
|
|
} |
|
|
VELOCITY = temp[0]; |
|
|
|
|
|
|
|
|
SvrData::getInstance()->setRobotVelocity(newVelocity); |
|
|
client.Send(SVR_TRUE_RSP); |
|
|
client.Send(SVR_TRUE_RSP); |
|
|
|
|
|
//SvrData::getInstance()->setState(SVR_STATE_RUNNING);
|
|
|
__SVR_CURRENT_JOB = false; |
|
|
__SVR_CURRENT_JOB = false; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
@ -703,7 +706,7 @@ void SvrHandling::SetAccel(SocketObject& client, string& args) |
|
|
__SETACCEL = true; |
|
|
__SETACCEL = true; |
|
|
__SVR_CURRENT_JOB = true; |
|
|
__SVR_CURRENT_JOB = true; |
|
|
string buf; |
|
|
string buf; |
|
|
float temp[1]; |
|
|
|
|
|
|
|
|
float newAcceleration; |
|
|
stringstream ss(args); |
|
|
stringstream ss(args); |
|
|
stringstream ss2f; |
|
|
stringstream ss2f; |
|
|
vector<string> tokens; |
|
|
vector<string> tokens; |
|
@ -725,15 +728,15 @@ void SvrHandling::SetAccel(SocketObject& client, string& args) |
|
|
ss2f.flush(); |
|
|
ss2f.flush(); |
|
|
ss2f.clear(); |
|
|
ss2f.clear(); |
|
|
ss2f << buf; |
|
|
ss2f << buf; |
|
|
ss2f >> temp[i]; |
|
|
|
|
|
|
|
|
ss2f >> newAcceleration; |
|
|
i++; |
|
|
i++; |
|
|
} |
|
|
} |
|
|
if ((temp[0] < 1)||(temp[0]>100)) |
|
|
|
|
|
|
|
|
if ((newAcceleration < 1.0f)||(newAcceleration >100.0f)) |
|
|
{ |
|
|
{ |
|
|
client.Send(SVR_FALSE_RSP); |
|
|
client.Send(SVR_FALSE_RSP); |
|
|
return; |
|
|
return; |
|
|
} |
|
|
} |
|
|
ACCELARATION = temp[0]; |
|
|
|
|
|
|
|
|
SvrData::getInstance()->setRobotAcceleration(newAcceleration); |
|
|
client.Send(SVR_TRUE_RSP); |
|
|
client.Send(SVR_TRUE_RSP); |
|
|
__SVR_CURRENT_JOB = false; |
|
|
__SVR_CURRENT_JOB = false; |
|
|
} |
|
|
} |
|
|