-fix several bugs

-add trajectory default constructor
-add lspb trajectory

lwrserv/BangBangTrajectroy.cpp

@@ -0,0 +1,32 @@
+#include <stdlib.h>
+#include <math.h>
+#include "Trajectroy.h"
+#include "BangBangTrajectroy.h"
+#include "vec.h"
+#include "SvrData.h"
+
+template <class T, unsigned SIZE> 
+BangBangJointTrajectroy::BangBangJointTrajectroy(
+        unsigned int steps_,
+        float totalTime_,
+        Vec<float,SIZE> jointMovement,
+        Vec<float,SIZE> maxJointRange,
+        Vec<float,SIZE> maxJointVelocityPerStep,
+        Vec<float,SIZE> maxJointAccelarationPerStep
+        )
+{
+    super(steps_, totalTime_,  jointMovement, maxJointVelocityPerStep, maxJointAccelarationPerStep);
+
+    //unused 
+    (void) maxJointRange;
+    Vec<float,LBR_MNJ> currentInk(0.0f);
+    Vec<float,LBR_MNJ> currentInkLast(0.0f);
+    Vec<float,LBR_MNJ> currentDist(0.0f);
+    Vec<float,LBR_MNJ> currentDistLast(0.0f);
+    for (unsigned int currStep = 0 ; currStep < steps ; ++currStep)
+    {
+        for (unsigned int currJoint = 0 ; currJoint < joints ; currJoint++)
+        {
+        }
+    }
+}

lwrserv/LinearTrajectory.cpp

@@ -0,0 +1,80 @@
+#include "LinearTrajectory.h"
+#include "vec.h"
+#include "stdlib.h"
+
+template <class T, unsigned SIZE> 
+LinearJointTrajectory::LinearJointTrajectory(
+        unsigned int steps_,
+        float totalTime_,
+        Vec<float,SIZE> jointMovement,
+        Vec<float,SIZE> maxJointVelocityPerStep,
+        Vec<float,SIZE> maxJointAccelarationPerStep
+        )
+{
+    Vec< float, SIZE> velocity = jointMovement / steps * 1.5f;
+    LinearJointTrajectory(
+            steps_,
+            totalTime_,
+            jointMovement,
+            maxJointVelocityPerStep,
+            maxJointAccelarationPerStep,
+            velocity);
+}
+
+template <class T, unsigned SIZE> 
+LinearJointTrajectory::LinearJointTrajectory(
+        unsigned int steps_,
+        float totalTime_,
+        Vec<float,SIZE> jointMovement,
+        Vec<float,SIZE> maxJointVelocityPerStep,
+        Vec<float,SIZE> maxJointAccelarationPerStep,
+        Vec<float,SIZE> velocity
+        )
+{
+    super(steps_, totalTime_, jointMovement, maxJointVelocityPerStep, maxJointAccelarationPerStep);
+
+    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 < joints ; 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 
+                nodes[currStep].jointPos     = acceleration/2.0f * currentTime * currentTime;
+                nodes[currStep].velocity     = acceleration * currentTime;
+                nodes[currStep].acceleration = acceleration;
+            } else
+            if ( currentTime <= totalTime - timeBlend)
+            {
+                // constant velocity 
+                nodes[currStep].jointPos     = (jointMovement(currJoint) - velocity(currJoint) * totalTime)/2.0f + velocity(currJoint) * currentTime;
+                nodes[currStep].velocity     = velocity(currJoint);
+                nodes[currStep].acceleration = 0.0f;
+            }
+            else
+            {
+                // slow down untill aim is reached 
+                // speed up till blend time is reached 
+                nodes[currStep].jointPos     = jointMovement(currJoint) - acceleration/2*totalTime*totalTime + acceleration * totalTime * currentTime - acceleration/2.0f * currentTime *currentTime;
+                nodes[currStep].velocity     = acceleration*timeBlend - acceleration * currentTime ;
+                nodes[currStep].acceleration = -acceleration;
+            }
+            // calculate  carthesian positions
+            nodes[currStep].cartPos = 0;
+        }
+    }
+}

lwrserv/SvrData.cpp

