Esempio n. 1
0
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;
}
Esempio n. 2
0
File: int2.c Progetto: crinq/cmot
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);
}