void test_spatial() {
//for timing
int n;
timeval t0, t1;
//construct Homogeneous Transform
VecEuler euler;
Vec3 translation;
HomogeneousTransform HT;
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);
//convert to Plucker transform
Mat6b P;
HTToPlucker(HT,P);
std::cout << "P=\n"; printMat6b(P,-1,-1);
//inverse Plucker transform
Mat6b P2;
if (1) {
//time it
n= (int) 1e8;
gettimeofday(&t0, NULL);
for (int i=0; i<n; i++) {
invHTToPlucker(HT,P2);
}
gettimeofday(&t1, NULL);
std::cout << "inv(P)=\n"; printMat6b(P2,-1,-1);
std::cout << "iterations: " << (Real) n << std::endl;
std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl;
std::cout << std::endl;
} else {
invHTToPlucker(HT,P2);
std::cout << "inv(P)=\n"; printMat6b(P2,-1,-1);
}
Vec6b m,f,v,v2;
setVec6b(1,2,3,4,5,6,m);
setVec6b(2,3,4,5,6,7,f);
std::cout << "m=\n"; printVec6b(m,-1,-1);
std::cout << "f=\n"; printVec6b(f,-1,-1);
//cross products
crossVec6bMotion(m,f,v);
std::cout << "crossMotion(m)*f=\n"; printVec6b(v,-1,-1);
crossVec6bForce(m,f,v);
std::cout << "crossForce(m)*f=\n"; printVec6b(v,-1,-1);
//multiply vector by Plucker transform
std::cout << "using spatial.h\n";
if (1) {
//time it
n= (int) 1e8;
gettimeofday(&t0, NULL);
for (int i=0; i<n; i++) {
multPluckerVec6b(P,m,v);
multPluckerTVec6b(P,m,v2);
}
gettimeofday(&t1, NULL);
std::cout << "P*m=\n"; printVec6b(v,-1,-1);
std::cout << "P'*m=\n"; printVec6b(v2,-1,-1);
std::cout << "iterations: " << (Real) n << std::endl;
std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl;
std::cout << std::endl;
} else {
multPluckerVec6b(P,m,v);
std::cout << "P*m=\n"; printVec6b(v,-1,-1);
multPluckerTVec6b(P,m,v2);
std::cout << "P'*m=\n"; printVec6b(v2,-1,-1);
}
if (1) {
//using Eigen
Eigen::Matrix<Real,6,6> P_;
Eigen::Matrix<Real,6,1> m_, v_, v2_;
//copy to Eigen matrices
copyMat6bToArray(P,P_.data());
copyVec6bToArray(m,m_.data());
gettimeofday(&t0, NULL);
for (int i=0; i<n; i++) {
v_ = P_*m_;
v2_ = P_.transpose()*m_;
}
gettimeofday(&t1, NULL);
std::cout << "using Eigen\n";
std::cout << "P*m=\n" << v_ << std::endl;
std::cout << "P'*m=\n" << v2_ << std::endl;
std::cout << "iterations: " << (Real) n << std::endl;
std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl;
}
//compute P'*I*P
//copy tricycle body frame
Real mass = 10;
Real L = 1;
Real W = .8;
Real H = .25;
Vec3 cm;
Mat3 I;
setVec3(L/2,0,H/2,cm);
inertiaBox(mass,L,W,H,I);
//using spatial.h
Mat6b Is; //spatial inertia
toSpatialInertia(mass,cm,I,Is);
std::cout << "Is=\n";
printMat6b(Is,-1,-1);
Mat6b Is2;
std::cout << "using spatial.h\n";
if (1) {
//time it
n = (int) 1e7;
gettimeofday(&t0, NULL);
for (int i=0; i<n; i++) {
//multPluckerTMat6bPlucker(P,Is,Is2); //deprecated
multPluckerTInertiaPlucker(P,Is,Is2);
}
gettimeofday(&t1, NULL);
std::cout << "P'*Is*P=\n"; printMat6b(Is2,-1,-1);
std::cout << "iterations: " << (Real) n << std::endl;
std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl;
std::cout << std::endl;
} else {
//multPluckerTMat6bPlucker(P,Is,Is2);
multPluckerTInertiaPlucker(P,Is,Is2);
std::cout << "P'*Is*P=\n"; printMat6b(Is2,-1,-1);
}
if (1) {
//using Eigen
Eigen::Matrix<Real,6,6> P_, Is_, Is2_;
//copy to Eigen matrices
copyMat6bToArray(P,P_.data());
copyMat6bToArray(Is,Is_.data());
gettimeofday(&t0, NULL);
for (int i=0; i<n; i++) {
Is2_ = P_.transpose()*Is_*P_;
}
gettimeofday(&t1, NULL);
std::cout << "using Eigen\n";
std::cout << "P'*Is*P=\n" << Is2_ << std::endl;
std::cout << "iterations: " << (Real) n << std::endl;
std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl;
}
}
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);
}
Quaternion::Quaternion(const EulerAngles &eulerAngles) {
setEuler(eulerAngles);
}