@@ -290,7 +290,7 @@ Vec<float, LBR_MNJ> SvrData::getMaxRange()
     pthread_mutex_lock(&dataLock);
     retval = robot.max.range;
     pthread_mutex_unlock(&dataLock);
-    return 0;
+    return retval;
 }
 
 int SvrData::setRobotVelocity(float newVelocity)

lwrserv/Trajectroy.cpp

@@ -1 +1,29 @@
+#include <stdlib.h>
 #include "Trajectroy.h"
+#include "vec.h"
+
+template <class T, unsigned SIZE> 
+Trajectory::Trajectory(
+        unsigned int steps_,
+        float totalTime_,
+        Vec<float,SIZE> jointMovement,
+        Vec<float,SIZE> maxJointVelocityPerStep,
+        Vec<float,SIZE> maxJointAccelarationPerStep
+        )
+{
+    steps = steps_;
+    joints = SIZE;
+    totalTime = totalTime_;
+
+    nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps_);
+
+    // unused
+    (void) jointMovement;
+    (void) maxJointVelocityPerStep;
+    (void) maxJointAccelarationPerStep;
+}
+
+Trajectory::~Trajectory()
+{
+    free(nodes);
+}

lwrserv/include/BangBangTrajectroy.h

@@ -1,14 +1,21 @@
 #ifndef _BANGBANGTRAJCETORY_H_
 #define _BANGBANGTRAJCETORY_H_
 #include "Trajectroy.h"
+#include "vec.h"
 
 class Trajectory;
 class BangBangJointTrajectroy : public Trajectory
 {
     public:
-        BangBangJointTrajectroy();
-        BangBangJointTrajectroy(unsigned int steps);
-        BangBangJointTrajectroy(unsigned int steps, unsigned int joints);
+        template <class T, unsigned SIZE> 
+        BangBangJointTrajectroy(
+                unsigned int steps_,
+                float totalTime_,
+                Vec<float,SIZE> jointMovement,
+                Vec<float,SIZE> maxJointRange,
+                Vec<float,SIZE> maxJointVelocityPerStep,
+                Vec<float,SIZE> maxJointAccelarationPerStep);
+
         ~BangBangJointTrajectroy();
     private:
 
@@ -17,7 +24,6 @@ class BangBangJointTrajectroy : public Trajectory
 };
 class BangBangCartTrajectory: public Trajectory
 {
-
 };
 
 #endif 

lwrserv/include/LinPolyTrajectory.h

@@ -1,21 +0,0 @@
-#ifndef _LINPOLYTRAJECTORY_H_
-#define _LINPOLYTRAJECTORY_H_
-#include "Trajectroy.h"
-
-class LinPolyJointTrajectory: public Trajectory
-{
-    public:
-        LinPolyJointTrajectory();
-        BangBangJointTrajectroy(unsigned int steps);
-        BangBangJointTrajectroy(unsigned int steps, unsigned int joints);
-        ~BangBangJointTrajectroy();
-    private:
-
-    protected:
-};
-class LinPolyCartTrajectory: public Trajectory
-{
-};
-
-
-#endif

lwrserv/include/LinearTrajectory.h

@@ -0,0 +1,34 @@
+#ifndef _LINPOLYTRAJECTORY_H_
+#define _LINPOLYTRAJECTORY_H_
+#include "Trajectroy.h"
+
+class LinearJointTrajectory: public Trajectory
+{
+    public:
+
+        template <class T, unsigned SIZE>
+        LinearJointTrajectory(
+                unsigned int steps_,
+                float totalTime_,
+                Vec<float,SIZE> jointMovement,
+                Vec<float,SIZE> maxJointVelocityPerStep,
+                Vec<float,SIZE> maxJointAccelarationPerStep);
+        template <class T, unsigned SIZE>
+        LinearJointTrajectory(
+                unsigned int steps_,
+                float totalTime_,
+                Vec<float,SIZE> jointMovement,
+                Vec<float,SIZE> maxJointVelocityPerStep,
+                Vec<float,SIZE> maxJointAccelarationPerStep,
+                Vec<float,SIZE> velocity);
+        ~LinearJointTrajectory();
+    private:
+
+    protected:
+};
+class LinearCartTrajectory: public Trajectory
+{
+};
+
+
+#endif

lwrserv/include/Trajectroy.h

