refactor trajectory

lwrserv/LinearTrajectory.cpp

@@ -1,23 +1,5 @@
-#include "LinearTrajectory.h"
-#include "vec.h"
-#include "stdlib.h"
-
-template <unsigned SIZE> 
-LinearJointTrajectory<SIZE>::LinearJointTrajectory(
-        float sampleTimeMs,
-        Vec<float,SIZE> maxJointVelocity,
-        Vec<float,SIZE> maxJointAcceleration,
-        Vec<float,SIZE> jointStart,
-        Vec<float,SIZE> jointEnd
-        )
-{
-    //
-    maxJointLocalVelocity = maxJointVelocity * sampleTimeMs;
-    maxJointLocalAcceleration = maxJointAcceleration * sampleTimeMs;
-    // calculate sample count
-    
-}
 
+#if 0 
 template <unsigned SIZE> 
 LinearJointTrajectory<SIZE>::LinearJointTrajectory(
         unsigned int steps_,
@@ -74,3 +56,4 @@ LinearJointTrajectory<SIZE>::LinearJointTrajectory(
         }
     }
 }
+#endif

lwrserv/SvrData.cpp

@@ -404,3 +404,18 @@ void SvrData::trajectoryCancelDone()
     pthread_mutex_unlock(&dataLock);
 }
 
+bool SvrData::getTrajectoryDone()
+{
+    bool retval = true;
+    pthread_mutex_lock(&dataLock);
+    retval = trajectory.done;
+    pthread_mutex_unlock(&dataLock);
+    return retval;
+}
+void SvrData::setTrajectoryDone(bool newState)
+{
+    pthread_mutex_lock(&dataLock);
+    trajectory.done = newState;
+    pthread_mutex_unlock(&dataLock);
+}
+

lwrserv/SvrHandling.cpp

@@ -10,6 +10,7 @@
 #include "SvrHandling.h"
 #include "SvrData.h"
 #include "Trajectroy.h"
+#include "LinearTrajectory.h"
 #include <boost/thread/thread.hpp>
 
 void printUsage(SocketObject& client)
@@ -45,9 +46,9 @@ void debugMat(Mat4f M)
     cout << M << "\n\r" << endl;
 }
 
-double* kukaBackwardCalc(double M_[12], float arg[3])
+VecJoint kukaBackwardCalc(double M_[12], float arg[3])
 {
-    double* Joints = new double[7];
+    VecJoint Joints(0.0f);
     //DH-Parameter
     float alp[7] = {-M_PI/2.0f,     M_PI/2.0f, -M_PI/2.0f,     M_PI/2.0f, -M_PI/2.0f,     M_PI/2.0f, 0.0f};
     float d[7]   = {310.4f/1000.0f, 0.0f,      400.1f/1000.0f, 0.0f,      390.0f/1000.0f, 0.0f,      78.0f/1000.0f};
@@ -95,19 +96,19 @@ double* kukaBackwardCalc(double M_[12], float arg[3])
     M06=M*Inv;
 
     //Joint 1 including given offset
-    Joints[0] = atan2(M06(1,3),M06(0,3))+J1os;
+    Joints(0) = atan2(M06(1,3),M06(0,3))+J1os;
 
     //Calculating M16
     Mat4f R01;
     i = 0;
-    R01(0,0) = cos(Joints[i]);
-    R01(0,1) = -sin(Joints[i])*cos(alp[i]);
-    R01(0,2) = sin(Joints[i])*sin(alp[i]);
+    R01(0,0) = cos(Joints(i));
+    R01(0,1) = -sin(Joints(i))*cos(alp[i]);
+    R01(0,2) = sin(Joints(i))*sin(alp[i]);
     R01(0,3) = 0;
 
-    R01(1,0) = sin(Joints[i]);
-    R01(1,1) = cos(Joints[i])*cos(alp[i]);
-    R01(1,2) = -cos(Joints[i])*sin(alp[i]);
+    R01(1,0) = sin(Joints(i));
+    R01(1,1) = cos(Joints(i))*cos(alp[i]);
+    R01(1,2) = -cos(Joints(i))*sin(alp[i]);
     R01(1,3) = 0;
 
     R01(2,0) = 0;
@@ -146,20 +147,20 @@ double* kukaBackwardCalc(double M_[12], float arg[3])
         -2*m14*m14*d3*d3*m34*m34+2*m14*m14*d5*d5*m34*m34+2*m14*m14*d5*d5*d3*d3
         +2*m24*m24*d5*d5*m14*m14)))/((m24*m24+m14*m14)*d3);
 
-    Joints[1] = atan2(arg1,arg2);
+    Joints(1) = atan2(arg1,arg2);
 
     //Calculating M26
     Mat4f R12;
     i = 1;
 
