diff --git a/lwrserv/include/BangBangTrajectory.h b/lwrserv/include/BangBangTrajectory.h index 68984d3..7d68d20 100644 --- a/lwrserv/include/BangBangTrajectory.h +++ b/lwrserv/include/BangBangTrajectory.h @@ -21,7 +21,6 @@ class BangBangJointTrajectory : public Trajectory // calculate maximum velocity and acceleration Vec maxJointLocalVelocity = maxJointVelocity * sampleTime; Vec maxJointLocalAcceleration = maxJointAcceleration * sampleTime; - std::cout << maxJointLocalVelocity << ", " << maxJointLocalAcceleration << "\n"; // calculate delta movement Vec jointMovement = jointEnd - jointStart; @@ -36,13 +35,12 @@ class BangBangJointTrajectory : public Trajectory // s = a * t^2 / 2 Vec minBangBangTime = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration) / sampleTime; //TODO check if mintime is neceesary - Vec time = ((jointMovementAbs/2.0f).celldivide(maxJointAcceleration) * 2.0f).sqrt() * 2.0f / sampleTime; - std::cout << time << "time \n "; - std::cout << minBangBangTime << "minBangBangTime \n "; + Vec timeMaxAcceleration = ((jointMovementAbs/2.0f).celldivide(maxJointAcceleration) * 2.0f).sqrt() * 2.0f / sampleTime; + Vec timeMaxVelocity = ((jointMovementAbs/2.0f).celldivide(maxJointVelocity) * 2.0f)*2.0f / sampleTime; + Vec time = timeMaxAcceleration.cellmax(timeMaxVelocity); Vec minStepsPerJoint = time;//jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f; minStepsPerJoint = minStepsPerJoint.ceil(); - std::cout << minStepsPerJoint << "minStepsPerJoint \n"; this->steps = minStepsPerJoint.max(); if (this->steps == 0) @@ -78,7 +76,6 @@ class BangBangJointTrajectory : public Trajectory 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; } diff --git a/lwrserv/include/LSPBTrajectory.h b/lwrserv/include/LSPBTrajectory.h index e25d9fa..87518c1 100644 --- a/lwrserv/include/LSPBTrajectory.h +++ b/lwrserv/include/LSPBTrajectory.h @@ -16,7 +16,6 @@ public: Vec jointEnd ) { - std::cout << "bang bang \n " ; float MStoSec = 1000.0f; float sampleTime = sampleTimeMs / MStoSec; // calculate maximum velocity and acceleration @@ -35,19 +34,57 @@ public: // 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); - maxJointLocalVelocity = (maxVeloReachable/sampleTime).cellmax(maxJointLocalVelocity); - Vec accelerationPhaseTime = maxJointVelocity.celldivide(maxJointAcceleration)/sampleTime; - Vec jointMovementRemaining = jointMovementAbs - maxJointVelocity.cellmultiply(maxJointVelocity).celldivide(maxJointAcceleration); - Vec maxVelocityPhaseThime = jointMovementRemaining.celldivide(maxJointLocalVelocity); + + + // v ^ + // | + // v_bang| /\ <- bang bang trajectory + // | / \ + // v_lspb| _____\___ <- lspb trajectory + // | / . . + // | / . . \ + // | / . . \ + // |/________.__.___\____> t + // | t1| t2 | t3| + // |tx| + // + // t1/t3 = acceleration phase + // t2 = constant speed phase + // + // t = t1 + t2 +t3 + // + // v_lspb = percent * v_bang + // + // t = t_bang + t_x + // t = t_bang + (v_bang - v_lspb)/a * 2 + // + // t = t_bang + (t_bang/2 * a - percent *t_bang/2*a) / a *2 + // t = t_bang + (1- percent) * (t_bang/2 * a) / a *2 + // t = t_bang + (1- percent) * t_bang + // t = (2-percent) * t_bang - 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(); + // calculate time for bang bang + Vec accelerationPhaseTime = maxJointVelocity.celldivide(maxJointAcceleration)/sampleTime; + Vec timeMaxAcceleration = ((jointMovementAbs/2.0f).celldivide(maxJointAcceleration) * 2.0f).sqrt() * 2.0f / sampleTime; + Vec timeMaxVelocity = ((jointMovementAbs/2.0f).celldivide(maxJointVelocity) * 2.0f)*2.0f / sampleTime; + Vec timeBangBang = timeMaxAcceleration.cellmax(timeMaxVelocity); + + std::cout << timeBangBang << "timeBangBang\n"; + float steps_bangbang = timeBangBang.ceil().max(); + + // calculate maxAcceleration for bangbang + Vec currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(steps_bangbang).celldivide(steps_bangbang)*2.0f; + std::cout << "currMaxAcceleration : " << currMaxAcceleration << "\n"; + + float percentLSPB = 0.7f; + Vec timeLSPB = timeBangBang * (1 + 0.25f * (1.0f - percentLSPB)); + float timeBlend = ((timeBangBang /2.0f) * percentLSPB).ceil().max(); + std::cout << timeBlend << "timeBlend \n"; + + std::cout << timeLSPB << "max time \n"; + Vec minStepsPerJoint = timeLSPB.ceil(); this->steps = minStepsPerJoint.max(); if (this->steps == 0) @@ -58,32 +95,25 @@ public: Vec jointLast = jointStart; Vec velocityLast(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"; + int count = 0; for( int i = 0 ; i < this->steps; ++i) { - for (int joint = 0 ; joint < SIZE; ++joint) + if (i < timeBlend) + { + count += 1; + this->nodes[i].acceleration = currMaxAcceleration; + }else if (i < this->steps - timeBlend ) + { + this->nodes[i].acceleration = Vec(0.0f); + }else { - if (i < accelerationPhaseTime(i)) - { - this->nodes[i].acceleration(i) = currMaxAcceleration(i); - }else if (i < accelerationPhaseTime(i) + jointMovementRemaining(i)) - { - this->nodes[i].acceleration(i) = 0.0f; - }else - { - this->nodes[i].acceleration(i) = currMaxAcceleration(i)* -1.0f; - } - this->nodes[i].velocity(i) = velocityLast(i) + jointMovementSgn(i) * currMaxAcceleration(i); + count -= 1; + this->nodes[i].acceleration = currMaxAcceleration* -1.0f; } + this->nodes[i].velocity = jointMovementSgn.cellmultiply(currMaxAcceleration) * count; this->nodes[i].jointPos = jointLast + this->nodes[i].velocity; - //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/include/Trajectroy.h b/lwrserv/include/Trajectroy.h index 4ed74b1..356c4e2 100644 --- a/lwrserv/include/Trajectroy.h +++ b/lwrserv/include/Trajectroy.h @@ -1,6 +1,7 @@ #ifndef _TRAJECTORY_H_ #define _TRAJECTORY_H_ #include "vec.h" +#include "mat.h" #include #include #include @@ -48,8 +49,6 @@ static struct { }; /// END OF TRAJECTORY LIST - - static std::string toString(enum TrajectoryType type) { int items = sizeof(TrajectoryTypeStr) / sizeof(TrajectoryTypeStr[0]); @@ -201,9 +200,9 @@ class Trajectory return retval; } - Mat getNextCartPos () + MatCarthesian getNextCartPos () { - Mat retval(0.0f,1.0f); + MatCarthesian retval(0.0f,1.0f); unsigned int pos = currentStep; if (steps == 0) return retval; @@ -233,7 +232,7 @@ class Trajectory return retval; } - if (nodes != NULL && movementType == MovementJointBased) + if (nodes != NULL) { retval = nodes[step].jointPos; } @@ -241,9 +240,9 @@ class Trajectory return retval; } - Mat getCartPos (unsigned int step) + MatCarthesian getCartPos (unsigned int step) { - Vec retval(0.0f); + MatCarthesian retval(0.0f, 1.0f); if (steps == 0) return retval; if (step >= steps) @@ -251,7 +250,7 @@ class Trajectory return retval; } - if (nodes != NULL && movementType == MovementJointBased) + if (nodes != NULL) { retval = nodes[step].cartPos; } @@ -268,7 +267,7 @@ class Trajectory return retval; } - if (nodes != NULL && movementType == MovementCartBased) + if (nodes != NULL) { retval = nodes[step].velocity; } @@ -285,7 +284,7 @@ class Trajectory return retval; } - if (nodes != NULL && movementType == MovementCartBased) + if (nodes != NULL) { retval = nodes[step].acceleration; } @@ -296,7 +295,7 @@ class Trajectory struct trajectoryNode { Vec jointPos; - Mat cartPos; + MatCarthesian cartPos; Vec velocity; Vec acceleration; }; diff --git a/lwrserv/include/mat.h b/lwrserv/include/mat.h index c7f89fc..df3cbde 100644 --- a/lwrserv/include/mat.h +++ b/lwrserv/include/mat.h @@ -16,6 +16,8 @@ typedef Mat Mat2d; typedef Mat Mat3d; typedef Mat Mat4d; +typedef Mat MatCarthesian; + // template square matrix class for SIMPLE data types template class Mat diff --git a/lwrserv/include/robot.h b/lwrserv/include/robot.h index cb16c8c..23f7162 100644 --- a/lwrserv/include/robot.h +++ b/lwrserv/include/robot.h @@ -5,7 +5,6 @@ #include "mat.h" #include "friComm.h" -typedef Mat MatCarthesian; typedef Vec VecTorque; class Robot diff --git a/lwrserv/program.cpp b/lwrserv/program.cpp index abc6ba0..96ee17d 100644 --- a/lwrserv/program.cpp +++ b/lwrserv/program.cpp @@ -206,7 +206,6 @@ void *threadRobotMovement(void *arg) } std::cout << "init position is " << currentJointPos << "\n"; - while(true) { if (REAL_ROBOT) @@ -312,7 +311,6 @@ end: static int trajcetorycount = 0; trajcetorycount +=1; - currentTrajectory->saveToFile(std::string("trajectory/a.csv")); // invalid trajectory skip it diff --git a/one_10_degree b/one_10_degree index aaaefda..05b9d3f 100755 --- a/one_10_degree +++ b/one_10_degree @@ -3,12 +3,11 @@ set timeout 1 spawn telnet localhost 8000 expect -set timeout 0.5 send "Hello Robot\n" expect -send "SS 10\n" +send "SS 0.0110\n" expect send "ST JointBangBang\n" expect -send "MPTPJ 0 0 0 0 0 0 100\n" +send "MPTPJ 0 0 0 0 0 0 10\n" expect diff --git a/one_10_degree_lspb b/one_10_degree_lspb index 0b0165f..f5b41f8 100755 --- a/one_10_degree_lspb +++ b/one_10_degree_lspb @@ -3,12 +3,12 @@ set timeout 1 spawn telnet localhost 8000 expect -set timeout 0.5 +set timeout 1 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" +send "MPTPJ 0 0 0 0 0 0 10\n" expect