fix bangbang

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 

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<JOINT_NUMBER>* 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<JOINT_NUMBER>*) new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); 
+    Trajectory<JOINT_NUMBER>* 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)

lwrserv/include/BangBangTrajectory.h

@@ -15,47 +15,62 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
                 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);
+            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 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<float,SIZE> minBangBangTime = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration) / sampleTime;
+            //TODO check if mintime is neceesary
+            
+
             Vec<float,SIZE> 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<SIZE>::trajectoryNode* ) calloc(sizeof(struct Trajectory<SIZE>::trajectoryNode),this->steps);
 
             Vec<float,SIZE> jointLast  = jointStart;
-            float percentStep = 1.0f / ((this->steps+1)/2);
+            Vec<float,SIZE> velocityLast(0.0f);
+
+            // 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;
+            Vec<float,SIZE> 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;

lwrserv/include/LinearTrajectory.h

@@ -18,6 +18,7 @@ class LinearJointTrajectory : public Trajectory<SIZE>
                 Vec<float,SIZE> jointEnd
                 )
         {
+            std::cout << "linear \n " ;
             float MStoSec = 1000.0f;
             // calculate maximum velocity and acceleration
             Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec);

lwrserv/include/Trajectroy.h

@@ -2,6 +2,8 @@
 #define _TRAJECTORY_H_
 #include "vec.h"
 #include <stdlib.h>
+#include <iostream>
+#include <fstream>
 
 template <unsigned SIZE> 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<float,SIZE> 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;
 

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

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

lwrserv/include/vec.h

@@ -170,6 +170,13 @@ public:
             vec(i) = m_atData[i]*tScale;
         return vec;
     }
+    Vec<T, SIZE> operator / (T tScale) 
+    {
+        Vec<T, SIZE> vec;
+        for (unsigned int i=0; i<SIZE; i++)
+            vec(i) = m_atData[i]/tScale;
+        return vec;
+    }
 
     Vec<T, SIZE> operator * (const Mat<T, SIZE> &mat)
     {
@@ -194,6 +201,20 @@ public:
             buff.m_atData[i] = m_atData[i]*vec.m_atData[i];
         return buff;
     }
+    Vec<T, SIZE> cellmin(const Vec<T, SIZE> & vec) 
+    {
+        Vec<T, SIZE> buff;
+        for (int i=0; i<SIZE; i++)
+            buff.m_atData[i] = std::min(m_atData[i],vec.m_atData[i]);
+        return buff;
+    }
+    Vec<T, SIZE> cellmax(const Vec<T, SIZE> & vec) 
+    {
+        Vec<T, SIZE> buff;
+        for (int i=0; i<SIZE; i++)
+            buff.m_atData[i] = std::max(m_atData[i],vec.m_atData[i]);
+        return buff;
+    }
 
     Vec<T, SIZE> abs ()
     {
@@ -203,6 +224,13 @@ public:
         return buff;
     }
     
+    Vec<T, SIZE> sqrt()
+    {
+        Vec<T, SIZE> buff;
+        for (int i=0; i<SIZE; i++)
+            buff.m_atData[i] = std::sqrt(m_atData[i]);
+        return buff;
+    }
     Vec<T, SIZE> floor ()
     {
         Vec<T, SIZE> buff;
@@ -214,9 +242,19 @@ public:
     {
         Vec<T, SIZE> buff;
         for (int i=0; i<SIZE; i++)
-            buff.m_atData[i] = ceil(m_atData[i]);
+            buff.m_atData[i] = std::ceil(m_atData[i]);
         return buff;
     }
+    float sgn(float x) 
+    { 
+        if ( x == 0 ) 
+            return 0.0f; 
+
+        if ( x > 0)
+            return 1.0f;
+        else
+            return -1.0f; 
+    }
     Vec<T, SIZE> sgn ()
     {
         Vec<T, SIZE> buff;

lwrserv/program.cpp

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

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 

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 

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