fix acceleration bug for bang bang

lwrserv/SvrData.cpp

@@ -24,7 +24,7 @@ SvrData::SvrData(void)
     double maxTrqJnt[] = {176.0,176.0,100.0,100.0,100.0,38.0,38.0};
     double maxRangeJnt[] = {170.0,120.0,170.0,120.0,170.0,120.0,170.0};
 
-    robot.currentVelocityPercentage = 20;
+    robot.currentVelocityPercentage = 10;
     robot.currentAccelerationPercentage = 10;
 
     for (unsigned int i = 0; i < JOINT_NUMBER; ++i)

lwrserv/include/BangBangTrajectory.h

@@ -33,12 +33,14 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
             // 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
+            // s = a * t^2 / 2
             Vec<float,SIZE> minBangBangTime = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration) / sampleTime;
             //TODO check if mintime is neceesary
+            Vec<float,SIZE> time = ((jointMovementAbs/2.0f).celldivide(maxJointAcceleration) * 2.0f).sqrt() * 2.0f / sampleTime;
+            std::cout << time << "time \n ";
             std::cout << minBangBangTime  << "minBangBangTime \n ";
-            
 
-            Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f;
+            Vec<float,SIZE> minStepsPerJoint = time;//jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f;
             minStepsPerJoint = minStepsPerJoint.ceil();
             std::cout << minStepsPerJoint << "minStepsPerJoint \n";
 
@@ -54,15 +56,14 @@ 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 = 1.0f;
-            Vec<float,SIZE> currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(this->steps).celldivide(this->steps)*2.0f*floaterror;
+            Vec<float,SIZE> currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(this->steps).celldivide(this->steps)*2.0f;
             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;

lwrserv/include/Trajectroy.h

@@ -5,6 +5,9 @@
 #include <iostream>
 #include <fstream>
 
+#include <iomanip>
+#include <cmath>
+#include <limits>
 template <unsigned SIZE> class Trajectory;
 
 // all types of trajectories
@@ -308,7 +311,7 @@ class Trajectory
                 {
                     if (currentJoint != 0)
                         file << ", ";
-                    file << nodes[currentStep].jointPos(currentJoint);
+                    file << std::fixed << std::setprecision(20) << nodes[currentStep].jointPos(currentJoint);
                 }
                 file << "\n";
             }