-    R12(0,0) = cos(Joints[i]);
-    R12(0,1) = -sin(Joints[i])*cos(alp[i]);
-    R12(0,2) = sin(Joints[i])*sin(alp[i]);
+    R12(0,0) = cos(Joints(i));
+    R12(0,1) = -sin(Joints(i))*cos(alp[i]);
+    R12(0,2) = sin(Joints(i))*sin(alp[i]);
     R12(0,3) = 0;
 
-    R12(1,0) = sin(Joints[i]);
-    R12(1,1) = cos(Joints[i])*cos(alp[i]);
-    R12(1,2) = -cos(Joints[i])*sin(alp[i]);
+    R12(1,0) = sin(Joints(i));
+    R12(1,1) = cos(Joints(i))*cos(alp[i]);
+    R12(1,2) = -cos(Joints(i))*sin(alp[i]);
     R12(1,3) = 0;
 
     R12(2,0) = 0;
@@ -179,20 +180,20 @@ double* kukaBackwardCalc(double M_[12], float arg[3])
     M26 = Inv*M06;
 
     //Joint 3
-    Joints[2] = atan2(ELBOW*M26(1,3),ELBOW*M26(0,3));
+    Joints(2) = atan2(ELBOW*M26(1,3),ELBOW*M26(0,3));
 
     //Calculating M36
     Mat4f R23;
     i = 2;
 
-    R23(0,0) = cos(Joints[i]);
-    R23(0,1) = -sin(Joints[i])*cos(alp[i]);
-    R23(0,2) = sin(Joints[i])*sin(alp[i]);
+    R23(0,0) = cos(Joints(i));
+    R23(0,1) = -sin(Joints(i))*cos(alp[i]);
+    R23(0,2) = sin(Joints(i))*sin(alp[i]);
     R23(0,3) = 0;
 
-    R23(1,0) = sin(Joints[i]);
-    R23(1,1) = cos(Joints[i])*cos(alp[i]);
-    R23(1,2) = -cos(Joints[i])*sin(alp[i]);
+    R23(1,0) = sin(Joints(i));
+    R23(1,1) = cos(Joints(i))*cos(alp[i]);
+    R23(1,2) = -cos(Joints(i))*sin(alp[i]);
     R23(1,3) = 0;
 
     R23(2,0) = 0;
@@ -212,19 +213,19 @@ double* kukaBackwardCalc(double M_[12], float arg[3])
     M36 = Inv*M06;
 
     //Joint 4
-    Joints[3] = atan2(M36(0,3),-M36(1,3));
+    Joints(3) = atan2(M36(0,3),-M36(1,3));
 
     //Calculating M47
     Mat4f R34;
     i = 3;
-    R34(0,0) = cos(Joints[i]);
-    R34(0,1) = -sin(Joints[i])*cos(alp[i]); 
-    R34(0,2) = sin(Joints[i])*sin(alp[i]);
+    R34(0,0) = cos(Joints(i));
+    R34(0,1) = -sin(Joints(i))*cos(alp[i]); 
+    R34(0,2) = sin(Joints(i))*sin(alp[i]);
     R34(0,3) = 0;
 
-    R34(1,0) = sin(Joints[i]);
-    R34(1,1) = cos(Joints[i])*cos(alp[i]);
-    R34(1,2) = -cos(Joints[i])*sin(alp[i]);
+    R34(1,0) = sin(Joints(i));
+    R34(1,1) = cos(Joints(i))*cos(alp[i]);
+    R34(1,2) = -cos(Joints(i))*sin(alp[i]);
     R34(1,3) = 0;
 
     R34(2,0) = 0;
@@ -246,28 +247,28 @@ double* kukaBackwardCalc(double M_[12], float arg[3])
     //Joint 5
     float th = 0.0001;
     if (abs(M47(1,2))<th && abs(M47(0,2))<th)
-        Joints[4] = 0;
+        Joints(4) = 0;
     else
-        Joints[4] = atan2(FLIP*M47(1,2),FLIP*M47(0,2));
+        Joints(4) = atan2(FLIP*M47(1,2),FLIP*M47(0,2));
 
     //Joint 6
-    Joints[5] = atan2(FLIP*sqrt(1-M47(2,2)*M47(2,2)),M47(2,2));
+    Joints(5) = atan2(FLIP*sqrt(1-M47(2,2)*M47(2,2)),M47(2,2));
 
     //Joint 7
     if (abs(M47(2,1))<th && abs(M47(2,0))<th)
-        Joints[6] = 0;
+        Joints(6) = 0;
     else
-        Joints[6] = atan2(FLIP*M47(2,1),FLIP*-M47(2,0));
+        Joints(6) = atan2(FLIP*M47(2,1),FLIP*-M47(2,0));
 
     for (int i = 0; i<7; i++)
     {
-        Joints[i] = Joints[i]*180.0/M_PI;
+        Joints(i) = Joints(i)*180.0/M_PI;
     }
 
     //Kuka Joints
     //Joints[3] = -Joints[3];
-    Joints[1] = -Joints[1];
-    Joints[5] = -Joints[5];
+    Joints(1) = -Joints(1);
+    Joints(5) = -Joints(5);
 
     return Joints;
 }
