00001 #include <robot_calibration/models/chain.h> 00002 #include <gtest/gtest.h> 00003 00004 using robot_calibration::rotation_from_axis_magnitude; 00005 using robot_calibration::axis_magnitude_from_rotation; 00006 00007 TEST(RotationTests, rotationsOK) 00008 { 00009 KDL::Rotation one_radian_axis_x1 = KDL::Rotation::RPY(1.0, 0.0, 0.0); 00010 KDL::Rotation one_radian_axis_x2 = rotation_from_axis_magnitude(1.0, 0.0, 0.0); 00011 00012 EXPECT_EQ(one_radian_axis_x1.UnitX(), one_radian_axis_x2.UnitX()); 00013 EXPECT_EQ(one_radian_axis_x1.UnitY(), one_radian_axis_x2.UnitY()); 00014 EXPECT_EQ(one_radian_axis_x1.UnitZ(), one_radian_axis_x2.UnitZ()); 00015 00016 KDL::Rotation neg_radian_axis_x1 = KDL::Rotation::RPY(-1.0, 0.0, 0.0); 00017 KDL::Rotation neg_radian_axis_x2 = rotation_from_axis_magnitude(-1.0, 0.0, 0.0); 00018 00019 EXPECT_EQ(neg_radian_axis_x1.UnitX(), neg_radian_axis_x2.UnitX()); 00020 EXPECT_EQ(neg_radian_axis_x1.UnitY(), neg_radian_axis_x2.UnitY()); 00021 EXPECT_EQ(neg_radian_axis_x1.UnitZ(), neg_radian_axis_x2.UnitZ()); 00022 00023 KDL::Rotation one_radian_axis_z1 = KDL::Rotation::RPY(0.0, 0.0, 1.0); 00024 KDL::Rotation one_radian_axis_z2 = rotation_from_axis_magnitude(0.0, 0.0, 1.0); 00025 00026 EXPECT_EQ(one_radian_axis_z1.UnitX(), one_radian_axis_z2.UnitX()); 00027 EXPECT_EQ(one_radian_axis_z1.UnitY(), one_radian_axis_z2.UnitY()); 00028 EXPECT_EQ(one_radian_axis_z1.UnitZ(), one_radian_axis_z2.UnitZ()); 00029 00030 double x, y, z; 00031 axis_magnitude_from_rotation(one_radian_axis_x1, x, y, z); 00032 EXPECT_DOUBLE_EQ(1.0, x); 00033 EXPECT_DOUBLE_EQ(0.0, y); 00034 EXPECT_DOUBLE_EQ(0.0, z); 00035 00036 axis_magnitude_from_rotation(neg_radian_axis_x1, x, y, z); 00037 EXPECT_DOUBLE_EQ(-1.0, x); 00038 EXPECT_DOUBLE_EQ(0.0, y); 00039 EXPECT_DOUBLE_EQ(0.0, z); 00040 00041 axis_magnitude_from_rotation(one_radian_axis_z1, x, y, z); 00042 EXPECT_DOUBLE_EQ(0.0, x); 00043 EXPECT_DOUBLE_EQ(0.0, y); 00044 EXPECT_DOUBLE_EQ(1.0, z); 00045 } 00046 00047 int main(int argc, char** argv) 00048 { 00049 testing::InitGoogleTest(&argc, argv); 00050 return RUN_ALL_TESTS(); 00051 }