GT2004HeadPathPlanner Class Reference

#include <GT2004HeadPathPlanner.h>

List of all members.

Public Member Functions

void init (const Vector3< double > *vectors=0, long *durations=0, int numberOfVectors=0, bool optimizeTimings=true)
void oldInit (const Vector3< double > *vectors=0, int numberOfVectors=0, long duration=0)
 GT2004HeadPathPlanner (const SensorDataBuffer &sensorDataBuffer)
bool getAngles (double &neckTilt, double &headPan, double &headTilt)
bool isLastPathFinished ()
long calculateHeadTiming (Vector3< double > &pos1, Vector3< double > &pos2)
bool headPositionReached (Vector3< double > pos)

Public Attributes

double lastNeckTilt
double lastHeadPan
double lastHeadTilt
double headPathSpeedNeckTilt
double headPathSpeedHeadPan
double headPathSpeedHeadTilt
const SensorDataBuffersensorDataBuffer

Static Public Attributes

static const double minimumHeadSpeed = 0.004

Private Types

 maxNumberOfPoints = 20
enum  { maxNumberOfPoints = 20 }

Private Member Functions

long calculateDurationsSum (long *duration, int durations)

Private Attributes

long currentPoint
long currentFrame
long numberOfFrames
long numberOfPoints
Vector3< double > points [maxNumberOfPoints]
double firstFrame [maxNumberOfPoints]


Detailed Description

Calculate a smooth series of head joint angles from a gives set of way points and an overall duration.

Author:
Uwe Düffert

Definition at line 23 of file GT2004HeadPathPlanner.h.


Member Enumeration Documentation

anonymous enum [private]

maximum number of allowed points in head path

Enumerator:
maxNumberOfPoints 

Definition at line 124 of file GT2004HeadPathPlanner.h.


Constructor & Destructor Documentation

GT2004HeadPathPlanner::GT2004HeadPathPlanner ( const SensorDataBuffer sensorDataBuffer  )  [inline]

default constructor

Definition at line 62 of file GT2004HeadPathPlanner.h.


Member Function Documentation

long GT2004HeadPathPlanner::calculateDurationsSum ( long *  duration,
int  durations 
) [private]

Return the whole Duration of the Headpath

Returns:
the sum of duration of the headpath

Definition at line 71 of file GT2004HeadPathPlanner.cpp.

Referenced by init().

long GT2004HeadPathPlanner::calculateHeadTiming ( Vector3< double > &  pos1,
Vector3< double > &  pos2 
)

Definition at line 141 of file GT2004HeadPathPlanner.cpp.

References headPathSpeedHeadPan, headPathSpeedHeadTilt, headPathSpeedNeckTilt, max, Vector3< V >::x, Vector3< V >::y, and Vector3< V >::z.

Referenced by init().

bool GT2004HeadPathPlanner::getAngles ( double &  neckTilt,
double &  headPan,
double &  headTilt 
)

Calculates the angles for tilt pan and roll

Returns:
true if the last point is reached.

Definition at line 101 of file GT2004HeadPathPlanner.cpp.

References currentFrame, currentPoint, firstFrame, numberOfFrames, numberOfPoints, points, Vector3< V >::x, Vector3< V >::y, and Vector3< V >::z.

bool GT2004HeadPathPlanner::headPositionReached ( Vector3< double >  pos  ) 

Definition at line 84 of file GT2004HeadPathPlanner.cpp.

References abs(), SensorData::data, SensorData::headPan, SensorData::headTilt, SensorDataBuffer::lastFrame(), SensorData::neckTilt, pi, sensorDataBuffer, toMicroRad(), Vector3< V >::x, Vector3< V >::y, and Vector3< V >::z.

Referenced by GT2004HeadControl::headPositionReached(), and GT2004HeadControl::setJoints().

void GT2004HeadPathPlanner::init ( const Vector3< double > *  vectors = 0,
long *  durations = 0,
int  numberOfVectors = 0,
bool  optimizeTimings = true 
)

Initializes a set of points to visit in a certain time

Parameters:
vectors set of arcs to visit
durations a set of timings which described the time between the arcs
numberOfVectors number of Vector3s in param vectors
optimizeTimings the timings will be optimized by the distance in angles for optimal moving speed

Definition at line 16 of file GT2004HeadPathPlanner.cpp.

References calculateDurationsSum(), calculateHeadTiming(), currentFrame, currentPoint, firstFrame, getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), RobotDimensions::jointLimitHeadPanN, RobotDimensions::jointLimitHeadPanP, RobotDimensions::jointLimitHeadTiltN, RobotDimensions::jointLimitHeadTiltP, RobotDimensions::jointLimitNeckTiltN, RobotDimensions::jointLimitNeckTiltP, lastHeadPan, lastHeadTilt, lastNeckTilt, Range< T >::limit(), maxNumberOfPoints, numberOfFrames, numberOfPoints, points, Vector3< V >::x, Vector3< V >::y, and Vector3< V >::z.

