44 lines
1.7 KiB
C++
44 lines
1.7 KiB
C++
#include "kinematics.hpp"
|
|
#include <chrono>
|
|
#include <iomanip>
|
|
#include <iostream>
|
|
#include <vector>
|
|
using namespace hexapod;
|
|
using Clock = std::chrono::steady_clock;
|
|
volatile double sink = 0;
|
|
int main() {
|
|
Model m;
|
|
constexpr int samples=20000, warmup=1000;
|
|
for(int mode=0;mode<3;++mode) {
|
|
Six seed{};
|
|
std::vector<double> times; times.reserve(samples);
|
|
int failures=0,maxIterations=0; double iterationSum=0;
|
|
for(int n=-warmup;n<samples;++n) {
|
|
const double t=(n+warmup)*.001;
|
|
Six q{10*std::sin(t),8*std::cos(t),5*std::sin(t),3*std::sin(t),2*std::cos(t),4*std::sin(t)};
|
|
const auto begin=Clock::now();
|
|
const auto command=m.inverse(q); // One IK plus one FK per timed call.
|
|
auto measured=command;
|
|
if(mode==2) measured[0]=1000; // Impossible: exercise nonconvergent path.
|
|
const auto result=forward(m,measured,mode==0 ? seed : Six{});
|
|
const auto end=Clock::now();
|
|
sink=command[0]+result.pose[0]+result.residual;
|
|
if(mode==0 && result.converged) seed=result.pose;
|
|
if(n>=0) {
|
|
times.push_back(std::chrono::duration<double,std::micro>(end-begin).count());
|
|
failures+=!result.converged;
|
|
maxIterations=std::max(maxIterations,result.iterations);
|
|
iterationSum+=result.iterations;
|
|
}
|
|
}
|
|
std::sort(times.begin(),times.end());
|
|
double sum=0; for(double t:times) sum+=t;
|
|
std::cout<<std::fixed<<std::setprecision(3)
|
|
<<(mode==0 ? "tracking" : mode==1 ? "home seed" : "nonconvergent")
|
|
<<": mean_us="<<sum/samples<<" median_us="<<times[samples/2]
|
|
<<" p99_us="<<times[samples*99/100]<<" max_us="<<times.back()
|
|
<<" mean_iterations="<<iterationSum/samples<<" max_iterations="<<maxIterations
|
|
<<" failures="<<failures<<"/"<<samples<<"\n";
|
|
}
|
|
}
|