|
|
|
@ -4,8 +4,10 @@ |
|
|
|
#include <error.h>
|
|
|
|
#include <errno.h>
|
|
|
|
#include <math.h>
|
|
|
|
#include <Trajectroy.h>
|
|
|
|
|
|
|
|
#include "SvrData.h"
|
|
|
|
#include <boost/thread/thread.hpp>
|
|
|
|
|
|
|
|
void printMat(Mat4f M) |
|
|
|
{ |
|
|
|
@ -18,22 +20,22 @@ float* abctomat(float a, float b, float c) |
|
|
|
float ca = cos(c); |
|
|
|
float sa = sin(c); |
|
|
|
|
|
|
|
rx(0,0) = 1; |
|
|
|
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; |
|
|
|
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; |
|
|
|
ry(1,1) = 1.0f; |
|
|
|
ry(2,0) = -sb; |
|
|
|
ry(2,2) = cb; |
|
|
|
ry(3,3) = 1; |
|
|
|
ry(3,3) = 1.0f; |
|
|
|
|
|
|
|
Mat4f rz; |
|
|
|
float cc = cos(a); |
|
|
|
@ -42,8 +44,8 @@ float* abctomat(float a, float b, float c) |
|
|
|
rz(0,1) = -sc; |
|
|
|
rz(1,0) = sc; |
|
|
|
rz(1,1) = cc; |
|
|
|
rz(2,2) = 1; |
|
|
|
rz(3,3) = 1; |
|
|
|
rz(2,2) = 1.0f; |
|
|
|
rz(3,3) = 1.0f; |
|
|
|
|
|
|
|
|
|
|
|
Mat4f result; |
|
|
|
@ -96,78 +98,6 @@ float* mattoabc(float M[12]) |
|
|
|
return abc; |
|
|
|
} |
|
|
|
|
|
|
|
void doRalf() |
|
|
|
{ |
|
|
|
// Global definitions
|
|
|
|
float lowStiff[6] = { 0.01f, 0.01f, 0.01f, 0.01f, 0.01f, 0.01f}; |
|
|
|
float highStiff[6] = {1000.0f, 1000.0f, 1000.0f, 10.0f, 10.0f, 10.0f}; |
|
|
|
float slowDamp[6] = { 0.007f, 0.007f, 0.007f, 0.007f, 0.007f, 0.007f}; |
|
|
|
float fastDamp[6] = { 0.7f, 0.7f, 0.7f, 0.7f, 0.7f, 0.7f}; |
|
|
|
|
|
|
|
float pos1[12] = { |
|
|
|
1.0f, 0.0f, 0.0f, 500.0f, |
|
|
|
0.0f, 1.0f, 0.0f, 0.0f, |
|
|
|
0.0f, 0.0f, 1.0f, 500.0f |
|
|
|
}; |
|
|
|
float pos2[12] = { |
|
|
|
1.0f, 0.0f, 0.0f , 500.0f, |
|
|
|
0.0f, 1.0f, 0.0f , 0.0f, |
|
|
|
0.0f, 0.0f ,1.0f , 500.0f |
|
|
|
}; |
|
|
|
|
|
|
|
friRemote friInst; |
|
|
|
double timeCounter=0; |
|
|
|
friInst.doDataExchange(); |
|
|
|
|
|
|
|
float* cartPos; |
|
|
|
while(true) |
|
|
|
{ |
|
|
|
// Time
|
|
|
|
timeCounter+=friInst.getSampleTime(); |
|
|
|
|
|
|
|
friInst.doReceiveData(); |
|
|
|
|
|
|
|
// Get all variables from FRI
|
|
|
|
float* jntPos = friInst.getMsrMsrJntPosition(); |
|
|
|
float* jacobian = friInst.getJacobian(); |
|
|
|
float* ftTcp = friInst.getFTTcp(); |
|
|
|
|
|
|
|
if (timeCounter < 20) |
|
|
|
cartPos = friInst.getMsrCartPosition(); |
|
|
|
//friInst.doDataExchange();
|
|
|
|
//friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, true);
|
|
|
|
#if 0
|
|
|
|
if (timeCounter>10 && timeCounter<10) |
|
|
|
{ |
|
|
|
friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, false); |
|
|
|
} else |
|
|
|
{ |
|
|
|
friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, false); |
|
|
|
} |
|
|
|
#endif
|
|
|
|
float npos[12]; |
|
|
|
memcpy(npos,cartPos,12); |
|
|
|
friInst.doCartesianImpedanceControl(npos, highStiff, fastDamp, NULL, NULL, false); |
|
|
|
|
|
|
|
#if 0
|
|
|
|
if (timeCounter<20) |
|
|
|
{ |
|
|
|
friInst.doCartesianImpedanceControl(Test, highStiff, fastDamp, NULL, NULL, true); |
|
|
|
} else if (timeCounter<30) |
|
|
|
{ |
|
|
|
friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, true); |
|
|
|
} else if (timeCounter<40) |
|
|
|
{ |
|
|
|
friInst.doCartesianImpedanceControl(cartPos , highStiff, fastDamp, NULL, NULL, true); |
|
|
|
} else |
|
|
|
{ |
|
|
|
friInst.doDataExchange(); |
|
|
|
} |
|
|
|
#endif
|
|
|
|
friInst.doSendData(); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
Mat4f vecToMat2(float vec[12]) |
|
|
|
{ |
|
|
|
Mat4f result; |
|
|
|
@ -178,7 +108,7 @@ Mat4f vecToMat2(float vec[12]) |
|
|
|
result(i,j) = (float)vec[i*4+j]; |
|
|
|
} |
|
|
|
} |
|
|
|
result(3,3)=(float)1; |
|
|
|
result(3,3)=1.0f; |
|
|
|
return result; |
|
|
|
} |
|
|
|
|
|
|
|
@ -248,57 +178,161 @@ float* quattovec(float quat[4]) |
|
|
|
return vec; |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void *threadFriDataExchange(void *arg) |
|
|
|
void *threadRobotMovement(void *arg) |
|
|
|
{ |
|
|
|
// unused
|
|
|
|
// unused parameters
|
|
|
|
(void) arg; |
|
|
|
|
|
|
|
//doRalf();
|
|
|
|
Trajectory<JOINT_NUMBER>* currentTrajectory = NULL; |
|
|
|
enum MovementType currentMovementType = MovementJointBased; |
|
|
|
|
|
|
|
friRemote friInst; |
|
|
|
VecJoint currentJointPos(0.0f); |
|
|
|
MatCarthesian currentCartPos(0.0f,1.0f); |
|
|
|
|
|
|
|
while (1) |
|
|
|
while(1) |
|
|
|
{ |
|
|
|
//#######################################
|
|
|
|
// Communication loop
|
|
|
|
// get current sample time cause it could be changed
|
|
|
|
float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs(); |
|
|
|
friRemote* friInst = SvrData::getInstance()->getFriRemote(); |
|
|
|
|
|
|
|
// get current joint positions
|
|
|
|
for ( int i= 0; i < LBR_MNJ; i++) |
|
|
|
// check if we have to cancel current trajectory
|
|
|
|
bool cancel = SvrData::getInstance()->getTrajectoryCancel(); |
|
|
|
if (cancel) |
|
|
|
{ |
|
|
|
MSRMSRJNTPOS[i] = friInst.getMsrCmdJntPosition()[i] + friInst.getMsrCmdJntPositionOffset()[i]; |
|
|
|
//MSRMSRJNTPOS[i] = friInst.getMsrMsrJntPosition()[i];
|
|
|
|
// free the current trajectory
|
|
|
|
if ( currentTrajectory != NULL) |
|
|
|
{ |
|
|
|
delete currentTrajectory; |
|
|
|
currentTrajectory = NULL; |
|
|
|
} |
|
|
|
// signal waiting thread the cancellation
|
|
|
|
SvrData::getInstance()->trajectoryCancelDone(); |
|
|
|
goto end; |
|
|
|
} |
|
|
|
|
|
|
|
// get current jacobian
|
|
|
|
float* friJacobian = friInst.getJacobian(); |
|
|
|
if ( friJacobian == NULL) |
|
|
|
// check for new trajectory
|
|
|
|
if (currentTrajectory == NULL) |
|
|
|
{ |
|
|
|
fprintf(stderr,"Failed: could not get jacobian\n"); |
|
|
|
break; |
|
|
|
// get next trajectory from svrData
|
|
|
|
currentTrajectory = SvrData::getInstance()->popTrajectory(); |
|
|
|
|
|
|
|
// no new trajectory
|
|
|
|
if(currentTrajectory == NULL) |
|
|
|
{ |
|
|
|
// stay on current position
|
|
|
|
goto end; |
|
|
|
} |
|
|
|
} |
|
|
|
for (int i = 0; i < FRI_CART_VEC*LBR_MNJ ; i++) |
|
|
|
|
|
|
|
// get next commanded trajectory type
|
|
|
|
switch (currentTrajectory->getMovementType()) |
|
|
|
{ |
|
|
|
MSRMSRJACOBIAN[i] = friJacobian[i]; |
|
|
|
case MovementCartBased: |
|
|
|
{ |
|
|
|
currentCartPos = currentTrajectory->getNextCartPos(); |
|
|
|
currentMovementType = MovementCartBased; |
|
|
|
} |
|
|
|
break; |
|
|
|
case MovementJointBased: |
|
|
|
{ |
|
|
|
currentJointPos = currentTrajectory->getNextJointPos(); |
|
|
|
currentMovementType = MovementJointBased; |
|
|
|
} |
|
|
|
default: |
|
|
|
{ |
|
|
|
// invalid trajectory skip it
|
|
|
|
delete currentTrajectory; |
|
|
|
currentTrajectory = NULL; |
|
|
|
goto end; |
|
|
|
} |
|
|
|
break; |
|
|
|
} |
|
|
|
|
|
|
|
// get current messured carthesian position
|
|
|
|
float* friMsrCartPosition = friInst.getMsrCartPosition(); |
|
|
|
if ( friMsrCartPosition == NULL) |
|
|
|
end: |
|
|
|
// TODO stop timer for last sent message
|
|
|
|
|
|
|
|
// time we used to come to this point caused by the calculation and lock time
|
|
|
|
// TODO calculate this time
|
|
|
|
float calculationTimeMs = 0.0f; |
|
|
|
|
|
|
|
// wait only the remaining time
|
|
|
|
float waitingTimeMs = sampleTimeMs - calculationTimeMs; |
|
|
|
|
|
|
|
// sleep the calculated waiting time
|
|
|
|
boost::this_thread::sleep( boost::posix_time::milliseconds(waitingTimeMs) ); |
|
|
|
|
|
|
|
|
|
|
|
switch (currentMovementType) |
|
|
|
{ |
|
|
|
fprintf(stderr,"Failed: could not get Messurement in Carthesian Position\n"); |
|
|
|
break; |
|
|
|
case MovementJointBased: |
|
|
|
{ |
|
|
|
// send the new joint values
|
|
|
|
float newJointPosToSend[JOINT_NUMBER] = {0.0f}; |
|
|
|
currentJointPos.getData(newJointPosToSend); |
|
|
|
friInst->doPositionControl(newJointPosToSend); |
|
|
|
} |
|
|
|
break; |
|
|
|
case MovementCartBased: |
|
|
|
{ |
|
|
|
#if 0
|
|
|
|
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 currentCartPosA[12] = {0.0f}; |
|
|
|
columncurrentCartPos.getData(newCartPosToSend); |
|
|
|
friInst->doCartesianImpedanceControl(currentCartPosA,Stiff, Damp, NULL, NULL, true); |
|
|
|
#endif
|
|
|
|
} |
|
|
|
break; |
|
|
|
} |
|
|
|
for ( int i =0; i < FRI_CART_FRM_DIM; i++) |
|
|
|
|
|
|
|
// start timer for the last sent msg
|
|
|
|
// TODO
|
|
|
|
|
|
|
|
// check if current trajectory is over
|
|
|
|
if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0) |
|
|
|
{ |
|
|
|
MSRMSRCARTPOS[i] = friMsrCartPosition[i]; |
|
|
|
// 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
|
|
|
|
VecJoint currentJointPos = SvrData::getInstance()->getMessuredJointPos(); |
|
|
|
|
|
|
|
MatCarthesian currentCartPost = SvrData::getInstance()->getMessuredCartPos(); |
|
|
|
|
|
|
|
// get current force and torque
|
|
|
|
float* friFTTcp = friInst.getFTTcp(); |
|
|
|
for ( int i = 0; i < FRI_CART_VEC; i++) |
|
|
|
VecJoint currentForceAndTorque = SvrData::getInstance()->getMessuredForceTorque(); |
|
|
|
|
|
|
|
// get current jacobian
|
|
|
|
// TODO use svr data
|
|
|
|
float* friJacobian = friInst->getJacobian(); |
|
|
|
if ( friJacobian == NULL) |
|
|
|
{ |
|
|
|
MSRMSRFTTCP[i] = friFTTcp[i]; |
|
|
|
fprintf(stderr,"Failed: could not get jacobian\n"); |
|
|
|
break; |
|
|
|
} |
|
|
|
for (int i = 0; i < FRI_CART_VEC*LBR_MNJ ; i++) |
|
|
|
{ |
|
|
|
MSRMSRJACOBIAN[i] = friJacobian[i]; |
|
|
|
} |
|
|
|
|
|
|
|
//#########################################################
|
|
|
|
@ -312,8 +346,7 @@ void *threadFriDataExchange(void *arg) |
|
|
|
Vec<float,LBR_MNJ> dMaxSpeed; |
|
|
|
Vec<float,LBR_MNJ> lMaxSpeed; |
|
|
|
Vec<float,LBR_MNJ> dGesamt; |
|
|
|
float sampleTime = 0.005f; |
|
|
|
|
|
|
|
float sampleTime = SvrData::getInstance()->getSampleTimeMs(); |
|
|
|
|
|
|
|
// get current robot constraints
|
|
|
|
Vec<float,LBR_MNJ> maxVelocityJoint = SvrData::getInstance()->getMaxVelocity(); |
|
|
|
@ -329,7 +362,7 @@ void *threadFriDataExchange(void *arg) |
|
|
|
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; |
|
|
|
@ -364,13 +397,13 @@ void *threadFriDataExchange(void *arg) |
|
|
|
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++) |
|
|
|
{ |
|
|
|
@ -389,10 +422,10 @@ void *threadFriDataExchange(void *arg) |
|
|
|
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); |
|
|
|
MSRMSRJNTPOS[j] += sgn(delta(j))*currentInk(j)*(1./180*M_PI); |
|
|
|
} |
|
|
|
// set new position
|
|
|
|
friInst.doPositionControl(MSRMSRJNTPOS); |
|
|
|
friInst->doPositionControl(MSRMSRJNTPOS); |
|
|
|
} |
|
|
|
} |
|
|
|
end: |
|
|
|
@ -424,8 +457,8 @@ end: |
|
|
|
{ |
|
|
|
for ( int i =0; i < FRI_CART_FRM_DIM; i++) |
|
|
|
{ |
|
|
|
Pos[i] = friInst.getMsrCartPosition()[i]; |
|
|
|
MSRMSRCARTPOS[i] = friInst.getMsrCartPosition()[i]; |
|
|
|
Pos[i] = friInst->getMsrCartPosition()[i]; |
|
|
|
MSRMSRCARTPOS[i] = friInst->getMsrCartPosition()[i]; |
|
|
|
deltaCart[i] = MSRCMDCARTPOS[i]-Pos[i]; |
|
|
|
} |
|
|
|
sum = 0; |
|
|
|
@ -484,24 +517,25 @@ end: |
|
|
|
{ |
|
|
|
Pos[i]+= deltaCart[i]*0.02; |
|
|
|
} |
|
|
|
friInst.doCartesianImpedanceControl(Pos, Stiff,Damp,NULL,NULL, true); |
|
|
|
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);
|
|
|
|
//friInst->doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
|
|
|
|
|
|
|
|
// innerhlab 5ms
|
|
|
|
friInst.doPositionControl(MSRMSRJNTPOS); |
|
|
|
friInst->doPositionControl(MSRMSRJNTPOS); |
|
|
|
|
|
|
|
//i = 0;
|
|
|
|
//}
|
|
|
|
//friInst.doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
|
|
|
|
//friInst->doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
|
|
|
|
} |
|
|
|
return NULL; |
|
|
|
} |
|
|
|
#endif
|
|
|
|
|
|
|
|
int main() |
|
|
|
{ |
|
|
|
@ -509,7 +543,7 @@ int main() |
|
|
|
//Setting pthread for FRI interface
|
|
|
|
pthread_t fri_t; |
|
|
|
//Start fri_thread
|
|
|
|
err = pthread_create(&fri_t,NULL,&threadFriDataExchange,NULL); |
|
|
|
err = pthread_create(&fri_t,NULL,&threadRobotMovement,NULL); |
|
|
|
if (err > 0 ) |
|
|
|
{ |
|
|
|
cerr << "Failed: could not create thread\n" << endl; |
|
|
|
|
philipp schoenberger
10 years ago