phschoen
/
lwr_server
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

doxygen trajectoy.h

master
philipp schoenberger 10 years ago
parent
b207716f96
commit
88b7b4e135
2 changed files with 273 additions and 25 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      lwrserv/doc/doxygenconfig
  2. 282
      lwrserv/include/Trajectroy.h

4
lwrserv/doc/doxygenconfig
View File

@ -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

282
lwrserv/include/Trajectroy.h
View File

@ -1,3 +1,9 @@
/**
* @file
*
* @author Philipp Schoenberger <ph.schoenberger@googlemail.com>
*/
#ifndef _TRAJECTORY_H_
#define _TRAJECTORY_H_
#include <stdlib.h>
@ -12,13 +18,25 @@
#include "Mat.h"
template <unsigned SIZE> 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 <unsigned SIZE>
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<float,SIZE> maxJointVelocity,
@ -132,18 +205,36 @@ class Trajectory
totalTime = steps * sampleTimeMs;
//this->nodes = new struct trajectoryNode [steps];
this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) calloc(sizeof(struct Trajectory<SIZE>::trajectoryNode),this->steps);
nodes[0].jointPos = jointEnd;
nodes[0].velocity = (jointEnd-jointStart)/sampleTimeMs;
nodes[0].acceleration = Vec<float,SIZE>(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<SIZE>*) new LinearJointTrajectory<SIZE>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd);
break;
@ -169,27 +262,39 @@ class Trajectory
case TrajectoryJointFivePoly:
break;
//@TODO implement Cartesian movement
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;
}
/**
* 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)
@ -198,27 +303,57 @@ class Trajectory
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<float,SIZE> getNextJointPos()
{
Vec<float,SIZE> 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,28 +364,60 @@ class Trajectory
return retval;
}
/**
* 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;
}
/**
* 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<float,SIZE> getJointPos(unsigned int step)
{
Vec<float,SIZE> retval(0.0f);
@ -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,51 +437,113 @@ 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);
// 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<float,SIZE> getJointVelocity (unsigned int step)
{
Vec<float,SIZE> 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<float,SIZE> getJointAcceleration(unsigned int step)
{
Vec<float,SIZE> 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;
}
@ -321,15 +551,26 @@ class Trajectory
return retval;
}
/**
* struct containing one step for the trajectory
*/
struct trajectoryNode
{
/// joint angles only used for joint based trajectories
Vec<float,SIZE> jointPos;
/// Cartesian position only used for Cartesian based trajectories
MatCarthesian cartPos;
/// velocity for all Joint
Vec<float,SIZE> velocity;
/// acceleration for all joints
Vec<float,SIZE> 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;
};

Write Preview
Loading…
Cancel
Save