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); }