You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
607 lines
19 KiB
607 lines
19 KiB
/**
|
|
* @file
|
|
*
|
|
* @author Philipp Schoenberger <ph.schoenberger@googlemail.com>
|
|
*/
|
|
|
|
#ifndef _TRAJECTORY_H_
|
|
#define _TRAJECTORY_H_
|
|
#include <stdlib.h>
|
|
#include <iostream>
|
|
#include <fstream>
|
|
|
|
#include <iomanip>
|
|
#include <cmath>
|
|
#include <limits>
|
|
|
|
#include "Vec.h"
|
|
#include "Mat.h"
|
|
template <unsigned SIZE> class Trajectory;
|
|
|
|
/// 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,
|
|
TrajectoryCartLinear,
|
|
|
|
TrajectoryJointBangBang,
|
|
TrajectoryCartBangBang,
|
|
|
|
TrajectoryJointFivePoly,
|
|
TrajectoryCartFivePoly,
|
|
|
|
TrajectoryJointLSPB,
|
|
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;
|
|
|
|
/**
|
|
* This function initalize the help structure for the String/Enum convert functions
|
|
*
|
|
* @see toEnum
|
|
* @see toString
|
|
*/
|
|
static void initTrajectoryType ()
|
|
{
|
|
if (trajectoryTypeStr == NULL)
|
|
{
|
|
trajectoryTypeStr = new struct TrajectoryTypeStr[__TrajectoryCount];
|
|
|
|
int i = 0;
|
|
|
|
trajectoryTypeStr[i].type = TrajectoryJointLinear;
|
|
trajectoryTypeStr[i].str = "JointLinear";
|
|
++i;
|
|
trajectoryTypeStr[i].type = TrajectoryJointLSPB;
|
|
trajectoryTypeStr[i].str = "JointLSPB";
|
|
++i;
|
|
trajectoryTypeStr[i].type = TrajectoryJointBangBang;
|
|
trajectoryTypeStr[i].str = "JointBangBang";
|
|
++i;
|
|
trajectoryTypeStr[i].type = TrajectoryJointFivePoly;
|
|
trajectoryTypeStr[i].str = "JointFivePoly";
|
|
++i;
|
|
|
|
trajectoryTypeStr[i].type = TrajectoryCartLinear;
|
|
trajectoryTypeStr[i].str = "CartLinear";
|
|
++i;
|
|
trajectoryTypeStr[i].type = TrajectoryCartLSPB;
|
|
trajectoryTypeStr[i].str = "CartLSPB";
|
|
++i;
|
|
trajectoryTypeStr[i].type = TrajectoryCartBangBang;
|
|
trajectoryTypeStr[i].str = "CartBangBang";
|
|
++i;
|
|
trajectoryTypeStr[i].type = TrajectoryCartFivePoly;
|
|
trajectoryTypeStr[i].str = "CartFivePoly";
|
|
++i;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 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();
|
|
int items = __TrajectoryCount;
|
|
for (int i = 0 ; i < items ; ++i)
|
|
{
|
|
if (trajectoryTypeStr[i].type == type)
|
|
return ""+ trajectoryTypeStr[i].str;
|
|
}
|
|
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();
|
|
int items = __TrajectoryCount;
|
|
for (int i = 0 ; i < items ; ++i)
|
|
{
|
|
if (trajectoryTypeStr[i].str == name)
|
|
return trajectoryTypeStr[i].type;
|
|
}
|
|
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;
|
|
|
|
steps = 0;
|
|
currentStep = 0;
|
|
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,
|
|
Vec<float,SIZE> maxJointAcceleration,
|
|
Vec<float,SIZE> jointStart,
|
|
Vec<float,SIZE> jointEnd
|
|
)
|
|
{
|
|
movementType = MovementJointBased;
|
|
|
|
steps = 1;
|
|
currentStep = 0;
|
|
|
|
totalTime = steps * sampleTimeMs;
|
|
|
|
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);
|
|
|
|
// 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,
|
|
Vec<float,SIZE> maxJointVelocity,
|
|
Vec<float,SIZE> maxJointAcceleration,
|
|
Vec<float,SIZE> jointStart,
|
|
Vec<float,SIZE> jointEnd
|
|
)
|
|
{
|
|
class Trajectory* retval = NULL;
|
|
|
|
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;
|
|
case TrajectoryJointBangBang:
|
|
retval = (class Trajectory<SIZE>*) new BangBangJointTrajectory<SIZE>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd);
|
|
break;
|
|
case TrajectoryJointLSPB:
|
|
retval = (class Trajectory<SIZE>*) new LSPBJointTrajectory<SIZE>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd);
|
|
break;
|
|
case TrajectoryJointFivePoly:
|
|
break;
|
|
|
|
//@TODO implement Cartesian movement
|
|
case TrajectoryCartLSPB:
|
|
case TrajectoryCartLinear:
|
|
case TrajectoryCartBangBang:
|
|
case TrajectoryCartFivePoly:
|
|
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)
|
|
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<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;
|
|
}
|
|
|
|
// increment step
|
|
currentStep ++;
|
|
|
|
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 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);
|
|
if (steps == 0)
|
|
return retval;
|
|
if (step >= steps)
|
|
{
|
|
return retval;
|
|
}
|
|
|
|
// only if there is a step container allocated and the trajectory is of type joint
|
|
if (nodes != NULL && movementType == MovementJointBased)
|
|
{
|
|
retval = nodes[step].jointPos;
|
|
}
|
|
|
|
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)
|
|
{
|
|
step = steps-1;
|
|
}
|
|
|
|
// 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)
|
|
{
|
|
step = steps - 1;
|
|
}
|
|
|
|
// 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)
|
|
{
|
|
step = steps - 1;
|
|
}
|
|
|
|
// 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
|
|
{
|
|
/// 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;
|
|
};
|
|
|
|
/**
|
|
* 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());
|
|
for (unsigned int currentStep = 0 ; currentStep < steps ; ++ currentStep)
|
|
{
|
|
for (unsigned int currentJoint = 0 ; currentJoint < SIZE; ++ currentJoint)
|
|
{
|
|
if (currentJoint != 0)
|
|
file << ", ";
|
|
file << std::fixed << std::setprecision(20) << nodes[currentStep].jointPos(currentJoint);
|
|
}
|
|
file << "\n";
|
|
}
|
|
file.close();
|
|
}
|
|
|
|
protected:
|
|
/// 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;
|
|
};
|
|
|
|
#endif
|