trivial

lwrserv/LinearTrajectory.cpp

@@ -4,17 +4,18 @@
 
 template <unsigned SIZE> 
 LinearJointTrajectory<SIZE>::LinearJointTrajectory(
-        unsigned int steps_,
-        float totalTime_,
-        Vec<float,SIZE> jointMovement_
+        float sampleTimeMs,
+        Vec<float,SIZE> maxJointVelocity,
+        Vec<float,SIZE> maxJointAcceleration,
+        Vec<float,SIZE> jointStart,
+        Vec<float,SIZE> jointEnd
         )
 {
-    Vec< float, SIZE> velocity = jointMovement_ / steps_ * 1.5f;
-    LinearJointTrajectory(
-            steps_,
-            totalTime_,
-            jointMovement_,
-            velocity);
+    //
+    maxJointLocalVelocity = maxJointVelocity * sampleTimeMs;
+    maxJointLocalAcceleration = maxJointAcceleration * sampleTimeMs;
+    // calculate sample count
+    
 }
 
 template <unsigned SIZE> 
@@ -25,6 +26,7 @@ LinearJointTrajectory<SIZE>::LinearJointTrajectory(
         Vec<float,SIZE> velocity
         )
 {
+    Vec< float, SIZE> velocity = jointMovement_ / steps_ * 1.5f;
     super(steps_, totalTime_, jointMovement);
 
     if ( velocity > jointMovement/totalTime_)

lwrserv/SvrData.cpp

@@ -20,8 +20,8 @@ SvrData::SvrData(void)
     double maxTrqJnt[] = {176.0,176.0,100.0,100.0,100.0,38.0,38.0};
     double maxRangeJnt[] = {170.0,120.0,170.0,120.0,170.0,120.0,170.0};
 
-    robot.currentVelocity = 20;
-    robot.currentAcceleration = 10;
+    robot.currentVelocityPercentage = 20;
+    robot.currentAccelerationPercentage = 10;
 
     for (unsigned int i = 0; i < JOINT_NUMBER; ++i)
     {
@@ -191,40 +191,38 @@ VecJoint SvrData::getMaxTorque()
 
 VecJoint SvrData::getMaxAcceleration()
 {
-    VecJoint buff;
+    VecJoint retval(0.0f);
     pthread_mutex_lock(&dataLock);
-    buff = robot.max.accelaration;
+    retval = robot.max.accelaration;
     pthread_mutex_unlock(&dataLock);
-    return buff;
+    return retval;
 }
 
 VecJoint SvrData::getMaxVelocity()
 {
-    VecJoint buff;
-
+    VecJoint retval(0.0f);
     pthread_mutex_lock(&dataLock);
-    buff = robot.max.velocity;
+    retval = robot.max.velocity;
     pthread_mutex_unlock(&dataLock);
-    return buff;
+    return retval;
 }
 VecJoint SvrData::getMaxRange()
 {
-    VecJoint buff;
-
+    VecJoint retval(0.0f);
     pthread_mutex_lock(&dataLock);
-    buff = robot.max.range;
+    retval = robot.max.range;
     pthread_mutex_unlock(&dataLock);
-    return buff;
+    return retval;
 }
 bool SvrData::checkJointRange(VecJoint vectorToCheck)
 {
-    bool retval = false;
-
+    // save temporal the max range for short time lock
     pthread_mutex_lock(&dataLock);
     Vec<float,LBR_MNJ> maxJointRange;
     maxJointRange = robot.max.range;
     pthread_mutex_unlock(&dataLock);
     
+    // check if the value is in plus or minus range 
     for (int i = 0; i<JOINT_NUMBER; i++)
     {
         if (abs(vectorToCheck(i)) > abs(maxJointRange(i)))
@@ -234,42 +232,76 @@ bool SvrData::checkJointRange(VecJoint vectorToCheck)
         }
     }
     // no join angle is too big 
-    return false;
+    return true;
 }
 
-int SvrData::setRobotVelocity(float newVelocity)
+int SvrData::setRobotVelocityPercentage(float newVelocityPercentage)
 {
     unsigned int retval = 0;
+    // percentage is to low
+    if (newVelocityPercentage <= 0.0f)
+        return -1;
+    // percentage is to high
+    if (newVelocityPercentage > 100.0f)
+        return -2;
 
     pthread_mutex_lock(&dataLock);
-    robot.currentVelocity = newVelocity;
+    robot.currentVelocityPercentage = newVelocityPercentage;
     pthread_mutex_unlock(&dataLock);
     return retval;
 }
-float SvrData::getRobotVelocity()
+VecJoint SvrData::getRobotVelocity()
+{
+    float percent = 0.0f;
+    VecJoint retval(0.0f);
+
+    pthread_mutex_lock(&dataLock);
+    percent = robot.currentVelocityPercentage / 100.0f;
+    retval = robot.max.velocity * percent;
+    pthread_mutex_unlock(&dataLock);
+    return retval;
+}
+float SvrData::getRobotVelocityPercentage()
 {
     float retval = 0;
 
     pthread_mutex_lock(&dataLock);
-    retval = robot.currentVelocity;
+    retval = robot.currentVelocityPercentage;
     pthread_mutex_unlock(&dataLock);
     return retval;
 }
-int SvrData::setRobotAcceleration(float newAcceleration)
+int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage)
 {
-    unsigned int retval = 0;
+    int retval = 0;
+    // percentage is to low
+    if (newAccelerationPercentage <= 0.0f)
+        return -1;
+    // percentage is to high
+    if (newAccelerationPercentage > 100.0f)
+        return -2;
+
+    pthread_mutex_lock(&dataLock);
+    robot.currentAccelerationPercentage = newAccelerationPercentage;
+    pthread_mutex_unlock(&dataLock);
+    return retval;
+}
+VecJoint SvrData::getRobotAcceleration()
+{
+    float percent = 0;
+    VecJoint retval(0.0f);
 
     pthread_mutex_lock(&dataLock);
-    robot.currentAcceleration = newAcceleration;
+    percent = robot.currentAccelerationPercentage / 100.0f;
+    retval  = robot.max.accelaration * percent;
     pthread_mutex_unlock(&dataLock);
     return retval;
 }
