bool twoLinkArm::unitTests()
{
point testpoint=point(-.0908,-2.2786);
point fcheck=fkin(testpoint);
std::cout<<"Fcheck: "<<std::endl;
crapPoint(fcheck);
std::cout<<std::endl;
point fcheck2;
std::cout<<"Fcheck2: "<<std::endl;
fkin(testpoint, fcheck, fcheck2);
crapPoint(fcheck);
crapPoint(fcheck2);
std::cout<<std::endl;
point icheck;
std::cout<<"Icheck: ";
std::cout<<ikin(fcheck,icheck)<<std::endl;;
crapPoint(icheck);
std::cout<<std::endl;
std::cout<<"Jacobian: "<<std::endl;
mat2 testjac=jacobian(point(5,6));
crapMat(testjac);
std::cout<<std::endl;
std::cout<<"Q double dot: "<<std::endl;
point qdd=getQDDot(point(1,2),point(3,4),point(5,6));
crapPoint(qdd);
std::cout<<std::endl;
std::cout<<"DC: "<<std::endl;
mat2 D;
point C;
computeInertiaCoriolis(point(1,2),point(3,4),D,C);
crapMat(D);
crapPoint(C);
std::cout<<std::endl;
return 1;
}
int intp(struct path* A, struct max_joint_steps max_j_s, struct min_axis_steps min_a_s){
struct path* p = A;
int steps = 0;
int tmp = 0;
if(!A){
return(0);
}
p->pos = fkin(p->pos);
while(p->next){
p->next->pos = fkin(p->next->pos);
p = p->next;
}
p = A;
while(p->next){
for(int i = 0; i < axis; i++){
tmp = ceil(abs((p->next->pos.axis_pos[i] - p->pos.axis_pos[i]) / min_a_s.step[i]));
if(steps < tmp){
steps = tmp;
}
}
for(int i = 0; i < steps; i++){
insert(p, split(p->pos, p->next->pos, steps - i)); // split
p->next->pos = fkin(p->next->pos); // axis -> joints
p = p->next;
}
steps = 0;
p = p->next;
}
return(0);
}
