first working move point to point

lwrserv/BangBangTrajectroy.cpp

@@ -1,22 +0,0 @@
-#include <stdlib.h>
-#include <math.h>
-#include "Trajectroy.h"
-#include "BangBangTrajectroy.h"
-#include "vec.h"
-#include "SvrData.h"
-
-template <unsigned SIZE> 
-BangBangJointTrajectroy<SIZE>::BangBangJointTrajectroy(
-        unsigned int steps_,
-        float totalTime_,
-        Vec<float,SIZE> jointMovement
-        )
-{
-    Vec< float, SIZE> velocity = jointMovement / steps_ * 1.5f;
-    //TODO
-    LinearJointTrajectory(
-            steps_,
-            totalTime_,
-            jointMovement,
-            velocity);
-}

lwrserv/LinearTrajectory.cpp

@@ -1,59 +0,0 @@
-
-#if 0 
-template <unsigned SIZE> 
-LinearJointTrajectory<SIZE>::LinearJointTrajectory(
-        unsigned int steps_,
-        float totalTime_,
-        Vec<float,SIZE> jointMovement,
-        Vec<float,SIZE> velocity
-        )
-{
-    Vec< float, SIZE> velocity = jointMovement_ / steps_ * 1.5f;
-    super(steps_, totalTime_, jointMovement);
-
-    if ( velocity > jointMovement/totalTime_)
-    {
-        std::cerr << "velocity is to small for Trajectory\n";
-    }
-
-    if ( velocity < (2.0f*jointMovement)/totalTime_)
-    {
-        std::cerr << "velocity is to big for Trajectory\n";
-    }
-
-    for (unsigned int currJoint = 0 ; currJoint < SIZE; currJoint++)
-    {
-        unsigned int timeBlend = (jointMovement(currJoint) + velocity(currJoint)*totalTime_)/ velocity(currJoint);
-        float acceleration =  velocity(currJoint) / timeBlend;
-
-        for (unsigned int currStep = 0 ; currStep < steps_ ; ++currStep)
-        {
-            float currentTime = currStep * totalTime_ / steps_;
-            if (currentTime <= timeBlend)
-            {
-                // speed up till blend time is reached 
-                this->nodes[currStep].jointPos     = acceleration/2.0f * currentTime * currentTime;
-                this->nodes[currStep].velocity     = acceleration * currentTime;
-                this->nodes[currStep].acceleration = acceleration;
-            } else
-            if ( currentTime <= totalTime_ - timeBlend)
-            {
-                // constant velocity 
-                this->nodes[currStep].jointPos     = (jointMovement(currJoint) - velocity(currJoint) * totalTime_)/2.0f + velocity(currJoint) * currentTime;
-                this->nodes[currStep].velocity     = velocity(currJoint);
-                this->nodes[currStep].acceleration = 0.0f;
-            }
-            else
-            {
-                // slow down until aim is reached 
-                // speed up till blend time is reached 
-                this->nodes[currStep].jointPos     = jointMovement(currJoint) - acceleration/2*totalTime_*totalTime_ + acceleration * totalTime_ * currentTime - acceleration/2.0f * currentTime *currentTime;
-                this->nodes[currStep].velocity     = acceleration*timeBlend - acceleration * currentTime ;
-                this->nodes[currStep].acceleration = -acceleration;
-            }
-            // calculate Cartesian positions
-            this->nodes[currStep].cartPos = 0;
-        }
-    }
-}
-#endif

lwrserv/SvrData.cpp

@@ -343,7 +343,16 @@ class Trajectory<JOINT_NUMBER>*  SvrData::popTrajectory()
 {
     class Trajectory<JOINT_NUMBER>* retval = NULL;
     pthread_mutex_lock(&dataLock);
+    if (!trajectory.list.empty() )
+    {
         retval = trajectory.list.front();
+        trajectory.list.pop();
+    }
+    else
+    {
+        // no trajectroy left
+        trajectory.done = true;
+    }
     pthread_mutex_unlock(&dataLock);
     return retval;
 }
@@ -354,6 +363,7 @@ int SvrData::pushTrajectory(class Trajectory<JOINT_NUMBER>* newTrajectory)
 
     pthread_mutex_lock(&dataLock);
     trajectory.list.push(newTrajectory);
+    trajectory.done = false;
     pthread_mutex_unlock(&dataLock);
     return 0;
 }
@@ -412,10 +422,19 @@ bool SvrData::getTrajectoryDone()
     pthread_mutex_unlock(&dataLock);
     return retval;
 }
