SpecialActions::SpecialActions() : wasEndOfSpecialAction(false), //stiffnessInterpolationStart(0), stiffnessInterpolationCounter(0), stiffnessInterpolationLength(0), wasActive(false), dataRepetitionCounter(0), lastSpecialAction(SpecialActionRequest::numOfSpecialActionIDs), mirror(false) { theInstance = this; std::vector<float> motionData; char errorBuffer[10000]; MofCompiler* mofCompiler = new MofCompiler; if(!mofCompiler->compileMofs(errorBuffer, sizeof(errorBuffer), motionData)) OUTPUT_TEXT("Error while parsing mof files:"); else motionNetData.load(motionData); delete mofCompiler; if(*errorBuffer) OUTPUT_TEXT(" " << errorBuffer); // create an uninitialised motion request to set startup motion currentNode = motionNetData.label_extern_start[SpecialActionRequest().specialAction]; // read entries from file InMapFile cm("specialActions.cfg"); if(!cm.exists()) { OUTPUT_ERROR("'specialActions.cfg' not found."); } else { OdometryParams infos; cm >> infos; for(std::vector<SpecialActionInfo>::const_iterator it = infos.specialActionInfos.begin(); it != infos.specialActionInfos.end(); it++) { infoTable[it->id] = SpecialActionInfo(*it); if(it->type == SpecialActionInfo::once || it->type == SpecialActionInfo::homogeneous) { infoTable[it->id].odometryOffset.rotation = it->odometryOffset.rotation; if(it->type == SpecialActionInfo::homogeneous) { // convert from mm/seconds to mm/tick float motionCycleTime = theFrameInfo.cycleTime; infoTable[it->type].odometryOffset.translation.x() *= motionCycleTime; infoTable[it->type].odometryOffset.translation.y() *= motionCycleTime; // convert from rad/seconds to rad/tick infoTable[it->type].odometryOffset.rotation *= motionCycleTime; } } } } }
SpecialActions::SpecialActions() : wasEndOfSpecialAction(false), //hardnessInterpolationStart(0), hardnessInterpolationCounter(0), hardnessInterpolationLength(0), wasActive(false), dataRepetitionCounter(0), lastSpecialAction(SpecialActionRequest::numOfSpecialActionIDs), mirror(false) { theInstance = this; InConfigFile file("specialActions.dat"); if(!file.exists() || file.eof()) { OUTPUT(idText, text, "SpecialActions : Error, 'specialActions.dat' not found."); } else motionNetData.load(file); // create an uninitialised motion request to set startup motion currentNode = motionNetData.label_extern_start[SpecialActionRequest().specialAction]; // read entries from file InMapFile cm("specialActions.cfg"); if(!cm.exists()) { OUTPUT(idText, text, "SpecialActions : Error, 'specialActions.cfg' not found."); } else { OdometryParams infos; cm >> infos; for(std::vector<SpecialActionInfo>::const_iterator it = infos.specialActionInfos.begin(); it != infos.specialActionInfos.end(); it++) { infoTable[it->id] = SpecialActionInfo(*it); if(it->type == SpecialActionInfo::once || it->type == SpecialActionInfo::homogeneous) { infoTable[it->id].odometryOffset.rotation = it->odometryOffset.rotation; if(it->type == SpecialActionInfo::homogeneous) { // convert from mm/seconds to mm/tick float motionCycleTime = theFrameInfo.cycleTime; infoTable[it->type].odometryOffset.translation.x *= motionCycleTime; infoTable[it->type].odometryOffset.translation.y *= motionCycleTime; // convert from rad/seconds to rad/tick infoTable[it->type].odometryOffset.rotation *= motionCycleTime; } } } } }
void MotionSelector::update(MotionSelection& motionSelection) { static int interpolationTimes[MotionRequest::numOfMotions]; interpolationTimes[MotionRequest::walk] = 790; interpolationTimes[MotionRequest::kick] = 200; interpolationTimes[MotionRequest::specialAction] = 200; interpolationTimes[MotionRequest::stand] = 600; interpolationTimes[MotionRequest::getUp] = 600; static const int playDeadDelay(2000); if(lastExecution) { MotionRequest::Motion requestedMotion = theMotionRequest.motion; if(theMotionRequest.motion == MotionRequest::walk && !theGroundContactState.contact) 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.contact)) || lastMotion == MotionRequest::stand || // stand can always be left (lastMotion == MotionRequest::specialAction && theSpecialActionsOutput.isLeavingPossible) || (lastMotion == MotionRequest::kick && theKickEngineOutput.isLeavingPossible) || (lastMotion == MotionRequest::getUp && theGetUpEngineOutput.isLeavingPossible)) //never immediatly leave kick or get up { motionSelection.targetMotion = requestedMotion; } 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)); const int interpolationTime = prevMotion == MotionRequest::specialAction && lastActiveSpecialAction == SpecialActionRequest::playDead ? playDeadDelay : 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); // normalize ratios for(int i = 0; i < MotionRequest::numOfMotions; i++) { motionSelection.ratios[i] /= sum; if(std::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; } 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); }
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); }