@@ -1,29 +1,46 @@
 #ifndef _TRAJECTORY_H_
 #define _TRAJECTORY_H_
+#include "vec.h"
 
 class Trajectory
 {
     public:
-        Trajectory();
-        Trajectory(unsigned int steps);
-        Trajectory(unsigned int steps, unsigned int joints);
+        template <class T, unsigned SIZE> 
+        Trajectory (
+            unsigned int steps_,
+            float totalTime_,
+            Vec<float,SIZE> jointMovement,
+            Vec<float,SIZE> maxJointVelocityPerStep,
+            Vec<float,SIZE> maxJointAccelarationPerStep
+            );
         ~Trajectory();
 
-        unsigned int update();
-
         unsigned int getSteps();
         unsigned int getJoints();
 
-        float getJointPos(unsigned int index, unsigned int joint);
-        float getCartPos(unsigned int index, unsigned int joint);
-        float getVelocity(unsigned int index, unsigned int joint);
-        float getAcceleration(unsigned int index, unsigned int joint);
+        /**
+         *  
+         */
+        template <class T, unsigned SIZE> Vec<float,SIZE> getJointPos    (unsigned int step);
+        template <class T, unsigned SIZE> Vec<float,SIZE> getCartPos     (unsigned int step);
+        template <class T, unsigned SIZE> Vec<float,SIZE> getVelocity    (unsigned int step);
+        template <class T, unsigned SIZE> Vec<float,SIZE> getAcceleration(unsigned int step);
+        
+        struct trajectoryNode
+        {
+            float jointPos;
+            float cartPos;
+            float velocity;
+            float acceleration;
+        };
     protected:
         static const unsigned int defaultSteps = 100;
-        static const unsigned int defaultJoints = 1;
 
         unsigned int steps;
+        float totalTime;
         unsigned int joints;
+        struct trajectoryNode* nodes;
+
 };
 
 #endif

lwrserv/include/trajectory.h

@@ -1,21 +0,0 @@
-#ifndef _TRAJECTORY_H_
-#define _TRAJECTORY_H_
-
-#include "vec.h"
-
-class Trajectory
-{
-    public:
-        virtual unsigned int getSteps();
-
-        virtual Vec<T, SIZE> getJointValues();
-        virtual Vec<T, SIZE> getJointVelocity(unsigned int jointID);
-        virtual Vec<T, SIZE> getJointAcceleration(unsigned int jointID);
-
-        virtual Vec<T, SIZE> getCarthesianValues();
-        virtual Vec<T, SIZE> getCarthesianVelocity(unsigned int jointID);
-        virtual Vec<T, SIZE> getCarthesianAcceleration(unsigned int jointID);
-};
-
-class TrajectoryBangBang :: Trajectory;
-#endif 

lwrserv/include/vec.h

@@ -20,6 +20,13 @@ public:
         for (unsigned int i=0; i<SIZE; i++)
             m_atData[i] = T(0);
     }
+    
+    // construction with data array
+    Vec<T, SIZE> (const T tData) 
+    {
+        for (unsigned int i=0; i<SIZE; i++)
+            m_atData[i] = tData;
+    }
 
     // construction with data array
     Vec<T, SIZE> (const T atData[SIZE]) 

lwrserv/program.cpp

@@ -352,6 +352,7 @@ void *threadFriDataExchange(void *arg)
 
             for (int j=0; j<LBR_MNJ; j++)
             {
+                // ceil maxsteps/2 - sqrt(maxsteps^2 - deltaAbs/maxaccelaration)
                 dMaxSpeed(j) = ceil( maxSteps /2.0f - sqrt((maxSteps/2.0f)*(maxSteps/2.0f) - deltaAbs(j)/maxAccelarationJoint(j)));
                 if (dMaxSpeed(j) == 0.0f)
                 {
@@ -370,6 +371,7 @@ void *threadFriDataExchange(void *arg)
                 Vec<float,LBR_MNJ> currentInkLast;
                 Vec<float,LBR_MNJ> currentDist;
                 Vec<float,LBR_MNJ> currentDistLast;
+                // bang bang
                 for (int i=0;i<maxSteps;i++)
                 {
                     for (int j=0;j<LBR_MNJ;j++)