fix startup behaviour and also rad/deg failure

lwrserv/SvrData.cpp

@@ -38,7 +38,7 @@ SvrData::SvrData(void)
     if (pthread_mutex_init(&dataLock, NULL) != 0)
         printf("mutex init failed\n");
 
-    this->friInst = new friRemote;
+    this->friInst = new friRemote();
 }
 
 SvrData::~SvrData()
@@ -87,15 +87,19 @@ void SvrData::updateMessurement()
 
     /// do the update using fri
     /// update current joint positions
+    friInst->getState();
+	FRI_STATE state = friInst->getState();
     VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition());
     VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset());
     newJointPos = newJointPosComanded + newJointPosOffset;
+    newJointPos = newJointPos * ( 180.0f / M_PI);
 
     /// update current cart position
     newCartPos = friInst->getMsrCartPosition();
     
     /// update current force and torque
     newForceAndTorque = friInst->getFTTcp();
+    std::cout << newJointPos << " current newJointPos \n" << state << "\n";
 
     updateMessurementData(newJointPos,newCartPos,newForceAndTorque);
 }
@@ -440,3 +444,32 @@ void SvrData::setTrajectoryType(enum TrajectoryType newType)
     pthread_mutex_unlock(&dataLock);
 }
 
+class Trajectory<JOINT_NUMBER>* SvrData::createTrajectory(VecJoint newJointPos)
+{
+    //set new target for next trajectory
+    VecJoint prevJointPos = getCommandedJointPos();
+    class Trajectory<JOINT_NUMBER>* newTrajectory;
+    
+    float sampleTimeMs = getSampleTimeMs();
+    VecJoint maxJointVelocity     =  getRobotVelocity();
+    VecJoint maxJointAcceleration =  getRobotAcceleration();
+    enum TrajectoryType type      =  getTrajectoryType();
+
+    //newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos);
+    newTrajectory = Trajectory<JOINT_NUMBER>::GetTrajectory(type,sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); 
+
+    return newTrajectory;
+}
+class Trajectory<JOINT_NUMBER>* SvrData::createTrajectory(MatCarthesian newJointPos)
+{
+
+}
+VecJoint SvrData::getMeasuredJointPos()
+{
+    VecJoint retval(0.0f);
+    pthread_mutex_lock(&dataLock);
+    retval = messured.jointPos;
+    pthread_mutex_unlock(&dataLock);
+    return retval;
+}
+MatCarthesian getMeasuredCartPos();

lwrserv/SvrHandling.cpp

@@ -383,6 +383,7 @@ SvrHandling::SvrHandling()
     commands[i].longVersion    = "SetTrajectoryType";
     commands[i].processCommand = &setTrajectoryType;
     commands[i].printHelp      = NULL;
+    i+=1;
 
     commandCount = i;
 }
@@ -628,6 +629,7 @@ void movePTPJoints(SocketObject& client, string& arg)
     VecJoint maxJointVelocity     =  SvrData::getInstance()->getRobotVelocity();
     VecJoint maxJointAcceleration =  SvrData::getInstance()->getRobotAcceleration();
 
+    //newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos);
     newTrajectory = (class Trajectory<JOINT_NUMBER>*) new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); 
 
     // add trajectory to queue for sender thread

lwrserv/include/BangBangTrajectroy.h → lwrserv/include/BangBangTrajectory.h

@@ -4,10 +4,10 @@
 #include "vec.h"
 
 template <unsigned SIZE> 
