doxygen for command functions

add helper functions for client + doxygen

lwrserv/include/BangBangTrajectory.h

@@ -3,10 +3,61 @@
 #include "Trajectroy.h"
 #include "Vec.h"
 
+/**
+ * @addtogroup trajectory
+ * @{
+ *
+ * @addtogroup BangBang
+ * @{
+ * @author  Philipp Schoenberger <ph.schoenberger@googlemail.com>
+ * @version 1.0
+ *
+ * @section LICENSE
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details at
+ * https://www.gnu.org/copyleft/gpl.html
+ *
+ * @section DESCRIPTION
+ *
+ * This file contains the Trajectory for a bang bang trajectory.
+ * The bang bang trajectory is a trajectory with linear acceleration
+ * phase followed by a direct de-acceleration phase
+ *
+ * The slowest joint is defining the speed of the other joints.
+ * By that all joints start and stop the movement synchronously
+ */
 template <unsigned SIZE> 
+
+/**
+ * Class for a Bang Bang Trajectory based on joint angles.
+ * The joint start and stop synchron
+ *
+ * This class is based on the Trajectory joint Template so it 
+ * all possible joint counts 
+ *
+ * @see Trajectory
+ */
 class BangBangJointTrajectory : public Trajectory<SIZE>
 {
     public:
+        /**
+         * standard constructor which will setup the class for a 
+         * trajectory from start to end position with a synchron movement
+         *
+         * @param sampleTimeMs         The discrete time interval for each step within the Trajectory with the unit [ milliseconds ]
+         * @param maxJointVelocity     The maximum velocity for each joint with the unit [ degree/ seconds ]
+         * @param maxJointAcceleration The maximum acceleration for each joint with the unit [ degree/(seconds^2) ]
+         * @param jointStart           The starting point for each joint in [ degree ]
+         * @param jointEnd             The point which should be reached at the end of the Trajectory [ degree ]
+         */
         BangBangJointTrajectory(
                 float sampleTimeMs,
                 Vec<float,SIZE> maxJointVelocity,
@@ -15,7 +66,6 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
                 Vec<float,SIZE> jointEnd
                 )
         {
-            std::cout << "bang bang \n " ;
             float MStoSec = 1000.0f;
             float sampleTime = sampleTimeMs / MStoSec;
             // calculate maximum velocity and acceleration
@@ -31,7 +81,7 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
 
             // calculate number of movement steps
             // one joint has to reach maxvelocity the others are stepped down to 
-            // calculate time if acceleration is enouth to reach max speed
+            // calculate time if acceleration is enough to reach max speed
             // s = a * t^2 / 2
             Vec<float,SIZE> minBangBangTime = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration) / sampleTime;
             //TODO check if mintime is neceesary
@@ -52,8 +102,9 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
             Vec<float,SIZE> velocityLast(0.0f);
 
             // percentage of max velocity 
-            //Vec<float,SIZE> currJointMovementOfMax = minStepsPerJoint / minStepsPerJoint.max() ;
-            // s =  a* t^2 / 2 => a = s* 2 / t^2
+            // s =  a* t^2 / 2 
+            // <=> 
+            // a = s* 2 / t^2
             Vec<float,SIZE> currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(this->steps).celldivide(this->steps)*2.0f;
             std::cout << "currMaxAcceleration : " << currMaxAcceleration << "\n";
 
@@ -66,7 +117,7 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
                     this->nodes[i].acceleration = currMaxAcceleration ;
                     count +=1.0f;
                 }
-                //  deacceleration phase
+                //  de-acceleration phase
                 else
                 {
                     this->nodes[i].acceleration = currMaxAcceleration * -1.0f;
@@ -76,18 +127,34 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
                 this->nodes[i].jointPos = jointLast + this->nodes[i].velocity; 
 
 
+                // check if we already reached end 
+                // This can happen if the steps calculation was
+                // to far off caused by floating point precision
+                 
+                float maxdiffLast = (jointEnd - jointLast).abs().max();
+                float maxdiffCurr = (jointEnd - this->nodes[i].jointPos).abs().max();
+                if ( maxdiffLast < maxdiffCurr)
+                {
+                    this->steps = i+1;
+
+                    this->nodes[i].jointPos = jointEnd;
+                    break;
+                }
+                
+                // save last joints
                 jointLast = this->nodes[i].jointPos;
                 velocityLast = this->nodes[i].velocity;
             }
         }