@@ -346,7 +347,7 @@ void SvrHandling::run(int port)
                 cout << timestamp() + "Client accepted!\n";
                 if (handshakeAccepted(*client))
                 {
-                    handle(*client);
+                    clientCommandLoop(*client);
                 }
                 client->Disconnect();
                 cout << timestamp() + "Client disconnected.\n";
@@ -382,7 +383,7 @@ bool SvrHandling::handshakeAccepted(SocketObject& client)
     return(c_state == accepted);
 }
 
-void SvrHandling::handle(SocketObject& client)
+void SvrHandling::clientCommandLoop(SocketObject& client)
 {
     string message, cmd, arg;
     while (c_state == accepted) 
@@ -582,14 +583,15 @@ void SvrHandling::MovePTPJoints(SocketObject& client, string& args)
     VecJoint maxJointVelocity     =  SvrData::getInstance()->getMaxVelocity();
     VecJoint maxJointAcceleration =  SvrData::getInstance()->getMaxAcceleration();
 
-    newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos);
+    newTrajectory = (class Trajectory<JOINT_NUMBER>*) new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); 
 
-    __MSRCMDJNTPOS = true;
+    // add trajectory to queue for sender thread
+    SvrData::getInstance()->pushTrajectory(newTrajectory);
 
     //Wait to end of movement
     while (true)
     {
-        if (SvrData::getInstance()->getMovementDone() == true)
+        if (SvrData::getInstance()->getTrajectoryDone() == true)
             break;
         boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) );
     }
@@ -598,39 +600,47 @@ void SvrHandling::MovePTPJoints(SocketObject& client, string& args)
 
 void SvrHandling::MoveHomRowWiseStatus(SocketObject& client, string& args)
 {
-    __SVR_CURRENT_JOB = true;
     string buf;
     double temp[15];
     stringstream ss(args);
     stringstream ss2f;
-    vector<string> tokens;
-    int i = 0;
+    int argc = 0;
+
+    // convert position parameters from string to float
     while (ss >> buf)
     {
-        if (i>15-1)
+        // only need 15 parameters not more
+        if (argc>15-1)
         {
             client.Send(SVR_FALSE_RSP);
             return;
         }
-        tokens.push_back(buf);
+
         ss2f.flush();
         ss2f.clear();
         ss2f << buf;
-        ss2f >> temp[i];
-        i++;
+        ss2f >> temp[argc];
+        argc++;
     }
-    if (i<15-1)
+
+    // check for exactly 15 parameters
+    if (argc != 15)
     {
         client.Send(SVR_FALSE_RSP);
         return ;
     }
+
+    // first part is homegenous position koordinates
+    MatCarthesian newCartPos(0.0f, 1.0f);
     for (int i=0 ;i<3; i++)
     {
         for (int j=0;j<4;j++)
         {
-            MSRCMDPOSE[i][j]=temp[i*4+j];
+            newCartPos(i,j)=temp[i*4+j];
         }
     }
+
+    // extract elbow flip and hand parameter
     float arg[3];
     for (int i=0;i<3;i++)
     {
@@ -638,120 +648,137 @@ void SvrHandling::MoveHomRowWiseStatus(SocketObject& client, string& args)
     }
 
     //Backward Calculation
+    VecJoint newJointPos = kukaBackwardCalc(temp, arg);
 
-    double* CalcJoints=NULL;
-    CalcJoints = kukaBackwardCalc(temp, arg);
 
     //Check for Joint Range
-    if (!checkJntRange(CalcJoints))
+    if (!SvrData::getInstance()->checkJointRange(newJointPos))
     {
         string out = "Error: Joints out of Range!";
         client.Send(out);
         return;
     }
 
-    // Jmove
-    for (int i=0 ;i<LBR_MNJ; i++)
-    {
-        MSRCMDJNTPOS[i]=CalcJoints[i];
-    }
-    __MSRCMDJNTPOS = true;
+    // calculate trajectory
+    VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos();
+    float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
+    VecJoint maxJointVelocity     =  SvrData::getInstance()->getMaxVelocity();
+    VecJoint maxJointAcceleration =  SvrData::getInstance()->getMaxAcceleration();
+
+    class Trajectory<JOINT_NUMBER>* newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos);
+
+    // set new commanded position for next trajectory
+    SvrData::getInstance()->setCommandedJointPos(newJointPos);
+
+    // add new trajectory to position
+    SvrData::getInstance()->pushTrajectory(newTrajectory);
 
     //Wait to end of movement
-    while (1)
-    {
-        if (!__MSRCMDJNTPOS)
+    while (true)
     {
+        if (SvrData::getInstance()->getTrajectoryDone() == true)
             break;
+        boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) );
     }
-    }
-    client.Send(SVR_TRUE_RSP);
 