-float SvrData::getRobotAcceleration()
+float SvrData::getRobotAccelerationPercentage()
 {
     float retval = 0;
 
     pthread_mutex_lock(&dataLock);
-    retval = robot.currentAcceleration;
+    retval = robot.currentAccelerationPercentage;
     pthread_mutex_unlock(&dataLock);
     return retval;
 }

lwrserv/SvrHandling.cpp

@@ -10,6 +10,7 @@
 #include "SvrHandling.h"
 #include "SvrData.h"
 #include "Trajectroy.h"
+#include <boost/thread/thread.hpp>
 
 void printUsage(SocketObject& client)
 {
@@ -46,7 +47,6 @@ void debugMat(Mat4f M)
 
 double* kukaBackwardCalc(double M_[12], float arg[3])
 {
-
     double* Joints = new double[7];
     //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};
@@ -460,6 +460,7 @@ void SvrHandling::handle(SocketObject& client)
     }
 }
 
+
 string SvrHandling::timestamp()
 {
     time_t rawtime;
@@ -480,15 +481,18 @@ void SvrHandling::GetPositionJoints(SocketObject& client)
     string out = "";
     stringstream ss (stringstream::in | stringstream::out);
 
+    // get current joint positions from database
     VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos();
 
-    // disassemble command
+    // assemble command feadback
     for (int i=0; i< JOINT_NUMBER; i++)
     {
         ss.str("");
         ss << (jointPos(i)*180/M_PI);
         out += ss.str() + " ";
     }
+
+    // send msg to client
     client.Send(out);
 }
 
@@ -497,17 +501,21 @@ void SvrHandling::GetPositionHomRowWise(SocketObject& client)
     string out = "";
     stringstream ss (stringstream::in | stringstream::out);
 
+    // get current Cartesianpositions from database 
     MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos();
 
