From bba9cb34ac79fde51dbe374fb9484af720fc4b4b Mon Sep 17 00:00:00 2001 From: philipp schoenberger Date: Thu, 20 Aug 2015 17:49:16 +0200 Subject: [PATCH] fix bangbang --- all_zero | 13 +++++++++ lwrserv/commands.cpp | 11 ++----- lwrserv/include/BangBangTrajectory.h | 43 +++++++++++++++++++--------- lwrserv/include/LinearTrajectory.h | 1 + lwrserv/include/Trajectroy.h | 21 ++++++++++++-- lwrserv/include/commands.h | 2 +- lwrserv/include/lwr4.h | 1 + lwrserv/include/vec.h | 40 +++++++++++++++++++++++++- lwrserv/program.cpp | 24 +++++++++------- one_10_degree | 13 +++++++++ two_10_degree | 13 +++++++++ two_20_degree | 13 +++++++++ 12 files changed, 158 insertions(+), 37 deletions(-) create mode 100755 all_zero create mode 100755 one_10_degree create mode 100755 two_10_degree create mode 100755 two_20_degree diff --git a/all_zero b/all_zero new file mode 100755 index 0000000..255f3ed --- /dev/null +++ b/all_zero @@ -0,0 +1,13 @@ +#!/usr/bin/expect +#Where the script should be run from. +set timeout 1 +spawn telnet localhost 8000 +expect +send "Hello Robot\n" +expect +send "SS 10\n" +expect +send "ST JointBangBang\n" +expect +send "MPTPJ 0 0 0 0 0 0 0\n" +expect diff --git a/lwrserv/commands.cpp b/lwrserv/commands.cpp index fc69f36..e18b1e1 100644 --- a/lwrserv/commands.cpp +++ b/lwrserv/commands.cpp @@ -125,16 +125,8 @@ void movePTPJoints(SocketObject& client, std::string& arg) 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); + Trajectory* newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos); // add trajectory to queue for sender thread SvrData::getInstance()->pushTrajectory(newTrajectory); @@ -142,6 +134,7 @@ void movePTPJoints(SocketObject& client, std::string& arg) SvrData::getInstance()->setCommandedJointPos(newJointPos); //Wait to end of movement + float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs(); while (true) { if (SvrData::getInstance()->getTrajectoryDone() == true) diff --git a/lwrserv/include/BangBangTrajectory.h b/lwrserv/include/BangBangTrajectory.h index f6df681..2a28804 100644 --- a/lwrserv/include/BangBangTrajectory.h +++ b/lwrserv/include/BangBangTrajectory.h @@ -15,47 +15,62 @@ class BangBangJointTrajectory : public Trajectory Vec jointEnd ) { + std::cout << "bang bang \n " ; float MStoSec = 1000.0f; + float sampleTime = sampleTimeMs / MStoSec; // calculate maximum velocity and acceleration - Vec maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec); - Vec maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec); + Vec maxJointLocalVelocity = maxJointVelocity * sampleTime; + Vec maxJointLocalAcceleration = maxJointAcceleration * sampleTime; + std::cout << maxJointLocalVelocity << ", " << maxJointLocalAcceleration << "\n"; // calculate delta movement Vec jointMovement = jointEnd - jointStart; Vec jointMovementAbs = jointMovement.abs(); + Vec jointMovementSgn = jointMovement.sgn(); // calculate sample count // calculate number of movement steps // one joint has to reach maxvelocity the others are stepped down to + // calculate time if acceleration is enouth to reach max speed + Vec minBangBangTime = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration) / sampleTime; + //TODO check if mintime is neceesary + + Vec minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f; - this->steps = ceil(minStepsPerJoint.max()); + minStepsPerJoint = minStepsPerJoint.ceil(); + std::cout << minStepsPerJoint << "minStepsPerJoint \n"; + + this->steps = minStepsPerJoint.max(); if (this->steps == 0) this->steps +=1; this->nodes = (struct Trajectory::trajectoryNode* ) calloc(sizeof(struct Trajectory::trajectoryNode),this->steps); Vec jointLast = jointStart; - float percentStep = 1.0f / ((this->steps+1)/2); + Vec velocityLast(0.0f); + + // percentage of max velocity + //Vec currJointMovementOfMax = minStepsPerJoint / minStepsPerJoint.max() ; + // s = a* t^2 / 2 => a = s* 2 / t^2 + float floaterror = 0.998; + Vec currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(this->steps).celldivide(this->steps)*2.0f*floaterror; for( int i = 0 ; i < this->steps; ++i) { // acceleration phase - if (i > this->steps /2 ) + if (i <= this->steps /2 ) { - float percent = i * percentStep; - this->nodes[i].velocity = maxJointLocalVelocity * percent; - this->nodes[i].acceleration = maxJointLocalVelocity * percentStep; + this->nodes[i].acceleration = currMaxAcceleration ; } // deacceleration phase else { - float percent = ( i - this->steps/2 ) * percentStep; - this->nodes[i].velocity = maxJointLocalVelocity * percent; - this->nodes[i].acceleration = maxJointLocalVelocity * (-1.0 * percentStep); + this->nodes[i].acceleration = currMaxAcceleration * -1.0f; } - - jointLast = jointLast + this->nodes[i].velocity; - this->nodes[i].jointPos = jointLast; + this->nodes[i].velocity = velocityLast + jointMovementSgn.cellmultiply(this->nodes[i].acceleration); +this->nodes[i].jointPos = jointLast + this->nodes[i].velocity; + jointLast = this->nodes[i].jointPos; + velocityLast = this->nodes[i].velocity; } // last step myth be to wide so cut it to the wanted position this->nodes[this->steps-1].jointPos = jointEnd; diff --git a/lwrserv/include/LinearTrajectory.h b/lwrserv/include/LinearTrajectory.h index 0e0aa66..39b9f28 100644 --- a/lwrserv/include/LinearTrajectory.h +++ b/lwrserv/include/LinearTrajectory.h @@ -18,6 +18,7 @@ class LinearJointTrajectory : public Trajectory Vec jointEnd ) { + std::cout << "linear \n " ; float MStoSec = 1000.0f; // calculate maximum velocity and acceleration Vec maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec); diff --git a/lwrserv/include/Trajectroy.h b/lwrserv/include/Trajectroy.h index 6d8488b..b9f05e0 100644 --- a/lwrserv/include/Trajectroy.h +++ b/lwrserv/include/Trajectroy.h @@ -2,6 +2,8 @@ #define _TRAJECTORY_H_ #include "vec.h" #include +#include +#include template class Trajectory; @@ -31,8 +33,6 @@ static struct { std::string str; } TrajectoryTypeStr[] = { - { .type = TrajectoryDefault , .str="default" }, - { .type = TrajectoryJointLinear , .str="JointLinear" }, { .type = TrajectoryJointLSPB , .str="JointLSPB" }, { .type = TrajectoryJointBangBang, .str="JointBangBang" }, @@ -298,6 +298,23 @@ class Trajectory Vec acceleration; }; + void saveToFile(std::string filename) + { + std::ofstream file; + file.open(filename.c_str()); + for (unsigned int currentStep = 0 ; currentStep < steps ; ++ currentStep) + { + for (unsigned int currentJoint = 0 ; currentJoint < SIZE; ++ currentJoint) + { + if (currentJoint != 0) + file << ", "; + file << nodes[currentStep].jointPos(currentJoint); + } + file << "\n"; + } + file.close(); + } + protected: static const unsigned int defaultSteps = 100; diff --git a/lwrserv/include/commands.h b/lwrserv/include/commands.h index 79a0e31..3f7ed83 100644 --- a/lwrserv/include/commands.h +++ b/lwrserv/include/commands.h @@ -10,7 +10,7 @@ static unsigned int commandCount; // registers all commands void registerCommands(); -// print out all registered commands to the client interface +// print out all commands to the client interface void printUsage(SocketObject& client, std::string& arg); //Handling request for current Joint Values diff --git a/lwrserv/include/lwr4.h b/lwrserv/include/lwr4.h index 76477a6..1c69296 100644 --- a/lwrserv/include/lwr4.h +++ b/lwrserv/include/lwr4.h @@ -23,6 +23,7 @@ class LWR4 : public Robot 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); diff --git a/lwrserv/include/vec.h b/lwrserv/include/vec.h index b078269..db212ff 100644 --- a/lwrserv/include/vec.h +++ b/lwrserv/include/vec.h @@ -170,6 +170,13 @@ public: vec(i) = m_atData[i]*tScale; return vec; } + Vec operator / (T tScale) + { + Vec vec; + for (unsigned int i=0; i operator * (const Mat &mat) { @@ -194,6 +201,20 @@ public: buff.m_atData[i] = m_atData[i]*vec.m_atData[i]; return buff; } + Vec cellmin(const Vec & vec) + { + Vec buff; + for (int i=0; i cellmax(const Vec & vec) + { + Vec buff; + for (int i=0; i abs () { @@ -203,6 +224,13 @@ public: return buff; } + Vec sqrt() + { + Vec buff; + for (int i=0; i floor () { Vec buff; @@ -214,9 +242,19 @@ public: { Vec buff; for (int i=0; i 0) + return 1.0f; + else + return -1.0f; + } Vec sgn () { Vec buff; diff --git a/lwrserv/program.cpp b/lwrserv/program.cpp index 1bffc61..934a620 100644 --- a/lwrserv/program.cpp +++ b/lwrserv/program.cpp @@ -183,8 +183,8 @@ void *threadRobotMovement(void *arg) Trajectory* currentTrajectory = NULL; enum MovementType currentMovementType = MovementJointBased; - Robot::VecJoint currentJointPos(0.0f); - MatCarthesian currentCartPos(0.0f,1.0f); + Robot::VecJoint currentJointPos(0.0f); + MatCarthesian currentCartPos(0.0f,1.0f); friRemote* friInst = SvrData::getInstance()->getFriRemote(); @@ -197,17 +197,17 @@ void *threadRobotMovement(void *arg) 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); } + std::cout << "init position is " << currentJointPos << "\n"; while(true) { + bool positionChanged = false; // check if we have to cancel current trajectory bool cancel = SvrData::getInstance()->getTrajectoryCancel(); if (cancel) @@ -250,13 +250,14 @@ void *threadRobotMovement(void *arg) case MovementCartBased: { currentCartPos = currentTrajectory->getNextCartPos(); + positionChanged = true; currentMovementType = MovementCartBased; } break; case MovementJointBased: { currentJointPos = currentTrajectory->getNextJointPos(); - + positionChanged = true; currentMovementType = MovementJointBased; } break; @@ -280,7 +281,8 @@ end: float newJointPosToSend[JOINT_NUMBER] = {0.0f}; Robot::VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI); newJointPosToSendRad.getData(newJointPosToSend); - std::cout << newJointPosToSendRad << "\n"; + if ( positionChanged) + std::cout << currentJointPos << "\n"; friInst->doPositionControl(newJointPosToSend); } break; @@ -299,15 +301,17 @@ end: // start timer for the last sent msg // TODO - if(currentTrajectory != NULL) - { - std::cout << "remaining Steps = " << currentTrajectory->getRemainingSteps()<<"\n"; - } // check if current trajectory is over if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0) { std::cout << " Trajectory finished \n "; + + static int trajcetorycount = 0; + trajcetorycount +=1; + + + currentTrajectory->saveToFile(std::string("trajectory/a.csv")); // invalid trajectory skip it delete currentTrajectory; currentTrajectory = NULL; diff --git a/one_10_degree b/one_10_degree new file mode 100755 index 0000000..a573b05 --- /dev/null +++ b/one_10_degree @@ -0,0 +1,13 @@ +#!/usr/bin/expect +#Where the script should be run from. +set timeout 1 +spawn telnet localhost 8000 +expect +send "Hello Robot\n" +expect +send "SS 10\n" +expect +send "ST JointBangBang\n" +expect +send "MPTPJ 0 0 0 0 0 0 10\n" +expect diff --git a/two_10_degree b/two_10_degree new file mode 100755 index 0000000..dbc8ddf --- /dev/null +++ b/two_10_degree @@ -0,0 +1,13 @@ +#!/usr/bin/expect +#Where the script should be run from. +set timeout 1 +spawn telnet localhost 8000 +expect +send "Hello Robot\n" +expect +send "SS 10\n" +expect +send "ST JointBangBang\n" +expect +send "MPTPJ 0 0 0 0 0 10 10\n" +expect diff --git a/two_20_degree b/two_20_degree new file mode 100755 index 0000000..58cd544 --- /dev/null +++ b/two_20_degree @@ -0,0 +1,13 @@ +#!/usr/bin/expect +#Where the script should be run from. +set timeout 1 +spawn telnet localhost 8000 +expect +send "Hello Robot\n" +expect +send "SS 10\n" +expect +send "ST JointBangBang\n" +expect +send "MPTPJ 0 0 0 0 0 20 20\n" +expect