+    // send a positive client feedback
+    client.Send(SVR_TRUE_RSP);
 }
 
 void SvrHandling::SetSpeed(SocketObject& client, string& args)
 {
-    __SETVELOCITY = true;
-    __SVR_CURRENT_JOB = true;
-    //SvrData::getInstance()->setState(SVR_STATE_SET_VELOCITY);
     string buf;
-    float newVelocity;
+    float newVelocityPercentage;
+    int argc = 0;
     stringstream ss(args);
     stringstream ss2f;
     vector<string> tokens;
-    int i = 0;
+
+    // no speed argument 
     if (args == "")
     {
         client.Send(SVR_FALSE_RSP);
         return;
     }
+
+    // convert speed argument to float
     while (ss >> buf)
     {
-        if (i>0)
+        // only need one speed argument
+        if (argc>0)
         {
             client.Send(SVR_FALSE_RSP);
             return;
         }
-        tokens.push_back(buf);
         ss2f.flush();
         ss2f.clear();
         ss2f << buf;
-        ss2f >> newVelocity;
-        i++;
+        ss2f >> newVelocityPercentage;
+        argc++;
     }
-    if ((newVelocity < 1.0f)||(newVelocity >1000.0))
+    
+    // check for a valid range for the new Velocity 
+    if ((newVelocityPercentage <= 0.0f)||(newVelocityPercentage >100.0f))
     {
         client.Send(SVR_FALSE_RSP);
         return;
     }
-    SvrData::getInstance()->setRobotVelocity(newVelocity);
+
+    // save new velocity into the database
+    SvrData::getInstance()->setRobotVelocityPercentage(newVelocityPercentage);
+
+    // signal a positive feedback to the client side
     client.Send(SVR_TRUE_RSP);
-    //SvrData::getInstance()->setState(SVR_STATE_RUNNING);
-    __SVR_CURRENT_JOB = false;
 }
 
 void SvrHandling::SetAccel(SocketObject& client, string& args)
 {
-    __SETACCEL = true;
-    __SVR_CURRENT_JOB = true;
     string buf;
-    float newAcceleration;
+    float newAccelerationPercentage;
     stringstream ss(args);
     stringstream ss2f;
-    vector<string> tokens;
-    int i = 0;
+    int argc = 0;
 
+    // need at least one argument
     if (args == "")
     {
         client.Send(SVR_FALSE_RSP);
         return;
     }
+    
+    // convert acceleration string into float
     while (ss >> buf)
     {
-        if (i>0)
+        // only one argument is necessary
+        if (argc>0)
         {
             client.Send(SVR_FALSE_RSP);
             return;
         }
-        tokens.push_back(buf);
         ss2f.flush();
         ss2f.clear();
         ss2f << buf;
-        ss2f >> newAcceleration;
-        i++;
+        ss2f >> newAccelerationPercentage;
+        argc++;
     }
-    if ((newAcceleration < 1.0f)||(newAcceleration >100.0f))
+
+    // check for a valid rang for the robot acceleration
+    if ((newAccelerationPercentage < 1.0f)||(newAccelerationPercentage >100.0f))
     {
         client.Send(SVR_FALSE_RSP);
             return;
     }
-    SvrData::getInstance()->setRobotAcceleration(newAcceleration);
+
+    // save new acceleration
+    SvrData::getInstance()->setRobotAccelerationPercentage(newAccelerationPercentage);
+
+    // signal a positive client feedback
     client.Send(SVR_TRUE_RSP);
-    __SVR_CURRENT_JOB = false;
 }
 
 void SvrHandling::StartPotFieldMode(SocketObject& client, string& args)
 {
+#if 0
     __MSRSTARTPOTFIELD = false;
     __MSRSTOPPOTFIELD = false;
     //Set current Joint Vals
@@ -767,11 +794,13 @@ void SvrHandling::StartPotFieldMode(SocketObject& client, string& args)
             break;
         }
     }
+#endif
     client.Send(SVR_TRUE_RSP);
 }
 
 void SvrHandling::StopPotFieldMode(SocketObject& client, string& args)
 {
+#if 0
     __POTFIELDMODE = false;
 
     while (1)
@@ -781,83 +810,78 @@ void SvrHandling::StopPotFieldMode(SocketObject& client, string& args)
             break;
         }
     }
