GTEST_TEST(DownhillSimplex, bench4)
{
std::array<Rangef, 4> ranges;
ranges[0] = Rangef(-50.f, 50);
ranges[1] = Rangef(-50.f, 50);
ranges[2] = Rangef(0, 1);
ranges[3] = Rangef(0, 1);
float timeToStepTarget = 0.3f;
float timeAfterStepTarget = 0.3f;
auto errorFunction = [timeToStepTarget, timeAfterStepTarget]
(const Vector4f& vals)
{
LIP currentState(250.f);
LIP targetState(250.f);
float footTrans = -50.f;
float posWeight = 1;
float velWeight = 50;
float timeWeight = 500;
const float currentZmp = vals(0);
const float nextZmp = vals(1);
const float timeToStep = vals(2);
const float timeAfterStep = vals(3);
LIP forwardedState = currentState.predict(timeToStep, currentZmp);
forwardedState.position += footTrans;
forwardedState.update(timeAfterStep, nextZmp);
return posWeight * sqr(targetState.position - forwardedState.position) +
velWeight * sqr(targetState.velocity - forwardedState.velocity) +
timeWeight * sqr(timeToStepTarget - timeToStep) +
timeWeight * sqr(timeAfterStepTarget - timeAfterStep);
};
auto optimizer = Optimizer::makeDownhillSimplexOptimizer(errorFunction, ranges);
Eigen::BenchTimer optTimer;
for(int i = 0; i < TRIES; ++i)
{
optimizer.initDelta(Vector4f(-1.f, 0.f, timeToStepTarget, timeAfterStepTarget),
Vector4f(10.f, 10.f, 0.1f, 0.1f));
// waste some cycles to get the return type automatically
auto res = optimizer.optimize(0.0001f, 1);
optTimer.start();
for(int j = 0; j < RUNS; ++j)
{
res = optimizer.optimize(0.0001f, 1000);
}
optTimer.stop();
}
PRINTF("overall - best: %.3fs, avg: %.3fs, worst: %.3fs \n", optTimer.best(), optTimer.total() / TRIES, optTimer.worst());
PRINTF("per run - best: %.6fs, avg: %.6fs, worst: %.6fs \n", optTimer.best() / RUNS, optTimer.total() / (TRIES * RUNS), optTimer.worst() / RUNS);
}
void Ferns::train( const Image & img )
{
RNG rng( time( NULL ) );
Eigen::Vector2i x0, x1;
for( uint32_t i = 0; i < _numFerns; i++ ){
_ferns.push_back( Fern( _testsPerFern, _patchSize ) );
for( uint32_t t = 0; t < _testsPerFern; t++ ){
x0[ 0 ] = rng.uniform( 0, _patchSize );
x0[ 1 ] = rng.uniform( 0, _patchSize );
x1[ 0 ] = rng.uniform( 0, _patchSize );
x1[ 1 ] = rng.uniform( 0, _patchSize );
_ferns.back().addTest( x0, x1 );
}
}
// detect features in the "model"-image
std::vector<Feature2Df> features;
VectorFeature2DInserter<float> inserter( features );
_featureDetector->extract( img, inserter );
int32_t patchHalfSize = _patchSize >> 1;
/* train the class */
PatchGenerator patchGen( Rangef( 0.0f, Math::TWO_PI ), Rangef( 0.6f, 1.5f ), _patchSize, 3.0 /* noise */ );
int x, y;
for( size_t i = 0; i < features.size(); i++ ){
std::cout << "FEATURE " << i+1 << " / " << features.size() << std::endl;
x = features[ i ].pt.x;
y = features[ i ].pt.y;
if( ( x - patchHalfSize ) < 0 ||
( x + patchHalfSize ) >= ( int32_t )img.width() ||
( y - patchHalfSize ) < 0 ||
( y + patchHalfSize ) >= ( int32_t )img.height() )
continue;
_modelFeatures.push_back( features[ i ] );
this->trainClass( _modelFeatures.size() - 1, patchGen, img );
}
for( size_t i = 0; i < _ferns.size(); i++ ){
_ferns[ i ].normalizeStatistics( _trainingSamples );
}
}
void CMA_ES_Population::init(const Configuration& configuration)
{
this->configuration = configuration;
n = static_cast<unsigned>(configuration.initialization.size());
sigma = configuration.sigma;
stopfitness = configuration.stopFitness;
stopeval = 1e3f * std::pow(static_cast<float>(n), 2.f);
lambda_ = 4 + static_cast<unsigned>(floorf(3.f * std::log(static_cast<float>(n))));
mu = lambda_ / 2.f;
weights = VectorXf(static_cast<unsigned>(mu));
for(int i = 1; i <= weights.size(); i++)
weights[i - 1] = std::log(mu + 0.5f) - std::log(static_cast<float>(i));
mu = floorf(mu);
weights /= weights.sum();
mueff = std::pow(weights.sum(), 2.f) / weights.array().pow(2).sum();
cc = (4 + mueff / static_cast<float>(n)) / (static_cast<float>(n) + 4.f + 2.f * mueff / static_cast<float>(n));
cs = (mueff + 2.f) / (static_cast<float>(n) + mueff + 5.f);
c1 = 2.f / (std::pow(static_cast<float>(n) + 1.3f, 2.f) + mueff);
cmu = 2.f * (mueff - 2.f + 1.f / mueff) / (std::pow(static_cast<float>(n) + 1.f, 2.f) + 2.f * mueff / 2.f);
damps = 1.f + 2.f * std::max(0.f, std::sqrt((mueff - 1.f) / (static_cast<float>(n) + 1.f)) - 1.f) + cs;
pc = MatrixXf::Zero(n, 1);
ps = MatrixXf::Zero(n, 1);
B = D = MatrixXf::Identity(n, n);
C = B* D* (B* D).transpose();
chiN = std::pow(static_cast<float>(n), 0.5f) * (1.f - 1.f / (4.f * static_cast<float>(n)) + 1.f / (21.f * std::pow(static_cast<float>(n), 2.f)));
xmean = Eigen::Map<const Eigen::VectorXf>(configuration.initialization.data(), n);
for(unsigned i = 0; i < n; i++)
{
xmean[i] = configuration.limits[i].limit(xmean[i]);
xmean[i] = std::atanh((xmean[i] - configuration.limits[i].min) / (configuration.limits[i].max - configuration.limits[i].min) * 2.f - 1.f);
xmean[i] = Rangef(-3.f, 3.f).limit(xmean[i]);
}
total = lambda_;
numFeatures = n;
fitness.resize(total);
updateIndividuals();
initialized = true;
somethingChanged = true;
}
GTEST_TEST(DownhillSimplex, bench1)
{
using Vector1f = Eigen::Matrix<float, 1, 1>;
auto errorFunction = []
(const Vector1f& vals)
{
const LIP currentState(250.f);
const LIP targetState(250.f);
float timeToStep = 0.3f;
float timeAfterStep = 0.3f;
float footTrans = -50.f;
float posWeight = 1;
float velWeight = 50;
const float currentZmp = vals(0);
LIP forwardedState = currentState.predict(timeToStep, currentZmp);
forwardedState.position += footTrans;
forwardedState.update(timeAfterStep, 0.f);
return posWeight * sqr(targetState.position - forwardedState.position) +
velWeight * sqr(targetState.velocity - forwardedState.velocity);
};
std::array<Rangef, 1> ranges;
ranges[0] = Rangef(-50.f, 50);
auto optimizer = Optimizer::makeDownhillSimplexOptimizer(errorFunction, ranges);
auto errorFunction2 = []
(const float& currentZmp)
{
const LIP currentState(250.f);
const LIP targetState(250.f);
float timeToStep = 0.3f;
float timeAfterStep = 0.3f;
float footTrans = -50.f;
float posWeight = 1;
float velWeight = 50;
LIP forwardedState = currentState.predict(timeToStep, currentZmp);
forwardedState.position += footTrans;
forwardedState.update(timeAfterStep, 0.f);
return posWeight * sqr(targetState.position - forwardedState.position) +
velWeight * sqr(targetState.velocity - forwardedState.velocity);
};
const Rangef range(-50.f, 50);
auto optimizer2 = Optimizer::makeDownhillSimplexOptimizer1(errorFunction2, range);
Eigen::BenchTimer optTimer;
Eigen::BenchTimer opt2Timer;
for(int i = 0; i < TRIES; ++i)
{
Vector1f start, delta;
start.x() = -1.f;
delta.x() = 10.f;
optimizer.initRandom(start);
auto copy = optimizer.simplex;
// waste some cycles to get the return type automatically
auto res = optimizer.optimize(0.0001f, 1);
optTimer.start();
for(int j = 0; j < RUNS; ++j)
{
optimizer.simplex = copy;
res = optimizer.optimize(0.0001f, 100);
}
optTimer.stop();
optimizer2.simplex[0] = copy[0].values[0];
optimizer2.simplex[1] = copy[1].values[0];
auto copy2 = optimizer2.simplex;
Optimizer::Result<float> res2;
opt2Timer.start();
for(int j = 0; j < RUNS; ++j)
{
optimizer2.simplex = copy2;
res2 = optimizer2.optimize(0.0001f, 100);
}
opt2Timer.stop();
if(res.best(0) == res2.best)
EXPECT_TRUE(res.best(0) == res2.best);
}
PRINTF("generic optimizer overall - best: %.3fs, avg: %.3fs, worst: %.3fs \n", optTimer.best(), optTimer.total() / TRIES, optTimer.worst());
PRINTF("generic optimizer per run - best: %.6fs, avg: %.6fs, worst: %.6fs \n", optTimer.best() / RUNS, optTimer.total() / (TRIES * RUNS), optTimer.worst() / RUNS);
PRINTF("special optimizer overall - best: %.3fs, avg: %.3fs, worst: %.3fs \n", opt2Timer.best(), opt2Timer.total() / TRIES, opt2Timer.worst());
PRINTF("special optimizer per run - best: %.6fs, avg: %.6fs, worst: %.6fs \n", opt2Timer.best() / RUNS, opt2Timer.total() / (TRIES * RUNS), opt2Timer.worst() / RUNS);
}
void HeadMotionEngine::update(HeadJointRequest& headJointRequest)
{
// update requested angles
requestedPan = theHeadAngleRequest.pan;
requestedTilt = theHeadAngleRequest.tilt;
//
float maxAcc = theGroundContactState.contact ? 10.f : 1.f; // arbitrary value that seems to be good...
MODIFY("module:HeadMotionEngine:maxAcceleration", maxAcc);
float pan = requestedPan == JointAngles::off ? JointAngles::off : Rangef(theHeadLimits.minPan(), theHeadLimits.maxPan()).limit(requestedPan);
float tilt = requestedTilt == JointAngles::off ? JointAngles::off : theHeadLimits.getTiltBound(pan).limit(requestedTilt);
const float deltaTime = theFrameInfo.cycleTime;
const Vector2f position(headJointRequest.pan == JointAngles::off ? theJointAngles.angles[Joints::headYaw] : headJointRequest.pan,
headJointRequest.tilt == JointAngles::off ? theJointAngles.angles[Joints::headPitch] : headJointRequest.tilt);
const Vector2f target(pan == JointAngles::off ? 0.f : pan, tilt == JointAngles::off ? 0.f : tilt);
Vector2f offset(target - position);
const float distanceToTarget = offset.norm();
// calculate max speed
const float maxSpeedForDistance = std::sqrt(2.f * distanceToTarget * maxAcc * 0.8f);
const float requestedSpeed = theHeadAngleRequest.stopAndGoMode
? theHeadAngleRequest.speed * (std::cos(pi2 / stopAndGoModeFrequenzy * theFrameInfo.time) / 2.f + .5f)
: static_cast<float>(theHeadAngleRequest.speed);
const float maxSpeed = std::min(maxSpeedForDistance, requestedSpeed);
// max speed clipping
if(distanceToTarget / deltaTime > maxSpeed)
offset *= maxSpeed * deltaTime / distanceToTarget; //<=> offset.normalize(maxSpeed * deltaTime);
// max acceleration clipping
Vector2f speed(offset / deltaTime);
Vector2f acc((speed - lastSpeed) / deltaTime);
const float accSquareAbs = acc.squaredNorm();
if(accSquareAbs > maxAcc * maxAcc)
{
acc *= maxAcc * deltaTime / std::sqrt(accSquareAbs);
speed = acc + lastSpeed;
offset = speed * deltaTime;
}
/* <=>
Vector2f speed(offset / deltaTime);
Vector2f acc((speed - lastSpeed) / deltaTime);
if(acc.squaredNorm() > maxAcc * maxAcc)
{
speed = acc.normalize(maxAcc * deltaTime) + lastSpeed;
offset = speed * deltaTime;
}
*/
PLOT("module:HeadMotionEngine:speed", toDegrees(speed.norm()));
// calculate new position
Vector2f newPosition(position + offset);
// set new position
headJointRequest.pan = pan == JointAngles::off ? JointAngles::off : newPosition.x();
headJointRequest.tilt = tilt == JointAngles::off ? JointAngles::off : newPosition.y();
headJointRequest.moving = pan != JointAngles::off && tilt != JointAngles::off && ((newPosition - position) / deltaTime).squaredNorm() > sqr(maxAcc * deltaTime * 0.5f);
// check reachability
headJointRequest.reachable = true;
if(pan != requestedPan || tilt != requestedTilt)
headJointRequest.reachable = false;
// store some values for the next iteration
lastSpeed = speed;
}
