From f625d6d1d1d26918019e3e9edb8b7e6ff76addf3 Mon Sep 17 00:00:00 2001 From: philipp schoenberger Date: Tue, 18 Aug 2015 16:28:41 +0200 Subject: [PATCH] first working move point to point --- lwrserv/BangBangTrajectroy.cpp | 22 ------ lwrserv/LinearTrajectory.cpp | 59 --------------- lwrserv/SvrData.cpp | 25 ++++++- lwrserv/SvrHandling.cpp | 40 ++++++++-- lwrserv/SvrHandling.h | 12 ++- lwrserv/include/LinearTrajectory.h | 26 ++++--- lwrserv/include/SvrData.h | 1 - lwrserv/include/Trajectroy.h | 113 +++++++++++++++++++++++++++-- lwrserv/include/vec.h | 1 - lwrserv/program.cpp | 57 ++++++++++----- 10 files changed, 224 insertions(+), 132 deletions(-) delete mode 100644 lwrserv/BangBangTrajectroy.cpp delete mode 100644 lwrserv/LinearTrajectory.cpp diff --git a/lwrserv/BangBangTrajectroy.cpp b/lwrserv/BangBangTrajectroy.cpp deleted file mode 100644 index 328b616..0000000 --- a/lwrserv/BangBangTrajectroy.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include -#include -#include "Trajectroy.h" -#include "BangBangTrajectroy.h" -#include "vec.h" -#include "SvrData.h" - -template -BangBangJointTrajectroy::BangBangJointTrajectroy( - unsigned int steps_, - float totalTime_, - Vec jointMovement - ) -{ - Vec< float, SIZE> velocity = jointMovement / steps_ * 1.5f; - //TODO - LinearJointTrajectory( - steps_, - totalTime_, - jointMovement, - velocity); -} diff --git a/lwrserv/LinearTrajectory.cpp b/lwrserv/LinearTrajectory.cpp deleted file mode 100644 index b8c595a..0000000 --- a/lwrserv/LinearTrajectory.cpp +++ /dev/null @@ -1,59 +0,0 @@ - -#if 0 -template -LinearJointTrajectory::LinearJointTrajectory( - unsigned int steps_, - float totalTime_, - Vec jointMovement, - Vec velocity - ) -{ - Vec< float, SIZE> velocity = jointMovement_ / steps_ * 1.5f; - super(steps_, totalTime_, jointMovement); - - if ( velocity > jointMovement/totalTime_) - { - std::cerr << "velocity is to small for Trajectory\n"; - } - - if ( velocity < (2.0f*jointMovement)/totalTime_) - { - std::cerr << "velocity is to big for Trajectory\n"; - } - - for (unsigned int currJoint = 0 ; currJoint < SIZE; currJoint++) - { - unsigned int timeBlend = (jointMovement(currJoint) + velocity(currJoint)*totalTime_)/ velocity(currJoint); - float acceleration = velocity(currJoint) / timeBlend; - - for (unsigned int currStep = 0 ; currStep < steps_ ; ++currStep) - { - float currentTime = currStep * totalTime_ / steps_; - if (currentTime <= timeBlend) - { - // speed up till blend time is reached - this->nodes[currStep].jointPos = acceleration/2.0f * currentTime * currentTime; - this->nodes[currStep].velocity = acceleration * currentTime; - this->nodes[currStep].acceleration = acceleration; - } else - if ( currentTime <= totalTime_ - timeBlend) - { - // constant velocity - this->nodes[currStep].jointPos = (jointMovement(currJoint) - velocity(currJoint) * totalTime_)/2.0f + velocity(currJoint) * currentTime; - this->nodes[currStep].velocity = velocity(currJoint); - this->nodes[currStep].acceleration = 0.0f; - } - else - { - // slow down until aim is reached - // speed up till blend time is reached - this->nodes[currStep].jointPos = jointMovement(currJoint) - acceleration/2*totalTime_*totalTime_ + acceleration * totalTime_ * currentTime - acceleration/2.0f * currentTime *currentTime; - this->nodes[currStep].velocity = acceleration*timeBlend - acceleration * currentTime ; - this->nodes[currStep].acceleration = -acceleration; - } - // calculate Cartesian positions - this->nodes[currStep].cartPos = 0; - } - } -} -#endif diff --git a/lwrserv/SvrData.cpp b/lwrserv/SvrData.cpp index 2ba4e1c..5a7e522 100644 --- a/lwrserv/SvrData.cpp +++ b/lwrserv/SvrData.cpp @@ -343,7 +343,16 @@ class Trajectory* SvrData::popTrajectory() { class Trajectory* retval = NULL; pthread_mutex_lock(&dataLock); - retval = trajectory.list.front(); + if (!trajectory.list.empty() ) + { + retval = trajectory.list.front(); + trajectory.list.pop(); + } + else + { + // no trajectroy left + trajectory.done = true; + } pthread_mutex_unlock(&dataLock); return retval; } @@ -354,6 +363,7 @@ int SvrData::pushTrajectory(class Trajectory* newTrajectory) pthread_mutex_lock(&dataLock); trajectory.list.push(newTrajectory); + trajectory.done = false; pthread_mutex_unlock(&dataLock); return 0; } @@ -412,10 +422,19 @@ bool SvrData::getTrajectoryDone() pthread_mutex_unlock(&dataLock); return retval; } -void SvrData::setTrajectoryDone(bool newState) + +enum TrajectoryType SvrData::getTrajectoryType() +{ + enum TrajectoryType retval = TrajectoryDefault; + pthread_mutex_lock(&dataLock); + retval = trajectory.type; + pthread_mutex_unlock(&dataLock); + return retval; +} +void SvrData::setTrajectoryType(enum TrajectoryType newType) { pthread_mutex_lock(&dataLock); - trajectory.done = newState; + trajectory.type = newType; pthread_mutex_unlock(&dataLock); } diff --git a/lwrserv/SvrHandling.cpp b/lwrserv/SvrHandling.cpp index 947056a..0e33396 100755 --- a/lwrserv/SvrHandling.cpp +++ b/lwrserv/SvrHandling.cpp @@ -330,7 +330,6 @@ void SvrHandling::run() void SvrHandling::run(int port) { cout << timestamp() + "Starting " << SVR_NAME << " on port "<< port <<"\n"; - port = SVR_DEFAULT_PORT; while (c_state !=done) { SocketObject *socket = new SocketObject; @@ -393,8 +392,6 @@ void SvrHandling::clientCommandLoop(SocketObject& client) cout<> newJointPos(i); i++; } + // to less joint values if (i!=JOINT_NUMBER) { @@ -587,6 +591,8 @@ void SvrHandling::MovePTPJoints(SocketObject& client, string& args) // add trajectory to queue for sender thread SvrData::getInstance()->pushTrajectory(newTrajectory); + + SvrData::getInstance()->setCommandedJointPos(newJointPos); //Wait to end of movement while (true) @@ -1004,6 +1010,24 @@ void SvrHandling::quit(SocketObject& client) client.Send(SVR_TRUE_RSP); client.Disconnect(); } +void SvrHandling::setTrajectoryType(SocketObject& client, 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 SvrHandling::getTrajectoryType(SocketObject& client) +{ + enum TrajectoryType currType = SvrData::getInstance()->getTrajectoryType(); + cout << "curr trajectory type : "<< toString(currType) << "\n"; + + client.Send("current trajectory: " + toString(currType)); +} void SvrHandling::debug(SocketObject& client) { diff --git a/lwrserv/SvrHandling.h b/lwrserv/SvrHandling.h index 0fb680f..3792640 100755 --- a/lwrserv/SvrHandling.h +++ b/lwrserv/SvrHandling.h @@ -16,9 +16,10 @@ #define SVR_TRUE_RSP "true" #define SVR_FALSE_RSP "false" // server error messages -#define SVR_INVALID_ARGUMENT_COUNT "The argument count is not correct." -#define SVR_OUT_OF_RANGE "At least one argument is out of Range." -#define SVR_HELP_MSG "GPJ - GetPositionJoints \n" +#define SVR_INVALID_ARGUMENT "ERROR: The argument is not correct." +#define SVR_INVALID_ARGUMENT_COUNT "ERROR: The argument count is not correct." +#define SVR_OUT_OF_RANGE "ERROR: At least one argument is out of Range." +#define SVR_HELP_MSG "GPJ - GetPositionJoints " //Begin SVR_COMMANDS #define CMD_GetPositionJoints "GPJ" @@ -38,6 +39,8 @@ #define CMD_HELP "help" #define CMD_ISKUKA "IsKukaLWR" #define CMD_MoveCartesian "MC" +#define CMD_SET_TRAJECTORY_TYPE "STRAJ" +#define CMD_GET_TRAJECTORY_TYPE "GTRAJ" //End SVR_COMMANDS typedef enum {waiting, handshake, accepted, done} svr_state; @@ -75,6 +78,9 @@ private: //Cartesian Movement //Move to given POSE position void MoveCartesian(SocketObject& client, std::string& args); + // set a specific trajectory type + void setTrajectoryType(SocketObject& client, std::string& arg); + void getTrajectoryType(SocketObject& client); //Quit void quit(SocketObject& client); //DEBUGGING diff --git a/lwrserv/include/LinearTrajectory.h b/lwrserv/include/LinearTrajectory.h index f4753ed..5eef713 100644 --- a/lwrserv/include/LinearTrajectory.h +++ b/lwrserv/include/LinearTrajectory.h @@ -18,10 +18,12 @@ class LinearJointTrajectory : public Trajectory Vec jointEnd ) { + float MStoSec = 1000.0f; // calculate maximum velocity and acceleration - Vec maxJointLocalVelocity = maxJointVelocity * sampleTimeMs; - Vec maxJointLocalAcceleration = maxJointAcceleration * sampleTimeMs; + Vec maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec); + Vec maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec); + std::cout << "maxv : " << maxJointLocalVelocity << "\n"; // calculate delta movement Vec jointMovement = jointEnd - jointStart; Vec jointMovementAbs = jointMovement.abs(); @@ -31,21 +33,25 @@ class LinearJointTrajectory : public Trajectory // calculate number of movement steps Vec minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity); this->steps = ceil(minStepsPerJoint.max()); + if (this->steps == 0) + this->steps +=1; - this->nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*this->steps); + std::cout << "steps : " << this->steps << minStepsPerJoint<< "\n"; + + this->nodes = (struct Trajectory::trajectoryNode* ) calloc(sizeof(struct Trajectory::trajectoryNode),this->steps); Vec jointLast = jointStart; for( int i = 0 ; i < this->steps; ++i) { jointLast = jointLast + jointMovement.celldivide((float) this->steps ); - this->nodes[0].jointPos = jointLast; - this->nodes[0].velocity = maxJointLocalVelocity; - this->nodes[0].acceleration; + this->nodes[i].jointPos = jointLast; + this->nodes[i].velocity = maxJointLocalVelocity; + this->nodes[i].acceleration = Vec(0.0f); } - } - ~LinearJointTrajectory() - { - free (this->nodes); + // 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); } private: diff --git a/lwrserv/include/SvrData.h b/lwrserv/include/SvrData.h index 2a57cb8..5675eb8 100644 --- a/lwrserv/include/SvrData.h +++ b/lwrserv/include/SvrData.h @@ -120,7 +120,6 @@ public: void trajectoryCancelDone(); bool getTrajectoryDone(); - void setTrajectoryDone(bool newState); enum TrajectoryType getTrajectoryType(); void setTrajectoryType(enum TrajectoryType newType); diff --git a/lwrserv/include/Trajectroy.h b/lwrserv/include/Trajectroy.h index 16857a9..b90c21b 100644 --- a/lwrserv/include/Trajectroy.h +++ b/lwrserv/include/Trajectroy.h @@ -2,8 +2,15 @@ #define _TRAJECTORY_H_ #include "vec.h" #include +#include + + +template class Trajectory; // all types of trajectories +/// START OF TRAJECTORY LIST +#include "LinearTrajectory.h" + enum TrajectoryType { TrajectoryDefault = 0, TrajectoryJointLinear = 0, @@ -18,6 +25,86 @@ enum TrajectoryType { TrajectoryJointLSPB, TrajectoryCartLSPB }; +static struct { + enum TrajectoryType type; + std::string str; +} TrajectoryTypeStr[] = +{ + { .type = TrajectoryDefault , .str="default" }, + + { .type = TrajectoryJointLinear , .str="JointLinear" }, + { .type = TrajectoryJointLSPB , .str="JointLSPB" }, + { .type = TrajectoryJointBangBang, .str="JointBangBang" }, + { .type = TrajectoryJointFivePoly, .str="JointFivePoly" }, + + { .type = TrajectoryCartLinear , .str="CartLinear" }, + { .type = TrajectoryCartLSPB , .str="CartLSPB" }, + { .type = TrajectoryCartBangBang, .str="CartBangBang" }, + { .type = TrajectoryCartFivePoly, .str="CartFivePoly" }, +}; +/// END OF TRAJECTORY LIST + + + +static std::string toString(enum TrajectoryType type) +{ + int items = sizeof(TrajectoryTypeStr) / sizeof(TrajectoryTypeStr[0]); + for (int i = 0 ; i < items ; ++i) + { + if (TrajectoryTypeStr[i].type == type) + return ""+ TrajectoryTypeStr[i].str; + } + return "default"; +} +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) + return TrajectoryTypeStr[i].type; + } + 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, @@ -52,7 +139,8 @@ class Trajectory totalTime = steps * sampleTimeMs; - nodes = (struct trajectoryNode* ) malloc(sizeof(struct trajectoryNode)*steps); + //this->nodes = new struct trajectoryNode [steps]; + this->nodes = (struct Trajectory::trajectoryNode* ) calloc(sizeof(struct Trajectory::trajectoryNode),this->steps); nodes[0].jointPos = jointEnd; nodes[0].velocity = (jointEnd-jointStart)/sampleTimeMs; @@ -64,9 +152,12 @@ class Trajectory (void)maxJointAcceleration; } - ~Trajectory() + virtual ~Trajectory() { - free(nodes); + std::cout << " test1 \n "; + std::cout << nodes[steps-1].jointPos << "\n"; + free (nodes); + std::cout << " test3 \n "< retval(0.0f); unsigned int pos = currentStep; + if (steps == 0) + return retval; if (pos >= steps) { - pos = steps; + pos = steps-1; } if (nodes != NULL && movementType == MovementJointBased) @@ -115,9 +208,11 @@ class Trajectory { Mat retval(0.0f,1.0f); unsigned int pos = currentStep; + if (steps == 0) + return retval; if (pos >= steps) { - pos = steps; + pos = steps-1; } if (nodes != NULL && movementType == MovementCartBased) @@ -134,6 +229,8 @@ class Trajectory Vec getJointPos (unsigned int step) { Vec retval(0.0f); + if (steps == 0) + return retval; if (step >= steps) { return retval; @@ -150,6 +247,8 @@ class Trajectory Mat getCartPos (unsigned int step) { Vec retval(0.0f); + if (steps == 0) + return retval; if (step >= steps) { return retval; @@ -165,6 +264,8 @@ class Trajectory Vec getJointVelocity (unsigned int step) { Vec retval(0.0f); + if (steps == 0) + return retval; if (step >= steps) { return retval; @@ -180,6 +281,8 @@ class Trajectory Vec getJointAcceleration(unsigned int step) { Vec retval(0.0f); + if (steps == 0) + return retval; if (step >= steps) { return retval; diff --git a/lwrserv/include/vec.h b/lwrserv/include/vec.h index d47f0ac..7ea833c 100644 --- a/lwrserv/include/vec.h +++ b/lwrserv/include/vec.h @@ -4,7 +4,6 @@ #include #include #include "mat.h" -#include "sgn.h" template class Vec; template class Mat; diff --git a/lwrserv/program.cpp b/lwrserv/program.cpp index 0db54ff..e508cc4 100644 --- a/lwrserv/program.cpp +++ b/lwrserv/program.cpp @@ -214,33 +214,44 @@ void *threadRobotMovement(void *arg) // no new trajectory if(currentTrajectory == NULL) { + // mark that we reached the end of the trajectory // stay on current position + cout << "end of trajectorys \n"; goto end; } + else + { + std::cout <<"new Trajectory with " << currentTrajectory->getSteps()<<"\n"; + } } // get next commanded trajectory type - switch (currentTrajectory->getMovementType()) + if (currentTrajectory != NULL) { - case MovementCartBased: - { - currentCartPos = currentTrajectory->getNextCartPos(); - currentMovementType = MovementCartBased; - } - break; - case MovementJointBased: - { - currentJointPos = currentTrajectory->getNextJointPos(); - currentMovementType = MovementJointBased; - } - default: + switch (currentTrajectory->getMovementType()) { - // invalid trajectory skip it - delete currentTrajectory; - currentTrajectory = NULL; - goto end; + case MovementCartBased: + { + currentCartPos = currentTrajectory->getNextCartPos(); + currentMovementType = MovementCartBased; + } + break; + case MovementJointBased: + { + currentJointPos = currentTrajectory->getNextJointPos(); + + currentMovementType = MovementJointBased; + } + break; + default: + { + // invalid trajectory skip it + delete currentTrajectory; + currentTrajectory = NULL; + goto end; + } + break; } - break; } end: @@ -254,7 +265,7 @@ end: float waitingTimeMs = sampleTimeMs - calculationTimeMs; // sleep the calculated waiting time - boost::this_thread::sleep( boost::posix_time::milliseconds(waitingTimeMs) ); + boost::this_thread::sleep( boost::posix_time::milliseconds(waitingTimeMs+100.0f) ); switch (currentMovementType) @@ -264,7 +275,8 @@ end: // send the new joint values float newJointPosToSend[JOINT_NUMBER] = {0.0f}; currentJointPos.getData(newJointPosToSend); - friInst->doPositionControl(newJointPosToSend); + std::cout << currentJointPos << "\n"; + //friInst->doPositionControl(newJointPosToSend); } break; case MovementCartBased: @@ -282,10 +294,15 @@ end: // start timer for the last sent msg // TODO + if(currentTrajectory != NULL) + { + cout << "remaining Steps = " << currentTrajectory->getRemainingSteps()<<"\n"; + } // check if current trajectory is over if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0) { + cout << " Trajectory finished \n "; // invalid trajectory skip it delete currentTrajectory; currentTrajectory = NULL;