00001 #include <gtest/gtest.h> 00002 #include "Eigen3d.h" 00003 00004 using namespace hrp; 00005 00006 TEST(Eigen3d, omegaFromRot) 00007 { 00008 Matrix33 m; 00009 Vector3 v; 00010 00011 m << 1.0, 0, 0, 00012 0, 1.0, 0, 00013 0, 0, 1.0; 00014 v = omegaFromRot(m); 00015 EXPECT_EQ(v[0], 0); 00016 EXPECT_EQ(v[1], 0); 00017 EXPECT_EQ(v[2], 0); 00018 00019 m << 1.0, 0, 0, 00020 0, 1.0, 0, 00021 0, 0, 1.0+2.0e-12; 00022 v = omegaFromRot(m); 00023 EXPECT_EQ(v[0], 0); 00024 EXPECT_EQ(v[1], 0); 00025 EXPECT_EQ(v[2], 0); 00026 00027 m << 1.0, 0, 0, 00028 0, 1.0, 0, 00029 0, 0, 1.0+2.0e-6; 00030 v = omegaFromRot(m); 00031 EXPECT_EQ(v[0], 0); 00032 EXPECT_EQ(v[1], 0); 00033 EXPECT_EQ(v[2], 0); 00034 00035 m = rotFromRpy(0, 0, 0); 00036 v = omegaFromRot(m); 00037 EXPECT_EQ(v[0], 0); 00038 EXPECT_EQ(v[1], 0); 00039 EXPECT_EQ(v[2], 0); 00040 00041 for (int i = 1; i <= 16; i++) { 00042 for (int j = 1; j <= 16; j++) { 00043 for (int k = 1; k <= 16; k++) { 00044 Vector3 rpy(pow(0.1, i), pow(0.1, j), pow(0.1, k)); 00045 m = rotFromRpy(rpy); 00046 v = omegaFromRot(m); 00047 EXPECT_FALSE(isnan(v[0])); 00048 EXPECT_FALSE(isnan(v[1])); 00049 EXPECT_FALSE(isnan(v[2])); 00050 } 00051 } 00052 } 00053 00054 for (int i = 0; i <= 180; i++) { 00055 for (int j = 0; j <= 180; j++) { 00056 for (int k = 0; k <= 180; k++) { 00057 Vector3 rpy(i*M_PI/180, j*M_PI/180, k*M_PI/180); 00058 m = rotFromRpy(rpy); 00059 v = omegaFromRot(m); 00060 EXPECT_FALSE(isnan(v[0])); 00061 EXPECT_FALSE(isnan(v[1])); 00062 EXPECT_FALSE(isnan(v[2])); 00063 } 00064 } 00065 } 00066 }