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.
585 lines
18 KiB
585 lines
18 KiB
#include "sgn.h"
|
|
#include <error.h>
|
|
#include <errno.h>
|
|
#include <math.h>
|
|
#include <Trajectroy.h>
|
|
|
|
#include "SvrData.h"
|
|
#include "SvrHandling.h"
|
|
#include <boost/thread/thread.hpp>
|
|
|
|
#include "mat.h"
|
|
#include "vec.h"
|
|
#include "robot.h"
|
|
|
|
float* abctomat(float a, float b, float c)
|
|
{
|
|
Mat4f rx;
|
|
float ca = cos(c);
|
|
float sa = sin(c);
|
|
|
|
rx(0,0) = 1.0f;
|
|
rx(1,1) = ca;
|
|
rx(1,2) = -sa;
|
|
rx(2,1) = sa;
|
|
rx(2,2) = ca;
|
|
rx(3,3) = 1.0f;
|
|
|
|
Mat4f ry;
|
|
float cb = cos(b);
|
|
float sb = sin(b);
|
|
ry(0,0) = cb;
|
|
ry(0,2) = sb;
|
|
ry(1,1) = 1.0f;
|
|
ry(2,0) = -sb;
|
|
ry(2,2) = cb;
|
|
ry(3,3) = 1.0f;
|
|
|
|
Mat4f rz;
|
|
float cc = cos(a);
|
|
float sc = sin(a);
|
|
rz(0,0) = cc;
|
|
rz(0,1) = -sc;
|
|
rz(1,0) = sc;
|
|
rz(1,1) = cc;
|
|
rz(2,2) = 1.0f;
|
|
rz(3,3) = 1.0f;
|
|
|
|
Mat4f result;
|
|
Mat4f temp;
|
|
temp = rz * ry;
|
|
result = temp * rx;
|
|
|
|
#ifdef __DEBUG__
|
|
printMat(rx);
|
|
printMat(ry);
|
|
printMat(rz);
|
|
printMat(result);
|
|
#endif
|
|
|
|
float *res = new float[12];
|
|
//TODO simple converter for mat
|
|
for (int j=0;j<3;j++)
|
|
{
|
|
for (int k=0;k<4;k++)
|
|
{
|
|
res[j*4+k] = result(j,k);
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
|
|
float* mattoabc(float M[12])
|
|
{
|
|
float norm;
|
|
float sa;
|
|
float ca;
|
|
float *abc = new float[3];
|
|
|
|
norm = sqrt((M[0]*M[0])+(M[4]*M[4]));
|
|
|
|
if (norm>1e-5)
|
|
{
|
|
sa = M[4]/norm;
|
|
ca = M[0]/norm;
|
|
abc[0] = atan2(sa,ca);
|
|
}
|
|
else
|
|
{
|
|
sa = 0;
|
|
ca = 1;
|
|
abc[0] = 0;
|
|
}
|
|
abc[1] = atan2(-M[8],ca*M[0]+sa*M[4]);
|
|
abc[2] = atan2(sa*M[2]-ca*M[6],-sa*M[1]+ca*M[5]);
|
|
return abc;
|
|
}
|
|
|
|
Mat4f vecToMat2(float vec[12])
|
|
{
|
|
Mat4f result;
|
|
for (int i=0; i<3; i++)
|
|
{
|
|
for (int j=0; j<4; j++)
|
|
{
|
|
result(i,j) = (float)vec[i*4+j];
|
|
}
|
|
}
|
|
result(3,3)=1.0f;
|
|
return result;
|
|
}
|
|
|
|
float* matToVec2(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;
|
|
}
|
|
|
|
float* vectoquat(float vec[12])
|
|
{
|
|
float *quat = new float[4];
|
|
float diag[3];
|
|
int v; int w; int u;
|
|
diag[0] = vec[0];
|
|
diag[1] = vec[5];
|
|
diag[2] = vec[10];
|
|
float u_ = *std::max_element(diag,diag+2);
|
|
|
|
if (u_==diag[0])
|
|
{
|
|
u = 1;
|
|
v = 2;
|
|
w = 3;
|
|
} else if (u_==diag[1])
|
|
{
|
|
u = 2;
|
|
v = 3;
|
|
w = 1;
|
|
} else
|
|
{
|
|
u = 3;
|
|
v = 1;
|
|
w = 2;
|
|
}
|
|
float r = sqrt(1+vec[(u-1)*4+(u-1)] - vec[(v-1)*4+(v-1)] - vec[(w-1)*4+(w-1)]);
|
|
quat[0] = (vec[(w-1)*4+(v-1)] - vec[(v-1)*4+(w-1)]) / (2*r);
|
|
quat[u] = r/2;
|
|
quat[v] = (vec[(u-1)*4+(v-1)] + vec[(v-1)*4+(u-1)]) / (2*r);
|
|
quat[w] = (vec[(w-1)*4+(u-1)] + vec[(u-1)*4+(w-1)]) / (2*r);
|
|
|
|
return quat;
|
|
}
|
|
|
|
float* quattovec(float quat[4])
|
|
{
|
|
float *vec = new float[12];
|
|
vec[0] = quat[0]*quat[0]+quat[1]*quat[1]-quat[2]*quat[2]-quat[3]*quat[3];
|
|
vec[4] = 2*(quat[1]*quat[2]+quat[0]*quat[3]);
|
|
vec[8] = 2*(quat[1]*quat[3]-quat[0]*quat[2]);
|
|
|
|
vec[1] = 2*(quat[1]*quat[2]-quat[0]*quat[3]);
|
|
vec[5] = quat[0]*quat[0]-quat[1]*quat[1]+quat[2]*quat[2]-quat[3]*quat[3];
|
|
vec[9] = 2*(quat[2]*quat[3]+quat[0]*quat[1]);
|
|
|
|
vec[2] = 2*(quat[1]*quat[3]+quat[0]*quat[2]);
|
|
vec[6] = 2*(quat[2]*quat[3]-quat[0]*quat[1]);
|
|
vec[10] = quat[0]*quat[0]-quat[1]*quat[1]-quat[2]*quat[2]+quat[3]*quat[3];
|
|
|
|
return vec;
|
|
}
|
|
|
|
void *threadRobotMovement(void *arg)
|
|
{
|
|
// unused parameters
|
|
(void) arg;
|
|
|
|
Trajectory<JOINT_NUMBER>* currentTrajectory = NULL;
|
|
enum MovementType currentMovementType = MovementJointBased;
|
|
|
|
Robot::VecJoint currentJointPos(0.0f);
|
|
MatCarthesian currentCartPos(0.0f,1.0f);
|
|
|
|
friRemote* friInst = SvrData::getInstance()->getFriRemote();
|
|
|
|
// get current robot position
|
|
int tries = 2;
|
|
|
|
for (int i= 0 ; i < tries ; ++i)
|
|
{
|
|
if (REAL_ROBOT)
|
|
SvrData::getInstance()->updateMessurement();
|
|
currentJointPos = SvrData::getInstance()->getMeasuredJointPos();
|
|
|
|
SvrData::getInstance()->setCommandedJointPos(currentJointPos);
|
|
|
|
float newJointPosToSend[JOINT_NUMBER] = {0.0f};
|
|
currentJointPos.getData(newJointPosToSend);
|
|
if (REAL_ROBOT)
|
|
friInst->doPositionControl(newJointPosToSend);
|
|
}
|
|
std::cout << "init position is " << currentJointPos << "\n";
|
|
|
|
while(true)
|
|
{
|
|
if (REAL_ROBOT)
|
|
SvrData::getInstance()->updateMessurement();
|
|
|
|
bool positionChanged = false;
|
|
// check if we have to cancel current trajectory
|
|
bool cancel = SvrData::getInstance()->getTrajectoryCancel();
|
|
if (cancel)
|
|
{
|
|
// free the current trajectory
|
|
if ( currentTrajectory != NULL)
|
|
{
|
|
delete currentTrajectory;
|
|
currentTrajectory = NULL;
|
|
}
|
|
// signal waiting thread the cancellation
|
|
SvrData::getInstance()->trajectoryCancelDone();
|
|
goto end;
|
|
}
|
|
|
|
// check for new trajectory
|
|
if (currentTrajectory == NULL)
|
|
{
|
|
// get next trajectory from svrData
|
|
currentTrajectory = SvrData::getInstance()->popTrajectory();
|
|
|
|
// no new trajectory
|
|
if(currentTrajectory == NULL)
|
|
{
|
|
// mark that we reached the end of the trajectory
|
|
// stay on current position
|
|
goto end;
|
|
}
|
|
else
|
|
{
|
|
std::cout <<"new Trajectory with " << currentTrajectory->getSteps()<<"\n";
|
|
}
|
|
}
|
|
|
|
// get next commanded trajectory type
|
|
if (currentTrajectory != NULL)
|
|
{
|
|
switch (currentTrajectory->getMovementType())
|
|
{
|
|
case MovementCartBased:
|
|
{
|
|
currentCartPos = currentTrajectory->getNextCartPos();
|
|
positionChanged = true;
|
|
currentMovementType = MovementCartBased;
|
|
}
|
|
break;
|
|
case MovementJointBased:
|
|
{
|
|
currentJointPos = currentTrajectory->getNextJointPos();
|
|
positionChanged = true;
|
|
currentMovementType = MovementJointBased;
|
|
}
|
|
break;
|
|
default:
|
|
{
|
|
// invalid trajectory skip it
|
|
delete currentTrajectory;
|
|
currentTrajectory = NULL;
|
|
goto end;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
end:
|
|
switch (currentMovementType)
|
|
{
|
|
case MovementJointBased:
|
|
{
|
|
// send the new joint values
|
|
float newJointPosToSend[JOINT_NUMBER] = {0.0f};
|
|
Robot::VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI);
|
|
newJointPosToSendRad.getData(newJointPosToSend);
|
|
if (REAL_ROBOT)
|
|
friInst->doPositionControl(newJointPosToSend);
|
|
}
|
|
break;
|
|
case MovementCartBased:
|
|
{
|
|
float Stiff[6] = {1000.0, 1000.0, 1000.0, 10.0, 10.0, 10.0};
|
|
float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7};
|
|
float newCartPosToSend[12] = {0.0f};
|
|
currentCartPos.getData(newCartPosToSend);
|
|
if (REAL_ROBOT)
|
|
friInst->doCartesianImpedanceControl(newCartPosToSend,Stiff, Damp, NULL, NULL, true);
|
|
}
|
|
break;
|
|
}
|
|
|
|
// start timer for the last sent msg
|
|
// TODO
|
|
|
|
// check if current trajectory is over
|
|
if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0)
|
|
{
|
|
std::cout << " Trajectory finished \n ";
|
|
|
|
static int trajcetorycount = 0;
|
|
trajcetorycount +=1;
|
|
|
|
currentTrajectory->saveToFile(std::string("trajectory/a.csv"));
|
|
// invalid trajectory skip it
|
|
delete currentTrajectory;
|
|
currentTrajectory = NULL;
|
|
}
|
|
}
|
|
}
|
|
|
|
#if 0
|
|
void *threadFriDataExchange(void *arg)
|
|
{
|
|
// unused
|
|
(void) arg;
|
|
|
|
friRemote* friInst = SvrData::getInstance()->getFriRemote();
|
|
|
|
while (1)
|
|
{
|
|
//#######################################
|
|
// Communication loop
|
|
|
|
// update current joint positions
|
|
SvrData::getInstance()->updateMessurement();
|
|
|
|
// get current joint position
|
|
Robot::VecJoint currentJointPos = SvrData::getInstance()->getMessuredJointPos();
|
|
|
|
MatCarthesian currentCartPost = SvrData::getInstance()->getMessuredCartPos();
|
|
|
|
// get current force and torque
|
|
Robot::VecJoint currentForceAndTorque = SvrData::getInstance()->getMessuredForceTorque();
|
|
|
|
// get current jacobian
|
|
// TODO use svr data
|
|
float* friJacobian = friInst->getJacobian();
|
|
if ( friJacobian == NULL)
|
|
{
|
|
fprintf(stderr,"Failed: could not get jacobian\n");
|
|
break;
|
|
}
|
|
for (int i = 0; i < FRI_CART_VEC*LBR_MNJ ; i++)
|
|
{
|
|
MSRMSRJACOBIAN[i] = friJacobian[i];
|
|
}
|
|
|
|
//#########################################################
|
|
// PTP Joint Movement
|
|
if (__MSRCMDJNTPOS)
|
|
{
|
|
Vec<float,LBR_MNJ> maxJointLocalVelocity;
|
|
Vec<float,LBR_MNJ> maxJointLocalAcceleration;
|
|
Vec<float,LBR_MNJ> delta;
|
|
Vec<float,LBR_MNJ> deltaAbs;
|
|
Vec<float,LBR_MNJ> dMaxSpeed;
|
|
Vec<float,LBR_MNJ> lMaxSpeed;
|
|
Vec<float,LBR_MNJ> dGesamt;
|
|
float sampleTime = SvrData::getInstance()->getSampleTimeMs();
|
|
|
|
// get current robot constraints
|
|
Vec<float,LBR_MNJ> maxVelocityJoint = SvrData::getInstance()->getMaxVelocity();
|
|
Vec<float,LBR_MNJ> maxAccelarationJoint = SvrData::getInstance()->getMaxAcceleration();
|
|
Vec<float,LBR_MNJ> messuredJointPos = SvrData::getInstance()->getMessuredJointPos();
|
|
Vec<float,LBR_MNJ> commandedJointPos = SvrData::getInstance()->getCommandedJointPos();
|
|
|
|
float velocity = SvrData::getInstance()->getRobotVelocity();
|
|
float accelaration = SvrData::getInstance()->getRobotAcceleration();
|
|
|
|
// calculate delta positions of movement
|
|
delta = commandedJointPos - messuredJointPos;
|
|
deltaAbs = delta.abs();
|
|
maxJointLocalVelocity = maxVelocityJoint * sampleTime * (velocity/100.0f);
|
|
maxJointLocalAcceleration = maxAccelarationJoint * sampleTime * (accelaration/100.0f);
|
|
|
|
// calculate number of movement steps
|
|
dMaxSpeed = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration);
|
|
lMaxSpeed = dMaxSpeed.cellmultiply(dMaxSpeed).cellmultiply(maxJointLocalAcceleration) * 0.5f;
|
|
|
|
for (int j=0; j<LBR_MNJ; j++)
|
|
{
|
|
if (lMaxSpeed(j) > deltaAbs(j)/(double)2.0)
|
|
{
|
|
dGesamt(j) = sqrt(deltaAbs(j) / maxJointLocalAcceleration (j))*2.0;
|
|
} else
|
|
{
|
|
dGesamt(j) = dMaxSpeed(j)*2 + (deltaAbs(j)-lMaxSpeed(j)*2.0)/maxJointLocalVelocity(j);
|
|
}
|
|
}
|
|
int maxSteps = ceil(dGesamt.max());
|
|
|
|
// there are atleast 2 steps otherwise it is no trajectory
|
|
if (maxSteps == 0 || maxSteps == 1)
|
|
goto end;
|
|
|
|
for (int j=0; j<LBR_MNJ; j++)
|
|
{
|
|
// ceil maxsteps/2 - sqrt(maxsteps^2 - deltaAbs/maxaccelaration)
|
|
dMaxSpeed(j) = ceil( maxSteps /2.0f - sqrt((maxSteps/2.0f)*(maxSteps/2.0f) - deltaAbs(j)/maxAccelarationJoint(j)));
|
|
if (dMaxSpeed(j) == 0.0f)
|
|
{
|
|
maxJointLocalAcceleration(j) = 0.0f;
|
|
} else
|
|
{
|
|
maxJointLocalAcceleration(j) = -deltaAbs(j)/(dMaxSpeed(j)*dMaxSpeed(j)-maxSteps*dMaxSpeed(j));
|
|
}
|
|
maxJointLocalVelocity(j) = dMaxSpeed(j)*maxJointLocalAcceleration(j);
|
|
}
|
|
|
|
// do the trajectory
|
|
{
|
|
Vec<float,LBR_MNJ> currentInk;
|
|
Vec<float,LBR_MNJ> currentInkLast;
|
|
Vec<float,LBR_MNJ> currentDist;
|
|
Vec<float,LBR_MNJ> currentDistLast;
|
|
|
|
// bang bang
|
|
for (int i=0;i<maxSteps;i++)
|
|
{
|
|
for (int j=0;j<LBR_MNJ;j++)
|
|
{
|
|
if (i+1 <= maxSteps/2.0f)
|
|
{
|
|
currentInk(j) = min(currentInkLast(j)+maxJointLocalAcceleration(j),maxJointLocalVelocity(j));
|
|
}else if (i+1 > maxSteps-dMaxSpeed(j))
|
|
{
|
|
currentInk(j) = max(currentInkLast(j)-maxJointLocalAcceleration(j),0.0f);
|
|
}else
|
|
{
|
|
currentInk(j) = currentInkLast(j);
|
|
}
|
|
currentDist(j) = currentDistLast(j) + sgn(delta(j))*currentInk(j);
|
|
currentInkLast(j) = currentInk(j);
|
|
currentDistLast(j) = currentDist(j);
|
|
MSRMSRJNTPOS[j] += sgn(delta(j))*currentInk(j)*(1./180*M_PI);
|
|
}
|
|
// set new position
|
|
friInst->doPositionControl(MSRMSRJNTPOS);
|
|
}
|
|
}
|
|
end:
|
|
// mark state to be done
|
|
__MSRCMDJNTPOS = false;
|
|
|
|
} else if (__CARTMOVE)
|
|
{
|
|
//##########################################################
|
|
//Cartesian Movement
|
|
|
|
const float Stiff[6] = {1000.0, 1000.0, 1000.0, 150.0, 150.0, 150.0};
|
|
const float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7};
|
|
float th = 0.000;
|
|
float deltaCart[12];
|
|
float Pos[12];
|
|
float dist;
|
|
float rot;
|
|
float sum;
|
|
float b;
|
|
float c;
|
|
float t_fact = 1;
|
|
float t_outerT = 0.007;
|
|
float t_innerT = 0.000;
|
|
float t_outerR = 0.007;
|
|
float t_innerR = 0.000;
|
|
|
|
while (true)
|
|
{
|
|
for ( int i =0; i < FRI_CART_FRM_DIM; i++)
|
|
{
|
|
Pos[i] = friInst->getMsrCartPosition()[i];
|
|
MSRMSRCARTPOS[i] = friInst->getMsrCartPosition()[i];
|
|
deltaCart[i] = MSRCMDCARTPOS[i]-Pos[i];
|
|
}
|
|
sum = 0;
|
|
for (int i=0; i<3 ; i++)
|
|
{
|
|
sum = sum + (deltaCart[(i*4)+3]*deltaCart[(i*4)+3]);
|
|
}
|
|
dist = sqrt(sum);
|
|
sum = 0;
|
|
for (int i=0;i<12;i++)
|
|
{
|
|
if (i != 3 && i != 7 && i != 11)
|
|
{
|
|
sum = sum + deltaCart[i]*deltaCart[i];
|
|
}
|
|
}
|
|
rot = sqrt(sum);
|
|
b = t_fact*max((float)0.0,(float)min((float)1.0,(t_outerT-dist+(t_innerT-t_outerT))*(1/(t_innerT-t_outerT))));
|
|
c = t_fact*max((float)0.0,(float)min((float)1.0,(float)(t_outerR-rot+(t_innerR-t_outerR))*(float)(1/(t_innerR-t_outerR))));
|
|
|
|
if (dist < th)
|
|
{
|
|
for (int i=0; i<3; i++)
|
|
{
|
|
deltaCart[(i*4)+3] = 0;
|
|
}
|
|
}else
|
|
{
|
|
for (int i=0; i<3; i++)
|
|
{
|
|
deltaCart[(i*4)+3] = (deltaCart[(i*4)+3] / dist)*b;
|
|
}
|
|
}
|
|
|
|
// check offset to point
|
|
if (rot < th)
|
|
{
|
|
for (int i=0;i<12;i++)
|
|
{
|
|
if (i != 3 && i != 7 && i != 11)
|
|
{
|
|
deltaCart[i] = 0;
|
|
}
|
|
}
|
|
}else
|
|
{
|
|
for (int i=0;i<12;i++)
|
|
{
|
|
if (i != 3 && i != 7 && i != 11)
|
|
{
|
|
deltaCart[i] = (deltaCart[i] / rot)*c;
|
|
}
|
|
}
|
|
}
|
|
for (int i=0;i<12;i++)
|
|
{
|
|
Pos[i]+= deltaCart[i]*0.02;
|
|
}
|
|
friInst->doCartesianImpedanceControl(Pos, Stiff,Damp,NULL,NULL, true);
|
|
|
|
}
|
|
}
|
|
//float Stiff[6] = {1000.0, 1000.0, 1000.0, 10.0, 10.0, 10.0};
|
|
//float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7};
|
|
//float Test[12] = {MSRMSRCARTPOS[0],MSRMSRCARTPOS[1],MSRMSRCARTPOS[2],MSRMSRCARTPOS[3],MSRMSRCARTPOS[4],MSRMSRCARTPOS[5],MSRMSRCARTPOS[6],MSRMSRCARTPOS[7],MSRMSRCARTPOS[8],MSRMSRCARTPOS[9],MSRMSRCARTPOS[10],MSRMSRCARTPOS[11]};
|
|
//friInst->doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
|
|
|
|
// innerhlab 5ms
|
|
friInst->doPositionControl(MSRMSRJNTPOS);
|
|
|
|
//i = 0;
|
|
//}
|
|
//friInst->doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
|
|
}
|
|
return NULL;
|
|
}
|
|
#endif
|
|
|
|
int main()
|
|
{
|
|
int err = 0;
|
|
//Setting pthread for FRI interface
|
|
pthread_t fri_t;
|
|
//Start fri_thread
|
|
err = pthread_create(&fri_t,NULL,&threadRobotMovement,NULL);
|
|
if (err > 0 )
|
|
{
|
|
std::cerr << "Failed: could not create thread\n" << std::endl;
|
|
return err;
|
|
}
|
|
|
|
//Start client handling
|
|
SvrHandling *svr = new SvrHandling();
|
|
if (svr == NULL)
|
|
{
|
|
std::cerr << "Failed: could not create server \n" << std::endl;
|
|
return -ENOMEM;
|
|
}
|
|
svr->run();
|
|
|
|
return 0;
|
|
}
|