51 lines
2.7 KiB
C++
51 lines
2.7 KiB
C++
#include "kinematics.hpp"
|
|
#include <iostream>
|
|
#include <random>
|
|
#include <stdexcept>
|
|
using namespace hexapod;
|
|
void check(bool value,const char* message) { if(!value) throw std::runtime_error(message); }
|
|
int main() {
|
|
Model m; Six home{}; auto lengths=m.inverse(home);
|
|
check(m.withinStroke(lengths),"home stroke");
|
|
for(int i=0;i<6;++i) {
|
|
double delta=40*rad;
|
|
double expected=std::sqrt(250*250+200*200+120*120-2*200*120*std::cos(delta));
|
|
check(std::abs(lengths[i]-expected)<1e-10,"analytic home length");
|
|
}
|
|
// Attachment order around the platform must match the base order; a
|
|
// reversal within a pair reproduces the original crossed-leg regression.
|
|
double winding=0;
|
|
for(int i=0;i<6;++i) {
|
|
const auto& a=m.top[i]; const auto& b=m.top[(i+1)%6];
|
|
double turn=std::atan2(a[0]*b[1]-a[1]*b[0],a[0]*b[0]+a[1]*b[1]);
|
|
check(turn>0,"platform attachment order must not reverse");
|
|
winding+=turn;
|
|
check(std::abs(lengths[i]-lengths[0])<1e-10,"symmetric home lengths");
|
|
}
|
|
check(std::abs(winding-360*rad)<1e-10,"platform winding");
|
|
auto p=rotate({1,0,0},{0,0,0,0,0,90}); check(std::abs(p[0])<1e-12 && std::abs(p[1]-1)<1e-12,"rotation convention");
|
|
std::mt19937 rng(1234); std::uniform_real_distribution<double> u(-1,1);
|
|
double worst=0; int maxIterations=0;
|
|
for(int n=0;n<1000;++n) {
|
|
Six q{}; for(int j=0;j<6;++j) q[j]=u(rng)*(j<3?20:8);
|
|
auto l=m.inverse(q); auto f=forward(m,l,home);
|
|
check(f.converged,"roundtrip convergence");
|
|
for(int j=0;j<6;++j) { worst=std::max(worst,std::abs(f.pose[j]-q[j])); check(std::abs(f.pose[j]-q[j])<1e-5,"roundtrip pose"); }
|
|
maxIterations=std::max(maxIterations,f.iterations);
|
|
}
|
|
Six invalid=home; invalid[0]=std::numeric_limits<double>::quiet_NaN();
|
|
check(!forward(m,invalid,home).converged,"NaN rejection");
|
|
check(!forward(m,lengths,invalid).converged,"NaN seed rejection");
|
|
auto impossible=lengths; impossible[0]=-1; check(!forward(m,impossible,home).converged,"negative length rejection");
|
|
check(!forward(m,m.inverse({10,5,2,3,1,1}),home,0).converged,"iteration limit");
|
|
check(!m.withinStroke(m.inverse({0,0,500,0,0,0})),"stroke rejection");
|
|
Geometry bad; bad.platformRadius=0; check(!forward(Model(bad),lengths,home).converged,"invalid geometry");
|
|
// Follow a smooth trajectory using the previous solution as seed.
|
|
Six seed{};
|
|
for(int n=0;n<1000;++n) {
|
|
double t=n*.01; 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)};
|
|
auto f=forward(m,m.inverse(q),seed); check(f.converged,"tracking"); seed=f.pose;
|
|
}
|
|
std::cout<<"PASS: 1000 random IK/FK roundtrips, trajectory and invalid inputs. Worst pose error "<<worst<<", max iterations "<<maxIterations<<"\n";
|
|
}
|