-class BangBangJointTrajectroy : public Trajectory<SIZE>
+class BangBangJointTrajectory : public Trajectory<SIZE>
 {
     public:
-        BangBangJointTrajectroy(
+        BangBangJointTrajectory(
                 float sampleTimeMs,
                 Vec<float,SIZE> maxJointVelocity,
                 Vec<float,SIZE> maxJointAcceleration,
@@ -24,30 +24,44 @@ class BangBangJointTrajectroy : public Trajectory<SIZE>
             Vec<float,SIZE> jointMovement = jointEnd - jointStart;
             Vec<float,SIZE> jointMovementAbs = jointMovement.abs();
 
+            // calculate sample count
+
             // calculate number of movement steps
-            Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity);
+            // one joint has to reach maxvelocity the others are stepped down to 
+            Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f;
             this->steps = ceil(minStepsPerJoint.max());
             if (this->steps == 0)
                 this->steps +=1;
 
-            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;
+            float percentStep = 1.0f / ((this->steps+1)/2);
             for( int i = 0 ; i < this->steps; ++i)
             {
-                jointLast = jointLast + jointMovement.celldivide((float) this->steps );
+                // acceleration phase
+                if (i > this->steps /2 )
+                {
+                    float percent = i * percentStep; 
+                    this->nodes[i].velocity = maxJointLocalVelocity * percent;
+                    this->nodes[i].acceleration = maxJointLocalVelocity * percentStep;
+                }
+                //  deacceleration phase
+                else
+                {
+                    float percent = ( i - this->steps/2 ) * percentStep; 
+                    this->nodes[i].velocity = maxJointLocalVelocity * percent;
+                    this->nodes[i].acceleration = maxJointLocalVelocity * (-1.0 * percentStep);
+                }
+
+                jointLast = jointLast + this->nodes[i].velocity;
                 this->nodes[i].jointPos = jointLast;
-                this->nodes[i].velocity = maxJointLocalVelocity; 
-                this->nodes[i].acceleration = Vec<float,SIZE>(0.0f);
             }
             // 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); 
         }
-        ~BangBangJointTrajectroy();
     private:
 
     protected:

lwrserv/include/FivePolyTrajectory.h

@@ -2,10 +2,12 @@
 #define _FIVEPOLYTRAJECTORY_H_
 #include "Trajectroy.h"
 
-class FivePolyJointTrajectory: public Trajectory
+template <unsigned SIZE>
+class FivePolyJointTrajectory: public Trajectory<SIZE>
 {
 };
-class FivePolyCartTrajectory: public Trajectory
+template <unsigned SIZE>
+class FivePolyCartTrajectory: public Trajectory<SIZE>
 {
 };
 

lwrserv/include/LSPBTrajectory.h

@@ -0,0 +1,127 @@
+#ifndef _LSPBTRAJCETORY_H_
+#define _LSPBTRAJCETORY_H_
+#include <math.h>
+#include "Trajectroy.h"
+#include "sgn.h"
+
+template <unsigned SIZE>
+class LSPBJointTrajectory: public Trajectory<SIZE>
+{
+public:
+    LSPBJointTrajectory(
+            float sampleTimeMs,
+            Vec<float,SIZE> maxJointVelocity,
+            Vec<float,SIZE> maxJointAcceleration,
+            Vec<float,SIZE> jointStart,
+            Vec<float,SIZE> jointEnd
+            )
+    {
+        float MStoSec = 1000.0f;
+        // calculate maximum velocity and acceleration
+        Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec);
+        Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec);
+        std::cout << "max : " << maxJointLocalVelocity << "\n";
+
+        // calculate delta movement
+        Vec<float,SIZE> jointMovement = jointEnd - jointStart;
+        Vec<float,SIZE> jointMovementAbs = jointMovement.abs();
+
+        // calculate number of movement steps
+        
+        // 2 same acceleration and deaceleration phases
+
+        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 ;
+
+                // one phase with constant velocity
+                float remainingjointMovementAbs = jointMovementAbs(j);
+                // v * t = s
+                remainingjointMovementAbs -= maxJointLocalVelocity(j) * minStepsPerJoint(j) * sampleTimeMs;
+
+                // s / v = t 
+                minStepsPerJoint(j) += remainingjointMovementAbs/maxJointLocalVelocity(j);
+            }
+        }
+
+        std::cout << "minsteps : " << minStepsPerJoint << "\n";
+        std::cout << "steps : " << minStepsPerJoint.max()  << "\n";
+        this->steps = ceil(minStepsPerJoint.max());
+        std::cout << "steps : " << this->steps << "\n";
+        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;
+
+        // 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);
+
+        for (int currStep=0; currStep<this->steps; currStep++)
+        {
+            for (int currJoint=0; currJoint<SIZE; currJoint++)
+            {
+                if (currStep+1 <= this->steps/2.0f)
+                {
+                    currentInk(currJoint) = std::min(currentInkLast(currJoint)+maxJointLocalAcceleration(currJoint),maxJointLocalVelocity(currJoint));
+                }else if (currStep+1 > this->steps-deltaToMaxSpeed(currJoint))
+                {
+                    currentInk(currJoint) = std::max(currentInkLast(currJoint)-maxJointLocalAcceleration(currJoint),0.0f);
+                }else
+                {
+                    currentInk(currJoint) = currentInkLast(currJoint);
+                }
+                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);
+            }
+        }
+
+        // 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); 
+    }
+};
+
+template <unsigned SIZE>
+class LSPBCartTrajectory: public Trajectory<SIZE>
+{
+};
+
+#endif

lwrserv/include/LSPBTrajectroy.h

@@ -1,12 +0,0 @@
-#ifndef _LSPBTRAJCETORY_H_
-#define _LSPBTRAJCETORY_H_
-#include "Trajectroy.h"
-
-class LSPBJointTrajectroy: public Trajectory
-{
-};
-class LSPBCartTrajectroy: public Trajectory
-{
-};
-
-#endif