+#endif
     client.Send(SVR_TRUE_RSP);
 }
 
 void SvrHandling::SetPos(SocketObject& client, string& args)
 {
-
     string buf;
     double temp[15];
     stringstream ss(args);
     stringstream ss2f;
-    vector<string> tokens;
-    int i = 0;
+    int argc = 0;
+
+    // convert position arguments from string to float
     while (ss >> buf)
     {
-        if (i>15-1)
+        // only 15 parameters are necessary
+        if (argc>=15)
         {
             client.Send(SVR_FALSE_RSP);
             return;
         }
-        tokens.push_back(buf);
+
         ss2f.flush();
         ss2f.clear();
         ss2f << buf;
-        ss2f >> temp[i];
-        i++;
+        ss2f >> temp[argc];
+        argc++;
     }
-    if (i<15-1)
+
+    // check for exactly 15 parameters
+    if (argc!=15)
     {
         client.Send(SVR_FALSE_RSP);
         return ;
     }
+
+
+    // the position is saved in the first parameter part
+    MatCarthesian newCartPos(0.0f,1.0f);
     for (int i=0 ;i<3; i++)
     {
         for (int j=0;j<4;j++)
         {
-            MSRCMDPOSE[i][j]=temp[i*4+j];
+            newCartPos(i,j)=temp[i*4+j];
         }
     }
+
+    // extract elbow flip and hand parameter
     float arg[3];
     for (int i=0;i<3;i++)
     {
         arg[i] = temp[12+i];
     }
 
-    //Backward Calculation
-
-    double* CalcJoints=NULL;
-    CalcJoints = kukaBackwardCalc(temp, arg);
+    //Backward Calculation of the position
+    VecJoint CalcJoints = kukaBackwardCalc(temp, arg);
 
-    //Check for Joint Range
-    if (!checkJntRange(CalcJoints))
+    //Check for a  valid joint range
+    if (!SvrData::getInstance()->checkJointRange(CalcJoints))
     {
-        string out = "Error: Joints out of Range!";
-        client.Send(out);
+        client.Send(SVR_OUT_OF_RANGE);
         return;
     }
 
-    // Set global Position
-    __MSRSETPOS = true;
-    globi = 1;
-    for (int i=0 ;i<LBR_MNJ; i++)
-    {
-        MSRCMDJNTPOS[i]=CalcJoints[i];
-    }
-    __MSRSETPOS = false;
-    sleep(5);
-    globi = 0;
-    //__MSRCMDJNTPOS = true;
+    // calculate new trajectory for next position
     
-    ////Wait to end of movement
-    //while (1)
-    //{
-    //  if (!__MSRCMDJNTPOS)
-    //  {
-    //      break;
-    //  }
-    //}
+    // set new commanded Position for next Trajectory
+    SvrData::getInstance()->setCommandedJointPos(CalcJoints);
+
+    //Wait to end of movement
+    
+    // send positive client feedback
     client.Send(SVR_TRUE_RSP);
 }
 
@@ -895,7 +919,7 @@ void SvrHandling::SetJoints(SocketObject& client, string& args)
     }
 
     //Check for Joint Range
