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