From 7f73d2969f2d2f9f8c3d6e3a61dc1b8c639646dc Mon Sep 17 00:00:00 2001 From: philipp schoenberger Date: Thu, 20 Aug 2015 09:15:18 +0200 Subject: [PATCH] add robot class for better handling future robots --- lwrserv/Makefile | 2 +- lwrserv/SocketObject.cpp | 40 +- lwrserv/SvrData.cpp | 60 +-- lwrserv/SvrHandling.cpp | 881 +---------------------------------- lwrserv/SvrHandling.h | 38 +- lwrserv/commands.cpp | 601 ++++++++++++++++++++++++ lwrserv/global.cpp | 36 -- lwrserv/header.h | 24 - lwrserv/include/SvrData.h | 55 +-- lwrserv/include/Trajectroy.h | 2 - lwrserv/include/commands.h | 49 ++ lwrserv/include/lwr4.h | 244 ++++++++++ lwrserv/include/robot.h | 96 ++++ lwrserv/program.cpp | 23 +- 14 files changed, 1097 insertions(+), 1054 deletions(-) create mode 100644 lwrserv/commands.cpp delete mode 100644 lwrserv/global.cpp delete mode 100644 lwrserv/header.h create mode 100644 lwrserv/include/commands.h create mode 100644 lwrserv/include/lwr4.h create mode 100644 lwrserv/include/robot.h diff --git a/lwrserv/Makefile b/lwrserv/Makefile index 7d91ca1..404e3d0 100644 --- a/lwrserv/Makefile +++ b/lwrserv/Makefile @@ -13,7 +13,7 @@ INCLUDE_DIR = -I . \ -I ./include # comment this out if you want debug compile info -#Q=@ +Q=@ ifeq ($(MAKECMDGOALS),test) GCOV := --coverage diff --git a/lwrserv/SocketObject.cpp b/lwrserv/SocketObject.cpp index 91f362b..0b8ec3c 100644 --- a/lwrserv/SocketObject.cpp +++ b/lwrserv/SocketObject.cpp @@ -7,13 +7,11 @@ #include #include -using namespace std; - #if defined(__msdos__) || defined(WIN32) SocketObject::SocketObject(void) { #ifdef __SOCKET_OBJECT_DEBUG - cout << "Windows SocketObject Konstruktor" << endl; + std::cout << "Windows SocketObject Konstruktor" << std::endl; #endif WSADATA wsaData; WORD wVersionRequested; @@ -73,7 +71,7 @@ int SocketObject::Bind(unsigned short int iPort) if (_skSocket == INVALID_SOCKET) { #ifdef __SOCKET_OBJECT_DEBUG - cout << "SocketObject::Bind(): INVALID SOCKET" << endl; + std::cout << "SocketObject::Bind(): INVALID SOCKET" << end::endl; #endif return 0; } @@ -90,7 +88,7 @@ int SocketObject::Bind(unsigned short int iPort) } else { #ifdef __SOCKET_OBJECT_DEBUG - cout << "SocketObject::Bind(): Alles ok" << endl; + std::cout << "SocketObject::Bind(): Alles ok" << std::endl; #endif return 0; } @@ -100,7 +98,7 @@ int SocketObject::Bind(unsigned short int iPort) struct sockaddr_in saServerAddress; #ifdef __SOCKET_OBJECT_DEBUG - std::cout << "Port: " << iPort << endl; + std::cout << "Port: " << iPort << std::endl; #endif // Socket erschaffen @@ -108,7 +106,7 @@ int SocketObject::Bind(unsigned short int iPort) if (_skSocket < 0) { #ifdef __SOCKET_OBJECT_DEBUG - std::cout << "Fehler bei Bind - socket()" << std::endl; + std::cout << "Error: on creating socket" << std::endl; #endif return -1; } @@ -133,7 +131,7 @@ int SocketObject::Bind(unsigned short int iPort) // Socket will nicht mehr lesen und schreiben shutdown(_skSocket,2); #ifdef __SOCKET_OBJECT_DEBUG - std::cout << "Fehler bei Bind!" << std::endl; + std::cout << "Error: on bind" << std::endl; #endif return -1; } else @@ -148,7 +146,7 @@ int SocketObject::Listen( void ) { // lauscht bei _skSocket, max Warteschlange = 2 return listen(_skSocket, 2); -// WIN32 : ,32 + // WIN32 : ,32 } @@ -178,7 +176,7 @@ bool SocketObject::Connect(const char* szServerAddress, unsigned short int iPort LPHOSTENT lpHost; int error; - // Open the socket + // open the socket _skSocket = socket(AF_INET, SOCK_STREAM, 0); if (_skSocket == INVALID_SOCKET) return false; @@ -217,7 +215,7 @@ bool SocketObject::Connect(const char* szServerAddress, unsigned short int iPort struct sockaddr_in server_addr; int status; - // Socket erschaffen + // create socket _skSocket = socket(AF_INET, SOCK_STREAM, 0); if (_skSocket < 0) return false; @@ -225,16 +223,13 @@ bool SocketObject::Connect(const char* szServerAddress, unsigned short int iPort if (nonblock) fcntl(_skSocket, F_SETFL, O_NONBLOCK); - // Initilisieren der sockaddr_in- Struktur + // initialize der sockaddr_in- Struktur memset(&server_addr, 0, sizeof(struct sockaddr_in)); server_addr.sin_family = AF_INET; - // server-Adresse bergeben server_addr.sin_addr.s_addr = inet_addr(szServerAddress); - // std::cout << inet_ntoa (server_addr.sin_addr)<< std::endl; - // Port server_addr.sin_port = htons(iPort); - // Verbindung herstellen + // connect to server status = connect(_skSocket, (struct sockaddr*)&server_addr, sizeof(sockaddr)); if (status < 0) @@ -247,7 +242,7 @@ bool SocketObject::Connect(const char* szServerAddress, unsigned short int iPort #endif } -bool SocketObject::Connect(const string szServerAddress, unsigned short int iPort, bool nonblock) +bool SocketObject::Connect(const std::string szServerAddress, unsigned short int iPort, bool nonblock) { return Connect((char*) szServerAddress.c_str(), iPort, nonblock); } @@ -270,10 +265,10 @@ int SocketObject::Recv(char *szBuffer, int iBufferLength, int iFlags) return recv(_skSocket, szBuffer, iBufferLength, iFlags); } -int SocketObject::Recv (string& s) +int SocketObject::Recv (std::string& s) { char buf [MAXRECV+1]; - // cout << WSAGetLastError() << " in SocketObject::Recv, vor recv" << endl; + // std::cout << WSAGetLastError() << " in SocketObject::Recv, vor recv" << std::endl; s = ""; memset(buf, 0, MAXRECV+1); int status = Recv(buf, MAXRECV, 0); @@ -282,7 +277,7 @@ int SocketObject::Recv (string& s) { case -1: #ifdef __SOCKET_OBJECT_DEBUG - cout << "status: -1 errno: " << errno << " in SocketObject::Recv" << endl; + std::cout << "status: -1 errno: " << errno << " in SocketObject::Recv" << std::endl; #endif break; case 0: @@ -309,7 +304,6 @@ int SocketObject::Recv (string& s) int SocketObject::receiveLine(char* array, int arraySize) { - //std::string ret; char r; int pos = 0; @@ -380,9 +374,9 @@ int SocketObject::Send(const char *szBuffer, int iBufferLength, int iFlags) return send(_skSocket,szBuffer,iBufferLength, iFlags); } -int SocketObject::Send (const string s) +int SocketObject::Send (const std::string s) { - string str = s; + std::string str = s; // add the (expected) CRLF if(str == "") { diff --git a/lwrserv/SvrData.cpp b/lwrserv/SvrData.cpp index 0806267..5338703 100644 --- a/lwrserv/SvrData.cpp +++ b/lwrserv/SvrData.cpp @@ -8,6 +8,10 @@ #include "friremote.h" #include +#include "mat.h" +#include "vec.h" +#include "robot.h" + bool SvrData::instanceFlag = false; SvrData* SvrData::single = 0; @@ -67,7 +71,7 @@ friRemote* SvrData::getFriRemote() } void SvrData::updateMessurementData( - VecJoint newJointPos, + Robot::VecJoint newJointPos, MatCarthesian newCartPos, VecTorque newForceAndTorque ) @@ -81,7 +85,7 @@ void SvrData::updateMessurementData( void SvrData::updateMessurement() { - VecJoint newJointPos(0.0f); + Robot::VecJoint newJointPos(0.0f); MatCarthesian newCartPos(0.0f,1.0f); VecTorque newForceAndTorque(0.0f); @@ -89,8 +93,8 @@ void SvrData::updateMessurement() /// update current joint positions friInst->getState(); FRI_STATE state = friInst->getState(); - VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition()); - VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset()); + Robot::VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition()); + Robot::VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset()); newJointPos = newJointPosComanded + newJointPosOffset; newJointPos = newJointPos * ( 180.0f / M_PI); @@ -115,9 +119,9 @@ void SvrData::unlock() pthread_mutex_unlock(&dataLock); } -VecJoint SvrData::getMessuredJointPos() +Robot::VecJoint SvrData::getMessuredJointPos() { - VecJoint buf; + Robot::VecJoint buf; pthread_mutex_lock(&dataLock); buf = messured.jointPos; pthread_mutex_unlock(&dataLock); @@ -154,15 +158,15 @@ int SvrData::getMessuredJacobian(float* data, size_t size) return 0; } -VecJoint SvrData::getCommandedJointPos() +Robot::VecJoint SvrData::getCommandedJointPos() { - VecJoint buff; + Robot::VecJoint buff; pthread_mutex_lock(&dataLock); buff = commanded.jointPos; pthread_mutex_unlock(&dataLock); return buff; } -void SvrData::setCommandedJointPos(VecJoint newJointPos) +void SvrData::setCommandedJointPos(Robot::VecJoint newJointPos) { pthread_mutex_lock(&dataLock); commanded.jointPos = newJointPos; @@ -184,41 +188,41 @@ void SvrData::setCommandedCartPos(MatCarthesian newCartPos) } -VecJoint SvrData::getMaxTorque() +Robot::VecJoint SvrData::getMaxTorque() { - VecJoint buff; + Robot::VecJoint buff; pthread_mutex_lock(&dataLock); buff = robot.max.torque; pthread_mutex_unlock(&dataLock); return buff; } -VecJoint SvrData::getMaxAcceleration() +Robot::VecJoint SvrData::getMaxAcceleration() { - VecJoint retval(0.0f); + Robot::VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); retval = robot.max.accelaration; pthread_mutex_unlock(&dataLock); return retval; } -VecJoint SvrData::getMaxVelocity() +Robot::VecJoint SvrData::getMaxVelocity() { - VecJoint retval(0.0f); + Robot::VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); retval = robot.max.velocity; pthread_mutex_unlock(&dataLock); return retval; } -VecJoint SvrData::getMaxRange() +Robot::VecJoint SvrData::getMaxRange() { - VecJoint retval(0.0f); + Robot::VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); retval = robot.max.range; pthread_mutex_unlock(&dataLock); return retval; } -bool SvrData::checkJointRange(VecJoint vectorToCheck) +bool SvrData::checkJointRange(Robot::VecJoint vectorToCheck) { // save temporal the max range for short time lock pthread_mutex_lock(&dataLock); @@ -254,10 +258,10 @@ int SvrData::setRobotVelocityPercentage(float newVelocityPercentage) pthread_mutex_unlock(&dataLock); return retval; } -VecJoint SvrData::getRobotVelocity() +Robot::VecJoint SvrData::getRobotVelocity() { float percent = 0.0f; - VecJoint retval(0.0f); + Robot::VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); percent = robot.currentVelocityPercentage / 100.0f; @@ -291,10 +295,10 @@ int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage) pthread_mutex_unlock(&dataLock); return retval; } -VecJoint SvrData::getRobotAcceleration() +Robot::VecJoint SvrData::getRobotAcceleration() { float percent = 0; - VecJoint retval(0.0f); + Robot::VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); percent = robot.currentAccelerationPercentage / 100.0f; @@ -444,15 +448,15 @@ void SvrData::setTrajectoryType(enum TrajectoryType newType) pthread_mutex_unlock(&dataLock); } -class Trajectory* SvrData::createTrajectory(VecJoint newJointPos) +class Trajectory* SvrData::createTrajectory(Robot::VecJoint newJointPos) { //set new target for next trajectory - VecJoint prevJointPos = getCommandedJointPos(); + Robot::VecJoint prevJointPos = getCommandedJointPos(); class Trajectory* newTrajectory; float sampleTimeMs = getSampleTimeMs(); - VecJoint maxJointVelocity = getRobotVelocity(); - VecJoint maxJointAcceleration = getRobotAcceleration(); + Robot::VecJoint maxJointVelocity = getRobotVelocity(); + Robot::VecJoint maxJointAcceleration = getRobotAcceleration(); enum TrajectoryType type = getTrajectoryType(); //newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos); @@ -464,9 +468,9 @@ class Trajectory* SvrData::createTrajectory(MatCarthesian newJoint { } -VecJoint SvrData::getMeasuredJointPos() +Robot::VecJoint SvrData::getMeasuredJointPos() { - VecJoint retval(0.0f); + Robot::VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); retval = messured.jointPos; pthread_mutex_unlock(&dataLock); diff --git a/lwrserv/SvrHandling.cpp b/lwrserv/SvrHandling.cpp index f4a2b42..d8dad02 100755 --- a/lwrserv/SvrHandling.cpp +++ b/lwrserv/SvrHandling.cpp @@ -1,6 +1,4 @@ -#include "header.h" #include -#include "global.h" #include "mat.h" #include "StringTool.h" #include @@ -9,6 +7,7 @@ #include "SvrHandling.h" #include "SvrData.h" +#include "commands.h" #include "Trajectroy.h" #include "LinearTrajectory.h" #include @@ -25,273 +24,6 @@ void printUsage(SocketObject& client, std::string& arg) } } -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))Accept(*client)) { - cout << timestamp() + "Client accepted!\n"; + std::cout << timestamp() + "Client accepted!\n"; if (handshakeAccepted(*client)) { clientCommandLoop(*client); } client->Disconnect(); - cout << timestamp() + "Client disconnected.\n"; + std::cout << timestamp() + "Client disconnected.\n"; } else { - cout << timestamp() + "Client bin error.\n"; + std::cout << timestamp() + "Client bin error.\n"; } delete client; } @@ -434,10 +166,10 @@ void SvrHandling::run(int port) bool SvrHandling::handshakeAccepted(SocketObject& client) { this->c_state = handshake; - string message = SVR_HELLO_MSG; + std::string message = SVR_HELLO_MSG; client.Send(message); client.Recv(message); - if (message.find(SVR_HANDSHAKE,0) != string::npos) + if (message.find(SVR_HANDSHAKE,0) != std::string::npos) { c_state = accepted; message = SVR_ACCEPTED; @@ -446,7 +178,7 @@ bool SvrHandling::handshakeAccepted(SocketObject& client) { c_state = waiting; message = SVR_FAILED; - cout << timestamp() + "Handshake failed. " << "Invalid recv msg \'" << message <<"\'"; + std::cout << timestamp() + "Handshake failed. " << "Invalid recv msg \'" << message <<"\'"; } client.Send(message); return(c_state == accepted); @@ -454,13 +186,13 @@ bool SvrHandling::handshakeAccepted(SocketObject& client) void SvrHandling::clientCommandLoop(SocketObject& client) { - string message, cmd, arg; + std::string message, cmd, arg; while (c_state == accepted) { if (client.Recv(message) > 0) { - cout<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 getPositionHomRowWise(SocketObject& client, std::string& arg) -{ - // unused - (void) arg; - - string out = ""; - stringstream ss (stringstream::in | stringstream::out); - - // get current Cartesian positions from database - MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos(); - - // assemble command feedback 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 getForceTorqueTcp(SocketObject& client, std::string& arg) -{ - // unused - (void) arg; - - 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 movePTPJoints(SocketObject& client, string& arg) -{ - string buf; - - stringstream ss(arg); - 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* newTrajectory; - - float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs(); - VecJoint maxJointVelocity = SvrData::getInstance()->getRobotVelocity(); - VecJoint maxJointAcceleration = SvrData::getInstance()->getRobotAcceleration(); - - //newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos); - newTrajectory = (class Trajectory*) new LinearJointTrajectory(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); - - // add trajectory to queue for sender thread - SvrData::getInstance()->pushTrajectory(newTrajectory); - - SvrData::getInstance()->setCommandedJointPos(newJointPos); - - //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 moveHomRowWiseStatus(SocketObject& client, string& arg) -{ - string buf; - double temp[15]; - stringstream ss(arg); - 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 configurationParameter[3]; - for (int i=0;i<3;i++) - { - configurationParameter[i] = temp[12+i]; - } - - //Backward Calculation - VecJoint newJointPos = kukaBackwardCalc(temp, configurationParameter); - - - //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()->getRobotVelocity(); - VecJoint maxJointAcceleration = SvrData::getInstance()->getRobotAcceleration(); - std::cout << "max velo is " << maxJointVelocity ; - - class Trajectory* newTrajectory = new LinearJointTrajectory(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 setSpeed(SocketObject& client, string& arg) -{ - string buf; - float newVelocityPercentage; - int argc = 0; - stringstream ss(arg); - stringstream ss2f; - vector tokens; - - // no speed argument - if (arg == "") - { - 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 setAccel(SocketObject& client, string& arg) -{ - string buf; - float newAccelerationPercentage; - stringstream ss(arg); - stringstream ss2f; - int argc = 0; - - // need at least one argument - if (arg == "") - { - 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 startPotFieldMode(SocketObject& client, string& arg) -{ - // unused - (void) arg; -#if 0 - __MSRSTARTPOTFIELD = false; - __MSRSTOPPOTFIELD = false; - //Set current Joint Vals - for (int i=0; i> 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 configurationParameter[3]; - for (int i=0;i<3;i++) - { - configurationParameter[i] = temp[12+i]; - } - - //Backward Calculation of the position - VecJoint CalcJoints = kukaBackwardCalc(temp, configurationParameter); - - //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 setJoints(SocketObject& client, string& arg) -{ - string buf; - VecJoint newJointPos(0.0f); - stringstream ss(arg); - stringstream ss2f; - int argc = 0; - - // convert to Joint vector - while (ss >> buf) - { - // to many input arguments or Joints - if (argc>=JOINT_NUMBER) - { - client.Send(SVR_INVALID_ARGUMENT_COUNT); - return; - } - - // convert string to float - ss2f.flush(); - ss2f.clear(); - ss2f << buf; - ss2f >> newJointPos(argc); - argc++; - } - - // to less joint values - if (argc != 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 moveCartesian(SocketObject& client, string& arg) -{ - string buf; - float temp[15]; - stringstream ss(arg); - 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 configurationParameter[3]; - for (int i=0;i<3;i++) - { - configurationParameter[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 quit(SocketObject& client , std::string& arg) -{ - //unused - (void) arg; - - client.Send(SVR_TRUE_RSP); - client.Disconnect(); -} - -void setTrajectoryType(SocketObject& client, std::string& arg) -{ - string buf; - enum TrajectoryType newType = TrajectoryDefault; - - newType = toEnum(arg); - SvrData::getInstance()->setTrajectoryType(newType); - cout << "new trajectory type : "<< toString(newType) << "\n"; - - client.Send(SVR_TRUE_RSP); -} -void getTrajectoryType(SocketObject& client, std::string& arg) -{ - enum TrajectoryType currType = SvrData::getInstance()->getTrajectoryType(); - cout << "curr trajectory type : "<< toString(currType) << "\n"; - - client.Send("current trajectory: " + toString(currType)); -} - -void debug(SocketObject& client, std::string& arg) -{ - - //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); - //} -} diff --git a/lwrserv/SvrHandling.h b/lwrserv/SvrHandling.h index cd6e583..1f90b41 100755 --- a/lwrserv/SvrHandling.h +++ b/lwrserv/SvrHandling.h @@ -1,5 +1,5 @@ -#include "defines.h" #include "StringTool.h" +#include "SocketObject.h" #include #include "friremote.h" @@ -60,40 +60,4 @@ struct ClientCommand void (*processCommand) ( SocketObject& client, std::string& argv); void (*printHelp) (void); }; -static struct ClientCommand* commands; -static unsigned int commandCount; - -void printUsage(SocketObject& client, std::string& arg); -//Handling request for current Joint Values -void getPositionJoints(SocketObject& client, std::string& arg); -//Get Position as POSE Matrix -void getPositionHomRowWise(SocketObject& client, std::string& arg); -//Get Force/torque values from TCP -void getForceTorqueTcp(SocketObject& client, std::string& arg); -//Move to given Joint combination -void movePTPJoints(SocketObject& client, std::string& arg); -//Move to given POSE position -void moveHomRowWiseStatus(SocketObject& client, std::string& arg); -//Set Velocity -void setSpeed(SocketObject& client, std::string& arg); -//Set Acceleration -void setAccel(SocketObject& client, std::string& arg); -//Starting Potential Field Movement Mode -void startPotFieldMode(SocketObject& client, std::string& arg); -//Stopping Potential Field Movement Mode -void stopPotFieldMode(SocketObject& client, std::string& arg); -//Setting Target Position HomRowWise for Potential Move -void setPos(SocketObject& client, std::string& arg); -//Setting Target Position as Joints for Potential Move -void setJoints(SocketObject& client, std::string& arg); -//Cartesian Movement -//Move to given POSE position -void moveCartesian(SocketObject& client, std::string& arg); -// set a specific trajectory type -void setTrajectoryType(SocketObject& client, std::string& arg); -void getTrajectoryType(SocketObject& client, std::string& arg); -//Quit -void quit(SocketObject& client, std::string& arg); -//DEBUGGING -void debug(SocketObject& client, std::string& arg); #endif diff --git a/lwrserv/commands.cpp b/lwrserv/commands.cpp new file mode 100644 index 0000000..fc69f36 --- /dev/null +++ b/lwrserv/commands.cpp @@ -0,0 +1,601 @@ +#include "commands.h" +#include "robot.h" +#include "lwr4.h" +#include "SocketObject.h" +#include "Trajectroy.h" +#include "SvrHandling.h" +#include "SvrData.h" + +#include +#include +#include "mat.h" +#include "vec.h" + +#include "StringTool.h" +#include +#include +#include + +#include + +void getPositionJoints(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + std::string out = ""; + std::stringstream ss (std::stringstream::in | std::stringstream::out); + + // get current joint positions from database + Robot::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 getPositionHomRowWise(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + std::string out = ""; + std::stringstream ss (std::stringstream::in | std::stringstream::out); + + // get current Cartesian positions from database + MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos(); + + // assemble command feedback 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 getForceTorqueTcp(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + std::string out = ""; + std::stringstream ss (std::stringstream::in | std::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 movePTPJoints(SocketObject& client, std::string& arg) +{ + std::string buf; + + std::stringstream ss(arg); + std::stringstream ss2f; + + // convert string to joint vector + Robot::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)) + { + std::string out = "Error: Joints out of Range!"; + client.Send(SVR_OUT_OF_RANGE); + return; + } + //set new target for next trajectory + Robot::VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos(); + class Trajectory* newTrajectory; + + float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs(); + Robot::VecJoint maxJointVelocity = SvrData::getInstance()->getRobotVelocity(); + Robot::VecJoint maxJointAcceleration = SvrData::getInstance()->getRobotAcceleration(); + + //newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos); + newTrajectory = (class Trajectory*) new LinearJointTrajectory(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); + + // add trajectory to queue for sender thread + SvrData::getInstance()->pushTrajectory(newTrajectory); + + SvrData::getInstance()->setCommandedJointPos(newJointPos); + + //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 moveHomRowWiseStatus(SocketObject& client, std::string& arg) +{ + std::string buf; + double temp[15]; + std::stringstream ss(arg); + std::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 + struct Robot::RobotConfig configurationParameter; + configurationParameter.elbow = temp[12] == 1.0f; + configurationParameter.flip = temp[13] == 1.0f; + configurationParameter.j1os = temp[14] == 1.0f; + + //Backward Calculation + Robot::VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter); + + + //Check for Joint Range + if (!SvrData::getInstance()->checkJointRange(newJointPos)) + { + client.Send(SVR_OUT_OF_RANGE); + return; + } + + // calculate trajectory + Robot::VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos(); + float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs(); + Robot::VecJoint maxJointVelocity = SvrData::getInstance()->getRobotVelocity(); + Robot::VecJoint maxJointAcceleration = SvrData::getInstance()->getRobotAcceleration(); + std::cout << "max velo is " << maxJointVelocity ; + + class Trajectory* newTrajectory = new LinearJointTrajectory(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 setSpeed(SocketObject& client, std::string& arg) +{ + std::string buf; + float newVelocityPercentage; + int argc = 0; + std::stringstream ss(arg); + std::stringstream ss2f; + + // no speed argument + if (arg == "") + { + 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 setAccel(SocketObject& client, std::string& arg) +{ + std::string buf; + float newAccelerationPercentage; + std::stringstream ss(arg); + std::stringstream ss2f; + int argc = 0; + + // need at least one argument + if (arg == "") + { + 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 startPotFieldMode(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; +#if 0 + __MSRSTARTPOTFIELD = false; + __MSRSTOPPOTFIELD = false; + //Set current Joint Vals + for (int i=0; i> 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 + struct Robot::RobotConfig configurationParameter; + configurationParameter.elbow = temp[12] == 1.0f; + configurationParameter.flip = temp[13] == 1.0f; + configurationParameter.j1os = temp[14] == 1.0f; + + //Backward Calculation + Robot::VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter); + + //Check for a valid joint range + if (!SvrData::getInstance()->checkJointRange(newJointPos)) + { + client.Send(SVR_OUT_OF_RANGE); + return; + } + + // calculate new trajectory for next position + + // set new commanded Position for next Trajectory + SvrData::getInstance()->setCommandedJointPos(newJointPos); + + //Wait to end of movement + + // send positive client feedback + client.Send(SVR_TRUE_RSP); +} + +void setJoints(SocketObject& client, std::string& arg) +{ + std::string buf; + Robot::VecJoint newJointPos(0.0f); + std::stringstream ss(arg); + std::stringstream ss2f; + int argc = 0; + + // convert to Joint vector + while (ss >> buf) + { + // to many input arguments or Joints + if (argc>=JOINT_NUMBER) + { + client.Send(SVR_INVALID_ARGUMENT_COUNT); + return; + } + + // convert string to float + ss2f.flush(); + ss2f.clear(); + ss2f << buf; + ss2f >> newJointPos(argc); + argc++; + } + + // to less joint values + if (argc != 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 + Robot::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 moveCartesian(SocketObject& client, std::string& arg) +{ +#if 0 + std::string buf; + float temp[15]; + std::stringstream ss(arg); + std::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 configurationParameter[3]; + for (int i=0;i<3;i++) + { + configurationParameter[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); + +#endif + // send a positive client feedback + client.Send(SVR_TRUE_RSP); +} + +void quit(SocketObject& client , std::string& arg) +{ + //unused + (void) arg; + + client.Send(SVR_TRUE_RSP); + client.Disconnect(); +} + +void setTrajectoryType(SocketObject& client, std::string& arg) +{ + std::string buf; + enum TrajectoryType newType = TrajectoryDefault; + + newType = toEnum(arg); + SvrData::getInstance()->setTrajectoryType(newType); + std::cout << "new trajectory type : "<< toString(newType) << "\n"; + + client.Send(SVR_TRUE_RSP); +} +void getTrajectoryType(SocketObject& client, std::string& arg) +{ + enum TrajectoryType currType = SvrData::getInstance()->getTrajectoryType(); + std::cout << "curr trajectory type : "<< toString(currType) << "\n"; + + client.Send("current trajectory: " + toString(currType)); +} + +void debug(SocketObject& client, std::string& arg) +{ + + //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); + //} +} diff --git a/lwrserv/global.cpp b/lwrserv/global.cpp deleted file mode 100644 index ec39519..0000000 --- a/lwrserv/global.cpp +++ /dev/null @@ -1,36 +0,0 @@ -#if 0 -bool __SVR_CURRENT_JOB = false; -bool __MSRMSRJNTPOS = false; -bool __MSRCMDJNTPOS = false; -bool __MSRCMDPOSE = false; -bool __SETVELOCITY = false; -bool __SETACCEL = false; -bool __DEBUG = false; -bool __POTFIELDMODE = false; -bool __MSRSTARTPOTFIELD = false; -bool __MSRSTOPPOTFIELD = false; -bool __MSRSETPOS = false; -bool __CARTMOVE = false; -bool __DOUS = false; -bool __DOUS2 = false; - -int globi = 0; - -float MSRMSRJNTPOS[7]; -double MSRCMDJNTPOS[7]; -double MSRMSRCARTPOS[12]; -float MSRCMDCARTPOS[12]; -double MSRMSRJACOBIAN[42]; -double MSRMSRFTTCP[6]; -double MSRCMDPOSE[3][4]; - -double VELOCITY = 20; -double ACCELARATION = 10; -//double maxVelJnt[] = {110.0,110.0,128.0,128.0,204.0,184.0,184.0}; -double maxAccJnt[] = {1.0,1.0,1.0,1.0,2.0,1.0,1.0}; -double maxTrqJnt[] = {176.0,176.0,100.0,100.0,100.0,38.0,38.0}; -double maxRangeJnt[] = {170.0,120.0,170.0,120.0,170.0,120.0,170.0}; -#endif - - - diff --git a/lwrserv/header.h b/lwrserv/header.h deleted file mode 100644 index 564af09..0000000 --- a/lwrserv/header.h +++ /dev/null @@ -1,24 +0,0 @@ -#include -#include -#include "defines.h" -#include "SocketObject.h" -#include "SvrHandling.h" -#include "StringTool.h" -#include -#include "pthread.h" -#include "friremote.h" -#include "math.h" -#include -#include "mat.h" -#include "vec.h" -//#include "opencv2\opencv.hpp" -//#pragma comment (lib, "opencv_core248d.lib") - - -//debug -#include -//end debug - -using namespace std; -//using namespace cv; - diff --git a/lwrserv/include/SvrData.h b/lwrserv/include/SvrData.h index 9057902..b3dd10d 100644 --- a/lwrserv/include/SvrData.h +++ b/lwrserv/include/SvrData.h @@ -12,31 +12,32 @@ #include "friComm.h" #include "friremote.h" +#include "mat.h" +#include "vec.h" +#include "robot.h" + #define JOINT_NUMBER (LBR_MNJ) -typedef Vec VecJoint; -typedef Vec VecTorque; -typedef Mat MatCarthesian; class SvrData: public Singleton { private: struct { - VecJoint jointPos; + Robot::VecJoint jointPos; MatCarthesian cartPos; VecTorque forceAndTorque; float jacobian[FRI_CART_VEC * JOINT_NUMBER]; } messured; struct { - VecJoint jointPos; + Robot::VecJoint jointPos; MatCarthesian cartPos; } commanded; struct { struct { - VecJoint velocity; - VecJoint accelaration; - VecJoint torque; - VecJoint range; + Robot::VecJoint velocity; + Robot::VecJoint accelaration; + Robot::VecJoint torque; + Robot::VecJoint range; } max; float currentVelocityPercentage; float currentAccelerationPercentage; @@ -62,7 +63,7 @@ private: void updateMessurementData( - VecJoint newJointPos, + Robot::VecJoint newJointPos, MatCarthesian newCartPos, VecTorque newForceAndTorque ); @@ -78,32 +79,32 @@ public: void lock(); void unlock(); - VecJoint getMeasuredJointPos(); + Robot::VecJoint getMeasuredJointPos(); MatCarthesian getMeasuredCartPos(); int getMessuredJacobian(float* data, size_t size); VecTorque getMessuredForceTorque(); - VecJoint getMessuredJointPos(); + Robot::VecJoint getMessuredJointPos(); MatCarthesian getMessuredCartPos(); - VecJoint getCommandedJointPos(); - void setCommandedJointPos(VecJoint newJointPos); + Robot::VecJoint getCommandedJointPos(); + void setCommandedJointPos(Robot::VecJoint newJointPos); MatCarthesian getCommandedCartPos(); void setCommandedCartPos(MatCarthesian newCartPos); - VecJoint getMaxTorque(); - VecJoint getMaxVelocity(); - VecJoint getMaxAcceleration(); - VecJoint getMaxRange(); + Robot::VecJoint getMaxTorque(); + Robot::VecJoint getMaxVelocity(); + Robot::VecJoint getMaxAcceleration(); + Robot::VecJoint getMaxRange(); - bool checkJointRange(VecJoint vectorToCheck); + bool checkJointRange(Robot::VecJoint vectorToCheck); - VecJoint getRobotVelocity(); + Robot::VecJoint getRobotVelocity(); float getRobotVelocityPercentage(); int setRobotVelocityPercentage(float newVelocityPercentage); - VecJoint getRobotAcceleration(); + Robot::VecJoint getRobotAcceleration(); float getRobotAccelerationPercentage(); int setRobotAccelerationPercentage(float newAccelerationPercentage); @@ -124,16 +125,16 @@ public: enum TrajectoryType getTrajectoryType(); void setTrajectoryType(enum TrajectoryType newType); - class Trajectory* createTrajectory(VecJoint newJointPos); + class Trajectory* createTrajectory(Robot::VecJoint newJointPos); class Trajectory* createTrajectory(MatCarthesian newJointPos); template class Trajectory* calculateTrajectory( - VecJoint maxJointVelocity, - VecJoint maxJointAcceleration, - VecJoint jointStart, - VecJoint jointEnd + Robot::VecJoint maxJointVelocity, + Robot::VecJoint maxJointAcceleration, + Robot::VecJoint jointStart, + Robot::VecJoint jointEnd ); - int calculateAndAddNewTrajectory(VecJoint jointEnd); + int calculateAndAddNewTrajectory(Robot::VecJoint jointEnd); }; #endif diff --git a/lwrserv/include/Trajectroy.h b/lwrserv/include/Trajectroy.h index f3490d1..6d8488b 100644 --- a/lwrserv/include/Trajectroy.h +++ b/lwrserv/include/Trajectroy.h @@ -2,7 +2,6 @@ #define _TRAJECTORY_H_ #include "vec.h" #include -#include template class Trajectory; @@ -61,7 +60,6 @@ static std::string toString(enum TrajectoryType type) static enum TrajectoryType toEnum(std::string name) { int items = sizeof(TrajectoryTypeStr) / sizeof(TrajectoryTypeStr[0]); - std::cout <<"items "<< items <<"\n"; for (int i = 0 ; i < items ; ++i) { if (TrajectoryTypeStr[i].str == name) diff --git a/lwrserv/include/commands.h b/lwrserv/include/commands.h new file mode 100644 index 0000000..79a0e31 --- /dev/null +++ b/lwrserv/include/commands.h @@ -0,0 +1,49 @@ +#ifndef _COMMANDS_H_ +#define _COMMANDS_H_ + +#include "SocketObject.h" +#include + +static struct ClientCommand* commands; +static unsigned int commandCount; + +// registers all commands +void registerCommands(); + +// print out all registered commands to the client interface +void printUsage(SocketObject& client, std::string& arg); + +//Handling request for current Joint Values +void getPositionJoints(SocketObject& client, std::string& arg); +//Get Position as POSE Matrix +void getPositionHomRowWise(SocketObject& client, std::string& arg); +//Get Force/torque values from TCP +void getForceTorqueTcp(SocketObject& client, std::string& arg); +//Move to given Joint combination +void movePTPJoints(SocketObject& client, std::string& arg); +//Move to given POSE position +void moveHomRowWiseStatus(SocketObject& client, std::string& arg); +//Set Velocity +void setSpeed(SocketObject& client, std::string& arg); +//Set Acceleration +void setAccel(SocketObject& client, std::string& arg); +//Starting Potential Field Movement Mode +void startPotFieldMode(SocketObject& client, std::string& arg); +//Stopping Potential Field Movement Mode +void stopPotFieldMode(SocketObject& client, std::string& arg); +//Setting Target Position HomRowWise for Potential Move +void setPos(SocketObject& client, std::string& arg); +//Setting Target Position as Joints for Potential Move +void setJoints(SocketObject& client, std::string& arg); +//Cartesian Movement +//Move to given POSE position +void moveCartesian(SocketObject& client, std::string& arg); +// set a specific trajectory type +void setTrajectoryType(SocketObject& client, std::string& arg); +void getTrajectoryType(SocketObject& client, std::string& arg); +//Quit +void quit(SocketObject& client, std::string& arg); +//DEBUGGING +void debug(SocketObject& client, std::string& arg); + +#endif diff --git a/lwrserv/include/lwr4.h b/lwrserv/include/lwr4.h new file mode 100644 index 0000000..76477a6 --- /dev/null +++ b/lwrserv/include/lwr4.h @@ -0,0 +1,244 @@ +#ifndef _LWR4_H_ +#define _LWR4_H_ + +#include "vec.h" +#include "mat.h" +#include "friComm.h" +#include "robot.h" + + +class LWR4 : public Robot +{ + public : + const static int joints = 7; + typedef Vec VecJoint; + + + static VecJoint getJointRange(void); + static VecJoint getJointVelocity(void); + static VecJoint getJointAcceleration(void); + + static Vec getDhParameter(unsigned int joint); + + static VecJoint backwardCalc(MatCarthesian pos, struct RobotConfig config ); + static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config ); +}; +Robot::VecJoint LWR4::backwardCalc(MatCarthesian M , struct RobotConfig config ) +{ + Robot::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 = (config.elbow ? 1.0f : -1.0f ); + int FLIP = (config.flip ? 1.0f : -1.0f ); + float J1os = (config.j1os ? 1.0f : -1.0f )/180.0*M_PI; + + //Calculating M06 + MatCarthesian 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.0f; + + 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.0f; + + R67(2,0) = 0.0f; + R67(2,1) = sin(alp[i]); + R67(2,2) = cos(alp[i]); + R67(2,3) = d[i]; + + R67(3,0) = 0.0f; + R67(3,1) = 0.0f; + R67(3,2) = 0.0f; + R67(3,3) = 1.0f; + + MatCarthesian M06; + MatCarthesian Inv; + Inv = R67.inv(); + M06=M*Inv; + + //Joint 1 including given offset + Joints(0) = atan2(M06(1,3),M06(0,3))+J1os; + + //Calculating M16 + MatCarthesian 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.0f; + + 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.0f; + + R01(2,0) = 0.0f; + R01(2,1) = sin(alp[i]); + R01(2,2) = cos(alp[i]); + R01(2,3) = d[i]; + + R01(3,0) = 0.0f; + R01(3,1) = 0.0f; + R01(3,2) = 0.0f; + R01(3,3) = 1.0f; + + Inv = R01.inv(); + MatCarthesian 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 + MatCarthesian 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.0f; + + 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.0f; + + R12(2,0) = 0.0f; + R12(2,1) = sin(alp[i]); + R12(2,2) = cos(alp[i]); + R12(2,3) = d[i]; + + R12(3,0) = 0.0f; + R12(3,1) = 0.0f; + R12(3,2) = 0.0f; + R12(3,3) = 1.0f; + + 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 + MatCarthesian 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.0f; + + 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.0f; + + R23(2,0) = 0.0f; + R23(2,1) = sin(alp[i]); + R23(2,2) = cos(alp[i]); + R23(2,3) = d[i]; + + R23(3,0) = 0.0f; + R23(3,1) = 0.0f; + R23(3,2) = 0.0f; + R23(3,3) = 1.0f; + + MatCarthesian R03; + R03 = R01*R12*R23; + Inv = R03.inv(); + MatCarthesian M36; + M36 = Inv*M06; + + //Joint 4 + Joints(3) = atan2(M36(0,3),-M36(1,3)); + + //Calculating M47 + MatCarthesian 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.0f; + + 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.0f; + + R34(2,0) = 0.0f; + R34(2,1) = sin(alp[i]); + R34(2,2) = cos(alp[i]); + R34(2,3) = d[i]; + + R34(3,0) = 0.0f; + R34(3,1) = 0.0f; + R34(3,2) = 0.0f; + R34(3,3) = 1.0f; + + MatCarthesian R04; + R04 = R01*R12*R23*R34; + Inv = R04.inv(); + MatCarthesian M47; + M47 = Inv*M; + + //Joint 5 + float th = 0.0001; + if (abs(M47(1,2)) MatCarthesian; +typedef Vec VecTorque; + +class Robot +{ + public : + const static int joints = 7; + typedef Vec VecJoint; + + static int getJointCount(void) + { + } + static VecJoint getJointRange(void) + { + return VecJoint(180.0f); + } + static VecJoint getJointVelocity(void) + { + return VecJoint(200.0f); + } + static VecJoint getJointAcceleration(void) + { + return VecJoint(10.0f); + } + + static Vec getDhParameter(unsigned int joint) + { + // unused parameter + (void) joint; + return Vec(0.0f); + } + + struct RobotConfig + { + bool elbow; + bool flip; + bool j1os; + }; + + static VecJoint backwardCalc(MatCarthesian, struct RobotConfig config ); + static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config ); +}; + + + +#if 0 +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; +} +#endif + +#endif diff --git a/lwrserv/program.cpp b/lwrserv/program.cpp index dc8f5d6..1bffc61 100644 --- a/lwrserv/program.cpp +++ b/lwrserv/program.cpp @@ -1,5 +1,3 @@ -#include "header.h" -#include "global.h" #include "sgn.h" #include #include @@ -7,8 +5,13 @@ #include #include "SvrData.h" +#include "SvrHandling.h" #include +#include "mat.h" +#include "vec.h" +#include "robot.h" + float* abctomat(float a, float b, float c) { Mat4f rx; @@ -180,7 +183,7 @@ void *threadRobotMovement(void *arg) Trajectory* currentTrajectory = NULL; enum MovementType currentMovementType = MovementJointBased; - VecJoint currentJointPos(0.0f); + Robot::VecJoint currentJointPos(0.0f); MatCarthesian currentCartPos(0.0f,1.0f); friRemote* friInst = SvrData::getInstance()->getFriRemote(); @@ -275,7 +278,7 @@ end: { // send the new joint values float newJointPosToSend[JOINT_NUMBER] = {0.0f}; - VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI); + Robot::VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI); newJointPosToSendRad.getData(newJointPosToSend); std::cout << newJointPosToSendRad << "\n"; friInst->doPositionControl(newJointPosToSend); @@ -298,13 +301,13 @@ end: // TODO if(currentTrajectory != NULL) { - cout << "remaining Steps = " << currentTrajectory->getRemainingSteps()<<"\n"; + std::cout << "remaining Steps = " << currentTrajectory->getRemainingSteps()<<"\n"; } // check if current trajectory is over if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0) { - cout << " Trajectory finished \n "; + std::cout << " Trajectory finished \n "; // invalid trajectory skip it delete currentTrajectory; currentTrajectory = NULL; @@ -329,12 +332,12 @@ void *threadFriDataExchange(void *arg) SvrData::getInstance()->updateMessurement(); // get current joint position - VecJoint currentJointPos = SvrData::getInstance()->getMessuredJointPos(); + Robot::VecJoint currentJointPos = SvrData::getInstance()->getMessuredJointPos(); MatCarthesian currentCartPost = SvrData::getInstance()->getMessuredCartPos(); // get current force and torque - VecJoint currentForceAndTorque = SvrData::getInstance()->getMessuredForceTorque(); + Robot::VecJoint currentForceAndTorque = SvrData::getInstance()->getMessuredForceTorque(); // get current jacobian // TODO use svr data @@ -560,7 +563,7 @@ int main() err = pthread_create(&fri_t,NULL,&threadRobotMovement,NULL); if (err > 0 ) { - cerr << "Failed: could not create thread\n" << endl; + std::cerr << "Failed: could not create thread\n" << std::endl; return err; } @@ -569,7 +572,7 @@ int main() SvrHandling *svr = new SvrHandling(); if (svr == NULL) { - cerr << "Failed: could not create server \n" << endl; + std::cerr << "Failed: could not create server \n" << std::endl; return -ENOMEM; } svr->run();