fixed bangbang

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"

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
 

lwrserv/include/BangBangTrajectory.h

@@ -35,6 +35,7 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
             // calculate time if acceleration is enouth to reach max speed
             Vec<float,SIZE> minBangBangTime = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration) / sampleTime;
             //TODO check if mintime is neceesary
+            std::cout << minBangBangTime  << "minBangBangTime \n ";
             
 
             Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f;
@@ -53,29 +54,33 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
             // percentage of max velocity 
             //Vec<float,SIZE> currJointMovementOfMax = minStepsPerJoint / minStepsPerJoint.max() ;
             // s =  a* t^2 / 2 => a = s* 2 / t^2
-            float floaterror = 0.998;
+            float floaterror = 1.0f;
             Vec<float,SIZE> 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<float,SIZE>(0.0f); 
         }
     private:
 

lwrserv/include/LSPBTrajectory.h

@@ -16,106 +16,77 @@ public:
             Vec<float,SIZE> jointEnd
             )
     {
+        std::cout << "bang bang \n " ;
         float MStoSec = 1000.0f;
+        float sampleTime = sampleTimeMs / MStoSec;
         // calculate maximum velocity and acceleration
-        Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec);
-        Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec);
-        std::cout << "max : " << maxJointLocalVelocity << "\n";
+        Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * sampleTime;
+        Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * sampleTime;
+        std::cout << maxJointLocalVelocity << ", " << maxJointLocalAcceleration << "\n";
 
         // calculate delta movement
         Vec<float,SIZE> jointMovement = jointEnd - jointStart;
         Vec<float,SIZE> jointMovementAbs = jointMovement.abs();
+        Vec<float,SIZE> jointMovementSgn = jointMovement.sgn();
 
-        // calculate number of movement steps
+        // calculate sample count
 
-        // 2 same acceleration and deaceleration phases
+        // 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<float,SIZE> maxVeloReachable = (jointMovementAbs.celldivide(maxJointAcceleration)*2.0f).sqrt().cellmultiply(maxJointVelocity);
 
-        Vec<float,SIZE> minStepsPerJoint(0.0f);
-        // check 
-        for (int j=0; j<SIZE; j++)
-        {
-            // only need acceleration and deacceleration phase
-            if (maxJointLocalVelocity(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 ;
+        maxJointLocalVelocity = (maxVeloReachable/sampleTime).cellmax(maxJointLocalVelocity);
 
-                // one phase with constant velocity
-                float remainingjointMovementAbs = jointMovementAbs(j);
-                // v * t = s
-                remainingjointMovementAbs -= maxJointLocalVelocity(j) * minStepsPerJoint(j) * sampleTimeMs;
+        Vec<float,SIZE> accelerationPhaseTime = maxJointVelocity.celldivide(maxJointAcceleration)/sampleTime;
+        Vec<float,SIZE> jointMovementRemaining = jointMovementAbs - maxJointVelocity.cellmultiply(maxJointVelocity).celldivide(maxJointAcceleration);
+        Vec<float,SIZE> maxVelocityPhaseThime = jointMovementRemaining.celldivide(maxJointLocalVelocity);
         
-                // s / v = t 
-                minStepsPerJoint(j) += remainingjointMovementAbs/maxJointLocalVelocity(j);
-            }
-        }
+        Vec<float,SIZE> 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<SIZE>::trajectoryNode* ) calloc(sizeof(struct Trajectory<SIZE>::trajectoryNode),this->steps);
 
         Vec<float,SIZE> jointLast  = jointStart;
+        Vec<float,SIZE> velocityLast(0.0f);
 
-        // calculate thime of max speed reaching
-        Vec<float,SIZE> deltaToMaxSpeed(0.0f);
-        Vec<float,SIZE> currJointLocalAcceleration(0.0f);
-        for (int j=0; j<SIZE; j++)
-        {
-            // ceil steps/2 - sqrt(steps^2 - jointmovementabs/maxaccelaration)
-            deltaToMaxSpeed(j) = ceil( this->steps /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<float,SIZE> currentInk(0.0f);
-        Vec<float,SIZE> currentInkLast(0.0f);
-        Vec<float,SIZE> currentDist(0.0f);
-        Vec<float,SIZE> currentDistLast(0.0f);
+        // percentage of max velocity 
+        //Vec<float,SIZE> currJointMovementOfMax = minStepsPerJoint / minStepsPerJoint.max() ;
+        // s =  a* t^2 / 2 => a = s* 2 / t^2
+        Vec<float,SIZE> currMaxAcceleration = (maxJointLocalAcceleration).cellmultiply(minStepsPerJoint) / minStepsPerJoint.max();
+        Vec<float,SIZE> currMaxVelocity     = (maxJointLocalVelocity ).cellmultiply(minStepsPerJoint) / minStepsPerJoint.max();
+        std::cout << "max velo curr : " << currMaxVelocity << "\n";
 
-        for (int currStep=0; currStep<this->steps; currStep++)
+        for( int i = 0 ; i < this->steps; ++i)
         {
-            for (int currJoint=0; currJoint<SIZE; currJoint++)
+            for (int joint = 0 ; joint < SIZE; ++joint)
             {
-                if (currStep+1 <= this->steps/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);
+                    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<float,SIZE>(0.0f); 
+            //std::cout << i << this->nodes[i].velocity << this->nodes[i].jointPos <<"\n";
+            jointLast = this->nodes[i].jointPos;
+            velocityLast = this->nodes[i].velocity;
+        }
     }
 };
 

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)

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);

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 

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 

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 

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