philipp schoenberger
10 years ago
31 changed files with 620 additions and 1097 deletions
-
1.gitignore
-
51lwrserv/Makefile
-
7lwrserv/doc/doxygenconfig
-
2lwrserv/include/BangBangTrajectory.h
-
3lwrserv/include/LinearTrajectory.h
-
2lwrserv/include/Mat.h
-
62lwrserv/include/Robot.h
-
0lwrserv/include/SocketObject.h
-
0lwrserv/include/StringTool.h
-
74lwrserv/include/SvrData.h
-
0lwrserv/include/SvrHandling.h
-
5lwrserv/include/Trajectroy.h
-
2lwrserv/include/Vec.h
-
11lwrserv/include/commands.h
-
1lwrserv/include/config.h
-
6lwrserv/include/defines.h
-
0lwrserv/include/friComm.h
-
0lwrserv/include/friremote.h
-
0lwrserv/include/friudp.h
-
34lwrserv/include/global.h
-
6lwrserv/include/lwr4.h
-
585lwrserv/program.cpp
-
0lwrserv/src/SocketObject.cpp
-
0lwrserv/src/StringTool.cpp
-
109lwrserv/src/SvrData.cpp
-
23lwrserv/src/SvrHandling.cpp
-
373lwrserv/src/commands.cpp
-
0lwrserv/src/friremote.cpp
-
0lwrserv/src/friudp.cpp
-
183lwrserv/src/main.cpp
-
177lwrserv/src/program.cpp
@ -1,2 +1,3 @@ |
|||||
build |
build |
||||
Release |
Release |
||||
|
html |
||||
@ -0,0 +1,7 @@ |
|||||
|
PROJECT_NAME = lwr_server |
||||
|
OUTPUT_DIRECTORY = html |
||||
|
WARNINGS = YES |
||||
|
INPUT = src |
||||
|
FILE_PATTERNS = *.cc *.h |
||||
|
INCLUDE_PATH = examples src |
||||
|
SEARCHENGINE = YES |
||||
@ -1,7 +1,6 @@ |
|||||
/* config.h. Generated from config.h.in by configure. */ |
/* config.h. Generated from config.h.in by configure. */ |
||||
/* config.h.in. Generated from configure.ac by autoheader. */ |
/* config.h.in. Generated from configure.ac by autoheader. */ |
||||
|
|
||||
|
|
||||
/* Compiling CppUTest itself */ |
/* Compiling CppUTest itself */ |
||||
#define CPPUTEST_COMPILATION 1 |
#define CPPUTEST_COMPILATION 1 |
||||
|
|
||||
@ -1,9 +1,11 @@ |
|||||
|
|
||||
#ifndef __DEFINES_H__ |
#ifndef __DEFINES_H__ |
||||
#define __DEFINES_H__ |
#define __DEFINES_H__ |
||||
|
|
||||
#define M_PI 3.141592654f |
|
||||
|
#ifndef M_PI |
||||
|
#define M_PI 3.141592654f |
||||
|
#endif |
||||
|
|
||||
|
#define REAL_ROBOT false |
||||
|
|
||||
#endif /* __DEFINES_H__ */ |
#endif /* __DEFINES_H__ */ |
||||
|
|
||||
@ -1,34 +0,0 @@ |
|||||
extern bool __SVR_CURRENT_JOB; |
|
||||
extern bool __MSRMSRJNTPOS; |
|
||||
extern bool __MSRCMDJNTPOS; |
|
||||
extern bool __MSRCMDPOSE; |
|
||||
extern bool __SETVELOCITY; |
|
||||
extern bool __SETACCEL; |
|
||||
extern bool __DEBUG; |
|
||||
extern bool __POTFIELDMODE; |
|
||||
extern bool __MSRSTARTPOTFIELD; |
|
||||
extern bool __MSRSTOPPOTFIELD; |
|
||||
extern bool __MSRSETPOS; |
|
||||
extern bool __CARTMOVE; |
|
||||
extern bool __DOUS; |
|
||||
extern bool __DOUS2; |
|
||||
|
|
||||
extern int globi; |
|
||||
|
|
||||
extern float MSRMSRJNTPOS[7]; |
|
||||
extern double MSRMSRCARTPOS[12]; |
|
||||
extern float MSRCMDCARTPOS[12]; |
|
||||
extern double MSRMSRJACOBIAN[42]; |
|
||||
extern double MSRMSRFTTCP[6]; |
|
||||
extern double MSRCMDJNTPOS[7]; |
|
||||
extern double MSRCMDPOSE[3][4]; |
|
||||
|
|
||||
#ifndef SSSS |
|
||||
#define SSSS |
|
||||
//void sleep(DWORD ms); |
|
||||
|
|
||||
//void sleep(DWORD ms) |
|
||||
//{ |
|
||||
// Sleep(ms); |
|
||||
//} |
|
||||
#endif |
|
||||
@ -1,585 +0,0 @@ |
|||||
#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; |
|
||||
} |
|
||||
@ -0,0 +1,183 @@ |
|||||
|
#include "sgn.h"
|
||||
|
#include <error.h>
|
||||
|
#include <errno.h>
|
||||
|
#include <math.h>
|
||||
|
#include <Trajectroy.h>
|
||||
|
|
||||
|
#include "Robot.h"
|
||||
|
#include "SvrData.h"
|
||||
|
#include "SvrHandling.h"
|
||||
|
#include <boost/thread/thread.hpp>
|
||||
|
|
||||
|
#include "Mat.h"
|
||||
|
#include "Vec.h"
|
||||
|
|
||||
|
void *threadRobotMovement(void *arg) |
||||
|
{ |
||||
|
// unused parameters
|
||||
|
(void) arg; |
||||
|
|
||||
|
Trajectory<jointNumber>* currentTrajectory = NULL; |
||||
|
enum MovementType currentMovementType = MovementJointBased; |
||||
|
|
||||
|
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[jointNumber] = {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[jointNumber] = {0.0f}; |
||||
|
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; |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
int main() |
||||
|
{ |
||||
|
int err = 0; |
||||
|
//Setting pthread for FRI interface
|
||||
|
pthread_t fri_t; |
||||
|
|
||||
|
//Start the thread for the robot movement
|
||||
|
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; |
||||
|
} |
||||
@ -0,0 +1,177 @@ |
|||||
|
#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; |
||||
|
} |
||||
|
|
||||
Write
Preview
Loading…
Cancel
Save
Reference in new issue