Referenced by GT2004HeadControl::beginBallSearchAt(), GT2004HeadControl::calibrateHeadSpeed(), oldInit(), GT2004HeadControl::searchForBallLeft(), and GT2004HeadControl::searchForBallRight().

bool GT2004HeadPathPlanner::isLastPathFinished (  )  [inline]

Return whether the last initialized path is already finished

Returns:
true if last path is finished.

Definition at line 75 of file GT2004HeadPathPlanner.h.

References currentFrame, and numberOfFrames.

Referenced by GT2004HeadControl::calibrateHeadSpeed(), and GT2004HeadControlSymbols::getLastHeadPathIsFinished().

void GT2004HeadPathPlanner::oldInit ( const Vector3< double > *  vectors = 0,
int  numberOfVectors = 0,
long  duration = 0 
) [inline]

Definition at line 34 of file GT2004HeadPathPlanner.h.

References init(), and maxNumberOfPoints.


Member Data Documentation

long GT2004HeadPathPlanner::currentFrame [private]

number of frames (a 8ms) since start of head path

Definition at line 117 of file GT2004HeadPathPlanner.h.

Referenced by getAngles(), init(), and isLastPathFinished().

long GT2004HeadPathPlanner::currentPoint [private]

index of the most recently reached point in head path, so the head is between points[currentPoint] and points[currentPoint+1]

Definition at line 114 of file GT2004HeadPathPlanner.h.

Referenced by getAngles(), and init().

double GT2004HeadPathPlanner::firstFrame[maxNumberOfPoints] [private]

number of the first frame after head path start for a certain point in head path

Definition at line 133 of file GT2004HeadPathPlanner.h.

Referenced by getAngles(), and init().

double GT2004HeadPathPlanner::headPathSpeedHeadPan

Definition at line 98 of file GT2004HeadPathPlanner.h.

Referenced by calculateHeadTiming(), GT2004HeadControl::calibrateHeadSpeed(), and GT2004HeadControl::GT2004HeadControl().

double GT2004HeadPathPlanner::headPathSpeedHeadTilt

Definition at line 98 of file GT2004HeadPathPlanner.h.

Referenced by calculateHeadTiming(), GT2004HeadControl::calibrateHeadSpeed(), and GT2004HeadControl::GT2004HeadControl().

double GT2004HeadPathPlanner::headPathSpeedNeckTilt

the maximum speed of all angles

Definition at line 98 of file GT2004HeadPathPlanner.h.

Referenced by calculateHeadTiming(), GT2004HeadControl::calibrateHeadSpeed(), and GT2004HeadControl::GT2004HeadControl().

double GT2004HeadPathPlanner::lastHeadPan

The pan calculated in the last frame

Definition at line 92 of file GT2004HeadPathPlanner.h.

Referenced by GT2004BasicBehaviorDirectedScanForLandmarks::execute(), GT2004HeadControl::execute(), GT2004HeadControl::GT2004HeadControl(), init(), GT2004HeadControl::setJoints(), and GT2004HeadControl::setJointsDirect().

double GT2004HeadPathPlanner::lastHeadTilt

The roll calculated in the last frame

Definition at line 95 of file GT2004HeadPathPlanner.h.

Referenced by GT2004HeadControl::execute(), GT2004HeadControl::GT2004HeadControl(), init(), GT2004HeadControl::setJoints(), and GT2004HeadControl::setJointsDirect().

double GT2004HeadPathPlanner::lastNeckTilt

The tilt calculated in the last frame

Definition at line 89 of file GT2004HeadPathPlanner.h.

Referenced by GT2004HeadControl::execute(), GT2004HeadControl::GT2004HeadControl(), init(), GT2004HeadControl::setJoints(), and GT2004HeadControl::setJointsDirect().

const double GT2004HeadPathPlanner::minimumHeadSpeed = 0.004 [static]

The minimum head speed in rad per frame: 0.004 = 28.6°/s

Definition at line 86 of file GT2004HeadPathPlanner.h.

long GT2004HeadPathPlanner::numberOfFrames [private]

requested duration of head path in frames (a 8ms)

Definition at line 120 of file GT2004HeadPathPlanner.h.

Referenced by getAngles(), init(), and isLastPathFinished().

long GT2004HeadPathPlanner::numberOfPoints [private]

number of points in requested path

Definition at line 127 of file GT2004HeadPathPlanner.h.

Referenced by getAngles(), and init().

Vector3<double> GT2004HeadPathPlanner::points[maxNumberOfPoints] [private]

the points the head shall visit during the head path

Definition at line 130 of file GT2004HeadPathPlanner.h.

Referenced by getAngles(), and init().

const SensorDataBuffer& GT2004HeadPathPlanner::sensorDataBuffer

the sensordatabuffer

Definition at line 101 of file GT2004HeadPathPlanner.h.

Referenced by headPositionReached().


The documentation for this class was generated from the following files:
Generated on Thu Dec 7 01:34:40 2006 for DT2005.panorama by  doxygen 1.4.7