lwrserv/include/SvrData.h

@@ -124,6 +124,8 @@ public:
     enum TrajectoryType getTrajectoryType();
     void setTrajectoryType(enum TrajectoryType newType);
 
+    class Trajectory<JOINT_NUMBER>* createTrajectory(VecJoint newJointPos);
+    class Trajectory<JOINT_NUMBER>* createTrajectory(MatCarthesian newJointPos);
 
     template <unsigned SIZE>
     class Trajectory<SIZE>* calculateTrajectory(

lwrserv/include/Trajectroy.h

@@ -4,12 +4,14 @@
 #include <stdlib.h>
 #include <string>
 
-
 template <unsigned SIZE> class Trajectory;
 
 // all types of trajectories
 /// START OF TRAJECTORY LIST
 #include "LinearTrajectory.h"
+#include "LSPBTrajectory.h"
+#include "BangBangTrajectory.h"
+#include "FivePolyTrajectory.h"
 
 enum TrajectoryType {
     TrajectoryDefault     = 0,
@@ -67,44 +69,6 @@ static enum TrajectoryType toEnum(std::string name)
     }
     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,
@@ -151,6 +115,41 @@ class Trajectory
             (void)maxJointVelocity;
             (void)maxJointAcceleration;
         }
+        static class Trajectory* GetTrajectory(
+            enum TrajectoryType type,
+            float sampleTimeMs,
+            Vec<float,SIZE> maxJointVelocity,
+            Vec<float,SIZE> maxJointAcceleration,
+            Vec<float,SIZE> jointStart,
+            Vec<float,SIZE> jointEnd
+            )
+        {
+            class Trajectory* retval = NULL;
+
+            switch ( type )
+            {
+                case TrajectoryJointLinear:
+                    retval = (class Trajectory<SIZE>*) new LinearJointTrajectory<SIZE>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); 
+                    break;
+                case TrajectoryJointBangBang:
+                    retval = (class Trajectory<SIZE>*) new BangBangJointTrajectory<SIZE>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); 
+                    break;
+                case TrajectoryJointLSPB:
+                    retval = (class Trajectory<SIZE>*) new LSPBJointTrajectory<SIZE>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); 
+                    break;
+                case TrajectoryJointFivePoly:
+                    break;
+
+                case TrajectoryCartLSPB:
+                case TrajectoryCartLinear:
+                case TrajectoryCartBangBang:
+                case TrajectoryCartFivePoly:
+                    // TODO implement carthesian movement
+                    retval = (class Trajectory<SIZE>*) new LinearJointTrajectory<SIZE>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); 
+                    break;
+            }
+            return retval;
+        }
 
         virtual ~Trajectory()
         {

lwrserv/program.cpp

@@ -183,12 +183,28 @@ void *threadRobotMovement(void *arg)
     VecJoint      currentJointPos(0.0f);
     MatCarthesian currentCartPos(0.0f,1.0f);
 
-    while(1)
-    {
-        // get current sample time cause it could be changed
-        float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
     friRemote* friInst = SvrData::getInstance()->getFriRemote();
 
+    // get current robot position
+    int tries = 2;
+    for (int i= 0 ; i < tries ; ++i)
+    {
+        SvrData::getInstance()->updateMessurement();
+        currentJointPos = SvrData::getInstance()->getMeasuredJointPos();
+    
+        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);
+    }
+
+    
+    while(true)
+    {
+
         // check if we have to cancel current trajectory
         bool cancel = SvrData::getInstance()->getTrajectoryCancel();
         if (cancel)
@@ -253,28 +269,16 @@ void *threadRobotMovement(void *arg)
         }
 
 end:
-        // TODO stop timer for last sent message 
-        
-        // time we used to come to this point caused by the calculation and lock time
-        // TODO calculate this time
-        float calculationTimeMs = 0.0f;
-        
-        // wait only the remaining time 
-        float waitingTimeMs = sampleTimeMs - calculationTimeMs;
-
-        // sleep the calculated waiting time
-        boost::this_thread::sleep( boost::posix_time::milliseconds(waitingTimeMs+100.0f) );
-        
-
         switch (currentMovementType)
         {
             case MovementJointBased:
                 {
                     // send the new joint values
                     float newJointPosToSend[JOINT_NUMBER] = {0.0f};
-                    currentJointPos.getData(newJointPosToSend);
-                    std::cout << currentJointPos << "\n";
-                    //friInst->doPositionControl(newJointPosToSend);
+                    VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI);
+                    newJointPosToSendRad.getData(newJointPosToSend);
+                    std::cout << newJointPosToSendRad << "\n";
+                    friInst->doPositionControl(newJointPosToSend);
                 }
                 break;
             case MovementCartBased: