diff --git a/lwrserv/SvrHandling.h b/lwrserv/SvrHandling.h index 1f90b41..5381fe8 100755 --- a/lwrserv/SvrHandling.h +++ b/lwrserv/SvrHandling.h @@ -6,7 +6,10 @@ #ifndef SVR_HANDLING #define SVR_HANDLING +#define REAL_ROBOT false + // Serve status + #define SVR_NAME "lwrsvr 4.11d" #define SVR_DEFAULT_PORT 8000 #define SVR_HANDSHAKE "Hello Robot" diff --git a/lwrserv/config.h b/lwrserv/config.h index 623c90a..67ace0c 100644 --- a/lwrserv/config.h +++ b/lwrserv/config.h @@ -1,6 +1,7 @@ /* config.h. Generated from config.h.in by configure. */ /* config.h.in. Generated from configure.ac by autoheader. */ + /* Compiling CppUTest itself */ #define CPPUTEST_COMPILATION 1 diff --git a/lwrserv/include/BangBangTrajectory.h b/lwrserv/include/BangBangTrajectory.h index 2a28804..b3fec41 100644 --- a/lwrserv/include/BangBangTrajectory.h +++ b/lwrserv/include/BangBangTrajectory.h @@ -35,6 +35,7 @@ class BangBangJointTrajectory : public Trajectory // calculate time if acceleration is enouth to reach max speed Vec minBangBangTime = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration) / sampleTime; //TODO check if mintime is neceesary + std::cout << minBangBangTime << "minBangBangTime \n "; Vec minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f; @@ -53,29 +54,33 @@ class BangBangJointTrajectory : public Trajectory // percentage of max velocity //Vec currJointMovementOfMax = minStepsPerJoint / minStepsPerJoint.max() ; // s = a* t^2 / 2 => a = s* 2 / t^2 - float floaterror = 0.998; + float floaterror = 1.0f; Vec currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(this->steps).celldivide(this->steps)*2.0f*floaterror; + std::cout << "currMaxAcceleration : " << currMaxAcceleration << "\n"; + + float count = 0.0f; for( int i = 0 ; i < this->steps; ++i) { // acceleration phase - if (i <= this->steps /2 ) + if (i < this->steps /2 ) { this->nodes[i].acceleration = currMaxAcceleration ; + count +=1.0f; } // deacceleration phase else { this->nodes[i].acceleration = currMaxAcceleration * -1.0f; + count -=1.0f; } - this->nodes[i].velocity = velocityLast + jointMovementSgn.cellmultiply(this->nodes[i].acceleration); -this->nodes[i].jointPos = jointLast + this->nodes[i].velocity; + this->nodes[i].velocity = jointMovementSgn.cellmultiply(currMaxAcceleration) * count; + this->nodes[i].jointPos = jointLast + this->nodes[i].velocity; + + + std::cout << count << this->nodes[i].velocity << this->nodes[i].jointPos <<"\n"; 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; - 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/LSPBTrajectory.h b/lwrserv/include/LSPBTrajectory.h index cb8432e..e25d9fa 100644 --- a/lwrserv/include/LSPBTrajectory.h +++ b/lwrserv/include/LSPBTrajectory.h @@ -16,106 +16,77 @@ public: 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); - std::cout << "max : " << maxJointLocalVelocity << "\n"; + 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 number of movement steps - - // 2 same acceleration and deaceleration phases + // calculate sample count - 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 ; + // 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 maxVeloReachable = (jointMovementAbs.celldivide(maxJointAcceleration)*2.0f).sqrt().cellmultiply(maxJointVelocity); - // one phase with constant velocity - float remainingjointMovementAbs = jointMovementAbs(j); - // v * t = s - remainingjointMovementAbs -= maxJointLocalVelocity(j) * minStepsPerJoint(j) * sampleTimeMs; + maxJointLocalVelocity = (maxVeloReachable/sampleTime).cellmax(maxJointLocalVelocity); - // s / v = t - minStepsPerJoint(j) += remainingjointMovementAbs/maxJointLocalVelocity(j); - } - } + Vec accelerationPhaseTime = maxJointVelocity.celldivide(maxJointAcceleration)/sampleTime; + Vec jointMovementRemaining = jointMovementAbs - maxJointVelocity.cellmultiply(maxJointVelocity).celldivide(maxJointAcceleration); + Vec maxVelocityPhaseThime = jointMovementRemaining.celldivide(maxJointLocalVelocity); + + Vec minStepsPerJoint = accelerationPhaseTime * 2.0f + maxVelocityPhaseThime; + std::cout << minStepsPerJoint << "minStepsPerJoint \n"; + std::cout << accelerationPhaseTime << "a time \n"; + std::cout << jointMovementRemaining << "reimain movement\n"; + std::cout << maxVelocityPhaseThime << "max time \n"; + minStepsPerJoint = minStepsPerJoint.ceil(); - std::cout << "minsteps : " << minStepsPerJoint << "\n"; - std::cout << "steps : " << minStepsPerJoint.max() << "\n"; - this->steps = ceil(minStepsPerJoint.max()); - std::cout << "steps : " << this->steps << "\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; + Vec velocityLast(0.0f); - // 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); + // percentage of max velocity + //Vec currJointMovementOfMax = minStepsPerJoint / minStepsPerJoint.max() ; + // s = a* t^2 / 2 => a = s* 2 / t^2 + Vec currMaxAcceleration = (maxJointLocalAcceleration).cellmultiply(minStepsPerJoint) / minStepsPerJoint.max(); + Vec currMaxVelocity = (maxJointLocalVelocity ).cellmultiply(minStepsPerJoint) / minStepsPerJoint.max(); + std::cout << "max velo curr : " << currMaxVelocity << "\n"; - for (int currStep=0; currStepsteps; currStep++) + for( int i = 0 ; i < this->steps; ++i) { - for (int currJoint=0; currJointsteps/2.0f) + if (i < accelerationPhaseTime(i)) { - currentInk(currJoint) = std::min(currentInkLast(currJoint)+maxJointLocalAcceleration(currJoint),maxJointLocalVelocity(currJoint)); - }else if (currStep+1 > this->steps-deltaToMaxSpeed(currJoint)) + this->nodes[i].acceleration(i) = currMaxAcceleration(i); + }else if (i < accelerationPhaseTime(i) + jointMovementRemaining(i)) { - currentInk(currJoint) = std::max(currentInkLast(currJoint)-maxJointLocalAcceleration(currJoint),0.0f); - }else + this->nodes[i].acceleration(i) = 0.0f; + }else { - currentInk(currJoint) = currentInkLast(currJoint); + this->nodes[i].acceleration(i) = currMaxAcceleration(i)* -1.0f; } - 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); + this->nodes[i].velocity(i) = velocityLast(i) + jointMovementSgn(i) * currMaxAcceleration(i); } - } + this->nodes[i].jointPos = jointLast + this->nodes[i].velocity; - // 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); + //std::cout << i << this->nodes[i].velocity << this->nodes[i].jointPos <<"\n"; + jointLast = this->nodes[i].jointPos; + velocityLast = this->nodes[i].velocity; + } } }; diff --git a/lwrserv/program.cpp b/lwrserv/program.cpp index 934a620..abc6ba0 100644 --- a/lwrserv/program.cpp +++ b/lwrserv/program.cpp @@ -190,22 +190,27 @@ void *threadRobotMovement(void *arg) // get current robot position int tries = 2; + for (int i= 0 ; i < tries ; ++i) { - SvrData::getInstance()->updateMessurement(); + if (REAL_ROBOT) + SvrData::getInstance()->updateMessurement(); currentJointPos = SvrData::getInstance()->getMeasuredJointPos(); SvrData::getInstance()->setCommandedJointPos(currentJointPos); float newJointPosToSend[JOINT_NUMBER] = {0.0f}; currentJointPos.getData(newJointPosToSend); - friInst->doPositionControl(newJointPosToSend); + if (REAL_ROBOT) + friInst->doPositionControl(newJointPosToSend); } std::cout << "init position is " << currentJointPos << "\n"; while(true) { + if (REAL_ROBOT) + SvrData::getInstance()->updateMessurement(); bool positionChanged = false; // check if we have to cancel current trajectory @@ -281,20 +286,18 @@ end: float newJointPosToSend[JOINT_NUMBER] = {0.0f}; Robot::VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI); newJointPosToSendRad.getData(newJointPosToSend); - if ( positionChanged) - std::cout << currentJointPos << "\n"; - friInst->doPositionControl(newJointPosToSend); + if (REAL_ROBOT) + friInst->doPositionControl(newJointPosToSend); } break; case MovementCartBased: { -#if 0 float Stiff[6] = {1000.0, 1000.0, 1000.0, 10.0, 10.0, 10.0}; float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7}; - float currentCartPosA[12] = {0.0f}; - columncurrentCartPos.getData(newCartPosToSend); - friInst->doCartesianImpedanceControl(currentCartPosA,Stiff, Damp, NULL, NULL, true); -#endif + float newCartPosToSend[12] = {0.0f}; + currentCartPos.getData(newCartPosToSend); + if (REAL_ROBOT) + friInst->doCartesianImpedanceControl(newCartPosToSend,Stiff, Damp, NULL, NULL, true); } break; } @@ -571,7 +574,6 @@ int main() return err; } - //Start client handling SvrHandling *svr = new SvrHandling(); if (svr == NULL) diff --git a/lwrserv/trajectory/start.m b/lwrserv/trajectory/start.m new file mode 100644 index 0000000..a1c9248 --- /dev/null +++ b/lwrserv/trajectory/start.m @@ -0,0 +1,22 @@ +clear all +close all + +filename = 'a.csv'; +M = csvread(filename); + +sampletime = 0.005 +t_max = size(M,1) * sampletime; + +t = linspace(0,t_max,size(M,1)); + + +[~,M_d] = gradient( M,0.005); +[~,M_dd] = gradient( M_d,0.005); + +figure(); +subplot(3,1,1); +plot (t,M); +subplot(3,1,2); +plot (t,M_d); +subplot(3,1,3); +plot (t,M_dd); \ No newline at end of file diff --git a/one_10_degree b/one_10_degree index a573b05..aaaefda 100755 --- a/one_10_degree +++ b/one_10_degree @@ -3,11 +3,12 @@ set timeout 1 spawn telnet localhost 8000 expect +set timeout 0.5 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" +send "MPTPJ 0 0 0 0 0 0 100\n" expect diff --git a/one_10_degree_lspb b/one_10_degree_lspb new file mode 100755 index 0000000..0b0165f --- /dev/null +++ b/one_10_degree_lspb @@ -0,0 +1,14 @@ +#!/usr/bin/expect +#Where the script should be run from. +set timeout 1 +spawn telnet localhost 8000 +expect +set timeout 0.5 +send "Hello Robot\n" +expect +send "SS 10\n" +expect +send "ST JointLSPB\n" +expect +send "MPTPJ 0 0 0 0 0 0 100\n" +expect diff --git a/test b/test new file mode 100755 index 0000000..a573b05 --- /dev/null +++ b/test @@ -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_20_degree b/two_20_degree index 58cd544..eb5932a 100755 --- a/two_20_degree +++ b/two_20_degree @@ -7,7 +7,7 @@ send "Hello Robot\n" expect send "SS 10\n" expect -send "ST JointBangBang\n" +send "ST JointLSPB\n" expect send "MPTPJ 0 0 0 0 0 20 20\n" expect