From 5c12d4de022c41450ab0124b0b77e0629d0f393f Mon Sep 17 00:00:00 2001 From: philipp schoenberger Date: Mon, 17 Aug 2015 23:11:12 +0200 Subject: [PATCH] trivial --- lwrserv/LinearTrajectory.cpp | 20 +- lwrserv/SvrData.cpp | 82 +++++--- lwrserv/SvrHandling.cpp | 98 +++++----- lwrserv/Trajectroy.cpp | 22 ++- lwrserv/friremote.cpp | 357 +++++++++++++++++------------------ lwrserv/include/SvrData.h | 14 +- lwrserv/include/Trajectroy.h | 2 +- lwrserv/program.cpp | 5 - 8 files changed, 315 insertions(+), 285 deletions(-) diff --git a/lwrserv/LinearTrajectory.cpp b/lwrserv/LinearTrajectory.cpp index e91bd79..632a909 100644 --- a/lwrserv/LinearTrajectory.cpp +++ b/lwrserv/LinearTrajectory.cpp @@ -4,17 +4,18 @@ template LinearJointTrajectory::LinearJointTrajectory( - unsigned int steps_, - float totalTime_, - Vec jointMovement_ + float sampleTimeMs, + Vec maxJointVelocity, + Vec maxJointAcceleration, + Vec jointStart, + Vec jointEnd ) { - Vec< float, SIZE> velocity = jointMovement_ / steps_ * 1.5f; - LinearJointTrajectory( - steps_, - totalTime_, - jointMovement_, - velocity); + // + maxJointLocalVelocity = maxJointVelocity * sampleTimeMs; + maxJointLocalAcceleration = maxJointAcceleration * sampleTimeMs; + // calculate sample count + } template @@ -25,6 +26,7 @@ LinearJointTrajectory::LinearJointTrajectory( Vec velocity ) { + Vec< float, SIZE> velocity = jointMovement_ / steps_ * 1.5f; super(steps_, totalTime_, jointMovement); if ( velocity > jointMovement/totalTime_) diff --git a/lwrserv/SvrData.cpp b/lwrserv/SvrData.cpp index c185d96..884a079 100644 --- a/lwrserv/SvrData.cpp +++ b/lwrserv/SvrData.cpp @@ -20,8 +20,8 @@ SvrData::SvrData(void) 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}; - robot.currentVelocity = 20; - robot.currentAcceleration = 10; + robot.currentVelocityPercentage = 20; + robot.currentAccelerationPercentage = 10; for (unsigned int i = 0; i < JOINT_NUMBER; ++i) { @@ -191,40 +191,38 @@ VecJoint SvrData::getMaxTorque() VecJoint SvrData::getMaxAcceleration() { - VecJoint buff; + VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); - buff = robot.max.accelaration; + retval = robot.max.accelaration; pthread_mutex_unlock(&dataLock); - return buff; + return retval; } VecJoint SvrData::getMaxVelocity() { - VecJoint buff; - + VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); - buff = robot.max.velocity; + retval = robot.max.velocity; pthread_mutex_unlock(&dataLock); - return buff; + return retval; } VecJoint SvrData::getMaxRange() { - VecJoint buff; - + VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); - buff = robot.max.range; + retval = robot.max.range; pthread_mutex_unlock(&dataLock); - return buff; + return retval; } bool SvrData::checkJointRange(VecJoint vectorToCheck) { - bool retval = false; - + // save temporal the max range for short time lock pthread_mutex_lock(&dataLock); Vec maxJointRange; maxJointRange = robot.max.range; pthread_mutex_unlock(&dataLock); + // check if the value is in plus or minus range for (int i = 0; i abs(maxJointRange(i))) @@ -234,42 +232,76 @@ bool SvrData::checkJointRange(VecJoint vectorToCheck) } } // no join angle is too big - return false; + return true; } -int SvrData::setRobotVelocity(float newVelocity) +int SvrData::setRobotVelocityPercentage(float newVelocityPercentage) { unsigned int retval = 0; + // percentage is to low + if (newVelocityPercentage <= 0.0f) + return -1; + // percentage is to high + if (newVelocityPercentage > 100.0f) + return -2; pthread_mutex_lock(&dataLock); - robot.currentVelocity = newVelocity; + robot.currentVelocityPercentage = newVelocityPercentage; pthread_mutex_unlock(&dataLock); return retval; } -float SvrData::getRobotVelocity() +VecJoint SvrData::getRobotVelocity() +{ + float percent = 0.0f; + VecJoint retval(0.0f); + + pthread_mutex_lock(&dataLock); + percent = robot.currentVelocityPercentage / 100.0f; + retval = robot.max.velocity * percent; + pthread_mutex_unlock(&dataLock); + return retval; +} +float SvrData::getRobotVelocityPercentage() { float retval = 0; pthread_mutex_lock(&dataLock); - retval = robot.currentVelocity; + retval = robot.currentVelocityPercentage; pthread_mutex_unlock(&dataLock); return retval; } -int SvrData::setRobotAcceleration(float newAcceleration) +int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage) { - unsigned int retval = 0; + int retval = 0; + // percentage is to low + if (newAccelerationPercentage <= 0.0f) + return -1; + // percentage is to high + if (newAccelerationPercentage > 100.0f) + return -2; + + pthread_mutex_lock(&dataLock); + robot.currentAccelerationPercentage = newAccelerationPercentage; + pthread_mutex_unlock(&dataLock); + return retval; +} +VecJoint SvrData::getRobotAcceleration() +{ + float percent = 0; + VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); - robot.currentAcceleration = newAcceleration; + percent = robot.currentAccelerationPercentage / 100.0f; + retval = robot.max.accelaration * percent; pthread_mutex_unlock(&dataLock); return retval; } -float SvrData::getRobotAcceleration() +float SvrData::getRobotAccelerationPercentage() { float retval = 0; pthread_mutex_lock(&dataLock); - retval = robot.currentAcceleration; + retval = robot.currentAccelerationPercentage; pthread_mutex_unlock(&dataLock); return retval; } diff --git a/lwrserv/SvrHandling.cpp b/lwrserv/SvrHandling.cpp index 44d77d1..5e762a8 100755 --- a/lwrserv/SvrHandling.cpp +++ b/lwrserv/SvrHandling.cpp @@ -10,6 +10,7 @@ #include "SvrHandling.h" #include "SvrData.h" #include "Trajectroy.h" +#include void printUsage(SocketObject& client) { @@ -46,7 +47,6 @@ void debugMat(Mat4f M) double* kukaBackwardCalc(double M_[12], float arg[3]) { - double* Joints = new double[7]; //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}; @@ -460,6 +460,7 @@ void SvrHandling::handle(SocketObject& client) } } + string SvrHandling::timestamp() { time_t rawtime; @@ -480,15 +481,18 @@ void SvrHandling::GetPositionJoints(SocketObject& client) string out = ""; stringstream ss (stringstream::in | stringstream::out); + // get current joint positions from database VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos(); - // disassemble command + // 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); } @@ -497,17 +501,21 @@ void SvrHandling::GetPositionHomRowWise(SocketObject& client) string out = ""; stringstream ss (stringstream::in | stringstream::out); + // get current Cartesianpositions from database MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos(); + // assemble command feadback row wise for (int i=0; i< FRI_CART_FRM_DIM; i++) { - int row = i / 4; - int column = i % 4; + int row = i % 4; + int column = i / 4; ss.str(""); ss << (cartPos(row,column)); out += ss.str() + " "; } + + // send msg to client client.Send(out); } @@ -570,33 +578,20 @@ void SvrHandling::MovePTPJoints(SocketObject& client, string& args) VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos(); class Trajectory* newTrajectory; - // calculate number of movement steps - dMaxSpeed = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration); - lMaxSpeed = dMaxSpeed.cellmultiply(dMaxSpeed).cellmultiply(maxJointLocalAcceleration) * 0.5f; - - for (int j=0; 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()); + float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs(); + VecJoint maxJointVelocity = SvrData::getInstance()->getMaxVelocity(); + VecJoint maxJointAcceleration = SvrData::getInstance()->getMaxAcceleration(); - newTrajectory = new LinearJointTrajectory(steps, totalTime, newJointPos); + newTrajectory = new LinearJointTrajectory(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); __MSRCMDJNTPOS = true; //Wait to end of movement - while (1) + while (true) { - if (!__MSRCMDJNTPOS) - { + if (SvrData::getInstance()->getMovementDone() == true) break; - } + boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) ); } client.Send(SVR_TRUE_RSP); } @@ -789,7 +784,8 @@ void SvrHandling::StopPotFieldMode(SocketObject& client, string& args) client.Send(SVR_TRUE_RSP); } -void SvrHandling::SetPos(SocketObject& client, string& args){ +void SvrHandling::SetPos(SocketObject& client, string& args) +{ string buf; double temp[15]; @@ -868,57 +864,55 @@ void SvrHandling::SetPos(SocketObject& client, string& args){ void SvrHandling::SetJoints(SocketObject& client, string& args) { string buf; - double temp[15]; + VecJoint newJointPos(0.0f); stringstream ss(args); stringstream ss2f; - vector tokens; int i = 0; + // convert to Joint vector while (ss >> buf) { - if (i>LBR_MNJ-1) + // to many input arguments or Joints + if (i>=JOINT_NUMBER) { - client.Send(SVR_FALSE_RSP); + client.Send(SVR_INVALID_ARGUMENT_COUNT); return; } - tokens.push_back(buf); + + // convert string to float ss2f.flush(); ss2f.clear(); ss2f << buf; - ss2f >> temp[i]; + ss2f >> newJointPos(i); i++; } - if (igetCommandedJointPos(); + + // set new trajectory + + // Set new target Position and also calculate cart pos + SvrData::getInstance()->setCommandedJointPos(newJointPos); + // TODO calculate and calculate position in Cartesian coordinates - ////Wait to end of movement - //while (1){ - // if (!__MSRCMDJNTPOS){ - // break;} - //} + // wait till position is reached + + // return positiv response to client client.Send(SVR_TRUE_RSP); } diff --git a/lwrserv/Trajectroy.cpp b/lwrserv/Trajectroy.cpp index e837ad5..2b97233 100644 --- a/lwrserv/Trajectroy.cpp +++ b/lwrserv/Trajectroy.cpp @@ -4,22 +4,30 @@ template Trajectory::Trajectory( - unsigned int steps_, - float totalTime_, - Vec jointMovement + float sampleTimeMs, + Vec maxJointVelocity, + Vec maxJointAcceleration, + Vec jointStart, + Vec jointEnd ) { movementType = MovementJointBased; - steps = steps_; + steps = 1; currentStep = 0; - totalTime = totalTime_; + totalTime = steps * sampleTimeMs; - nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps_); + nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps); + + nodes[0].jointPos = jointEnd; + nodes[0].cartPos = kukaBackwardCalc(); + nodes[0].velocity = (jointEnd-jointStart)/sampleTimeMs; + nodes[0].acceleration = Vec(9999.0f); // unused - (void) jointMovement; + (void)jointStart; + (void)jointEnd; } template diff --git a/lwrserv/friremote.cpp b/lwrserv/friremote.cpp index 8dd217b..6032202 100644 --- a/lwrserv/friremote.cpp +++ b/lwrserv/friremote.cpp @@ -24,13 +24,13 @@ friRemote.cpp NOTE: This sample, as the corresponding FRI (Fast Research inteface) is subject to radical change - + [FH]}} */ /** - \addtogroup friRemoteLib - \brief Library for FRI (FastResearchInterface) + \addtogroup friRemoteLib + \brief Library for FRI (FastResearchInterface) */ /* @{ */ @@ -47,32 +47,32 @@ friRemote::friRemote(int port,char *hintToRemoteHost):remote(port,hintToRemoteHost) { - //std::cout << __FUNCTION__ << " " < the respective cmd.cmd.cmdFlags field is set properly + value is ignored -> the respective cmd.cmd.cmdFlags field is set properly Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !! */ int friRemote::doJntImpedanceControl(const float newJntPosition[LBR_MNJ], - const float newJntStiff[LBR_MNJ], - const float newJntDamp[LBR_MNJ], - const float newJntAddTorque[LBR_MNJ], - bool flagDataExchange) + const float newJntStiff[LBR_MNJ], + const float newJntDamp[LBR_MNJ], + const float newJntAddTorque[LBR_MNJ], + bool flagDataExchange) { - // Helper, if not properly initialized or the like... - cmd.cmd.cmdFlags=0; - if (newJntPosition) - { - cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS; - // Note:: If the interface is not in Command mode, - // The commands have to be "mirrored" to get in sync - if ((getState() != FRI_STATE_CMD) || (!isPowerOn())) - { - for (int i = 0; i < LBR_MNJ; i++) - { - cmd.cmd.jntPos[i]=msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i]; - } - } - else - { - // compute new values here ... - for (int i = 0; i < LBR_MNJ; i++) - cmd.cmd.jntPos[i]=newJntPosition[i]; - } - } - if (newJntStiff) - { - cmd.cmd.cmdFlags|=FRI_CMD_JNTSTIFF; - for (int i = 0; i < LBR_MNJ; i++) - cmd.cmd.jntStiffness[i]=newJntStiff[i]; - } - if (newJntDamp) - { - cmd.cmd.cmdFlags|=FRI_CMD_JNTDAMP; - for (int i = 0; i < LBR_MNJ; i++) - cmd.cmd.jntDamping[i]=newJntDamp[i]; - } - if (newJntAddTorque) - { - cmd.cmd.cmdFlags|=FRI_CMD_JNTTRQ; - for (int i = 0; i < LBR_MNJ; i++) - cmd.cmd.addJntTrq[i]=newJntAddTorque[i]; - } - - if (flagDataExchange) - { - return doDataExchange(); - } - return 1; + // Helper, if not properly initialized or the like... + cmd.cmd.cmdFlags=0; + if (newJntPosition) + { + cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS; + // Note:: If the interface is not in Command mode, + // The commands have to be "mirrored" to get in sync + if ((getState() != FRI_STATE_CMD) || (!isPowerOn())) + { + for (int i = 0; i < LBR_MNJ; i++) + { + cmd.cmd.jntPos[i]=msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i]; + } + } + else + { + // compute new values here ... + for (int i = 0; i < LBR_MNJ; i++) + cmd.cmd.jntPos[i]=newJntPosition[i]; + } + } + if (newJntStiff) + { + cmd.cmd.cmdFlags|=FRI_CMD_JNTSTIFF; + for (int i = 0; i < LBR_MNJ; i++) + cmd.cmd.jntStiffness[i]=newJntStiff[i]; + } + if (newJntDamp) + { + cmd.cmd.cmdFlags|=FRI_CMD_JNTDAMP; + for (int i = 0; i < LBR_MNJ; i++) + cmd.cmd.jntDamping[i]=newJntDamp[i]; + } + if (newJntAddTorque) + { + cmd.cmd.cmdFlags|=FRI_CMD_JNTTRQ; + for (int i = 0; i < LBR_MNJ; i++) + cmd.cmd.addJntTrq[i]=newJntAddTorque[i]; + } + + if (flagDataExchange) + { + return doDataExchange(); + } + return 1; } - /** automatically do data exchange, if not otherwise specified - if flagDataExchange is set to false, call doDataExchange() - or doReceiveData()/doSendData() on your own - IN: newJntPosition - joint positions - newJntStiff - joint stiffness (Spring factor) - newJntDamp - joint damping (Damping factor) - newJntAddTorque - additional torque - - Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the - value is ignored -> the respective cmd.cmd.cmdFlags field is set properly - Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !! - */ + /** automatically do data exchange, if not otherwise specified + if flagDataExchange is set to false, call doDataExchange() + or doReceiveData()/doSendData() on your own + IN: newJntPosition - joint positions + newJntStiff - joint stiffness (Spring factor) + newJntDamp - joint damping (Damping factor) + newJntAddTorque - additional torque + + Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the + value is ignored -> the respective cmd.cmd.cmdFlags field is set properly + Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !! + */ int friRemote::doCartesianImpedanceControl(const float newCartPosition[FRI_CART_FRM_DIM], - const float newCartStiff[FRI_CART_VEC], - const float newCartDamp[FRI_CART_VEC], - const float newAddTcpFT[FRI_CART_VEC], - const float newJntNullspace[LBR_MNJ], - bool flagDataExchange) + const float newCartStiff[FRI_CART_VEC], + const float newCartDamp[FRI_CART_VEC], + const float newAddTcpFT[FRI_CART_VEC], + const float newJntNullspace[LBR_MNJ], + bool flagDataExchange) { - // Helper, if not properly initialized or the like... - cmd.cmd.cmdFlags=0; - if ( newCartPosition ) - { - cmd.cmd.cmdFlags|=FRI_CMD_CARTPOS; - for ( int i = 0; i < FRI_CART_FRM_DIM; i++) - { - cmd.cmd.cartPos[i]=newCartPosition[i]; - - } - } - if ( newCartStiff) - { - cmd.cmd.cmdFlags|=FRI_CMD_CARTSTIFF; - for ( int i = 0; i < FRI_CART_VEC; i++) - { - cmd.cmd.cartStiffness[i]=newCartStiff[i]; - - } - - } - if ( newCartDamp) - { - cmd.cmd.cmdFlags|=FRI_CMD_CARTDAMP; - ; - for ( int i = 0; i < FRI_CART_VEC; i++) - { - cmd.cmd.cartDamping[i]=newCartDamp[i]; - - } - } - if ( newAddTcpFT) - { - cmd.cmd.cmdFlags|=FRI_CMD_TCPFT; - ; - for ( int i = 0; i < FRI_CART_VEC; i++) - { - cmd.cmd.addTcpFT[i]=newAddTcpFT[i]; - - } - } - - if (newJntNullspace) - { - cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS; - // Note:: If the interface is not in Command mode, - // The commands have to be "mirrored" to get in sync - - // compute new values here ... - for (int i = 0; i < LBR_MNJ; i++) - cmd.cmd.jntPos[i]=newJntNullspace[i]; - } - - -if (flagDataExchange) - { - return doDataExchange(); - } - return 1; + // Helper, if not properly initialized or the like... + cmd.cmd.cmdFlags=0; + if ( newCartPosition ) + { + cmd.cmd.cmdFlags|=FRI_CMD_CARTPOS; + for ( int i = 0; i < FRI_CART_FRM_DIM; i++) + { + cmd.cmd.cartPos[i]=newCartPosition[i]; + + } + } + if ( newCartStiff) + { + cmd.cmd.cmdFlags|=FRI_CMD_CARTSTIFF; + for ( int i = 0; i < FRI_CART_VEC; i++) + { + cmd.cmd.cartStiffness[i]=newCartStiff[i]; + + } + + } + if ( newCartDamp) + { + cmd.cmd.cmdFlags|=FRI_CMD_CARTDAMP; + for ( int i = 0; i < FRI_CART_VEC; i++) + { + cmd.cmd.cartDamping[i]=newCartDamp[i]; + + } + } + if ( newAddTcpFT) + { + cmd.cmd.cmdFlags|=FRI_CMD_TCPFT; + for ( int i = 0; i < FRI_CART_VEC; i++) + { + cmd.cmd.addTcpFT[i]=newAddTcpFT[i]; + + } + } + + if (newJntNullspace) + { + cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS; + // Note:: If the interface is not in Command mode, + // The commands have to be "mirrored" to get in sync + + // compute new values here ... + for (int i = 0; i < LBR_MNJ; i++) + cmd.cmd.jntPos[i]=newJntNullspace[i]; + } + + if (flagDataExchange) + { + return doDataExchange(); + } + return 1; } diff --git a/lwrserv/include/SvrData.h b/lwrserv/include/SvrData.h index 0890914..c007688 100644 --- a/lwrserv/include/SvrData.h +++ b/lwrserv/include/SvrData.h @@ -38,8 +38,8 @@ private: VecJoint torque; VecJoint range; } max; - float currentVelocity; - float currentAcceleration; + float currentVelocityPercentage; + float currentAccelerationPercentage; } robot; struct { @@ -97,11 +97,13 @@ public: bool checkJointRange(VecJoint vectorToCheck); - float getRobotVelocity(); - int setRobotVelocity(float newVelocity); + VecJoint getRobotVelocity(); + float getRobotVelocityPercentage(); + int setRobotVelocityPercentage(float newVelocityPercentage); - float getRobotAcceleration(); - int setRobotAcceleration(float newVelocity); + VecJoint getRobotAcceleration(); + float getRobotAccelerationPercentage(); + int setRobotAccelerationPercentage(float newAccelerationPercentage); unsigned int getJoints(); diff --git a/lwrserv/include/Trajectroy.h b/lwrserv/include/Trajectroy.h index 0a0bbfe..be01072 100644 --- a/lwrserv/include/Trajectroy.h +++ b/lwrserv/include/Trajectroy.h @@ -57,7 +57,7 @@ class Trajectory struct trajectoryNode { Vec jointPos; - Mat* cartPos; + Mat cartPos; Vec velocity; Vec acceleration; }; diff --git a/lwrserv/program.cpp b/lwrserv/program.cpp index 08defe6..0db54ff 100644 --- a/lwrserv/program.cpp +++ b/lwrserv/program.cpp @@ -9,11 +9,6 @@ #include "SvrData.h" #include -void printMat(Mat4f M) -{ - cout << M << endl; -} - float* abctomat(float a, float b, float c) { Mat4f rx;