-    if (!checkJntRange(newJointPos))
+    if (!SvrData::getInstance()->checkJointRange(newJointPos))
     {
         client.Send(SVR_OUT_OF_RANGE);
         return;
@@ -906,28 +930,29 @@ void SvrHandling::SetJoints(SocketObject& client, string& args)
     
     // set new trajectory
     
-    // Set new target Position and also calculate cart pos
+    // Set new target Position and also calculate Cartesian position
     SvrData::getInstance()->setCommandedJointPos(newJointPos);
     // TODO calculate and calculate position in Cartesian coordinates
 
     // wait till position is reached
 
-    // return positiv response to client
+    // return positive response to client
     client.Send(SVR_TRUE_RSP);
 }
 
 void SvrHandling::MoveCartesian(SocketObject& client, string& args)
 {
-    __SVR_CURRENT_JOB = true;
     string buf;
     float temp[15];
     stringstream ss(args);
     stringstream ss2f;
-    int i = 0;
+    int argc = 0;
 
+    // convert Cartesian coordinates from string into float
     while (ss >> buf)
     {
-        if (i>15-1)
+        // only need maximum 16 coordinates for a homogeneous matrix
+        if (argc>=15)
         {
             client.Send(SVR_FALSE_RSP);
             return;
@@ -935,10 +960,12 @@ void SvrHandling::MoveCartesian(SocketObject& client, string& args)
         ss2f.flush();
         ss2f.clear();
         ss2f << buf;
-        ss2f >> temp[i];
-        i++;
+        ss2f >> temp[argc];
+        argc++;
     }
-    if (i<15-1)
+
+    // check for exactly 15 coordinates
+    if (argc != 15)
     {
         client.Send(SVR_FALSE_RSP);
         return ;
@@ -968,8 +995,8 @@ void SvrHandling::MoveCartesian(SocketObject& client, string& args)
     tempVec = matToVec(TRobot);
     //tempVec = matToVec(ApRobot);
     
+    // send a positive client feedback
     client.Send(SVR_TRUE_RSP);
-    __DOUS2 = true;
 }
 
 void SvrHandling::quit(SocketObject& client)
@@ -981,8 +1008,6 @@ void SvrHandling::quit(SocketObject& client)
 void SvrHandling::debug(SocketObject& client)
 {
 
-    __DEBUG = true;
-    
     //check = true;
     //float temp[7];
     //while (1){

lwrserv/SvrHandling.h

@@ -48,8 +48,8 @@ private:
     svr_state c_state;
     //Handshake
     bool handshakeAccepted(SocketObject& client);
-    //Handle client
-    void handle(SocketObject& client);
+    //handle client commands he is disconected
+    void clientCommandLoop(SocketObject& client);
     //Handling request for current Joint Values
     void GetPositionJoints(SocketObject& client);
     //Get Position as POSE Matrix

lwrserv/Trajectroy.cpp

@@ -1,193 +0,0 @@
-#include <stdlib.h>
-#include "Trajectroy.h"
-#include "vec.h"
-
-template <unsigned SIZE> 
-Trajectory<SIZE>::Trajectory(
-        float sampleTimeMs,
-        Vec<float,SIZE> maxJointVelocity,
-        Vec<float,SIZE> maxJointAcceleration,
-        Vec<float,SIZE> jointStart,
-        Vec<float,SIZE> jointEnd
-        )
-{
-    movementType = MovementJointBased;
-
-    steps = 1;
-    currentStep = 0;
-
-    totalTime = steps * sampleTimeMs;
-
-    nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps);
-
-    nodes[0].jointPos = jointEnd;
-    nodes[0].cartPos  = kukaBackwardCalc();
-    nodes[0].velocity = (jointEnd-jointStart)/sampleTimeMs;
-    nodes[0].acceleration = Vec<float,SIZE>(9999.0f);
-
-    // unused
-    (void)jointStart;
-    (void)jointEnd;
-}
-
-template <unsigned SIZE> 
-Trajectory<SIZE>::Trajectory(
-        unsigned int steps_,
-        float totalTime_,
-        Mat<float,4> cartStart,
-        Mat<float,4> cartEnd
-        )
-{
-    movementType = MovementCartBased;
-
-    steps = steps_;
-    currentStep = 0;
-
-    totalTime = totalTime_;
-
-    nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps_);
-
-    // unused
-    (void) cartStart;
-    (void) cartEnd;
-}
-
-template <unsigned SIZE> 
-Trajectory<SIZE>::~Trajectory()
-{
-    free(nodes);
-}
-
-
-template <unsigned SIZE> 
-unsigned int Trajectory<SIZE>::getSteps()
-{
-    return steps;
-}
-
-template <unsigned SIZE> 
-unsigned int Trajectory<SIZE>::getRemainingSteps()
-{
-    if (steps >= currentStep )
-        return steps - currentStep;
-    else 
-        return 0;
-}
-
-template <unsigned SIZE> 
-unsigned int Trajectory<SIZE>::getCurrentStep()
-{
-    return currentStep;
-}
-
-template <unsigned SIZE> 
-enum MovementType Trajectory<SIZE>::getMovementType()
-{
-    return movementType;
-}
-
-template <unsigned SIZE> 
-Vec<float,SIZE> Trajectory<SIZE>::getNextJointPos ()
-{
-    Vec<float,SIZE> retval(0.0f);
-    unsigned int pos = currentStep;
-    if (pos >= steps)
-    {
-        pos = steps;
-    }
-
-    if (nodes != NULL && movementType == MovementJointBased)
-    {
-        retval = nodes[pos].jointPos;
-    }
-
-    // increment step
-    currentStep ++;
-
-    return retval;
-}
-template <unsigned SIZE> 
-Mat<float,4> Trajectory<SIZE>::getNextCartPos ()
-{
-    Mat<float,4> retval(0.0f,1.0f);
-    unsigned int pos = currentStep;
-    if (pos >= steps)
-    {
-        pos = steps;
-    }
-
-    if (nodes != NULL && movementType == MovementCartBased)
-    {
-        retval = nodes[pos].cartPos;
-    }
-
-    // increment step
-    currentStep ++;
-
-    return retval;
-}
-
-template <unsigned SIZE> 
-Vec<float,SIZE> Trajectory<SIZE>::getJointPos (unsigned int step)
-{
-    Vec<float,SIZE> retval(0.0f);
-    if (step >= steps)
-    {
-        return retval;
-    }
-
-    if (nodes != NULL && movementType == MovementJointBased)
-    {
-        retval = nodes[step].cartPos;
-    }
-
-    return retval;
-}
-template <unsigned SIZE> 
-Mat<float,4> Trajectory<SIZE>::getCartPos (unsigned int step)
-{
-    Mat<float,4> retval(0.0f, 1.0f);
-    if (step >= steps)
-    {
-        return retval;
-    }
-
-    if (nodes != NULL && movementType == MovementCartBased)
-    {
-        retval = nodes[step].cartPos;
-    }
-
-    return retval;
-}
-template <unsigned SIZE> 
-Vec<float,SIZE> Trajectory<SIZE>::getJointVelocity    (unsigned int step)
-{
-    Vec<float,SIZE> retval(0.0f);
-    if (step >= steps)
-    {
-        return retval;
-    }
-
-    if (nodes != NULL && movementType == MovementCartBased)
-    {
-        retval = nodes[step].velocity;
-    }
-
-    return retval;
-}
-template <unsigned SIZE> 
-Vec<float,SIZE> Trajectory<SIZE>::getJointAcceleration(unsigned int step)
-{
-    Vec<float,SIZE> retval(0.0f);
-    if (step >= steps)
-    {
-        return retval;
-    }
-
-    if (nodes != NULL && movementType == MovementCartBased)
-    {
-        retval = nodes[step].acceleration;
-    }
-
-    return retval;
-}

lwrserv/global.cpp

@@ -1,3 +1,4 @@
+#if 0
 bool __SVR_CURRENT_JOB = false;
 bool __MSRMSRJNTPOS = false;
 bool __MSRCMDJNTPOS = false;
@@ -15,7 +16,6 @@ bool __DOUS2 = false;
 
 int globi = 0;
 
-#if 0
 float MSRMSRJNTPOS[7];
 double MSRCMDJNTPOS[7];
 double MSRMSRCARTPOS[12];

lwrserv/include/LinearTrajectory.h

@@ -1,6 +1,9 @@
-#ifndef _LINPOLYTRAJECTORY_H_
-#define _LINPOLYTRAJECTORY_H_
+#ifndef _LINEAR_TRAJECTORY_H_
+#define _LINEAR_TRAJECTORY_H_
+
+#include "vec.h"
 #include "Trajectroy.h"
+#include "stdlib.h"
 
 template <unsigned SIZE>
 class LinearJointTrajectory : public Trajectory<SIZE>
@@ -8,23 +11,50 @@ class LinearJointTrajectory: public Trajectory<SIZE>
     public:
 
         LinearJointTrajectory(
-                unsigned int steps_,
-                float totalTime_,
-                Vec<float,SIZE> jointMovement_);
-        LinearJointTrajectory(
-                unsigned int steps_,
-                float totalTime_,
-                Vec<float,SIZE> jointMovement_,
-                Vec<float,SIZE> velocity_);
-        ~LinearJointTrajectory();
+                float sampleTimeMs,
+                Vec<float,SIZE> maxJointVelocity,
+                Vec<float,SIZE> maxJointAcceleration,
+                Vec<float,SIZE> jointStart,
+                Vec<float,SIZE> jointEnd
+                )
+        {
+            // calculate maximum velocity and acceleration
+            Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * sampleTimeMs;
+            Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * sampleTimeMs;
+
+            // calculate delta movement
+            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);
+            this->steps = ceil(minStepsPerJoint.max());
+
+            this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) malloc(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;
+            }
+        }
+        ~LinearJointTrajectory()
+        {
+            free (this->nodes);
+        }
     private:
 
     protected:
 };