+    // assemble command feadback row wise
     for (int i=0; i< FRI_CART_FRM_DIM; i++)
     {
-        int row = i / 4;
-        int column = i % 4;
+        int row = i % 4;
+        int column = i / 4;
 
         ss.str("");
         ss << (cartPos(row,column));
         out += ss.str() + " ";
     }
+    
+    // send msg to client
     client.Send(out);
 }
 
@@ -570,33 +578,20 @@ void SvrHandling::MovePTPJoints(SocketObject& client, string& args)
     VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos();
     class Trajectory<JOINT_NUMBER>* newTrajectory;
     
-    // calculate number of movement steps
-    dMaxSpeed = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration);
-    lMaxSpeed = dMaxSpeed.cellmultiply(dMaxSpeed).cellmultiply(maxJointLocalAcceleration) * 0.5f;
+    float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
+    VecJoint maxJointVelocity     =  SvrData::getInstance()->getMaxVelocity();
+    VecJoint maxJointAcceleration =  SvrData::getInstance()->getMaxAcceleration();
 
-            for (int j=0; j<JOINT_NUMBER; j++)
-            {
-                if (lMaxSpeed(j) > deltaAbs(j)/(double)2.0)
-                {
-                    dGesamt(j) = sqrt(deltaAbs(j) / maxJointLocalAcceleration (j))*2.0;
-                } else 
-                {
-                    dGesamt(j) = dMaxSpeed(j)*2 + (deltaAbs(j)-lMaxSpeed(j)*2.0)/maxJointLocalVelocity(j);
-                }
-            }
-            int maxSteps = ceil(dGesamt.max());
-
-    newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(steps, totalTime, newJointPos);
+    newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos);
 
     __MSRCMDJNTPOS = true;
 
     //Wait to end of movement
-    while (1)
-    {
-        if (!__MSRCMDJNTPOS)
+    while (true)
     {
+        if (SvrData::getInstance()->getMovementDone() == true)
             break;
-        }
+        boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) );
     }
     client.Send(SVR_TRUE_RSP);
 }
@@ -789,7 +784,8 @@ void SvrHandling::StopPotFieldMode(SocketObject& client, string& args)
     client.Send(SVR_TRUE_RSP);
 }
 
-void SvrHandling::SetPos(SocketObject& client, string& args){
+void SvrHandling::SetPos(SocketObject& client, string& args)
+{
 
     string buf;
     double temp[15];
@@ -868,57 +864,55 @@ void SvrHandling::SetPos(SocketObject& client, string& args){
 void SvrHandling::SetJoints(SocketObject& client, string& args)
 {
     string buf;
-    double temp[15];
+    VecJoint newJointPos(0.0f);
     stringstream ss(args);
     stringstream ss2f;
-    vector<string> tokens;
     int i = 0;
 
+    // convert to Joint vector
     while (ss >> buf)
     {
-        if (i>LBR_MNJ-1)
+        // to many input arguments or Joints
+        if (i>=JOINT_NUMBER)
         {
-            client.Send(SVR_FALSE_RSP);
+            client.Send(SVR_INVALID_ARGUMENT_COUNT);
             return;
         }
-        tokens.push_back(buf);
+
+        // convert string to float
         ss2f.flush();
         ss2f.clear();
         ss2f << buf;
-        ss2f >> temp[i];
+        ss2f >> newJointPos(i);
         i++;
     }
-    if (i<LBR_MNJ-1)
+
+    // to less joint values
+    if (i != JOINT_NUMBER)
     {
-        client.Send(SVR_FALSE_RSP);
+        client.Send(SVR_INVALID_ARGUMENT_COUNT);
         return;
     }
 
     //Check for Joint Range
-    if (!checkJntRange(temp))
+    if (!checkJntRange(newJointPos))
     {
-        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]=temp[i];
-    }
-    __MSRSETPOS = false;
-    sleep(5);
-    globi = 0;
-    //__MSRCMDJNTPOS = true;
+    // calculate trajectory from last commanded joint position
+    VecJoint currentComandedJoint = SvrData::getInstance()->getCommandedJointPos();
     
-    ////Wait to end of movement
-    //while (1){
-    //  if (!__MSRCMDJNTPOS){
-    //      break;}
-    //}
+    // set new trajectory
+    
+    // Set new target Position and also calculate cart pos
+    SvrData::getInstance()->setCommandedJointPos(newJointPos);
+    // TODO calculate and calculate position in Cartesian coordinates
+
+    // wait till position is reached
+
+    // return positiv response to client
     client.Send(SVR_TRUE_RSP);
 }
 

lwrserv/Trajectroy.cpp

@@ -4,22 +4,30 @@
 
 template <unsigned SIZE> 
 Trajectory<SIZE>::Trajectory(
-        unsigned int steps_,
-        float totalTime_,
-        Vec<float,SIZE> jointMovement
+        float sampleTimeMs,
+        Vec<float,SIZE> maxJointVelocity,
+        Vec<float,SIZE> maxJointAcceleration,
+        Vec<float,SIZE> jointStart,
+        Vec<float,SIZE> jointEnd
         )
 {
     movementType = MovementJointBased;
 
-    steps = steps_;
+    steps = 1;
     currentStep = 0;
 
-    totalTime = totalTime_;
+    totalTime = steps * sampleTimeMs;
 
-    nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps_);
+    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) jointMovement;
+    (void)jointStart;
+    (void)jointEnd;
 }
 
 template <unsigned SIZE> 