-    private:
-
-    protected:
-
 };
 template <unsigned SIZE> 
 class BangBangCartTrajectory: public Trajectory<SIZE>
 {
 };
 
+/**
+ * @}
+ * @}
+ */
+
 #endif 

lwrserv/include/commands.h

@@ -4,6 +4,14 @@
 #include "SocketObject.h"
 #include <stdlib.h>
 
+/**
+ * @addtogroup commands
+ * functions to process the request from client side and respond to them
+ * @{
+ *  @addtogroup processing
+ *  @{
+ */
+
 static struct ClientCommand* commands;
 static unsigned int commandCount;
 
@@ -39,4 +47,9 @@ void quit(SocketObject& client, std::string& arg);
 // check if we use a kuka
 void isKukaLwr(SocketObject& client, std::string& arg);
 
+/**
+ *  @}
+ * @}
+ */
+
 #endif

lwrserv/include/commandsHelp.h

@@ -0,0 +1,45 @@
+#ifndef _COMMANDSHELP_H_
+#define _COMMANDSHELP_H_
+
+#include "SocketObject.h"
+#include <stdlib.h>
+/**
+ * @addtogroup commands
+ * Additional functions for the client commands to print help and usage of them
+ * @{
+ *  @addtogroup help
+ *  @{
+ */
+
+//Handling request for current Joint Values
+void getPositionJointsHelp (SocketObject& client, std::string& arg);
+//Get Position as POSE Matrix
+void getPositionHomRowWiseHelp (SocketObject& client, std::string& arg);
+//Get Force/torque values from TCP
+void getForceTorqueTcpHelp (SocketObject& client, std::string& arg);
+//Move to given Joint combination
+void movePTPJointsHelp (SocketObject& client, std::string& arg);
+//Move to given POSE position
+void moveHomRowWiseStatusHelp (SocketObject& client, std::string& arg);
+//Set Velocity
+void setSpeedHelp (SocketObject& client, std::string& arg);
+//Set Acceleration
+void setAccelHelp (SocketObject& client, std::string& arg);
+//Starting Potential Field Movement Mode
+void startPotFieldModeHelp (SocketObject& client, std::string& arg);
+//Stopping Potential Field Movement Mode
+void stopPotFieldModeHelp (SocketObject& client, std::string& arg);
+// set the current trajectory type 
+void setTrajectoryTypeHelp (SocketObject& client, std::string& arg);
+// get the current trajectory type 
+void getTrajectoryTypeHelp (SocketObject& client, std::string& arg);
+//Quit
+void quitHelp (SocketObject& client, std::string& arg);
+// check if we use a kuka
+void isKukaLwrHelp (SocketObject& client, std::string& arg);
+
+/**
+ *  @}
+ * @}
+ */
+#endif

lwrserv/include/lwr4.h

@@ -1,6 +1,5 @@
 #ifndef _LWR4_H_
 #define _LWR4_H_
-
 #include "Vec.h"
 #include "Mat.h"
 #include "friComm.h"

lwrserv/src/commands.cpp

@@ -18,7 +18,8 @@
  *
  * @section DESCRIPTION
  *
- * This file contains the commands for the client interface
+ * This file contains functions to process the commands
+ * comming from the client interface
  */
 #include <stdlib.h>
 #include <string>
@@ -68,6 +69,16 @@ void moveRobotTo(Robot::VecJoint newJointPos)
         boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) );
     }
 }