+
 template <unsigned SIZE>
 class LinearCartTrajectory : public Trajectory<SIZE>
 {
-};
-
 
+};
 #endif

lwrserv/include/SvrData.h

@@ -45,7 +45,9 @@ private:
     struct {
         float sampleTimeMs;
         bool cancel;
+        bool done;
         std::queue< Trajectory<JOINT_NUMBER>* > list;
+        enum TrajectoryType type;
     }trajectory;
 
     friRemote* friInst;
@@ -116,5 +118,21 @@ public:
     int  pushTrajectory(class Trajectory<JOINT_NUMBER>* newFrajectory);
     bool cancelCurrentTrajectory();
     void trajectoryCancelDone();
+    
+    bool getTrajectoryDone();
+    void setTrajectoryDone(bool newState);
+    
+    enum TrajectoryType getTrajectoryType();
+    void setTrajectoryType(enum TrajectoryType newType);
+
+
+    template <unsigned SIZE>
+    class Trajectory<SIZE>* calculateTrajectory(
+            VecJoint maxJointVelocity,
+            VecJoint maxJointAcceleration,
+            VecJoint jointStart,
+            VecJoint jointEnd
+            );
+    int  calculateAndAddNewTrajectory(VecJoint jointEnd);
 };
 #endif 

lwrserv/include/Trajectroy.h

@@ -1,6 +1,7 @@
 #ifndef _TRAJECTORY_H_
 #define _TRAJECTORY_H_
 #include "vec.h"
