void test_transform() {
VecEuler euler;
Vec3 translation;
HomogeneousTransform HT,HT2;
setEuler(DEGTORAD(10),DEGTORAD(20),DEGTORAD(30),euler);
setVec3(1,2,3,translation);
poseToHT(euler,translation,HT);
std::cout << "HT=\n"; printHT(HT,-1,-1);
//test rotation matrix
Mat3 Rx,Ry,Rz,Rot,Tmp;
Rotx(euler[0],Rx);
Roty(euler[1],Ry);
Rotz(euler[2],Rz);
multMatMat3(Ry,Rx,Tmp);
multMatMat3(Rz,Tmp,Rot);
std::cout << "Rotz(yaw)*Roty(pit)*Rotx(rol)=\n"; printMat3(Rot,-1,-1);
//test invert transform
invertHT(HT,HT2);
std::cout << "inv(HT)=\n"; printHT(HT2,-1,-1);
//test compose HT
if (1) {
//time it
int n = (int) 1e8;
timeval t0, t1;
//compose Homogeneous Transforms
gettimeofday(&t0, NULL);
for (int i=0; i<n; i++) {
composeHT(HT,HT,HT2);
}
gettimeofday(&t1, NULL);
std::cout << "HT*HT=\n"; printHT(HT2,-1,-1);
std::cout << "iterations: " << (Real) n << std::endl;
std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl;
} else {
composeHT(HT,HT,HT2);
std::cout << "HT*HT=\n"; printHT(HT2,-1,-1);
}
composeInvHT(HT,HT,HT2);
std::cout << "inv(HT)*HT=\n"; printHT(HT2,-1,-1);
//test transform point
Vec3 p,q;
setVec3(2,3,4,p);
std::cout << "p=\n"; printVec3(p,-1,-1);
applyHT(HT,p,q);
std::cout << "q=HT*p=\n"; printVec3(q,-1,-1);
applyInvHT(HT,p,q);
std::cout << "q=inv(HT)*p=\n"; printVec3(q,-1,-1);
//test converting between angular velocity and Euler rates
Vec3 vel={1,2,3};
VecEuler eulerrate;
MatEuler T;
std::cout << "angular velocity=\n"; printVec3(vel,-1,-1);
velToEulerrate(euler,vel,eulerrate,T);
std::cout << "eulerrate=\n"; printEuler(eulerrate,-1,-1);
std::cout << "T_vel_to_eulerrate=\n"; printMatReal(3,3,T,-1,-1);
std::cout << "convert back:\n";
eulerrateToVel(euler,eulerrate,vel,0);
std::cout << "angular velocity=\n"; printVec3(vel,-1,-1);
std::cout << std::endl;
//test at singularity
setEuler(0,M_PI/2,0,euler);
setVec3(0,0,1,vel);
velToEulerrate(euler,vel,eulerrate,T);
std::cout << "test at singularity:\n";
std::cout << "euler=\n"; printEuler(euler,-1,-1);
std::cout << "angular velocity=\n"; printVec3(vel,-1,-1);
std::cout << "eulerrate=\n"; printEuler(eulerrate,-1,-1);
}
int main(){
Mecanismo P = Mecanismo(2);
cube I1__; I1__.zeros(3,3,2);
I1__.slice(0) << 0 << 0 << 0 << endr
<< 0 << 107.307e-6 + 146.869e-6 << 0 << endr
<< 0 << 0 << 107.307e-6 + 146.869e-6 << endr;
I1__.slice(1) << 0 << 0 << 0 << endr
<< 0 << 438.0e-6 << 0 << endr
<< 0 << 0 << 438.0e-6 << endr;
cube I2__; I2__.zeros(3,3,2);
I2__.slice(0) << 0 << 0 << 0 << endr
<< 0 << 107.307e-6 + 188.738e-6 << 0 << endr
<< 0 << 0 << 107.307e-6 + 188.738e-6 << endr;
I2__.slice(1) << 0 << 0 << 0 << endr
<< 0 << 301.679e-6 << 0 << endr
<< 0 << 0 << 301.679e-6 << endr;
Serial RR1 = Serial(2, {0.12, 0.16}, {0.06, 0.078},{0.062, 0.124}, I1__ , {0, 0, 9.8}, &fDH_RR);
Serial RR2 = Serial(2, {0.12, 0.16}, {0.06, 0.058},{0.062, 0.097}, I2__ , {0, 0, 9.8}, &fDH_RR);
Serial **RR_ = new Serial* [2];
RR_[0] = &RR1;
RR_[1] = &RR2;
//Matrizes que descrevem a arquitetura do mecanismo
double l0 = 0.05;
mat D_ = join_vert((mat)eye(2,2),2);
mat E_ = join_diag( Roty(0)(span(0,1),span(0,2)), Roty(PI)(span(0,1),span(0,2)) );
mat F_ = zeros(4,4);
vec f_ = {l0,0,-l0,0};
Parallel Robot = Parallel(2, &P, RR_, 2, {2,4}, D_, E_, F_, f_);
Reference RefObj = Reference(0.12, {0.08, 0.16}, {-0.08, 0.4});
//Plotar área de trabalho
uint nx = 96.0;
uint ny = 56.0;
double lx = 0.24;
double ly = 0.28;
double xi = -lx;
double xf = lx;
double yi = 0.0;
double yf = ly;
double dx = (xf-xi)/(nx-1);
double dy = (yf-yi)/(ny-1);
double dl = 0.5*(dx+dy);
Mat<int> M; M.zeros(nx,ny);
field<mat> fZ_(nx,ny);
field<mat> fMh_(nx,ny);
field<vec> fgh_(nx,ny);
field<vec> fa1_(nx,ny);
field<vec> fa2_(nx,ny);
field<vec> fa12_(nx,ny);
for(uint i=0; i<nx; i++){
for(uint j=0; j<ny; j++){
fZ_(i,j).zeros(2,2);
fMh_(i,j).zeros(2,2);
fgh_(i,j).zeros(2);
fa1_(i,j).zeros(2);
fa2_(i,j).zeros(2);
fa12_(i,j).zeros(2);
}
}
vec v1_ = {1,0};
vec v2_ = {0,1};
vec v12_ = {1,1};
double r = 0.07;
double x0 = 0.0;
double y0 = 0.17;
uint rows = nx;
uint cols = ny;
vec q0_ = {0.823167, 1.81774, 0.823167, 1.81774};
GNR2 gnr2 = GNR2("RK6", &Robot, 1e-6, 30);
//gnr2.Doit(q0_, {0.05,0.08});
//cout << gnr2.convergiu << endl;
//cout << gnr2.x_ << endl;
//cout << gnr2.res_ << endl;
//cout << gnr2.n << endl;
double x;
double y;
mat A2_;
for(uint i=0; i<rows; i++){
for(uint j=0; j<cols; j++){
x = xi + i*dx;
y = yi + j*dy;
gnr2.Doit(q0_, {x, y});
if(gnr2.convergiu){
q0_ = gnr2.x_;
A2_ = join_horiz(Robot.Ah_, join_horiz(Robot.Ao_.col(1), Robot.Ao_.col(3)) );
if(abs(det(Robot.Ao_)) < 1.6*1e-6 || abs(det(A2_)) < 1e-11 ) M(i,j) = 2;
else{
M(i,j) = 1;
Robot.Doit(Robot.q0_, join_vert(v1_, -solve(Robot.Ao_, Robot.Ah_*v1_)) );
fZ_(i,j) = Robot.Z_;
fMh_(i,j) = Robot.dy->Mh_;
fgh_(i,j) = Robot.dy->gh_;
fa1_(i,j) = Robot.dy->vh_;
Robot.Doit(Robot.q0_, Robot.C_*v2_);
fa2_(i,j) = Robot.dy->vh_;
Robot.Doit(Robot.q0_, Robot.C_*v12_);
fa12_(i,j) = Robot.dy->vh_ - fa1_(i,j) - fa2_(i,j);
}
}
gnr2.convergiu = false;
if( ((x - x0)*(x - x0) + (y - y0)*(y - y0) <= (r+dl)*(r+dl) ) && ((x - x0)*(x - x0) + (y - y0)*(y - y0) >= (r-dl)*(r-dl) ) && (M(i,j) != 2) )
M(i,j) = 3;
}
}
for(uint i=0; i<rows; i++){
for(uint j=0; j<cols; j++){
cout << M(i,j) << ";" ;
if(j==cols-1) cout << endl;
}
}
fZ_.save("fZ_field");
fMh_.save("fMh_field");
fgh_.save("fgh_field");
fa1_.save("fa1_field");
fa2_.save("fa2_field");
fa12_.save("fa12_field");
return 0;
}