+
+/**
+ * This function processes the request from client side to
+ * get the current joint positions of the robot
+ *
+ * @param client connection to client which will recive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see getPositionJointsHelp
+ */
 void getPositionJoints(SocketObject& client, std::string& arg)
 {
     // unused
@@ -80,7 +91,7 @@ void getPositionJoints(SocketObject& client, std::string& arg)
     Robot::VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos();
 
     // assemble command feadback
-    for (int i=0; i< Robot::joints; i++)
+    for (unsigned int i = 0 ; i < Robot::joints ; i++)
     {
         ss.str("");
         ss << (jointPos(i)*180/M_PI);
@@ -91,11 +102,21 @@ void getPositionJoints(SocketObject& client, std::string& arg)
     client.Send(out);
 }
 
+/**
+ * This function processes the request from client side to
+ * get the current homogenious carthesian coordinates
+ *
+ * @param client connection to client which will recive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see getPositionHomRowWiseHelp
+ */
 void getPositionHomRowWise(SocketObject& client, std::string& arg)
 {
     // unused
     (void) arg;
 
+    // set up the string buffers
     std::string out = "";
     std::stringstream ss (std::stringstream::in | std::stringstream::out);
 
@@ -103,52 +124,79 @@ void getPositionHomRowWise(SocketObject& client, std::string& arg)
     MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos();
 
     // assemble command feedback row wise
-    for (int i=0; i< FRI_CART_FRM_DIM; i++)
+    // leav out the 4th row cause its containing only " 0 0 0 1 "
+    for (int row = 0; row < 3 ; row++)
+    {
+        for (int column = 0; column < 4 ; column++)
         {
-        int row = i % 4;
-        int column = i / 4;
-
             ss.str("");
             ss << (cartPos(row,column));
             out += ss.str() + " ";
         }
+    }
 
     // send msg to client
     client.Send(out);
 }
 
+/**
+ * This function processes the request from client side to
+ * get the current force and torque based on the endefector 
+ * of the robot 
+ *
+ * @param client connection to client which will recive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see getForceTorqueTcpHelp
+ */
 void getForceTorqueTcp(SocketObject& client, std::string& arg)
 {
     // unused
     (void) arg;
 
+    // set up the string buffers
     std::string out = "";
     std::stringstream ss (std::stringstream::in | std::stringstream::out);
 
+    // get current force and torque parametes from database
     VecTorque torque = SvrData::getInstance()->getMessuredForceTorque();
+
+    // assemble command feedback
     for (int i=0; i< FRI_CART_VEC; i++)
     {
         ss.str("");
         ss << torque(i);
         out += ss.str() + " ";
     }
+
+    // send msg to client
     client.Send(out);
 }
 
+/**
+ * This function processes the request from client side to
+ * move the robot from the current position to a 
+ * new position defined by the new joint angles
+ *
+ * @param client connection to client which will recive the response
+ * @param arg    array containg the new joint angles from fist to last one
+ *
+ * @see movePTPJointsHelp
+ */
 void movePTPJoints(SocketObject& client, std::string& arg)
 {
+    // set up the string buffers
     std::string buf;
-
     std::stringstream ss(arg);
     std::stringstream ss2f;
 
     // convert string to joint vector
     Robot::VecJoint newJointPos(0.0f);
-    int i = 0;
+    unsigned int i = 0;
     while (ss >> buf)
     {
         // to many joint values
-        if (i>=Robot::joints)
+        if (i >= Robot::joints)
         {
             client.Send(SVR_INVALID_ARGUMENT_COUNT);
             return;
@@ -161,10 +209,10 @@ void movePTPJoints(SocketObject& client, std::string& arg)
     }
 
     // to less joint values
-    if (i!=Robot::joints)
+    if (i != Robot::joints)
     {
         client.Send(SVR_INVALID_ARGUMENT_COUNT);
-        return ;
+        return;
     }
 
     //check for joint range
@@ -175,12 +223,24 @@ void movePTPJoints(SocketObject& client, std::string& arg)
         return;
     }
 
+    // to actually move the robot to new joint angles
     moveRobotTo(newJointPos);
     client.Send(SVR_TRUE_RSP);
 }
 
+/**
+ * This function processes the request from client side to
+ * move the robot from the current position to a 
+ * new position defined by the new homogeneous coordinates and configuration parameters
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    array containing the new carthesian positions and configuration parameters (elbow, flip, j1os)
+ *
+ * @see moveHomRowWiseStatusHelp
+ */
 void moveHomRowWiseStatus(SocketObject& client, std::string& arg)
 {
+    // set up the string buffers
     std::string buf;
     double temp[15];
     std::stringstream ss(arg);
@@ -223,9 +283,9 @@ void moveHomRowWiseStatus(SocketObject& client, std::string& arg)
 
     // extract elbow flip and hand parameter
     struct Robot::RobotConfig configurationParameter;
-    configurationParameter.elbow = temp[12] == 1.0f;
-    configurationParameter.flip  = temp[13] == 1.0f;
-    configurationParameter.j1os  = temp[14] == 1.0f;
+    configurationParameter.elbow = (temp[12] == 1.0f);
+    configurationParameter.flip  = (temp[13] == 1.0f);
+    configurationParameter.j1os  = (temp[14] == 1.0f);
 
     //Backward Calculation
     Robot::VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter);
@@ -243,6 +303,15 @@ void moveHomRowWiseStatus(SocketObject& client, std::string& arg)
     client.Send(SVR_TRUE_RSP);
 }
 