+#include <stdlib.h>
 
 // all types of trajectories
 enum TrajectoryType {
@@ -27,32 +28,170 @@ template <unsigned SIZE>
 class Trajectory
 {
     public:
+        Trajectory()
+        {
+            movementType = MovementJointBased;
+
+            steps = 0;
+            currentStep = 0;
+            totalTime = 0;
+            nodes = NULL;
+        }
         Trajectory(
-            unsigned int steps_,
-            float totalTime_,
-            Vec<float,SIZE> jointMovement
-            );
-        Trajectory(
-            unsigned int steps_,
-            float totalTime_,
-            Mat<float,4> cartStart,
-            Mat<float,4> cartEnd
-        );
-        ~Trajectory();
-
-        unsigned int getSteps();
-        unsigned int getRemainingSteps();
-        unsigned int getCurrentStep();
-
-        enum MovementType getMovementType();
-
-        Vec<float,SIZE> getNextJointPos    ();
-        Mat<float,4>    getNextCartPos     ();
-
-        Vec<float,SIZE> getJointPos         (unsigned int step);
-        Mat<float,4>    getCartPos          (unsigned int step);
-        Vec<float,SIZE> getJointVelocity    (unsigned int step);
-        Vec<float,SIZE> getJointAcceleration(unsigned int step);
+            float sampleTimeMs,
+            Vec<float,SIZE> maxJointVelocity,
+            Vec<float,SIZE> maxJointAcceleration,
+            Vec<float,SIZE> jointStart,
+            Vec<float,SIZE> jointEnd
+            )
+        {
+            movementType = MovementJointBased;
+
+            steps = 1;
+            currentStep = 0;
+
+            totalTime = steps * sampleTimeMs;
+
+            nodes = (struct trajectoryNode* ) malloc(sizeof(struct trajectoryNode)*steps);
+
+            nodes[0].jointPos = jointEnd;
+            nodes[0].velocity = (jointEnd-jointStart)/sampleTimeMs;
+            nodes[0].acceleration = Vec<float,SIZE>(9999.0f);
+
+            // unused
+            (void)jointStart;
+            (void)maxJointVelocity;
+            (void)maxJointAcceleration;
+        }
+
+        ~Trajectory()
+        {
+            free(nodes);
+        }
+
+        unsigned int getSteps()
+        {
+            return steps;
+        }
+
+        unsigned int getRemainingSteps()
+        {
+            if (steps >= currentStep )
+                return steps - currentStep;
+            else 
+                return 0;
+        }
+
+        unsigned int getCurrentStep()
+        {
+            return currentStep;
+        }
+
+        enum MovementType getMovementType()
+        {
+            return movementType;
+        }
+
+        Vec<float,SIZE> getNextJointPos()
+        {
+            Vec<float,SIZE> retval(0.0f);
+            unsigned int pos = currentStep;
+            if (pos >= steps)
+            {
+                pos = steps;
+            }
+
+            if (nodes != NULL && movementType == MovementJointBased)
+            {
+                retval = nodes[pos].jointPos;
+            }
+
+            // increment step
+            currentStep ++;
+
+            return retval;
+        }
+        Mat<float,4>    getNextCartPos     ()
+        {
+            Mat<float,4> retval(0.0f,1.0f);
+            unsigned int pos = currentStep;
+            if (pos >= steps)
+            {
+                pos = steps;
+            }
+
+            if (nodes != NULL && movementType == MovementCartBased)
+            {
+                retval = nodes[pos].cartPos;
+            }
+
+            // increment step
+            currentStep ++;
+
+            return retval;
+        }
+
+        Vec<float,SIZE> getJointPos         (unsigned int step)
+        {
+            Vec<float,SIZE> retval(0.0f);
+            if (step >= steps)
+            {
+                return retval;
+            }
+
+            if (nodes != NULL && movementType == MovementJointBased)
+            {
+                retval = nodes[step].jointPos;
+            }
+
+            return retval;
+        }
+
+        Mat<float,4>    getCartPos          (unsigned int step)
+        {
+            Vec<float,SIZE> retval(0.0f);
+            if (step >= steps)
+            {
+                return retval;
+            }
+
+            if (nodes != NULL && movementType == MovementJointBased)
+            {
+                retval = nodes[step].cartPos;
+            }
+
+            return retval;
+        }
+        Vec<float,SIZE> getJointVelocity    (unsigned int step)
+        {
+            Vec<float,SIZE> retval(0.0f);
+            if (step >= steps)
+            {
+                return retval;
+            }
+
+            if (nodes != NULL && movementType == MovementCartBased)
+            {
+                retval = nodes[step].velocity;
+            }
+
+            return retval;
+        }
+        Vec<float,SIZE> getJointAcceleration(unsigned int step)
+        {
+            Vec<float,SIZE> retval(0.0f);
+            if (step >= steps)
+            {
+                return retval;
+            }
+
+            if (nodes != NULL && movementType == MovementCartBased)
+            {
+                retval = nodes[step].acceleration;
+            }
+
+            return retval;
+        }
         
         struct trajectoryNode
         {

lwrserv/include/vec.h

@@ -4,6 +4,7 @@
 #include <ostream>
 #include <cmath>
 #include "mat.h"
+#include "sgn.h"
 
 template<class T, unsigned SIZE> class Vec;
 template <class T, unsigned SIZE> class Mat;
@@ -217,6 +218,13 @@ public:
             buff.m_atData[i] = ceil(m_atData[i]);
         return buff;
     }
+    Vec<T, SIZE> sgn ()
+    {
+        Vec<T, SIZE> buff;
+        for (int i=0; i<SIZE; i++)
+            buff.m_atData[i] = sgn(m_atData[i]);
+        return buff;
+    }
     
     T max ()
     {