lwrserv/friremote.cpp

@@ -242,7 +242,6 @@ int friRemote::doCartesianImpedanceControl(const float newCartPosition[FRI_CART_
     if ( newCartDamp)
     {
         cmd.cmd.cmdFlags|=FRI_CMD_CARTDAMP;
-			;
         for ( int i = 0; i < FRI_CART_VEC; i++)
         {
             cmd.cmd.cartDamping[i]=newCartDamp[i];
@@ -252,7 +251,6 @@ int friRemote::doCartesianImpedanceControl(const float newCartPosition[FRI_CART_
     if ( newAddTcpFT)
     {
         cmd.cmd.cmdFlags|=FRI_CMD_TCPFT;
-			;
         for ( int i = 0; i < FRI_CART_VEC; i++)
         {
             cmd.cmd.addTcpFT[i]=newAddTcpFT[i];
@@ -271,7 +269,6 @@ int friRemote::doCartesianImpedanceControl(const float newCartPosition[FRI_CART_
                 cmd.cmd.jntPos[i]=newJntNullspace[i];
     }
 
-
     if (flagDataExchange)
     {
         return doDataExchange();

lwrserv/include/SvrData.h

@@ -38,8 +38,8 @@ private:
             VecJoint torque;
             VecJoint range;
         } max;
-        float currentVelocity;
-        float currentAcceleration;
+        float currentVelocityPercentage;
+        float currentAccelerationPercentage;
     } robot;
 
     struct {
@@ -97,11 +97,13 @@ public:
     
     bool     checkJointRange(VecJoint vectorToCheck);
 
-    float getRobotVelocity();
-    int   setRobotVelocity(float newVelocity);
+    VecJoint getRobotVelocity();
+    float    getRobotVelocityPercentage();
+    int      setRobotVelocityPercentage(float newVelocityPercentage);
 
-    float getRobotAcceleration();
-    int   setRobotAcceleration(float newVelocity);
+    VecJoint getRobotAcceleration();
+    float    getRobotAccelerationPercentage();
+    int      setRobotAccelerationPercentage(float newAccelerationPercentage);
 
     unsigned int getJoints();
 

lwrserv/include/Trajectroy.h

@@ -57,7 +57,7 @@ class Trajectory
         struct trajectoryNode
         {
             Vec<float,SIZE> jointPos;
-            Mat<float,4>*    cartPos;
+            Mat<float,4>    cartPos;
             Vec<float,SIZE> velocity;
             Vec<float,SIZE> acceleration;
         };

lwrserv/program.cpp

@@ -9,11 +9,6 @@
 #include "SvrData.h"
 #include <boost/thread/thread.hpp>
 
-void printMat(Mat4f M)
-{
-    cout << M << endl;
-}
-
 float* abctomat(float a, float b, float c)
 {
     Mat4f rx;