From b0953b7ce27767209c490e768d2ff7388801a82c Mon Sep 17 00:00:00 2001 From: philipp schoenberger Date: Wed, 19 Aug 2015 16:34:29 +0200 Subject: [PATCH] fix startup behaviour and also rad/deg failure --- lwrserv/SvrData.cpp | 35 ++++- lwrserv/SvrHandling.cpp | 2 + ...gBangTrajectroy.h => BangBangTrajectory.h} | 32 +++-- lwrserv/include/FivePolyTrajectory.h | 6 +- lwrserv/include/LSPBTrajectory.h | 127 ++++++++++++++++++ lwrserv/include/LSPBTrajectroy.h | 12 -- lwrserv/include/SvrData.h | 2 + lwrserv/include/Trajectroy.h | 77 ++++++----- lwrserv/program.cpp | 44 +++--- 9 files changed, 254 insertions(+), 83 deletions(-) rename lwrserv/include/{BangBangTrajectroy.h => BangBangTrajectory.h} (64%) create mode 100644 lwrserv/include/LSPBTrajectory.h delete mode 100644 lwrserv/include/LSPBTrajectroy.h diff --git a/lwrserv/SvrData.cpp b/lwrserv/SvrData.cpp index dc4e10b..0806267 100644 --- a/lwrserv/SvrData.cpp +++ b/lwrserv/SvrData.cpp @@ -38,7 +38,7 @@ SvrData::SvrData(void) if (pthread_mutex_init(&dataLock, NULL) != 0) printf("mutex init failed\n"); - this->friInst = new friRemote; + this->friInst = new friRemote(); } SvrData::~SvrData() @@ -87,15 +87,19 @@ void SvrData::updateMessurement() /// do the update using fri /// update current joint positions + friInst->getState(); + FRI_STATE state = friInst->getState(); VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition()); VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset()); newJointPos = newJointPosComanded + newJointPosOffset; + newJointPos = newJointPos * ( 180.0f / M_PI); /// update current cart position newCartPos = friInst->getMsrCartPosition(); /// update current force and torque newForceAndTorque = friInst->getFTTcp(); + std::cout << newJointPos << " current newJointPos \n" << state << "\n"; updateMessurementData(newJointPos,newCartPos,newForceAndTorque); } @@ -440,3 +444,32 @@ void SvrData::setTrajectoryType(enum TrajectoryType newType) pthread_mutex_unlock(&dataLock); } +class Trajectory* SvrData::createTrajectory(VecJoint newJointPos) +{ + //set new target for next trajectory + VecJoint prevJointPos = getCommandedJointPos(); + class Trajectory* newTrajectory; + + float sampleTimeMs = getSampleTimeMs(); + VecJoint maxJointVelocity = getRobotVelocity(); + VecJoint maxJointAcceleration = getRobotAcceleration(); + enum TrajectoryType type = getTrajectoryType(); + + //newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos); + newTrajectory = Trajectory::GetTrajectory(type,sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); + + return newTrajectory; +} +class Trajectory* SvrData::createTrajectory(MatCarthesian newJointPos) +{ + +} +VecJoint SvrData::getMeasuredJointPos() +{ + VecJoint retval(0.0f); + pthread_mutex_lock(&dataLock); + retval = messured.jointPos; + pthread_mutex_unlock(&dataLock); + return retval; +} +MatCarthesian getMeasuredCartPos(); diff --git a/lwrserv/SvrHandling.cpp b/lwrserv/SvrHandling.cpp index 9e3eb6b..f4a2b42 100755 --- a/lwrserv/SvrHandling.cpp +++ b/lwrserv/SvrHandling.cpp @@ -383,6 +383,7 @@ SvrHandling::SvrHandling() commands[i].longVersion = "SetTrajectoryType"; commands[i].processCommand = &setTrajectoryType; commands[i].printHelp = NULL; + i+=1; commandCount = i; } @@ -628,6 +629,7 @@ void movePTPJoints(SocketObject& client, string& arg) 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 diff --git a/lwrserv/include/BangBangTrajectroy.h b/lwrserv/include/BangBangTrajectory.h similarity index 64% rename from lwrserv/include/BangBangTrajectroy.h rename to lwrserv/include/BangBangTrajectory.h index 11083f7..f6df681 100644 --- a/lwrserv/include/BangBangTrajectroy.h +++ b/lwrserv/include/BangBangTrajectory.h @@ -4,10 +4,10 @@ #include "vec.h" template -class BangBangJointTrajectroy : public Trajectory +class BangBangJointTrajectory : public Trajectory { public: - BangBangJointTrajectroy( + BangBangJointTrajectory( float sampleTimeMs, Vec maxJointVelocity, Vec maxJointAcceleration, @@ -24,30 +24,44 @@ class BangBangJointTrajectroy : public Trajectory Vec jointMovement = jointEnd - jointStart; Vec jointMovementAbs = jointMovement.abs(); + // calculate sample count + // calculate number of movement steps - Vec minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity); + // one joint has to reach maxvelocity the others are stepped down to + Vec minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f; this->steps = ceil(minStepsPerJoint.max()); if (this->steps == 0) this->steps +=1; - std::cout << "steps : " << this->steps << minStepsPerJoint<< "\n"; - this->nodes = (struct Trajectory::trajectoryNode* ) calloc(sizeof(struct Trajectory::trajectoryNode),this->steps); Vec jointLast = jointStart; + float percentStep = 1.0f / ((this->steps+1)/2); for( int i = 0 ; i < this->steps; ++i) { - jointLast = jointLast + jointMovement.celldivide((float) this->steps ); + // acceleration phase + if (i > this->steps /2 ) + { + float percent = i * percentStep; + this->nodes[i].velocity = maxJointLocalVelocity * percent; + this->nodes[i].acceleration = maxJointLocalVelocity * percentStep; + } + // deacceleration phase + else + { + float percent = ( i - this->steps/2 ) * percentStep; + this->nodes[i].velocity = maxJointLocalVelocity * percent; + this->nodes[i].acceleration = maxJointLocalVelocity * (-1.0 * percentStep); + } + + jointLast = jointLast + this->nodes[i].velocity; this->nodes[i].jointPos = jointLast; - this->nodes[i].velocity = maxJointLocalVelocity; - this->nodes[i].acceleration = Vec(0.0f); } // last step myth be to wide so cut it to the wanted position this->nodes[this->steps-1].jointPos = jointEnd; this->nodes[this->steps-1].velocity = maxJointLocalVelocity; // TODO this is not the truth this->nodes[this->steps-1].acceleration = Vec(0.0f); } - ~BangBangJointTrajectroy(); private: protected: diff --git a/lwrserv/include/FivePolyTrajectory.h b/lwrserv/include/FivePolyTrajectory.h index ed79613..c87244e 100644 --- a/lwrserv/include/FivePolyTrajectory.h +++ b/lwrserv/include/FivePolyTrajectory.h @@ -2,10 +2,12 @@ #define _FIVEPOLYTRAJECTORY_H_ #include "Trajectroy.h" -class FivePolyJointTrajectory: public Trajectory +template +class FivePolyJointTrajectory: public Trajectory { }; -class FivePolyCartTrajectory: public Trajectory +template +class FivePolyCartTrajectory: public Trajectory { }; diff --git a/lwrserv/include/LSPBTrajectory.h b/lwrserv/include/LSPBTrajectory.h new file mode 100644 index 0000000..cb8432e --- /dev/null +++ b/lwrserv/include/LSPBTrajectory.h @@ -0,0 +1,127 @@ +#ifndef _LSPBTRAJCETORY_H_ +#define _LSPBTRAJCETORY_H_ +#include +#include "Trajectroy.h" +#include "sgn.h" + +template +class LSPBJointTrajectory: public Trajectory +{ +public: + LSPBJointTrajectory( + float sampleTimeMs, + Vec maxJointVelocity, + Vec maxJointAcceleration, + Vec jointStart, + Vec jointEnd + ) + { + float MStoSec = 1000.0f; + // calculate maximum velocity and acceleration + Vec maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec); + Vec maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec); + std::cout << "max : " << maxJointLocalVelocity << "\n"; + + // calculate delta movement + Vec jointMovement = jointEnd - jointStart; + Vec jointMovementAbs = jointMovement.abs(); + + // calculate number of movement steps + + // 2 same acceleration and deaceleration phases + + Vec minStepsPerJoint(0.0f); + // check + for (int j=0; j jointMovementAbs(j)/2.0f) + { + // s = (a * t^2 )/2 + // => t = sqrt( s * 2 / a) + minStepsPerJoint(j) = sqrt(jointMovementAbs(j) / maxJointLocalAcceleration (j))*2.0f; + } else + { + // 2 speedup and slow down phases + minStepsPerJoint(j) = maxJointLocalVelocity(j)/maxJointLocalAcceleration(j) * 2.0f ; + + // one phase with constant velocity + float remainingjointMovementAbs = jointMovementAbs(j); + // v * t = s + remainingjointMovementAbs -= maxJointLocalVelocity(j) * minStepsPerJoint(j) * sampleTimeMs; + + // s / v = t + minStepsPerJoint(j) += remainingjointMovementAbs/maxJointLocalVelocity(j); + } + } + + std::cout << "minsteps : " << minStepsPerJoint << "\n"; + std::cout << "steps : " << minStepsPerJoint.max() << "\n"; + this->steps = ceil(minStepsPerJoint.max()); + std::cout << "steps : " << this->steps << "\n"; + if (this->steps == 0) + this->steps +=1; + + this->nodes = (struct Trajectory::trajectoryNode* ) calloc(sizeof(struct Trajectory::trajectoryNode),this->steps); + + Vec jointLast = jointStart; + + // calculate thime of max speed reaching + Vec deltaToMaxSpeed(0.0f); + Vec currJointLocalAcceleration(0.0f); + for (int j=0; jsteps /2.0f - sqrt((this->steps/2.0f)*(this->steps/2.0f) - jointMovementAbs(j)/maxJointAcceleration(j))); + if (deltaToMaxSpeed(j) <= 0.0f) + { + currJointLocalAcceleration(j) = 0.0f; + } else + { + currJointLocalAcceleration(j) = -jointMovementAbs(j)/(deltaToMaxSpeed(j)*deltaToMaxSpeed(j)-this->steps*deltaToMaxSpeed(j)); + } + maxJointLocalVelocity(j) = deltaToMaxSpeed(j)*currJointLocalAcceleration(j); + } + + Vec currentInk(0.0f); + Vec currentInkLast(0.0f); + Vec currentDist(0.0f); + Vec currentDistLast(0.0f); + + for (int currStep=0; currStepsteps; currStep++) + { + for (int currJoint=0; currJointsteps/2.0f) + { + currentInk(currJoint) = std::min(currentInkLast(currJoint)+maxJointLocalAcceleration(currJoint),maxJointLocalVelocity(currJoint)); + }else if (currStep+1 > this->steps-deltaToMaxSpeed(currJoint)) + { + currentInk(currJoint) = std::max(currentInkLast(currJoint)-maxJointLocalAcceleration(currJoint),0.0f); + }else + { + currentInk(currJoint) = currentInkLast(currJoint); + } + currentDist(currJoint) = currentDistLast(currJoint) + sgn(jointMovement(currJoint))*currentInk(currJoint); + this->nodes[currStep].jointPos(currJoint) += sgn(jointMovement(currJoint))*currentInk(currJoint); + this->nodes[currStep].velocity(currJoint) = currentInk(currJoint); + this->nodes[currStep].acceleration(currJoint) = currentInkLast(currJoint) - currentInk(currJoint); + + currentDistLast(currJoint) = currentDist(currJoint); + currentInkLast(currJoint) = currentInk(currJoint); + } + } + + // last step myth be to wide so cut it to the wanted position + this->nodes[this->steps-1].jointPos = jointEnd; + this->nodes[this->steps-1].velocity = maxJointLocalVelocity; // TODO this is not the truth + this->nodes[this->steps-1].acceleration = Vec(0.0f); + } +}; + +template +class LSPBCartTrajectory: public Trajectory +{ +}; + +#endif diff --git a/lwrserv/include/LSPBTrajectroy.h b/lwrserv/include/LSPBTrajectroy.h deleted file mode 100644 index 0da6cf0..0000000 --- a/lwrserv/include/LSPBTrajectroy.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef _LSPBTRAJCETORY_H_ -#define _LSPBTRAJCETORY_H_ -#include "Trajectroy.h" - -class LSPBJointTrajectroy: public Trajectory -{ -}; -class LSPBCartTrajectroy: public Trajectory -{ -}; - -#endif diff --git a/lwrserv/include/SvrData.h b/lwrserv/include/SvrData.h index 5675eb8..9057902 100644 --- a/lwrserv/include/SvrData.h +++ b/lwrserv/include/SvrData.h @@ -124,6 +124,8 @@ public: enum TrajectoryType getTrajectoryType(); void setTrajectoryType(enum TrajectoryType newType); + class Trajectory* createTrajectory(VecJoint newJointPos); + class Trajectory* createTrajectory(MatCarthesian newJointPos); template class Trajectory* calculateTrajectory( diff --git a/lwrserv/include/Trajectroy.h b/lwrserv/include/Trajectroy.h index dc88ca5..f3490d1 100644 --- a/lwrserv/include/Trajectroy.h +++ b/lwrserv/include/Trajectroy.h @@ -4,12 +4,14 @@ #include #include - template class Trajectory; // all types of trajectories /// START OF TRAJECTORY LIST #include "LinearTrajectory.h" +#include "LSPBTrajectory.h" +#include "BangBangTrajectory.h" +#include "FivePolyTrajectory.h" enum TrajectoryType { TrajectoryDefault = 0, @@ -67,44 +69,6 @@ static enum TrajectoryType toEnum(std::string name) } return TrajectoryDefault; } -template -class Trajectory* calculateTrajectory(enum TrajectoryType type, - float sampleTimeMs, - Vec maxJointVelocity, - Vec maxJointAcceleration, - Vec jointStart, - Vec jointEnd) - -{ - class Trajectory* retval = NULL; - switch(type) - { - case TrajectoryJointLinear: - retval = (class Trajectory*) new LinearJointTrajectory( - sampleTimeMs, - maxJointVelocity, - maxJointAcceleration, - jointStart, - jointEnd); - break; - case TrajectoryCartLinear: - case TrajectoryJointBangBang: - retval = (class Trajectory*) new LinearJointTrajectory( - sampleTimeMs, - maxJointVelocity, - maxJointAcceleration, - jointStart, - jointEnd); - break; - case TrajectoryCartBangBang: - case TrajectoryJointFivePoly: - case TrajectoryCartFivePoly: - case TrajectoryJointLSPB: - case TrajectoryCartLSPB: - break; - } - return retval; -} enum MovementType { MovementJointBased= 0, @@ -151,6 +115,41 @@ class Trajectory (void)maxJointVelocity; (void)maxJointAcceleration; } + static class Trajectory* GetTrajectory( + enum TrajectoryType type, + float sampleTimeMs, + Vec maxJointVelocity, + Vec maxJointAcceleration, + Vec jointStart, + Vec jointEnd + ) + { + class Trajectory* retval = NULL; + + switch ( type ) + { + case TrajectoryJointLinear: + retval = (class Trajectory*) new LinearJointTrajectory(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); + break; + case TrajectoryJointBangBang: + retval = (class Trajectory*) new BangBangJointTrajectory(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); + break; + case TrajectoryJointLSPB: + retval = (class Trajectory*) new LSPBJointTrajectory(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); + break; + case TrajectoryJointFivePoly: + break; + + case TrajectoryCartLSPB: + case TrajectoryCartLinear: + case TrajectoryCartBangBang: + case TrajectoryCartFivePoly: + // TODO implement carthesian movement + retval = (class Trajectory*) new LinearJointTrajectory(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); + break; + } + return retval; + } virtual ~Trajectory() { diff --git a/lwrserv/program.cpp b/lwrserv/program.cpp index 6261746..dc8f5d6 100644 --- a/lwrserv/program.cpp +++ b/lwrserv/program.cpp @@ -183,11 +183,27 @@ void *threadRobotMovement(void *arg) VecJoint currentJointPos(0.0f); MatCarthesian currentCartPos(0.0f,1.0f); - while(1) + friRemote* friInst = SvrData::getInstance()->getFriRemote(); + + // get current robot position + int tries = 2; + for (int i= 0 ; i < tries ; ++i) + { + SvrData::getInstance()->updateMessurement(); + currentJointPos = SvrData::getInstance()->getMeasuredJointPos(); + + SvrData::getInstance()->setCommandedJointPos(currentJointPos); + + std::cout << "init position is " << currentJointPos << "\n"; + float newJointPosToSend[JOINT_NUMBER] = {0.0f}; + currentJointPos.getData(newJointPosToSend); + //std::cout << currentJointPos << "\n"; + friInst->doPositionControl(newJointPosToSend); + } + + + while(true) { - // get current sample time cause it could be changed - float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs(); - friRemote* friInst = SvrData::getInstance()->getFriRemote(); // check if we have to cancel current trajectory bool cancel = SvrData::getInstance()->getTrajectoryCancel(); @@ -253,28 +269,16 @@ void *threadRobotMovement(void *arg) } end: - // TODO stop timer for last sent message - - // time we used to come to this point caused by the calculation and lock time - // TODO calculate this time - float calculationTimeMs = 0.0f; - - // wait only the remaining time - float waitingTimeMs = sampleTimeMs - calculationTimeMs; - - // sleep the calculated waiting time - boost::this_thread::sleep( boost::posix_time::milliseconds(waitingTimeMs+100.0f) ); - - switch (currentMovementType) { case MovementJointBased: { // send the new joint values float newJointPosToSend[JOINT_NUMBER] = {0.0f}; - currentJointPos.getData(newJointPosToSend); - std::cout << currentJointPos << "\n"; - //friInst->doPositionControl(newJointPosToSend); + VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI); + newJointPosToSendRad.getData(newJointPosToSend); + std::cout << newJointPosToSendRad << "\n"; + friInst->doPositionControl(newJointPosToSend); } break; case MovementCartBased: