bool KickEngineData::checkPhaseTime(const FrameInfo& frame, const RobotDimensions& rd, const FilteredJointData& jd, const TorsoMatrix& torsoMatrix)
{
timeSinceTimeStamp = frame.getTimeSince(timeStamp);
if(motionID > -1)
{
if(phaseNumber < currentParameters.numberOfPhases)
{
if((unsigned int) timeSinceTimeStamp > currentParameters.phaseParameters[phaseNumber].duration)
{
phaseNumber++;
timeStamp = frame.time;
timeSinceTimeStamp = frame.getTimeSince(timeStamp);
if(currentKickRequest.dynamical && !currentKickRequest.dynPoints.empty())
for(unsigned int i = 0; i < currentKickRequest.dynPoints.size(); i++)
if(currentKickRequest.dynPoints[i].phaseNumber == phaseNumber)
addDynPoint(currentKickRequest.dynPoints[i], rd, torsoMatrix);
}
}
else if(currentParameters.loop && phaseNumber == currentParameters.numberOfPhases)
{
phaseNumber = 0;
calculateOrigins(rd, jd);
currentParameters.initFirstPhaseLoop(origins, currentParameters.phaseParameters[currentParameters.numberOfPhases - 1].comTra[2], Vector2<>(jd.angles[JointData::HeadPitch], jd.angles[JointData::HeadYaw]));
if(currentKickRequest.dynamical && !currentKickRequest.dynPoints.empty())
for(unsigned int i = 0; i < currentKickRequest.dynPoints.size(); i++)
if(currentKickRequest.dynPoints[i].phaseNumber == phaseNumber)
addDynPoint(currentKickRequest.dynPoints[i], rd, torsoMatrix);
}
return (phaseNumber < currentParameters.numberOfPhases);
}
else
return false;
}
void GameDataProvider::update(RobotInfo& robotInfo, const FrameInfo& theFrameInfo, const BehaviorControlOutput& theBehaviorControlOutput)
{
unsigned timeStamp = gameControlTimeStamp;
if(RECEIVE_GAME_CONTROL(gameControlData))
timeStamp = theFrameInfo.time;
bool received = false;
if(theFrameInfo.getTimeSince(timeStamp) < 500)
{
for(int i = 0; i < 2; ++i)
//if(Global::getSettings().teamNumber == (int)gameControlData.teams[i].teamNumber)
//TODO:
if( 0 == (int)gameControlData.teams[i].teamNumber)
{
gameControlTimeStamp = timeStamp;
const RoboCup::TeamInfo& t = gameControlData.teams[i];
//ASSERT(Global::getSettings().playerNumber >= 1 && Global::getSettings().playerNumber <= MAX_NUM_PLAYERS);
ASSERT(2 >= 1 && 2 <= MAX_NUM_PLAYERS);
RoboCup::RoboCupGameControlReturnData returnPacket;
memcpy(returnPacket.header, GAMECONTROLLER_RETURN_STRUCT_HEADER, sizeof(returnPacket.header));
returnPacket.version = GAMECONTROLLER_RETURN_STRUCT_VERSION;
//returnPacket.team = Global::getSettings().teamNumber;
returnPacket.team = 0;
//returnPacket.player = Global::getSettings().playerNumber;
returnPacket.player = 2;
if(&theBehaviorControlOutput && robotInfo.penalty != theBehaviorControlOutput.robotInfo.penalty)
{
returnPacket.message = theBehaviorControlOutput.robotInfo.penalty != PENALTY_NONE
? GAMECONTROLLER_RETURN_MSG_MAN_PENALISE
: GAMECONTROLLER_RETURN_MSG_MAN_UNPENALISE;
lastSent = theFrameInfo.time;
(void) SEND_GAME_CONTROL(returnPacket);
}
else if(theFrameInfo.getTimeSince(lastSent) > 500)
{
returnPacket.message = GAMECONTROLLER_RETURN_MSG_ALIVE;
lastSent = theFrameInfo.time;
(void) SEND_GAME_CONTROL(returnPacket);
}
//(RoboCup::RobotInfo&) robotInfo = t.players[Global::getSettings().playerNumber - 1];
(RoboCup::RobotInfo&) robotInfo = t.players[2 - 1];
received = true;
break;
}
}
//if(!received && &theBehaviorControlOutput && Global::getSettings().teamNumber == (int)theBehaviorControlOutput.ownTeamInfo.teamNumber)
if(!received && &theBehaviorControlOutput && 0 == (int)theBehaviorControlOutput.ownTeamInfo.teamNumber)
robotInfo = theBehaviorControlOutput.robotInfo;
//robotInfo.number = Global::getSettings().playerNumber;
robotInfo.number = 2;
}
void GameDataProvider::update(GameInfo& gameInfo, const FrameInfo& theFrameInfo, const BehaviorControlOutput& theBehaviorControlOutput)
{
//if((theFrameInfo.getTimeSince(gameControlTimeStamp) < 500 || !&theBehaviorControlOutput || Global::getSettings().teamNumber != (int)theBehaviorControlOutput.ownTeamInfo.teamNumber) &&
// (Global::getSettings().teamNumber == (int)gameControlData.teams[0].teamNumber || Global::getSettings().teamNumber == (int)gameControlData.teams[1].teamNumber))
if((theFrameInfo.getTimeSince(gameControlTimeStamp) < 500 || !&theBehaviorControlOutput || 0 != (int)theBehaviorControlOutput.ownTeamInfo.teamNumber) &&
(0 == (int)gameControlData.teams[0].teamNumber || 0 == (int)gameControlData.teams[1].teamNumber))
memcpy(&(RoboCup::RoboCupGameControlData&) gameInfo, &gameControlData, (char*) gameControlData.teams - (char*) &gameControlData);
//else if(&theBehaviorControlOutput && Global::getSettings().teamNumber == (int)theBehaviorControlOutput.ownTeamInfo.teamNumber)
else if(&theBehaviorControlOutput && 0 == (int)theBehaviorControlOutput.ownTeamInfo.teamNumber)
gameInfo = theBehaviorControlOutput.gameInfo;
gameInfo.timeSinceLastPackageReceived = (float) theFrameInfo.getTimeSince(gameControlTimeStamp);
}
bool KickEngineData::checkPhaseTime(const FrameInfo& frame, const JointAngles& ja, const TorsoMatrix& torsoMatrix)
{
timeSinceTimeStamp = frame.getTimeSince(timeStamp);
if(motionID < 0)
return false;
if(phaseNumber < currentParameters.numberOfPhases)
{
if(static_cast<unsigned int>(timeSinceTimeStamp) >= currentParameters.phaseParameters[phaseNumber].duration)
{
phaseNumber++;
timeStamp = frame.time;
timeSinceTimeStamp = frame.getTimeSince(timeStamp);
if(phaseNumber < currentParameters.numberOfPhases)
{
if(currentKickRequest.armsBackFix)
{
if(lElbowFront)
{
Vector3f inverse = currentParameters.phaseParameters[phaseNumber].controlPoints[Phase::leftHandRot][2];
inverse.x() *= -1.f;
addDynPoint(DynPoint(Phase::leftHandRot, phaseNumber, inverse), torsoMatrix);
}
if(rElbowFront)
{
Vector3f inverse = currentParameters.phaseParameters[phaseNumber].controlPoints[Phase::rightHandRot][2];
inverse.x() *= -1.f;
addDynPoint(DynPoint(Phase::rightHandRot, phaseNumber, inverse), torsoMatrix);
}
}
for(unsigned int i = 0; i < currentKickRequest.dynPoints.size(); i++)
if(currentKickRequest.dynPoints[i].phaseNumber == phaseNumber)
addDynPoint(currentKickRequest.dynPoints[i], torsoMatrix);
}
}
}
else if(currentParameters.loop && phaseNumber == currentParameters.numberOfPhases)
{
phaseNumber = 0;
//calculateOrigins(currentKickRequest, ja, torsoMatrix);
currentParameters.initFirstPhaseLoop(origins, currentParameters.phaseParameters[currentParameters.numberOfPhases - 1].comTra[2], Vector2f(ja.angles[Joints::headPitch], ja.angles[Joints::headYaw]));
for(unsigned int i = 0; i < currentKickRequest.dynPoints.size(); i++)
if(currentKickRequest.dynPoints[i].phaseNumber == phaseNumber)
addDynPoint(currentKickRequest.dynPoints[i], torsoMatrix);
}
return phaseNumber < currentParameters.numberOfPhases;
}
void GameDataProvider::update(OwnTeamInfo& ownTeamInfo, const FrameInfo& theFrameInfo, const BehaviorControlOutput& theBehaviorControlOutput, const FieldDimensions& theFieldDimensions)
{
if(!ownTeamInfoSet)
{
(RoboCup::TeamInfo&) ownTeamInfo = gameControlData.teams[0];
//ownTeamInfo.teamNumber = Global::getSettings().teamNumber;
ownTeamInfo.teamNumber = 0;
ownTeamInfoSet = true;
}
bool received = false;
if(theFrameInfo.getTimeSince(gameControlTimeStamp) < 500)
{
for(int i = 0; i < 2; ++i)
//if(Global::getSettings().teamNumber == (int)gameControlData.teams[i].teamNumber)
if(0 == (int)gameControlData.teams[i].teamNumber)
{
(RoboCup::TeamInfo&) ownTeamInfo = gameControlData.teams[i];
received = true;
break;
}
}
//if(!received && &theBehaviorControlOutput && Global::getSettings().teamNumber == (int)theBehaviorControlOutput.ownTeamInfo.teamNumber)
if(!received && &theBehaviorControlOutput && 0 == (int)theBehaviorControlOutput.ownTeamInfo.teamNumber)
ownTeamInfo = theBehaviorControlOutput.ownTeamInfo;
//MODIFY("representation:OwnTeamInfo", ownTeamInfo);
//theFieldDimensions.drawPolygons(ownTeamInfo.teamColor);
}
void TeamDataProvider::update(TeamMateData& teamMateData,
const FrameInfo& theFrameInfo,
const MotionRequest& theMotionRequest,
const MotionInfo& theMotionInfo,
const RobotInfo& theRobotInfo)
{
/*
DECLARE_PLOT("module:TeamDataProvider:sslVisionOffset");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset1"); // 1-4: players
DECLARE_PLOT("module:TeamDataProvider:ntpOffset2");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset3");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset4");
DECLARE_PLOT("module:TeamDataProvider:ntpOffset12"); // SSL vision data
PLOT("module:TeamDataProvider:ntpOffset1", ntp.timeSyncBuffers[1].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset2", ntp.timeSyncBuffers[2].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset3", ntp.timeSyncBuffers[3].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset4", ntp.timeSyncBuffers[4].bestOffset);
PLOT("module:TeamDataProvider:ntpOffset12", ntp.timeSyncBuffers[12].bestOffset);
*/
teamMateData = theTeamMateData;
teamMateData.numOfConnectedPlayers = 0;
teamMateData.firstTeamMate = teamMateData.secondTeamMate = teamMateData.thirdTeamMate = TeamMateData::numOfPlayers;
for(int i = TeamMateData::firstPlayer; i < TeamMateData::numOfPlayers; ++i)
if(teamMateData.timeStamps[i] && theFrameInfo.getTimeSince(teamMateData.timeStamps[i]) < networkTimeout)
{
if(!teamMateData.isPenalized[i] && teamMateData.hasGroundContact[i])
{
++teamMateData.numOfConnectedPlayers;
if(teamMateData.numOfConnectedPlayers == 1) teamMateData.firstTeamMate = i;
else if(teamMateData.numOfConnectedPlayers == 2) teamMateData.secondTeamMate = i;
else if(teamMateData.numOfConnectedPlayers == 3) teamMateData.thirdTeamMate = i;
}
else
teamMateData.timeStamps[i] = 0;
}
if(teamMateData.numOfConnectedPlayers > 0)
teamMateData.wasConnected = theTeamMateData.wasConnected = true;
/*teamMateData.sendThisFrame = (ntp.doSynchronization(theFrameInfo.time, Global::getTeamOut()) ||
theFrameInfo.getTimeSince(lastSentTimeStamp) >= 100) */ // TODO config file?
//#ifdef TARGET_ROBOT
// && !(theMotionRequest.motion == MotionRequest::specialAction && theMotionRequest.specialActionRequest.specialAction == SpecialActionRequest::playDead)
// && !(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::playDead)
//#endif
;
if(teamMateData.sendThisFrame)
{
lastSentTimeStamp = theFrameInfo.time;
//TEAM_OUTPUT(idTeamMateIsPenalized, bin, (theRobotInfo.penalty != PENALTY_NONE));
//TEAM_OUTPUT(idTeamMateHasGroundContact, bin, (theGroundContactState.contact || !theDamageConfiguration.useGroundContactDetection));
//TEAM_OUTPUT(idTeamMateIsUpright, bin, (theFallDownState.state == theFallDownState.upright));
//if(theGroundContactState.contact || !theDamageConfiguration.useGroundContactDetection)
// TEAM_OUTPUT(idTeamMateTimeSinceLastGroundContact, bin, lastSentTimeStamp);
}
}
bool KickEngineData::sitOutTransitionDisturbance(bool& compensate, bool& compensated, const FilteredSensorData& sd, KickEngineOutput& kickEngineOutput, const JointData& jd, const FrameInfo& frame)
{
if(compensate)
{
if(!startComp)
{
timeStamp = frame.time;
lSupp = false;
rSupp = false;
toLeftSupport = false;
comOffset = Vector2<>(0.f, 0.f);
origin = Vector2<>(0.f, 0.f);
balanceSum = Vector2<>(0.f, 0.f);
gyro = Vector2<>(0.f, 0.f);
lastGyroLeft = Vector2<>(0.f, 0.f);
lastGyroRight = Vector2<>(0.f, 0.f);
gyroErrorLeft = Vector2<>(0.f, 0.f);
gyroErrorRight = Vector2<>(0.f, 0.f);
bodyError = Vector2<>(0.f, 0.f);
lastBody = Vector2<>(0.f, 0.f);
lastCom = Vector3<>(0.f, 0.f, 0.f);
motionID = -1;
kickEngineOutput.isLeavingPossible = false;
for(int i = 0; i < JointData::numOfJoints; i++)
{
lastBalancedJointRequest.angles[i] = jd.angles[i];
compenJoints.angles[i] = jd.angles[i];
}
}
for(int i = 0; i < JointData::numOfJoints; i++)
{
kickEngineOutput.angles[i] = compenJoints.angles[i];
kickEngineOutput.jointHardness.hardness[i] = 100;
}
int time = frame.getTimeSince(timeStamp);
if((abs(sd.data[SensorData::gyroX]) < 0.1f && abs(sd.data[SensorData::gyroY]) < 0.1f && time > 200) || time > 1000)
{
compensate = false;
compensated = true;
return true;
}
else
{
return false;
}
}
return true;
}
void CameraMatrixProvider::update(CameraMatrixPrev& cameraMatrixPrev,
const TorsoMatrix& theTorsoMatrix,
const RobotCameraMatrixPrev& theRobotCameraMatrixPrev,
const CameraCalibration& theCameraCalibration,
const FrameInfo& theFrameInfo,
const FilteredJointDataPrev theFilteredJointDataPrev)
{
Pose3D torsoMatrixPrev(theTorsoMatrix);
torsoMatrixPrev.conc(theTorsoMatrix.offset.invert());
cameraMatrixPrev.computeCameraMatrix(torsoMatrixPrev, theRobotCameraMatrixPrev, theCameraCalibration);
cameraMatrixPrev.isValid = theFrameInfo.getTimeSince(theFilteredJointDataPrev.timeStamp) < 500;
}
void CameraMatrixProvider::update(CameraMatrixOther& cameraMatrixOther,
const TorsoMatrix& theTorsoMatrix,
const RobotCameraMatrixOther& theRobotCameraMatrixOther,
const CameraCalibration& theCameraCalibration,
const MotionInfo& theMotionInfo,
const FallDownState& theFallDownState,
const FrameInfo& theFrameInfo,
const FilteredJointData& theFilteredJointData,
const RobotInfo& theRobotInfo)
{
cameraMatrixOther.computeCameraMatrix(theTorsoMatrix, theRobotCameraMatrixOther, theCameraCalibration);
cameraMatrixOther.isValid = theTorsoMatrix.isValid && theMotionInfo.isMotionStable &&
theFallDownState.state == FallDownState::upright &&
theFrameInfo.getTimeSince(theFilteredJointData.timeStamp) < 500 &&
theRobotInfo.penalty == PENALTY_NONE;
}
void USObstacleGridProvider::ageCellState(const FrameInfo& theFrameInfo)
{
for(int i = 0; i < GRID_SIZE; ++i)
{
USObstacleGrid::Cell& c = cells[i];
if(c.state)
{
if(theFrameInfo.getTimeSince(c.lastUpdate) > parameters.cellFreeInterval)
{
c.state--;
c.lastUpdate = theFrameInfo.time;
}
c.cluster = 0;
}
}
}
bool KickEngineData::sitOutTransitionDisturbance(bool& compensate, bool& compensated, const InertialData& id, KickEngineOutput& kickEngineOutput, const JointAngles& ja, const FrameInfo& frame)
{
if(compensate)
{
if(!startComp)
{
timeStamp = frame.time;
gyro = Vector2f::Zero();
lastGyroLeft = Vector2f::Zero();
lastGyroRight = Vector2f::Zero();
gyroErrorLeft = Vector2f::Zero();
gyroErrorRight = Vector2f::Zero();
bodyError = Vector2f::Zero();
lastBody = Vector2f::Zero();
lastCom = Vector3f::Zero();
motionID = -1;
kickEngineOutput.isLeavingPossible = false;
for(int i = 0; i < Joints::numOfJoints; i++)
{
lastBalancedJointRequest.angles[i] = ja.angles[i];
compenJoints.angles[i] = ja.angles[i];
}
}
for(int i = 0; i < Joints::numOfJoints; i++)
{
kickEngineOutput.angles[i] = compenJoints.angles[i];
kickEngineOutput.stiffnessData.stiffnesses[i] = 100;
}
int time = frame.getTimeSince(timeStamp);
if((std::abs(id.gyro.x()) < 0.1f && std::abs(id.gyro.y()) < 0.1f && time > 200) || time > 1000)
{
compensate = false;
compensated = true;
return true;
}
else
return false;
}
return true;
}
void CameraMatrixProvider::update(CameraMatrix& cameraMatrix,
const TorsoMatrix& theTorsoMatrix,
const RobotCameraMatrix& theRobotCameraMatrix,
const CameraCalibration& theCameraCalibration,
const MotionInfo& theMotionInfo,
const FallDownState& theFallDownState,
const FrameInfo& theFrameInfo,
const FilteredJointData& theFilteredJointData,
const RobotInfo& theRobotInfo)
{
cameraMatrix.computeCameraMatrix(theTorsoMatrix, theRobotCameraMatrix, theCameraCalibration);
cameraMatrix.isValid = theTorsoMatrix.isValid && theMotionInfo.isMotionStable &&
theFallDownState.state == FallDownState::upright &&
theFrameInfo.getTimeSince(theFilteredJointData.timeStamp) < 500 &&
theRobotInfo.penalty == PENALTY_NONE;
//cout << "CameraMatrix z: " << cameraMatrix.translation.z << endl;
//DECLARE_DEBUG_DRAWING("module:CameraMatrixProvider:calibrationHelper", "drawingOnImage");
//COMPLEX_DRAWING("module:CameraMatrixProvider:calibrationHelper", drawFieldLines(cameraMatrix););
//TEAM_OUTPUT(idTeamCameraHeight, bin, cameraMatrix.translation.z);
}
void USObstacleGridProvider::update(USObstacleGrid& usObstacleGrid, const OdometryData& theOdometryData, const GameInfo& theGameInfo,
const RobotInfo& theRobotInfo, const MotionRequest& theMotionRequest,
const FrameInfo& theFrameInfo, const RobotPose& theRobotPose, const FilteredSensorData& theFilteredSensorData)
{
if(!initialized)
{
lastOdometry = theOdometryData;
cells = usObstacleGrid.cells;
for(int i = 0; i < GRID_SIZE; ++i)
cells[i].state = USObstacleGrid::Cell::FREE;
initialized = true;
}
else if((gameInfoGameStateLastFrame == STATE_SET) && (theGameInfo.state == STATE_PLAYING))
{
for(int i = 0; i < GRID_SIZE; ++i)
cells[i].state = USObstacleGrid::Cell::FREE;
}
//MODIFY("parameters:USObstacleGridProvider", parameters);
//DECLARE_DEBUG_DRAWING("module:USObstacleGridProvider:us", "drawingOnField");
ageCellState(theFrameInfo);
if((theRobotInfo.penalty == PENALTY_NONE) &&
(theMotionRequest.motion != MotionRequest::specialAction) &&
(theFrameInfo.getTimeSince(lastTimePenalized) > 3000))
{
moveGrid(theOdometryData);
checkUS(theFilteredSensorData, theFrameInfo, theOdometryData);
}
else if(theRobotInfo.penalty != PENALTY_NONE)
{
lastTimePenalized = theFrameInfo.time;
}
usObstacleGrid.drawingOrigin = theRobotPose;
usObstacleGrid.drawingOrigin.rotation = normalize(usObstacleGrid.drawingOrigin.rotation - accumulatedOdometry.rotation);
usObstacleGrid.cellOccupiedThreshold = parameters.cellOccupiedThreshold;
usObstacleGrid.cellMaxOccupancy = parameters.cellMaxOccupancy;
gameInfoGameStateLastFrame = theGameInfo.state;
}
void ArmContactModelProvider::update(ArmContactModel& ArmContactModel,
const JointRequest& theJointRequest,
const FallDownState& theFallDownState,
const MotionInfo& theMotionInfo,
const FilteredJointData& theFilteredJointData,
const FrameInfo& theFrameInfo)
{
//MODIFY("module:ArmContactModelProvider:parameters", p);
/*
DECLARE_PLOT("module:ArmContactModelProvider:contactLeft");
DECLARE_PLOT("module:ArmContactModelProvider:contactRight");
PLOT("module:ArmContactModelProvider:contactLeft", ArmContactModel.contactLeft ? 7.5 : 2.5);
PLOT("module:ArmContactModelProvider:contactRight", ArmContactModel.contactRight ? 7.5 : 2.5);
DECLARE_PLOT("module:ArmContactModelProvider:errorLeftX");
DECLARE_PLOT("module:ArmContactModelProvider:errorRightX");
DECLARE_PLOT("module:ArmContactModelProvider:errorLeftY");
DECLARE_PLOT("module:ArmContactModelProvider:errorRightY");
PLOT("module:ArmContactModelProvider:errorLeftX", errorLeftX);
PLOT("module:ArmContactModelProvider:errorRightX", errorRightX);
PLOT("module:ArmContactModelProvider:errorLeftY", errorLeftY);
PLOT("module:ArmContactModelProvider:errorRightY", errorRightY);
*/
/* Buffer arm angles */
struct ArmAngles angles;
angles.leftX = theJointRequest.angles[JointData::LShoulderPitch];
angles.leftY = theJointRequest.angles[JointData::LShoulderRoll];
angles.rightX = theJointRequest.angles[JointData::RShoulderPitch];
angles.rightY = theJointRequest.angles[JointData::RShoulderRoll];
angleBuffer.add(angles);
/* Decrease errors */
errorLeftX = max(0.0f, errorLeftX - p.errorXDecrease);
errorLeftY = max(0.0f, errorLeftY - p.errorYDecrease);
errorRightX = max(0.0f, errorRightX - p.errorXDecrease);
errorRightY = max(0.0f, errorRightY - p.errorYDecrease);
/* Reset in case of a fall */
if(theFallDownState.state == FallDownState::onGround)
{
errorLeftX = 0.0f;
errorLeftY = 0.0f;
errorRightX = 0.0f;
errorRightY = 0.0f;
}
/* Check for arm contact */
// motion types to take into account: stand, walk (if the robot is upright)
if((theMotionInfo.motion == MotionInfo::stand || theMotionInfo.motion == MotionInfo::walk) &&
(theFallDownState.state == FallDownState::upright || theFallDownState.state == FallDownState::staggering))
{
bool left = checkArm(true, theFilteredJointData);
bool right = checkArm(false, theFilteredJointData);
if(p.debugMode && theFrameInfo.getTimeSince(lastSoundTime) > soundDelay &&
((left && !ArmContactModel.contactLeft) || (right && !ArmContactModel.contactRight)))
{
lastSoundTime = theFrameInfo.time;
//SoundPlayer::play("arm.wav");
}
ArmContactModel.contactLeft = left;
ArmContactModel.contactRight = right;
}
else
{
ArmContactModel.contactLeft = false;
ArmContactModel.contactRight = false;
}
}
void MotionSelector::update(MotionSelection& motionSelection,
const MotionRequest& theMotionRequest,
const WalkingEngineOutput& theWalkingEngineOutput,
const GroundContactState& theGroundContactState,
const DamageConfiguration& theDamageConfiguration,
const FrameInfo& theFrameInfo)
{
static const int interpolationTimes[MotionRequest::numOfMotions] =
{
10, //790, // to walk
600, // to Bike, (could be 0)
10, // to specialAction
10, // to stand
};
static const int playDeadDelay(2000);
if(lastExecution)
{
MotionRequest::Motion requestedMotion = theMotionRequest.motion;
if(theMotionRequest.motion == MotionRequest::walk && ((!theGroundContactState.contactSafe && theDamageConfiguration.useGroundContactDetectionForSafeStates) || theWalkingEngineOutput.enforceStand))
requestedMotion = MotionRequest::stand;
if(forceStand && (lastMotion == MotionRequest::walk || lastMotion == MotionRequest::stand))
{
requestedMotion = MotionRequest::stand;
forceStand = false;
}
// check if the target motion can be the requested motion (mainly if leaving is possible)
if((lastMotion == MotionRequest::walk && (!&theWalkingEngineOutput || theWalkingEngineOutput.isLeavingPossible || (!theGroundContactState.contactSafe && theDamageConfiguration.useGroundContactDetectionForSafeStates))) ||
lastMotion == MotionRequest::stand || // stand can always be left
(lastMotion == MotionRequest::specialAction) || //&& (!&theSpecialActionsOutput || theSpecialActionsOutput.isLeavingPossible)) ||
// (lastMotion == MotionRequest::bike && (!&theBikeEngineOutput || theBikeEngineOutput.isLeavingPossible)) ||
(requestedMotion == MotionRequest::specialAction &&
(theMotionRequest.specialActionRequest.specialAction == SpecialActionRequest::standUpBackNao ||
theMotionRequest.specialActionRequest.specialAction == SpecialActionRequest::standUpFrontNao/* ||
theMotionRequest.specialActionRequest.specialAction == SpecialActionRequest::layDownKeeper*/)))
{
motionSelection.targetMotion = requestedMotion;
}
if(requestedMotion == MotionRequest::bike)
motionSelection.bikeRequest = theMotionRequest.bikeRequest;
else
motionSelection.bikeRequest = BikeRequest();
if(requestedMotion == MotionRequest::walk)
motionSelection.walkRequest = theMotionRequest.walkRequest;
else
motionSelection.walkRequest = WalkRequest();
if(requestedMotion == MotionRequest::specialAction)
{
motionSelection.specialActionRequest = theMotionRequest.specialActionRequest;
}
else
{
motionSelection.specialActionRequest = SpecialActionRequest();
if(motionSelection.targetMotion == MotionRequest::specialAction)
motionSelection.specialActionRequest.specialAction = SpecialActionRequest::numOfSpecialActionIDs;
}
// increase / decrease all ratios according to target motion
const unsigned deltaTime(theFrameInfo.getTimeSince(lastExecution));
int interpolationTime = prevMotion == MotionRequest::specialAction && lastActiveSpecialAction == SpecialActionRequest::playDead ? playDeadDelay : interpolationTimes[motionSelection.targetMotion];
// no play dead for walk
if (motionSelection.targetMotion == MotionRequest::walk)
interpolationTime = interpolationTimes[motionSelection.targetMotion];
float delta((float)deltaTime / interpolationTime);
// ASSERT(SystemCall::getMode() == SystemCall::logfileReplay || delta > 0.00001f);
float sum(0);
for(int i = 0; i < MotionRequest::numOfMotions; i++)
{
if(i == motionSelection.targetMotion)
motionSelection.ratios[i] += delta;
else
motionSelection.ratios[i] -= delta;
motionSelection.ratios[i] = std::max(motionSelection.ratios[i], 0.0f); // clip ratios
sum += motionSelection.ratios[i];
}
ASSERT(sum != 0);
// normalizeBH ratios
for(int i = 0; i < MotionRequest::numOfMotions; i++)
{
motionSelection.ratios[i] /= sum;
if(abs(motionSelection.ratios[i] - 1.f) < 0.00001f)
motionSelection.ratios[i] = 1.f; // this should fix a "motionSelection.ratios[motionSelection.targetMotion] remains smaller than 1.f" bug
}
if(motionSelection.ratios[MotionRequest::specialAction] < 1.f)
{
if(motionSelection.targetMotion == MotionRequest::specialAction)
motionSelection.specialActionMode = MotionSelection::first;
else
motionSelection.specialActionMode = MotionSelection::deactive;
}
else
motionSelection.specialActionMode = MotionSelection::active;
if(motionSelection.specialActionMode == MotionSelection::active && motionSelection.specialActionRequest.specialAction != SpecialActionRequest::numOfSpecialActionIDs)
lastActiveSpecialAction = motionSelection.specialActionRequest.specialAction;
if(motionSelection.ratios[MotionRequest::walk] < 1.f)
motionSelection.walkRequest = WalkRequest();
}
lastExecution = theFrameInfo.time;
if(lastMotion != motionSelection.targetMotion)
prevMotion = lastMotion;
lastMotion = motionSelection.targetMotion;
// PLOT("module:MotionSelector:ratios:walk", motionSelection.ratios[MotionRequest::walk]);
// PLOT("module:MotionSelector:ratios:stand", motionSelection.ratios[MotionRequest::stand]);
// PLOT("module:MotionSelector:ratios:specialAction", motionSelection.ratios[MotionRequest::specialAction]);
// PLOT("module:MotionSelector:lastMotion", lastMotion);
// PLOT("module:MotionSelector:prevMotion", prevMotion);
// PLOT("module:MotionSelector:targetMotion", motionSelection.targetMotion);
}
