34 lines
1.9 KiB
C++
34 lines
1.9 KiB
C++
#include "kinematics.hpp"
|
|
#include <iostream>
|
|
#include <random>
|
|
#include <stdexcept>
|
|
void check(bool ok,const char* msg) { if(!ok) throw std::runtime_error(msg); }
|
|
int main() {
|
|
arm::Model m;
|
|
arm::Six zero{}; const auto home=m.pose(m.home);
|
|
for(double x:home) check(std::abs(x)<1e-10,"home pose offsets");
|
|
// Independent straight-arm and base-yaw geometry checks.
|
|
auto t=m.forward(zero); check(std::abs(t.p[0]-630)<1e-10 && t.p[2]==300,"straight arm dimensions");
|
|
zero[0]=90; t=m.forward(zero); check(std::abs(t.p[0])<1e-9 && std::abs(t.p[1]-630)<1e-9,"base rotation");
|
|
auto half=arm::logRotation(arm::rotation(0,arm::pi)); check(std::abs(std::abs(half[0])-arm::pi)<1e-8,"180 degree orientation error");
|
|
std::mt19937 rng(42); std::uniform_real_distribution<double> jitter(-8,8);
|
|
for(int sample=0;sample<500;++sample) {
|
|
auto q=m.home; for(double& v:q) v+=jitter(rng);
|
|
const auto pose=m.pose(q); auto r=m.inverse(pose,m.home);
|
|
check(r.converged && m.withinLimits(r.joints),"near-home IK round trip");
|
|
check(r.residual<1e-5,"round trip residual");
|
|
}
|
|
auto seed=m.home;
|
|
for(int i=0;i<1000;++i) {
|
|
auto q=m.home; for(int k=0;k<6;++k) q[k]+=10*std::sin(i*.005+k*.4);
|
|
auto r=m.inverse(m.pose(q),seed); check(r.converged,"continuous IK tracking"); seed=r.joints;
|
|
}
|
|
check(!m.inverse({10000,0,0,0,0,0},m.home).converged,"unreachable target");
|
|
auto bad=m.home; bad[0]=1000; check(!m.inverse({},bad).converged,"invalid seed");
|
|
arm::Geometry invalid; invalid.tool=-1; check(!arm::Model(invalid).inverse({},m.home).converged,"invalid geometry");
|
|
auto nan=arm::Six{}; nan[0]=NAN; check(!m.inverse(nan,m.home).converged,"nonfinite pose");
|
|
// Straight wrist is singular but holding its current pose must remain valid.
|
|
arm::Six singular{}; check(m.inverse(m.pose(singular),singular).converged,"singular pose hold");
|
|
std::cout<<"PASS: geometry, orientation, 500 IK round trips, 1000 tracking samples, invalid and unreachable inputs\n";
|
|
}
|