#include <GT2004HeadPathPlanner.h>
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 SensorDataBuffer & | sensorDataBuffer |
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] |
Definition at line 23 of file GT2004HeadPathPlanner.h.
anonymous enum [private] |
maximum number of allowed points in head path
Definition at line 124 of file GT2004HeadPathPlanner.h.
GT2004HeadPathPlanner::GT2004HeadPathPlanner | ( | const SensorDataBuffer & | sensorDataBuffer | ) | [inline] |
default constructor
Definition at line 62 of file GT2004HeadPathPlanner.h.
long GT2004HeadPathPlanner::calculateDurationsSum | ( | long * | duration, | |
int | durations | |||
) | [private] |
Return the whole 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
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
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
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] |
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().
Definition at line 98 of file GT2004HeadPathPlanner.h.
Referenced by calculateHeadTiming(), GT2004HeadControl::calibrateHeadSpeed(), and GT2004HeadControl::GT2004HeadControl().
Definition at line 98 of file GT2004HeadPathPlanner.h.
Referenced by calculateHeadTiming(), GT2004HeadControl::calibrateHeadSpeed(), and GT2004HeadControl::GT2004HeadControl().
the maximum speed of all angles
Definition at line 98 of file GT2004HeadPathPlanner.h.
Referenced by calculateHeadTiming(), GT2004HeadControl::calibrateHeadSpeed(), and GT2004HeadControl::GT2004HeadControl().
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().
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().
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().
the sensordatabuffer
Definition at line 101 of file GT2004HeadPathPlanner.h.
Referenced by headPositionReached().