+/**
+ * This function processes the request from client side to
+ * set a new percentage for the speed value.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    new percentage
+ *
+ * @see moveHomRowWiseStatusHelp
+ */
 void setSpeed(SocketObject& client, std::string& arg)
 {
     std::string buf;
@@ -391,17 +460,15 @@ void getTrajectoryType(SocketObject& client, std::string& arg)
 }
 
 /**
- * clients can check if the robot is of Type Kuka LWR
+ *
  * @param client connection to client which will recive the response
  * @param arg    aguments which are unused in this case
- *
- * @author Philipp Schoenberger <ph.schoenberger@googlemail.com>
- * @date
  */
 void isKukaLwr(SocketObject& client, std::string& arg)
 {
     //unused parameter 
     (void) arg;
 
+    // we are a kuka so send true 
     client.Send(SVR_TRUE_RSP);
 }

lwrserv/src/commandsHelp.cpp

@@ -0,0 +1,336 @@
+/**
+ * @file
+ * @author  Philipp Schoenberger <ph.schoenberger@googlemail.com>
+ * @version 1.0
+ *
+ * @section LICENSE
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details at
+ * https://www.gnu.org/copyleft/gpl.html
+ *
+ * @section DESCRIPTION
+ *
+ * This file contains functions to process the commands
+ * coming from the client interface
+ */
+#include <stdlib.h>
+#include <string>
+#include <iostream>
+#include "SocketObject.h"
+
+/**
+ * This function returns the help information text to the client
+ * for the getPositionJoints function.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see getPositionJoints
+ */
+void getPositionJointsHelp(SocketObject& client, std::string& arg)
+{
+    // unused
+    (void) arg;
+
+    // set up the string buffers
+    std::string out = "";
+
+    // add the help text to the buffer
+    out += "Request for the current joint angles from database.\n";
+    out += "Return string is formated like: \n \t 1.0 2.0 ... 4.0 \n ";
+
+    // send message to client
+    client.Send(out);
+}
+
+/**
+ * This function returns the help information text to the client
+ * for the getPositionHomRowWise function.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see getPositionHomRowWise
+ */
+void getPositionHomRowWiseHelp(SocketObject& client, std::string& arg)
+{
+    // unused
+    (void) arg;
+
+    // set up the string buffers
+    std::string out = "";
+
+    // add the help text to the buffer
+    out += "Request for the current positions in form of a homogeneous cartesian matrix.\n";
+    out += "The return string is formated row wise with leaving out the last line.\n";
+    out += "\n";
+    out += "R11 R12 R13 Tx \n";
+    out += "R21 R22 R23 Ty \n";
+    out += "R31 R32 R33 Tz \n";
+    out += "0   0   0   1  \n";
+    out += "\n";
+    out += "Example Matrix above will be assembled to a line like the following one\n";
+    out += "\n";
+    out += "R11 R12 R13 Tx R21 R22 R23 Ty R31 R32 R33 Tz\n";
+    out += "\n";
+
+    // send message to client
+    client.Send(out);
+}
+
+/**
+ * This function returns the help information text to the client
+ * for the getForceTorqueTcp function.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see getForceTorqueTcp
+ */
+void getForceTorqueTcpHelp(SocketObject& client, std::string& arg)
+{
+    // unused
+    (void) arg;
+
+    // set up the string buffers
+    std::string out = "";
+
+    // add the help text to the buffer
+    out += "Request for the current force and torque of the Robot\n";
+    out += "The first 3 Values are the x,y,z forces of the robot endefektor\n";
+    out += "The second 3 values  are the x,y,z rotational torques on the endefektor\n";
+    out += "The returned string will be formated like this: \n";
+    out += "\t Fx Fy Fz Tx Ty Tz\n";
+
+    // send message to client
+    client.Send(out);
+}
+
+/**
+ * This function returns the help information text to the client
+ * for the movePTPJoints function.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see movePTPJoints
+ */
+void movePTPJointsHelp(SocketObject& client, std::string& arg)
+{
+    // unused
+    (void) arg;
+
+    // set up the string buffers
+    std::string out = "";
+
+    // add the help text to the buffer
+    out += "Request the server to move the robot from the current measured Position\n";
+    out += "to the new position defined by the given parameters.\n";
+    out += "The amount of parameters have to match exactly the number of joints of the\n";
+    out += "Robot. Otherwise the command will return \"false\".\n";
+    out += "Command interface is blocked after sending the command and will be up\n";
+    out += "when the robot reached the commanded destination.\n";
+    out += "When the movement was successfully finished the return value will be \"true\"\n";
+    out += "The entered command should be formated like this: \n";
+    out += "MPTP J1 J2 J3 J4 J5 J6 J7 ... \n";
+
+    // send msg to client
+    client.Send(out);
+}
+
+/**
+ * This function returns the help information text to the client
+ * for the moveHomRowWiseStatus function.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see moveHomRowWiseStatus
+ */
+void moveHomRowWiseStatusHelp(SocketObject& client, std::string& arg)
+{
+    // unused
+    (void) arg;
+
+    // set up the string buffers
+    std::string out = "";
+
+    // send message to client
+    client.Send(out);
+}
+
+/**
+ * This function returns the help information text to the client
+ * for the setSpeed function.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see setSpeed
+ */
+void setSpeedHelp(SocketObject& client, std::string& arg)
+{
+    // unused
+    (void) arg;
+
+    // set up the string buffers
+    std::string out = "";
+
+    // send message to client
+    client.Send(out);
+}
+
+/**
+ * This function returns the help information text to the client
+ * for the setAccel function.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see setAccel
+ */
+void setAccelHelp(SocketObject& client, std::string& arg)
+{
+    // unused
+    (void) arg;
+
+    // set up the string buffers
+    std::string out = "";
+
+    // send message to client
+    client.Send(out);
+}
+
+/**
+ * This function returns the help information text to the client
+ * for the startPotFieldMode function.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see startPotFieldMode
+ */
+void startPotFieldModeHelp(SocketObject& client, std::string& arg)
+{
+    // unused
+    (void) arg;
+
+    // set up the string buffers
+    std::string out = "";
+
+    // send message to client
+    client.Send(out);
+}
+
+/**
+ * This function returns the help information text to the client
+ * for the stopPotFieldMode function.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see stopPotFieldMode
+ */
+void stopPotFieldModeHelp(SocketObject& client, std::string& arg)
+{
+    // unused
+    (void) arg;
+
+    // set up the string buffers
+    std::string out = "";
+
+    // send message to client
+    client.Send(out);
+}
+
+/**
+ * This function returns the help information text to the client
+ * for the quit function.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see quit
+ */
+void quitHelp(SocketObject& client , std::string& arg)
+{
+    // unused
+    (void) arg;
+
+    // set up the string buffers
+    std::string out = "";
+
+    // send message to client
+    client.Send(out);
+}
+
+/**
+ * This function returns the help information text to the client
+ * for the quit function.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see quit
+ */
+void setTrajectoryTypeHelp(SocketObject& client, std::string& arg)
+{
+    // unused
+    (void) arg;
+
+    // set up the string buffers
+    std::string out = "";
+
+    // send message to client
+    client.Send(out);
+}
+
+/**
+ * This function returns the help information text to the client
+ * for the getTrajectoryType function.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see getTrajectoryType
+ */
+void getTrajectoryTypeHelp(SocketObject& client, std::string& arg)
+{
+    // unused
+    (void) arg;
+
+    // set up the string buffers
+    std::string out = "";
+
+    // send message to client
+    client.Send(out);
+}
+
+/**
+ * This function returns the help information text to the client
+ * for the isKukaLwr function.
+ *
+ * @param client connection to client which will receive the response
+ * @param arg    unused but has to be there for matching the signature
+ *
+ * @see isKukaLwr
+ */
+void isKukaLwrHelp(SocketObject& client, std::string& arg)
+{
+    // unused
+    (void) arg;
+
+    // set up the string buffers
+    std::string out = "";
+
+    // send message to client
+    client.Send(out);
+}