std::vector<float> EHCLSOptimiser::getNextParameters()
{
m_previous_parameters = m_current_parameters;
mutateBestParameters(m_current_parameters);
return Parameter::getAsVector(m_current_parameters);
}
void Optimiser::tickOptimiser(float speed, float power)
{
static bool initialised = false; // we need to initialise the optimisation on the first tick
static float cost = 0; // the specific cost of transport
cost = power/(NAO_WEIGHT*speed);
CurrentSpeed = speed;
CurrentCost = cost;
if (initialised == false)
{
if (LeftStep == NULL || RightStep == NULL) // This is highly unlikely, but just in case; we are not initialised unless we have both the left and right step
return;
SpeedImprovement = 0;
Alpha = 0;
BestSpeed = speed;
BestCost = cost;
initBestParameters();
CountSinceLastImprovement = 0;
SpeedPreviousImprovement = 2.5;
CostPreviousImprovement = 0.3;
initialised = true;
#if OPTIMISER_VERBOSITY > 3
thelog << "OPTIMISER: tickOptimiser initialised." << endl;
#endif
}
#if OPTIMISER_VERBOSITY > 3
thelog << "OPTIMISER: tickOptimiser on " << LeftStep->Name << " and " << RightStep->Name << " with speed " << speed << ", power " << power << ", and cost " << cost << endl;
#endif
if (cost < BestCost && balanceFallenCount == 0) // speed > BestSpeed
{
#if OPTIMISER_VERBOSITY > 2
thelog << "OPTIMISER: tickOptimiser Improvement." << endl;
#endif
SpeedImprovement = speed - BestSpeed;
CostImprovement = cost - BestCost;
//Alpha = 0.9*fabs(tanh(SpeedImprovement/SpeedPreviousImprovement));
Alpha = 0.9*fabs(tanh(fabs(CostImprovement/CostPreviousImprovement)));
copyToBestParameters();
BestSpeed = speed;
BestCost = cost;
CountSinceLastImprovement = 0;
#if OPTIMISER_VERBOSITY > 2
thelog << "OPTIMISER: tickOptimiser BestSpeed:" << BestSpeed << " BestCost: " << BestCost << "SpeedImprovement:" << SpeedImprovement << " PreviousSpeedImprovement:" << SpeedPreviousImprovement << "CostImprovement:" << CostImprovement << "PreviousCostImprovement:" << CostPreviousImprovement << " Alpha:" << Alpha << endl;
#endif
SpeedPreviousImprovement = SpeedImprovement;
CostPreviousImprovement = CostImprovement;
}
else
CountSinceLastImprovement++;
if (CountSinceLastImprovement > ResetLimit)
{
Alpha *= 0.9;
CountSinceLastImprovement = 0;
BestSpeed *= 0.97;
BestCost *= 1.1;
#if OPTIMISER_VERBOSITY > 2
thelog << "OPTIMISER: tickOptimiser Reseting" << endl;
#endif
}
mutateBestParameters();
writeOptimiserLog();
Iteration++;
return;
}
