#include <GT2004HeadControl.h>
Inheritance diagram for GT2004HeadControl:
Jan Hoffmann
Uwe Düffert
Martin Lötzsch
Definition at line 31 of file GT2004HeadControl.h.
anonymous enum |
The minimum head speed in urad per frame: 4000 = 28.6°/s
Definition at line 84 of file GT2004HeadControl.h.
anonymous enum |
Definition at line 168 of file GT2004HeadControl.h.
GT2004HeadControl::GT2004HeadControl | ( | HeadControlInterfaces & | interfaces | ) |
Constructor.
interfaces | The paramters of the HeadControl module. |
Definition at line 34 of file GT2004HeadControl.cpp.
References calibrationReset(), CameraInfo::CameraInfo(), cameraInfo, SensorData::data, getRobotConfiguration(), RobotConfiguration::getRobotDimensions(), SensorData::headPan, headPathPlanner, GT2004HeadPathPlanner::headPathSpeedHeadPan, GT2004HeadPathPlanner::headPathSpeedHeadTilt, GT2004HeadPathPlanner::headPathSpeedNeckTilt, SensorData::headTilt, GTXabsl2EngineExecutor::init(), RobotDimensions::jointLimitHeadPanN, RobotDimensions::jointLimitHeadPanP, RobotDimensions::jointLimitHeadTiltN, RobotDimensions::jointLimitHeadTiltP, RobotDimensions::jointLimitNeckTiltN, RobotDimensions::jointLimitNeckTiltP, jointRangeHeadPan, jointRangeHeadTilt, jointRangeNeckTilt, SensorDataBuffer::lastFrame(), GT2004HeadPathPlanner::lastHeadPan, GT2004HeadPathPlanner::lastHeadTilt, GT2004HeadPathPlanner::lastNeckTilt, SensorData::neckTilt, HeadControlInterfaces::sensorDataBuffer, setupMainAngles(), SPEED_FACTOR, speedHeadPan, speedHeadTilt, speedNeckTilt, and useCommunicatedBall.
GT2004HeadControl::~GT2004HeadControl | ( | ) | [inline] |
Destructor.
Definition at line 41 of file GT2004HeadControl.h.
void GT2004HeadControl::aimAtLandmark | ( | int | landmark, | |
double & | neckTilt, | |||
double & | headPan, | |||
double & | headTilt | |||
) |
call look-at-point depending on whether the landmark has has a z-dimension the offset in the image is adjusted: landmarks on the ground are aimed at differently from goals and corner posts
Definition at line 310 of file GT2004HeadControl.cpp.
References cameraInfo, CameraInfo::resolutionHeight, simpleLookAtPointOnField(), and targetPointOnLandmark.
Referenced by GT2004BasicBehaviorDirectedScanForLandmarks::execute().
void GT2004HeadControl::beginBallSearchAt | ( | Vector2< double > | ballPosition2d | ) |
begin a ball search by the given start position
Definition at line 187 of file GT2004HeadControl.cpp.
References headDown, headLeft, headLeftDown, headMiddleLeft, headMiddleLeftDown, headMiddleRight, headMiddleRightDown, headPathPlanner, headRight, headRightDown, headUp, GT2004HeadPathPlanner::init(), lastScanWasLeft, normalize(), HeadControlInterfaces::robotPose, Pose2D::rotation, simpleLookAtPointOnField(), Pose2D::translation, Vector2< V >::x, and Vector2< V >::y.
int GT2004HeadControl::calculateClosestLandmark | ( | double | direction = 0 , |
|
double | nextLeftOrRight = 0 | |||
) |
return the closest landmark w.r.t. a given direction; also, the next landmark to the left (or right) can be calculated.
Definition at line 447 of file GT2004HeadControl.cpp.
References Vector2< V >::abs(), Drawings::blue, CIRCLE, LINE, normalize(), numOfLandmarks, Landmark::position, HeadControlInterfaces::robotPose, Pose2D::rotation, sgn, targetPointOnLandmark, Pose2D::translation, Vector2< V >::x, Vector3< V >::y, and Vector2< V >::y.
Referenced by GT2004BasicBehaviorDirectedScanForLandmarks::execute().
void GT2004HeadControl::calibrateHeadSpeed | ( | ) |
Definition at line 542 of file GT2004HeadControl.cpp.
References calibrationRoundCount, calibrationState, calibrationStateDownTilt1, calibrationStateDownTilt1Wait, calibrationStateDownTilt2, calibrationStateDownTilt2Wait, calibrationStateLeft, calibrationStateLeftWait, calibrationStateReady, calibrationStateRight, calibrationStateRightWait, calibrationStateStart, calibrationStateUpTilt1, calibrationStateUpTilt1Wait, calibrationStateUpTilt2, calibrationStateUpTilt2Wait, calibrationStateUseResults, calibrationSuccessfulRounds, calibrationTime, calibrationTimeOutsPan, calibrationTimeOutsTilt1, calibrationTimeOutsTilt2, SystemCall::getCurrentSystemTime(), SystemCall::getTimeSince(), headPathPlanner, GT2004HeadPathPlanner::headPathSpeedHeadPan, GT2004HeadPathPlanner::headPathSpeedHeadTilt, GT2004HeadPathPlanner::headPathSpeedNeckTilt, headPositionReached(), headUp, idText, GT2004HeadPathPlanner::init(), GT2004HeadPathPlanner::isLastPathFinished(), isTimedOut(), OUTPUT, speedHeadPan, speedHeadTilt, speedNeckTilt, text, Vector3< V >::x, Vector3< V >::y, and Vector3< V >::z.
bool GT2004HeadControl::calibrateHeadSpeedIsReady | ( | ) | [inline] |
Definition at line 219 of file GT2004HeadControl.h.
References calibrationState, and calibrationStateReady.
void GT2004HeadControl::calibrationReset | ( | ) | [inline] |
Definition at line 191 of file GT2004HeadControl.h.
References calibrationState, calibrationStateStart, calibrationTimeOutsPan, calibrationTimeOutsTilt1, and calibrationTimeOutsTilt2.
Referenced by GT2004HeadControl().
void GT2004HeadControl::execute | ( | ) | [virtual] |
Executes the module
Implements Module.
Definition at line 142 of file GT2004HeadControl.cpp.
References BodyPercept::backBack, BodyPercept::backFront, SensorData::chin, GTXabsl2Profiler::collectProfiles, SensorData::data, GTXabsl2Profiler::dontCollectProfiles, GTXabsl2EngineExecutor::executeEngine(), RobotState::getButtonDuration(), RobotState::getButtonPressed(), HeadControlMode::headControlMode, HeadControlInterfaces::headControlMode, HeadControlInterfaces::headIsBlockedBySpecialActionOrWalk, SensorData::headPan, headPathPlanner, SensorData::headTilt, SensorDataBuffer::lastFrame(), lastHeadControlMode, GT2004HeadPathPlanner::lastHeadPan, GT2004HeadPathPlanner::lastHeadTilt, GT2004HeadPathPlanner::lastNeckTilt, SensorData::neckTilt, GTXabsl2EngineExecutor::profiler, GTXabsl2Profiler::profilerCollectMode, GTXabsl2Profiler::profilerWriteMode, HeadControlInterfaces::robotState, HeadControlInterfaces::sensorDataBuffer, symbols, GT2004HeadControlSymbols::update(), and GTXabsl2Profiler::writeCompleteProfiles.
int GT2004HeadControl::getLastSeenBeaconIndex | ( | ) |
return the index of the last seen beacon
Definition at line 76 of file GT2004HeadControl.cpp.
References HeadControlInterfaces::landmarksState, and LandmarksState::lastSeenBeaconIndex().
Referenced by GT2004HeadControlSymbols::update().
void GT2004HeadControl::getLookAtBallAngles | ( | const Vector2< double > | ballOnField, | |
double & | neckTilt, | |||
double & | headPan, | |||
double & | headTilt | |||
) |
returns the angles that are good for looking at the ball
Definition at line 330 of file GT2004HeadControl.cpp.
References Vector2< V >::abs(), cameraInfo, Geometry::fieldCoord2Relative(), CameraInfo::resolutionHeight, HeadControlInterfaces::robotPose, simpleLookAtPointRelativeToRobot(), Vector2< V >::x, and Vector2< V >::y.
void GT2004HeadControl::getSensorHeadAngles | ( | Vector3< double > & | pos | ) |
put the sensor values in var pos
Definition at line 95 of file GT2004HeadControl.cpp.
References SensorData::data, SensorData::headPan, SensorData::headTilt, SensorDataBuffer::lastFrame(), SensorData::neckTilt, HeadControlInterfaces::sensorDataBuffer, Vector3< V >::x, Vector3< V >::y, and Vector3< V >::z.
long GT2004HeadControl::getTimeBetweenSeen2LastBeacons | ( | int | index | ) |
return the time between the two last seen beacons
Definition at line 85 of file GT2004HeadControl.cpp.
References HeadControlInterfaces::landmarksState, and LandmarksState::timeBetweenSeen2LastBeacons().
Referenced by GT2004HeadControlSymbols::getTimeBetweenSeen2LastBeacons().
long GT2004HeadControl::getTimeOfLastSeenBeacon | ( | int | index | ) |
return the time of the last seen beacon
Definition at line 80 of file GT2004HeadControl.cpp.
References HeadControlInterfaces::landmarksState, and LandmarksState::timeOfLastSeenBeacon().
Referenced by GT2004HeadControlSymbols::getTimeSinceLastSeenABeacon().
bool GT2004HeadControl::handleMessage | ( | InMessage & | message | ) | [virtual] |
Is called for every incoming debug message.
message | An interface to read the message from the queue |
Reimplemented from Xabsl2HeadControl.
Definition at line 181 of file GT2004HeadControl.cpp.
References Xabsl2HeadControl::handleMessage().
bool GT2004HeadControl::headPanIsLeft | ( | ) |
Is true, if the head is on the left side
Definition at line 90 of file GT2004HeadControl.cpp.
References SensorData::data, SensorData::headPan, SensorDataBuffer::lastFrame(), and HeadControlInterfaces::sensorDataBuffer.
double GT2004HeadControl::headPositionDistanceToActualPosition | ( | Vector3< double > | comp, | |
bool | leftSide | |||
) |
returns a distance between actual position and comp. the small results are better
Definition at line 128 of file GT2004HeadControl.cpp.
References SensorData::data, SensorData::headPan, SensorDataBuffer::lastFrame(), HeadControlInterfaces::sensorDataBuffer, and Vector3< V >::y.
bool GT2004HeadControl::headPositionReached | ( | Vector3< double > | pos | ) | [inline] |
Definition at line 199 of file GT2004HeadControl.h.
References headPathPlanner, and GT2004HeadPathPlanner::headPositionReached().
Referenced by calibrateHeadSpeed().
bool GT2004HeadControl::isTimedOut | ( | ) | [inline] |
Definition at line 214 of file GT2004HeadControl.h.
References calibrationTime, and SystemCall::getTimeSince().
Referenced by calibrateHeadSpeed().
void GT2004HeadControl::lookAtPoint | ( | const Vector3< double > & | pos, | |
const Vector2< int > & | offset, | |||
double & | tilt, | |||
double & | pan, | |||
double & | roll | |||
) |
look at 3d-point on field with offset point in camera image this is straight-forward for the ERS210, but the ERS7 has two tilt joints that can both be used to look at something. Lookatpoint uses the two following methods to find a 'good' solution.
bool GT2004HeadControl::lookAtPointFixHeadTilt | ( | const Vector3< double > & | aim, | |
const double & | xtan, | |||
const double & | ytan, | |||
double & | tilt1, | |||
double & | pan, | |||
const double & | tilt2 | |||
) |
look at 3d-point subroutine trying to find tilt1 solution for given tilt2/roll. this is useful on ERS210 and ERS7
bool GT2004HeadControl::lookAtPointFixNeckTilt | ( | const Vector3< double > & | aim, | |
const double & | xtan, | |||
const double & | ytan, | |||
const double & | tilt, | |||
double & | pan, | |||
double & | tilt2 | |||
) |
look at 3d-point subroutine trying to find tilt2 solution for given tilt1. this is only useful on ERS7
void GT2004HeadControl::registerSymbolsAndBasicBehaviors | ( | ) | [virtual] |
Registers symbols and basic behaviors at the engine
Implements GTXabsl2EngineExecutor.
Definition at line 136 of file GT2004HeadControl.cpp.
References basicBehaviors, GTXabsl2EngineExecutor::pEngine, GT2004HeadControlBasicBehaviors::registerBasicBehaviors(), GT2004HeadControlSymbols::registerSymbols(), and symbols.
void GT2004HeadControl::searchForBallLeft | ( | ) |
looks to the left/right side an aearch for ball. Used for kicks
Definition at line 115 of file GT2004HeadControl.cpp.
References headDown, headLeft, headMiddleLeft, headPathPlanner, GT2004HeadPathPlanner::init(), and lastScanWasLeft.
void GT2004HeadControl::searchForBallRight | ( | ) |
Definition at line 102 of file GT2004HeadControl.cpp.
References headDown, headMiddleRight, headPathPlanner, headRight, GT2004HeadPathPlanner::init(), and lastScanWasLeft.
void GT2004HeadControl::setJoints | ( | double | tilt, | |
double | pan, | |||
double | roll, | |||
double | speed = 0 , |
|||
double | mouth = 0 | |||
) |
deals with setting the head joints and performs optimizations so that the head does not move too fast angles are in RAD and NOT EVER AGAIN(!!) IN MICRORAD!
Definition at line 251 of file GT2004HeadControl.cpp.
References Vector2< V >::abs(), HeadControlInterfaces::currentOdometryData, HeadControlInterfaces::headMotionRequest, headPathPlanner, GT2004HeadPathPlanner::headPositionReached(), jointRangeHeadPan, jointRangeHeadTilt, jointRangeNeckTilt, GT2004HeadPathPlanner::lastHeadPan, GT2004HeadPathPlanner::lastHeadTilt, GT2004HeadPathPlanner::lastNeckTilt, lastOdometryData, lastRobotPose, Range< T >::limit(), Range< T >::max, HeadMotionRequest::mouth, Vector2< V >::normalize(), HeadMotionRequest::pan, HeadControlInterfaces::robotPose, HeadMotionRequest::roll, setJointsIsCloseToDestination, setJointsMaxPanReached, HeadMotionRequest::tilt, Vector2< V >::x, and Vector2< V >::y.
Referenced by GT2004BasicBehaviorDirectedScanForLandmarks::execute().
void GT2004HeadControl::setJointsDirect | ( | double | tilt, | |
double | pan, | |||
double | roll, | |||
double | mouth = 0 | |||
) |
Definition at line 239 of file GT2004HeadControl.cpp.
References HeadControlInterfaces::headMotionRequest, headPathPlanner, jointRangeHeadPan, jointRangeHeadTilt, jointRangeNeckTilt, GT2004HeadPathPlanner::lastHeadPan, GT2004HeadPathPlanner::lastHeadTilt, GT2004HeadPathPlanner::lastNeckTilt, Range< T >::limit(), HeadMotionRequest::mouth, HeadMotionRequest::pan, HeadMotionRequest::roll, HeadMotionRequest::tilt, and toMicroRad().
void GT2004HeadControl::setupMainAngles | ( | ) |
initial the main angles for the head movement
Definition at line 498 of file GT2004HeadControl.cpp.
References headDown, headLeft, headLeftDown, headMiddleLeft, headMiddleLeftDown, headMiddleRight, headMiddleRightDown, headRight, headRightDown, headUp, Vector3< V >::x, Vector3< V >::y, and Vector3< V >::z.
Referenced by GT2004HeadControl().
bool GT2004HeadControl::simpleLookAtPointFixNeckTilt | ( | const Vector3< double > & | aim, | |
const double & | tilt1, | |||
double & | headPan, | |||
double & | headTilt | |||
) |
Simplified "look at 3d-point" subroutine trying to find tilt1 solution for given tilt2/roll. this is useful on ERS210 and ERS7
transformation from "neck-coord" into cam. coord., i.e.
translation by distanceNeckToPanCenter
a rotation by the neck-joint Angle followed by
get the pan angle from looking at where the target is in the xy plane
never mind clipping: if (headPan>=pi) headPan -= pi2; else if (headPan<-pi) headPan += pi2;
perform (pan-) rotation on target
get the headTilt angle by taking the angle to the target in the XZ-Plane
again, no clipping: if (headTilt >= pi) headTilt -= pi2; else if (headTilt < -pi) headTilt += pi2;
For debugging: Rotate camera coord. system by the tilt just found. If everything was done correctly, target should have y = z = 0.0
RotationMatrix rotationByHeadTilt; rotationByHeadTilt.rotateY(headTilt); target = rotationByHeadTilt*target;
if values are our of bounds, return 0 so a different method can be applied!
Definition at line 418 of file GT2004HeadControl.cpp.
References RobotDimensions::distanceNeckToPanCenter, RobotDimensions::jointLimitHeadPanN, RobotDimensions::jointLimitHeadPanP, RobotDimensions::jointLimitHeadTiltN, RobotDimensions::jointLimitHeadTiltP, RotationMatrix::rotateY(), RotationMatrix::rotateZ(), and Vector3< V >::z.
Referenced by simpleLookAtPointRelativeToRobot().
void GT2004HeadControl::simpleLookAtPointOnField | ( | const Vector3< double > | pos, | |
Vector2< int > | offset, | |||
double & | neckTilt, | |||
double & | headPan, | |||
double & | headTilt | |||
) |
Simplified "look at 3d-point" on field with offset point in camera image this is straight-forward for the ERS210, but the ERS7 has two tilt joints that can both be used to look at something. Lookatpoint uses the two following methods to find a 'good' solution.
transform the targets position to the robot's "coord. system":
transform from world to relative coordinates
what about the order of the rotations?
rotate by the body's rotation(s)
Definition at line 401 of file GT2004HeadControl.cpp.
References Pose2D::getAngle(), RobotPose::getPose(), HeadControlInterfaces::robotPose, RotationMatrix::rotateZ(), simpleLookAtPointRelativeToRobot(), Pose2D::translation, Vector2< V >::x, Vector3< V >::x, Vector2< V >::y, and Vector3< V >::y.
Referenced by aimAtLandmark(), and beginBallSearchAt().
void GT2004HeadControl::simpleLookAtPointRelativeToRobot | ( | const Vector3< double > | pos, | |
Vector2< int > | offset, | |||
double & | neckTilt, | |||
double & | headPan, | |||
double & | headTilt | |||
) |
Definition at line 349 of file GT2004HeadControl.cpp.
References HeadControlInterfaces::bodyPosture, BodyPosture::bodyRollCalculatedFromLegSensors, BodyPosture::bodyTiltCalculatedFromLegSensors, cameraInfo, RobotDimensions::jointLimitHeadPanN, RobotDimensions::jointLimitHeadPanP, RobotDimensions::jointLimitHeadTiltN, RobotDimensions::jointLimitHeadTiltP, RobotDimensions::jointLimitNeckTiltN, RobotDimensions::jointLimitNeckTiltP, jointRangeHeadPan, jointRangeHeadTilt, jointRangeNeckTilt, Range< T >::limit(), BodyPosture::neckHeightCalculatedFromLegSensors, CameraInfo::openingAngleHeight, CameraInfo::openingAngleWidth, CameraInfo::resolutionHeight, CameraInfo::resolutionWidth, RotationMatrix::rotateY(), simpleLookAtPointFixNeckTilt(), Vector2< V >::x, Vector2< V >::y, and Vector3< V >::z.
Referenced by getLookAtBallAngles(), and simpleLookAtPointOnField().
The basic behaviors used by the Xabsl2Engine
Definition at line 81 of file GT2004HeadControl.h.
Referenced by registerSymbolsAndBasicBehaviors().
enum { ... } GT2004HeadControl::calibrationState |
Referenced by calibrateHeadSpeed(), calibrateHeadSpeedIsReady(), and calibrationReset().
Definition at line 187 of file GT2004HeadControl.h.
Referenced by calibrateHeadSpeed(), and isTimedOut().
Definition at line 188 of file GT2004HeadControl.h.
Referenced by calibrateHeadSpeed(), and calibrationReset().
Definition at line 188 of file GT2004HeadControl.h.
Referenced by calibrateHeadSpeed(), and calibrationReset().
Definition at line 188 of file GT2004HeadControl.h.
Referenced by calibrateHeadSpeed(), and calibrationReset().
Information about the used camera
Definition at line 75 of file GT2004HeadControl.h.
Referenced by aimAtLandmark(), getLookAtBallAngles(), GT2004HeadControl(), and simpleLookAtPointRelativeToRobot().
Vector3<double> GT2004HeadControl::headDown |
Definition at line 153 of file GT2004HeadControl.h.
Referenced by beginBallSearchAt(), searchForBallLeft(), searchForBallRight(), and setupMainAngles().
Vector3<double> GT2004HeadControl::headLeft |
basic headpositions for the gaze
Definition at line 144 of file GT2004HeadControl.h.
Referenced by beginBallSearchAt(), searchForBallLeft(), and setupMainAngles().
Definition at line 151 of file GT2004HeadControl.h.
Referenced by beginBallSearchAt(), and setupMainAngles().
Definition at line 146 of file GT2004HeadControl.h.
Referenced by beginBallSearchAt(), searchForBallLeft(), and setupMainAngles().
Definition at line 147 of file GT2004HeadControl.h.
Referenced by beginBallSearchAt(), and setupMainAngles().
Definition at line 148 of file GT2004HeadControl.h.
Referenced by beginBallSearchAt(), searchForBallRight(), and setupMainAngles().
Definition at line 149 of file GT2004HeadControl.h.
Referenced by beginBallSearchAt(), and setupMainAngles().
An instance of the head path planner
Definition at line 57 of file GT2004HeadControl.h.
Referenced by beginBallSearchAt(), calibrateHeadSpeed(), execute(), GT2004HeadControlSymbols::getLastHeadPathIsFinished(), GT2004HeadControl(), headPositionReached(), searchForBallLeft(), searchForBallRight(), setJoints(), and setJointsDirect().
Vector3<double> GT2004HeadControl::headRight |
Definition at line 145 of file GT2004HeadControl.h.
Referenced by beginBallSearchAt(), searchForBallRight(), and setupMainAngles().
Definition at line 150 of file GT2004HeadControl.h.
Referenced by beginBallSearchAt(), and setupMainAngles().
Vector3<double> GT2004HeadControl::headUp |
Definition at line 152 of file GT2004HeadControl.h.
Referenced by beginBallSearchAt(), calibrateHeadSpeed(), and setupMainAngles().
Range<double> GT2004HeadControl::jointRangeHeadPan [private] |
Definition at line 232 of file GT2004HeadControl.h.
Referenced by GT2004HeadControl(), setJoints(), setJointsDirect(), and simpleLookAtPointRelativeToRobot().
Range<double> GT2004HeadControl::jointRangeHeadTilt [private] |
Definition at line 232 of file GT2004HeadControl.h.
Referenced by GT2004HeadControl(), setJoints(), setJointsDirect(), and simpleLookAtPointRelativeToRobot().
Range<double> GT2004HeadControl::jointRangeNeckTilt [private] |
Definition at line 232 of file GT2004HeadControl.h.
Referenced by GT2004HeadControl(), setJoints(), setJointsDirect(), and simpleLookAtPointRelativeToRobot().
The head control mode that was executed in the last frame
Definition at line 72 of file GT2004HeadControl.h.
Referenced by execute().
RobotPose GT2004HeadControl::lastRobotPose [private] |
If true, the last head movement was directed to the left side (of the ball)
Definition at line 60 of file GT2004HeadControl.h.
Referenced by beginBallSearchAt(), searchForBallLeft(), and searchForBallRight().
stores the number of frames "setjoints" has been close to its destination
Definition at line 134 of file GT2004HeadControl.h.
Referenced by GT2004HeadControlSymbols::getSetJointsIsCloseToDestination(), and setJoints().
true if the max pan of the head joint is reached
Definition at line 137 of file GT2004HeadControl.h.
Referenced by GT2004HeadControlSymbols::getSetJointsMaxPanReached(), and setJoints().
Definition at line 157 of file GT2004HeadControl.h.
Referenced by calibrateHeadSpeed(), and GT2004HeadControl().
Definition at line 158 of file GT2004HeadControl.h.
Referenced by calibrateHeadSpeed(), and GT2004HeadControl().
speed in rad/s for head movement. its used for headpathplanner and the calibration
Definition at line 156 of file GT2004HeadControl.h.
Referenced by calibrateHeadSpeed(), and GT2004HeadControl().
The symbols used by the Xabsl2Engine
Definition at line 78 of file GT2004HeadControl.h.
Referenced by execute(), and registerSymbolsAndBasicBehaviors().