From 88b7b4e135e1354ed3ed3fd203f99e2c7f9dfe8c Mon Sep 17 00:00:00 2001 From: philipp schoenberger Date: Sun, 30 Aug 2015 21:18:09 +0200 Subject: [PATCH] doxygen trajectoy.h --- lwrserv/doc/doxygenconfig | 4 +- lwrserv/include/Trajectroy.h | 294 ++++++++++++++++++++++++++++++++--- 2 files changed, 273 insertions(+), 25 deletions(-) diff --git a/lwrserv/doc/doxygenconfig b/lwrserv/doc/doxygenconfig index e729215..a75318c 100644 --- a/lwrserv/doc/doxygenconfig +++ b/lwrserv/doc/doxygenconfig @@ -1,7 +1,7 @@ PROJECT_NAME = lwr_server OUTPUT_DIRECTORY = html WARNINGS = YES -INPUT = src -FILE_PATTERNS = *.cc *.h +INPUT = src include +FILE_PATTERNS = *.cc *.h *.cpp *.hpp INCLUDE_PATH = examples src SEARCHENGINE = YES diff --git a/lwrserv/include/Trajectroy.h b/lwrserv/include/Trajectroy.h index fdd2aa6..e074c6c 100644 --- a/lwrserv/include/Trajectroy.h +++ b/lwrserv/include/Trajectroy.h @@ -1,3 +1,9 @@ +/** + * @file + * + * @author Philipp Schoenberger + */ + #ifndef _TRAJECTORY_H_ #define _TRAJECTORY_H_ #include @@ -12,13 +18,25 @@ #include "Mat.h" template class Trajectory; -// all types of trajectories -/// START OF TRAJECTORY LIST +/// all types of trajectories should als be included here #include "LinearTrajectory.h" #include "LSPBTrajectory.h" #include "BangBangTrajectory.h" #include "FivePolyTrajectory.h" +/** + * enumeration of all suppored Trajectorys. + * @info How to add a new Trajectory + * First of all add a Include line in this file. + * You should add the new Trajectories in this Enumeration + * Also in the initTrajectoryType initalisation Routine and GetTrajectory should be extended + * for the new Trajectory Type. + * + * @warning __TrajectoryCount should alsways be the last item in the list + * + * @see initTrajectoryType + * @see GetTrajectory + */ enum TrajectoryType { TrajectoryDefault = 0, TrajectoryJointLinear = 0, @@ -34,12 +52,24 @@ enum TrajectoryType { TrajectoryCartLSPB, __TrajectoryCount }; + +/** + * Helper structure for the String/Enum convert functions + * + * @see toEnum + * @see toString + */ static struct TrajectoryTypeStr{ enum TrajectoryType type; std::string str; }* trajectoryTypeStr = NULL; -/// END OF TRAJECTORY LIST +/** + * This function initalize the help structure for the String/Enum convert functions + * + * @see toEnum + * @see toString + */ static void initTrajectoryType () { if (trajectoryTypeStr == NULL) @@ -76,6 +106,15 @@ static void initTrajectoryType () } } +/** + * This function converts the enumeration type of a Trajectory to a std::string + * + * @parameter type Type of the Trajectory + * @retval string of the Trajectory of "default" if no trajectory was matched + * + * @see toEnum + * @see initTrajectoryType + */ static std::string toString(enum TrajectoryType type) { initTrajectoryType(); @@ -87,6 +126,16 @@ static std::string toString(enum TrajectoryType type) } return "default"; } + +/** + * This function converts the string of a Trajectory back to a enumeration id + * + * @parameter name String containing the Trajectory type + * @retval Trajectory id or default if no Trajectory was found + * + * @see toString + * @see initTrajectoryType + */ static enum TrajectoryType toEnum(std::string name) { initTrajectoryType(); @@ -98,16 +147,26 @@ static enum TrajectoryType toEnum(std::string name) } return TrajectoryDefault; } + enum MovementType { MovementJointBased= 0, MovementCartBased }; +/** + * Trajectory class with a template + * The unsigned SIZE is representing the number of joints + * + * Use this class to implement a new trajectory + */ template class Trajectory { public: + /** + * Default constructor + */ Trajectory() { movementType = MovementJointBased; @@ -117,6 +176,20 @@ class Trajectory totalTime = 0; nodes = NULL; } + /** + * Normal constructor for a Joint based Trajectory + * Trajectory is computed within the constructor and contains a stepwise + * movement from jointStart to the jointEnd. + * + * The Trajectoy class just uses a one step Trajectory + * for smarter Trajectories use BangBang, LSPB or Linear + * + * @param sampleTimeMs The timestep for each step within the Trajectory + * @param maxJointVelocity The maximum velocity for each Joint of the Robot + * @param maxJointAcceleration The maximum acceleration for each Joint of the Robot + * @param jointStart The joint angles of the starting point + * @param jointEnd The end position for each joint after running the trajectory + */ Trajectory( float sampleTimeMs, Vec maxJointVelocity, @@ -132,18 +205,36 @@ class Trajectory totalTime = steps * sampleTimeMs; - //this->nodes = new struct trajectoryNode [steps]; this->nodes = (struct Trajectory::trajectoryNode* ) calloc(sizeof(struct Trajectory::trajectoryNode),this->steps); nodes[0].jointPos = jointEnd; nodes[0].velocity = (jointEnd-jointStart)/sampleTimeMs; nodes[0].acceleration = Vec(9999.0f); - // unused + // cast unused parameters to void to supress compiler warnings (void)jointStart; (void)maxJointVelocity; (void)maxJointAcceleration; } + /** + * Normal constructor for a Joint based Trajectory + * Trajectory is computed within the constructor and contains a stepwise + * movement from jointStart to the jointEnd. + * + * The Trajectory class just uses a one step Trajectory + * for smarter Trajectories use BangBang, LSPB or Linear + * + * @param type The enumeration id of the Trajectory to convert to + * @param sampleTimeMs The time step for each step within the Trajectory + * @param maxJointVelocity The maximum velocity for each Joint of the Robot + * @param maxJointAcceleration The maximum acceleration for each Joint of the Robot + * @param jointStart The joint angles of the starting point + * @param jointEnd The end position for each joint after running the trajectory + * + * @return A valid pointer to a new Instance for a Trajectory between the start and end point. + * + * @info memory deletion has to be done by the receiver for the class + */ static class Trajectory* GetTrajectory( enum TrajectoryType type, float sampleTimeMs, @@ -157,6 +248,8 @@ class Trajectory switch ( type ) { + case __TrajectoryCount: + // fall through to linear joint trajectory case TrajectoryJointLinear: retval = (class Trajectory*) new LinearJointTrajectory(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); break; @@ -169,56 +262,98 @@ class Trajectory case TrajectoryJointFivePoly: break; + //@TODO implement Cartesian movement case TrajectoryCartLSPB: case TrajectoryCartLinear: case TrajectoryCartBangBang: case TrajectoryCartFivePoly: - // TODO implement carthesian movement retval = (class Trajectory*) new LinearJointTrajectory(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); break; } return retval; } + /** + * Destructor for each Trajectory which frees the step container + * Inherent Classes does not need any destructor if no extra fields are allocated within these. + */ virtual ~Trajectory() { free (nodes); } + /** + * Returns the number of steps for the trajectory + * @retval total step count + */ unsigned int getSteps() { return steps; } + /** + * Returns the number of remaining steps for the trajectory + * @retval remaining steps for this trajectory + */ unsigned int getRemainingSteps() { - if (steps >= currentStep ) + if (steps >= currentStep) return steps - currentStep; else return 0; } + /** + * Returns the current step number for the trajectory + * @retval current step number for this trajectory + */ unsigned int getCurrentStep() { return currentStep; } + /** + * Returns the movement type of the trajectory + * This can be Cartesian or joint based + * + * @retval type of movement for this trajectory + * + * @see MovementType + */ enum MovementType getMovementType() { return movementType; } + /** + * Returns a Vector with new angles for the Robot + * If end of the Trajectory is reached it always returns the + * last joint angles. + * If there is not even one step in the Trajectory it returns a Vector + * containing zeros. + * + * @info only returns valid values if this is a joint based Trajectory + * + * @retval Vector containing the next joint angles + * + * @see getJointPos + */ Vec getNextJointPos() { Vec retval(0.0f); unsigned int pos = currentStep; + + // trajectory does not contain even one step (should not happen) if (steps == 0) return retval; + + // cap on maximum steps if (pos >= steps) { pos = steps-1; } + // only if there is a step container allocated and the trajectory is of type joint if (nodes != NULL && movementType == MovementJointBased) { retval = nodes[pos].jointPos; @@ -229,29 +364,61 @@ class Trajectory return retval; } - MatCarthesian getNextCartPos () + + /** + * Returns a homogeneous matrix with the new Cartesian position for the robot + * If end of the trajectory is reached it always returns the + * last Cartesian position. + * If there is not even one step in the trajectory it returns the + * identity matrix + * + * @info only returns valid values if this is a Cartesian based trajectory + * + * @retval Cartesian matrix containing the next position + * + * @see getCartPos + */ + MatCarthesian getNextCartPos() { MatCarthesian retval(0.0f,1.0f); unsigned int pos = currentStep; + + // trajectory does not contain even one step (should not happen) if (steps == 0) return retval; + + // cap on maximum steps if (pos >= steps) { pos = steps-1; } + // only if there is a step container allocated and the trajectory is of type joint if (nodes != NULL && movementType == MovementCartBased) { retval = nodes[pos].cartPos; } - // increment step + // increment step for next query currentStep ++; return retval; } - Vec getJointPos (unsigned int step) + /** + * Returns a joint position for the robot without + * incrementing the next position + * If there is not even one step in the trajectory it returns the + * zero vector + * + * @info only returns valid values if this is a joint based trajectory + * + * @param step id of the needed position + * @retval joint vector containing the position + * + * @see getNextJointPos + */ + Vec getJointPos(unsigned int step) { Vec retval(0.0f); if (steps == 0) @@ -261,7 +428,8 @@ class Trajectory return retval; } - if (nodes != NULL) + // only if there is a step container allocated and the trajectory is of type joint + if (nodes != NULL && movementType == MovementJointBased) { retval = nodes[step].jointPos; } @@ -269,67 +437,140 @@ class Trajectory return retval; } + /** + * Returns a current joint position for the robot without + * incrementing the next position + * If end of the trajectory is reached it always returns the + * last joint position. + * If there is not even one step in the trajectory it returns the + * zero vector + * + * @info only returns valid values if this is a Cartesian based trajectory + * + * @param step id of the needed position + * @retval joint vector containing the position + * + * @see getNextJointPos + */ MatCarthesian getCartPos (unsigned int step) { - MatCarthesian retval(0.0f, 1.0f); + MatCarthesian retval(0.0f,1.0f); + + // trajectory does not contain even one step (should not happen) if (steps == 0) return retval; + + // cap on maximum steps if (step >= steps) { - return retval; + step = steps-1; } - if (nodes != NULL) + // only if there is a step container allocated and the trajectory is of type Cartesian + if (nodes != NULL && movementType == MovementCartBased) { retval = nodes[step].cartPos; } return retval; } + + /** + * Returns a joint velocity for the robot without + * incrementing the next position + * If end of the trajectory is reached it always returns the + * last velocity. + * If there is not even one step in the trajectory it returns the + * zero vector + * + * @info only returns valid values if this is a joint based trajectory + * + * @param step id of the needed position + * @retval joint vector containing the position + * + * @see getNextJointPos + */ Vec getJointVelocity (unsigned int step) { Vec retval(0.0f); + + // trajectory does not contain even one step (should not happen) if (steps == 0) return retval; + + // cap on maximum steps if (step >= steps) { - return retval; + step = steps - 1; } - if (nodes != NULL) + // only if there is a step container allocated and the trajectory is of type joint + if (nodes != NULL && movementType == MovementJointBased) { retval = nodes[step].velocity; } return retval; } + + /** + * Returns a joint acceleration for the robot without + * incrementing the next position + * If end of the trajectory is reached it always returns the + * last velocity. + * If there is not even one step in the trajectory it returns the + * zero vector + * + * @info only returns valid values if this is a joint based trajectory + * + * @param step id of the needed position + * @retval joint vector containing the position + * + * @see getNextJointPos + */ Vec getJointAcceleration(unsigned int step) { Vec retval(0.0f); + + // trajectory does not contain even one step (should not happen) if (steps == 0) return retval; + + // cap on maximum steps if (step >= steps) { - return retval; + step = steps - 1; } - if (nodes != NULL) + // only if there is a step container allocated and the trajectory is of type joint + if (nodes != NULL && movementType == MovementJointBased) { retval = nodes[step].acceleration; } return retval; } - + + /** + * struct containing one step for the trajectory + */ struct trajectoryNode { - Vec jointPos; + /// joint angles only used for joint based trajectories + Vec jointPos; + /// Cartesian position only used for Cartesian based trajectories MatCarthesian cartPos; + /// velocity for all Joint Vec velocity; + /// acceleration for all joints Vec acceleration; }; - void saveToFile(std::string filename) + /** + * Saves each joint step to the file line wise + * @param filename filename the trajectory is saved to including the path + */ + void saveJointToFile(std::string filename) { std::ofstream file; file.open(filename.c_str()); @@ -347,12 +588,19 @@ class Trajectory } protected: - static const unsigned int defaultSteps = 100; - + /// step count for this trajectory which is used for the nodes array unsigned int steps; + + /// current position for the trajectory unsigned int currentStep; + + /// total time of the trajectory in ms float totalTime; + + /// array of the trajectory containing parameters for each step struct trajectoryNode* nodes; + + /// movement type of the trajectory which is Cartesian or Joint based enum MovementType movementType; };