-void SvrData::setTrajectoryDone(bool newState)
+
+enum TrajectoryType SvrData::getTrajectoryType()
+{
+    enum TrajectoryType retval = TrajectoryDefault;
+    pthread_mutex_lock(&dataLock);
+    retval = trajectory.type;
+    pthread_mutex_unlock(&dataLock);
+    return retval;
+}
+void SvrData::setTrajectoryType(enum TrajectoryType newType)
 {
     pthread_mutex_lock(&dataLock);
-    trajectory.done = newState;
+    trajectory.type = newType;
     pthread_mutex_unlock(&dataLock);
 }
 

lwrserv/SvrHandling.cpp

@@ -330,7 +330,6 @@ void SvrHandling::run()
 void SvrHandling::run(int port)
 {
     cout << timestamp() + "Starting " << SVR_NAME << " on port "<< port <<"\n";
-    port = SVR_DEFAULT_PORT;
     while (c_state !=done)
     {
         SocketObject *socket = new SocketObject;
@@ -393,8 +392,6 @@ void SvrHandling::clientCommandLoop(SocketObject& client)
             cout<<message;
             StringTool::String2CmdArg((const string) message, cmd, arg);
 
-            //SvrData::getInstance().lock();
-
             if (cmd == CMD_GetPositionJoints)
             {
                 GetPositionJoints(client);
@@ -437,22 +434,29 @@ void SvrHandling::clientCommandLoop(SocketObject& client)
                 debug(client);
             } else if (cmd == "") 
             {
+                // ignore blank lines
                 continue;
+            } else if (cmd == CMD_ISKUKA)
+            {
+                client.Send(SVR_TRUE_RSP);
+            } else if (cmd == CMD_SET_TRAJECTORY_TYPE)
+            {
+                setTrajectoryType(client, arg);
+            } else if (cmd == CMD_GET_TRAJECTORY_TYPE)
+            {
+                getTrajectoryType(client);
             } else if (cmd == CMD_HELP)
             {
                 printUsage(client);
             } else if (cmd == CMD_QUIT || cmd == CMD_QUIT1 || cmd == CMD_QUIT2)
             {
                 quit(client);
-            } else if (cmd == CMD_ISKUKA)
-            {
-                client.Send(SVR_TRUE_RSP);
             } else 
             {
                 message = "Error: Unknown command!";
                 client.Send(message);
+                printUsage(client);
             }
-            //SvrData::getInstance().unlock();
         }else 
         {
             cout << timestamp() + "Connection to client lost..\n";
@@ -537,7 +541,6 @@ void SvrHandling::GetForceTorqueTcp(SocketObject& client)
 
 void SvrHandling::MovePTPJoints(SocketObject& client, string& args)
 {
-    //__SVR_CURRENT_JOB = true;
     string buf;
 
     stringstream ss(args);
@@ -560,6 +563,7 @@ void SvrHandling::MovePTPJoints(SocketObject& client, string& args)
         ss2f >> newJointPos(i);
         i++;
     }
+
     // to less joint values
     if (i!=JOINT_NUMBER)
     {
@@ -588,6 +592,8 @@ void SvrHandling::MovePTPJoints(SocketObject& client, string& args)
     // add trajectory to queue for sender thread
     SvrData::getInstance()->pushTrajectory(newTrajectory);
     
+    SvrData::getInstance()->setCommandedJointPos(newJointPos);
+
     //Wait to end of movement
     while (true)
     {
@@ -1004,6 +1010,24 @@ void SvrHandling::quit(SocketObject& client)
     client.Send(SVR_TRUE_RSP);
     client.Disconnect();
 }
+void SvrHandling::setTrajectoryType(SocketObject& client, string& arg)
+{
+    string buf;
+    enum TrajectoryType newType = TrajectoryDefault;
+
+    newType = toEnum(arg);
+    SvrData::getInstance()->setTrajectoryType(newType);
+    cout << "new trajectory type : "<<  toString(newType) << "\n";
+    
+    client.Send(SVR_TRUE_RSP);
+}
+void SvrHandling::getTrajectoryType(SocketObject& client)
+{
+    enum TrajectoryType currType =  SvrData::getInstance()->getTrajectoryType();
+    cout << "curr trajectory type : "<<  toString(currType) << "\n";
+
+    client.Send("current trajectory: " + toString(currType));
+}
 
 void SvrHandling::debug(SocketObject& client)
 {

lwrserv/SvrHandling.h

@@ -16,9 +16,10 @@
 #define SVR_TRUE_RSP		"true"
 #define SVR_FALSE_RSP		"false"
 // server error messages 
-#define SVR_INVALID_ARGUMENT_COUNT  "The argument count is not correct."
-#define SVR_OUT_OF_RANGE            "At least one argument is out of Range."
-#define SVR_HELP_MSG                "GPJ - GetPositionJoints \n"
+#define SVR_INVALID_ARGUMENT        "ERROR: The argument is not correct."
+#define SVR_INVALID_ARGUMENT_COUNT  "ERROR: The argument count is not correct."
+#define SVR_OUT_OF_RANGE            "ERROR: At least one argument is out of Range."
+#define SVR_HELP_MSG                "GPJ - GetPositionJoints " 
 
 //Begin SVR_COMMANDS
 #define CMD_GetPositionJoints			"GPJ"
@@ -38,6 +39,8 @@
 #define CMD_HELP                        "help"
 #define CMD_ISKUKA						"IsKukaLWR"
 #define CMD_MoveCartesian				"MC"
+#define CMD_SET_TRAJECTORY_TYPE         "STRAJ"
+#define CMD_GET_TRAJECTORY_TYPE         "GTRAJ"
 //End SVR_COMMANDS
 
 typedef enum {waiting, handshake, accepted, done} svr_state;
@@ -75,6 +78,9 @@ private:
     //Cartesian Movement
     //Move to given POSE position
     void MoveCartesian(SocketObject& client, std::string& args);
+    // set a specific trajectory type
+    void setTrajectoryType(SocketObject& client, std::string& arg);
+    void getTrajectoryType(SocketObject& client);
     //Quit
     void quit(SocketObject& client);
     //DEBUGGING

lwrserv/include/LinearTrajectory.h

@@ -18,10 +18,12 @@ class LinearJointTrajectory : public Trajectory<SIZE>
                 Vec<float,SIZE> jointEnd
                 )
         {
+            float MStoSec = 1000.0f;
             // calculate maximum velocity and acceleration
-            Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * sampleTimeMs;
-            Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * sampleTimeMs;
+            Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec);
+            Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec);
 
+            std::cout << "maxv : " << maxJointLocalVelocity << "\n";
             // calculate delta movement
             Vec<float,SIZE> jointMovement = jointEnd - jointStart;
             Vec<float,SIZE> jointMovementAbs = jointMovement.abs();
@@ -31,21 +33,25 @@ class LinearJointTrajectory : public Trajectory<SIZE>
             // calculate number of movement steps
             Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity);
             this->steps = ceil(minStepsPerJoint.max());
+            if (this->steps == 0)
+                this->steps +=1;
 
-            this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) malloc(sizeof(struct Trajectory<SIZE>::trajectoryNode)*this->steps);
+            std::cout << "steps : " << this->steps << minStepsPerJoint<< "\n";
+
+            this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) calloc(sizeof(struct Trajectory<SIZE>::trajectoryNode),this->steps);
 
             Vec<float,SIZE> jointLast  = jointStart;
             for( int i = 0 ; i < this->steps; ++i)
             {
                 jointLast = jointLast + jointMovement.celldivide((float) this->steps );
-                this->nodes[0].jointPos = jointLast;
-                this->nodes[0].velocity = maxJointLocalVelocity; 
-                this->nodes[0].acceleration;
-            }
+                this->nodes[i].jointPos = jointLast;
+                this->nodes[i].velocity = maxJointLocalVelocity; 
+                this->nodes[i].acceleration = Vec<float,SIZE>(0.0f);
             }
-        ~LinearJointTrajectory()
-        {
-            free (this->nodes);
+            // 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/SvrData.h

@@ -120,7 +120,6 @@ public:
     void trajectoryCancelDone();
     
     bool getTrajectoryDone();
-    void setTrajectoryDone(bool newState);
     
     enum TrajectoryType getTrajectoryType();
     void setTrajectoryType(enum TrajectoryType newType);

lwrserv/include/Trajectroy.h

@@ -2,8 +2,15 @@
 #define _TRAJECTORY_H_
 #include "vec.h"
 #include <stdlib.h>
+#include <string>
+
+
+template <unsigned SIZE> class Trajectory;
 
 // all types of trajectories
+/// START OF TRAJECTORY LIST
+#include "LinearTrajectory.h"
+
 enum TrajectoryType {
     TrajectoryDefault     = 0,
     TrajectoryJointLinear = 0,
@@ -18,6 +25,86 @@ enum TrajectoryType {
     TrajectoryJointLSPB,
     TrajectoryCartLSPB
 };
+static struct {
+    enum TrajectoryType type;
+    std::string str;
+} TrajectoryTypeStr[] =
+{
+    {  .type = TrajectoryDefault     , .str="default" },
+
+    {  .type = TrajectoryJointLinear  , .str="JointLinear" },
+    {  .type = TrajectoryJointLSPB    , .str="JointLSPB" },
+    {  .type = TrajectoryJointBangBang, .str="JointBangBang" },
+    {  .type = TrajectoryJointFivePoly, .str="JointFivePoly" },
+
+    {  .type = TrajectoryCartLinear  , .str="CartLinear" },
+    {  .type = TrajectoryCartLSPB    , .str="CartLSPB" },
+    {  .type = TrajectoryCartBangBang, .str="CartBangBang" },
+    {  .type = TrajectoryCartFivePoly, .str="CartFivePoly" },
+};
+/// END OF TRAJECTORY LIST
+
+
+
+static std::string toString(enum TrajectoryType type)
+{
+    int items = sizeof(TrajectoryTypeStr) / sizeof(TrajectoryTypeStr[0]);
+    for (int i = 0 ; i < items ; ++i)
+    {
+        if (TrajectoryTypeStr[i].type == type)
+            return ""+ TrajectoryTypeStr[i].str;
+    }
+    return "default";
+}
+static enum TrajectoryType toEnum(std::string name)
+{
+    int items = sizeof(TrajectoryTypeStr) / sizeof(TrajectoryTypeStr[0]);
+    std::cout <<"items "<< items <<"\n";
+    for (int i = 0 ; i < items ; ++i)
+    {
+        if (TrajectoryTypeStr[i].str == name)
+            return TrajectoryTypeStr[i].type;
+    }
+    return TrajectoryDefault;
+}
+template <unsigned SIZE>
+class Trajectory<SIZE>* calculateTrajectory(enum TrajectoryType type,
+            float sampleTimeMs,
+            Vec<float,SIZE> maxJointVelocity,
+            Vec<float,SIZE> maxJointAcceleration,
+            Vec<float,SIZE> jointStart,
+            Vec<float,SIZE> jointEnd)
+
+{
+    class Trajectory<SIZE>* retval = NULL;
+    switch(type)
+    {
+        case TrajectoryJointLinear:
+            retval = (class Trajectory<SIZE>*) new LinearJointTrajectory<SIZE>(
+                    sampleTimeMs,
+                    maxJointVelocity,
+                    maxJointAcceleration,
+                    jointStart,
+                    jointEnd); 
+            break;
+        case TrajectoryCartLinear:
+        case TrajectoryJointBangBang:
+            retval = (class Trajectory<SIZE>*) new LinearJointTrajectory<SIZE>(
+                    sampleTimeMs,
+                    maxJointVelocity,
+                    maxJointAcceleration,
+                    jointStart,
+                    jointEnd); 
+            break;
+        case TrajectoryCartBangBang:
+        case TrajectoryJointFivePoly:
+        case TrajectoryCartFivePoly:
+        case TrajectoryJointLSPB:
+        case TrajectoryCartLSPB:
+            break;
+    }
+    return retval;
+}
 enum MovementType
 {
     MovementJointBased= 0,
@@ -52,7 +139,8 @@ class Trajectory
 
             totalTime = steps * sampleTimeMs;
 
-            nodes = (struct trajectoryNode* ) malloc(sizeof(struct trajectoryNode)*steps);
+            //this->nodes = new struct trajectoryNode [steps];
+            this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) calloc(sizeof(struct Trajectory<SIZE>::trajectoryNode),this->steps);
 
             nodes[0].jointPos = jointEnd;
             nodes[0].velocity = (jointEnd-jointStart)/sampleTimeMs;
@@ -64,9 +152,12 @@ class Trajectory
             (void)maxJointAcceleration;
         }
 
-        ~Trajectory()
+        virtual ~Trajectory()
         {
-            free(nodes);
+            std::cout << " test1 \n ";
+            std::cout << nodes[steps-1].jointPos << "\n";
+            free (nodes);
+            std::cout << " test3 \n "<<std::endl;
         }
 
         unsigned int getSteps()
@@ -96,9 +187,11 @@ class Trajectory
         {
             Vec<float,SIZE> retval(0.0f);
             unsigned int pos = currentStep;
+            if (steps == 0)
+                return retval;
             if (pos >= steps)
             {
-                pos = steps;
+                pos = steps-1;
             }
 
             if (nodes != NULL && movementType == MovementJointBased)
@@ -115,9 +208,11 @@ class Trajectory
         {
             Mat<float,4> retval(0.0f,1.0f);
             unsigned int pos = currentStep;
+            if (steps == 0)
+                return retval;
             if (pos >= steps)
             {
-                pos = steps;
+                pos = steps-1;
             }
 
             if (nodes != NULL && movementType == MovementCartBased)
@@ -134,6 +229,8 @@ class Trajectory
         Vec<float,SIZE> getJointPos         (unsigned int step)
         {
             Vec<float,SIZE> retval(0.0f);
+            if (steps == 0)
+                return retval;
             if (step >= steps)
             {
                 return retval;
@@ -150,6 +247,8 @@ class Trajectory
         Mat<float,4>    getCartPos          (unsigned int step)
         {
             Vec<float,SIZE> retval(0.0f);
+            if (steps == 0)
+                return retval;
             if (step >= steps)
             {
                 return retval;
@@ -165,6 +264,8 @@ class Trajectory
         Vec<float,SIZE> getJointVelocity    (unsigned int step)
         {
             Vec<float,SIZE> retval(0.0f);
+            if (steps == 0)
+                return retval;
             if (step >= steps)
             {
                 return retval;
@@ -180,6 +281,8 @@ class Trajectory
         Vec<float,SIZE> getJointAcceleration(unsigned int step)
         {
             Vec<float,SIZE> retval(0.0f);
+            if (steps == 0)
+                return retval;
             if (step >= steps)
             {
                 return retval;

lwrserv/include/vec.h

@@ -4,7 +4,6 @@
 #include <ostream>
 #include <cmath>
 #include "mat.h"
-#include "sgn.h"
 
 template<class T, unsigned SIZE> class Vec;
 template <class T, unsigned SIZE> class Mat;

lwrserv/program.cpp

@@ -214,12 +214,20 @@ void *threadRobotMovement(void *arg)
             // no new trajectory 
             if(currentTrajectory == NULL)
             {
+                // mark that we reached the end of the trajectory
                 // stay on current position
+                cout << "end of trajectorys \n";
                 goto end;
             }
+            else
+            {
+                std::cout <<"new Trajectory with " << currentTrajectory->getSteps()<<"\n";
+            }
         }
 
         // get next commanded trajectory type
+        if (currentTrajectory != NULL)
+        {
             switch (currentTrajectory->getMovementType())
             {
                 case MovementCartBased:
@@ -231,8 +239,10 @@ void *threadRobotMovement(void *arg)
                 case MovementJointBased:
                 {
                     currentJointPos = currentTrajectory->getNextJointPos();
+                    
                     currentMovementType = MovementJointBased;
                 }
+                break;
                 default:
                 {
                     // invalid trajectory skip it
@@ -242,6 +252,7 @@ void *threadRobotMovement(void *arg)
                 }
                 break;
             }
+        }
 
 end:
         // TODO stop timer for last sent message 
@@ -254,7 +265,7 @@ end:
         float waitingTimeMs = sampleTimeMs - calculationTimeMs;
 
         // sleep the calculated waiting time
-        boost::this_thread::sleep( boost::posix_time::milliseconds(waitingTimeMs) );
+        boost::this_thread::sleep( boost::posix_time::milliseconds(waitingTimeMs+100.0f) );
         
 
         switch (currentMovementType)
@@ -264,7 +275,8 @@ end:
                     // send the new joint values
                     float newJointPosToSend[JOINT_NUMBER] = {0.0f};
                     currentJointPos.getData(newJointPosToSend);
-                    friInst->doPositionControl(newJointPosToSend);
+                    std::cout << currentJointPos << "\n";
+                    //friInst->doPositionControl(newJointPosToSend);
                 }
                 break;
             case MovementCartBased:
@@ -282,10 +294,15 @@ end:
 
         // start timer for the last sent msg
         // TODO 
+        if(currentTrajectory != NULL)
+        {
+            cout << "remaining Steps = " << currentTrajectory->getRemainingSteps()<<"\n";
+        }
         
         // check if current trajectory is over 
         if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0)
         {
+            cout << " Trajectory finished \n ";
             // invalid trajectory skip it
             delete currentTrajectory;
             currentTrajectory = NULL;