From bab303e83c33e9e1f7c9119e919da5e77e20dbcf Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 1 Apr 2019 10:08:29 +0200 Subject: [PATCH] Cleaned up tests --- test/calibration_test.cpp | 36 ++++++++++++------------------------ 1 file changed, 12 insertions(+), 24 deletions(-) diff --git a/test/calibration_test.cpp b/test/calibration_test.cpp index d30c266..2bdaf51 100644 --- a/test/calibration_test.cpp +++ b/test/calibration_test.cpp @@ -84,44 +84,33 @@ TEST(UrRtdeDriver, calibration) DHRobot my_robot_calibration; // clang-format off // d, a, theta, alpha - //my_robot_calibration.segments_.push_back(DHSegment(0 , 0 , 0 , 0.0)); - //my_robot_calibration.segments_.push_back(DHSegment(0.5 , 0 , 0.1 , 0.2)); - //my_robot_calibration.segments_.push_back(DHSegment(0.8 , 0 , 0.3 , 0.4)); - //my_robot_calibration.segments_.push_back(DHSegment(-1.3 , 0 , 0 , 0.0)); - //my_robot_calibration.segments_.push_back(DHSegment(0 , 0 , 0 , 0.0)); - //my_robot_calibration.segments_.push_back(DHSegment(0 , 0 , 0 , 0.0)); - my_robot_calibration.segments_.push_back(DHSegment( 0.00065609212979853 , 4.6311376834935676e-05 , -7.290070070824746e-05 , 0.000211987863869334 )); - my_robot_calibration.segments_.push_back(DHSegment( 1.4442162376284788 , -0.00012568315331862312 , -0.01713897289704999 , -0.0072553625957652995)); - my_robot_calibration.segments_.push_back(DHSegment( 0.854147723854608 , 0.00186216581161458 , -0.03707159413492756 , -0.013483226769541364 )); - my_robot_calibration.segments_.push_back(DHSegment(-2.2989425877563705 , 9.918593870679266e-05 , 0.054279462160583214 , 0.0013495820227329425 )); - my_robot_calibration.segments_.push_back(DHSegment(-1.573498686836816e-05 , 4.215462720453189e-06 , 1.488984257025741e-07 , -0.001263136163679901 )); - my_robot_calibration.segments_.push_back(DHSegment( 1.9072435590711256e-05 , 0 , 1.551499479707493e-05 , 0 )); + my_robot_calibration.segments_.push_back(DHSegment( 0.00065609212979853 , 4.6311376834935676e-05 , -7.290070070824746e-05 , 0.000211987863869334 )); + my_robot_calibration.segments_.push_back(DHSegment( 1.4442162376284788 , -0.00012568315331862312 , -0.01713897289704999 , -0.0072553625957652995)); + my_robot_calibration.segments_.push_back(DHSegment( 0.854147723854608 , 0.00186216581161458 , -0.03707159413492756 , -0.013483226769541364 )); + my_robot_calibration.segments_.push_back(DHSegment(-2.2989425877563705 , 9.918593870679266e-05 , 0.054279462160583214 , 0.0013495820227329425 )); + my_robot_calibration.segments_.push_back(DHSegment(-1.573498686836816e-05 , 4.215462720453189e-06 , 1.488984257025741e-07 , -0.001263136163679901 )); + my_robot_calibration.segments_.push_back(DHSegment( 1.9072435590711256e-05 , 0 , 1.551499479707493e-05 , 0 )); // clang-format on - Calibration calibration(my_robot + my_robot_calibration); - Eigen::Matrix jointvalues; jointvalues << 0, 0, 0, 0, 0, 0; - // First let's see, whether our calibration input does make sense. - //{ - // Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); - // EXPECT_PRED3(isApproximately, -1.25672673098643, fk(0, 3), 1e-11); - // EXPECT_PRED3(isApproximately, -0.320928557210126, fk(1, 3), 1e-11); - // EXPECT_PRED3(isApproximately, 0.158086917698569, fk(2, 3), 1e-11); - //} for (size_t i = 0; i < 1000; ++i) { Calibration calibration(my_robot + my_robot_calibration); jointvalues = 2 * pi * Eigen::Matrix::Random(); - // TODO: Remove this output - std::cout << "Testing with jointvalues: [" << jointvalues.transpose() << "]" << std::endl; Eigen::Matrix4d fk_orig = calibration.calcForwardKinematics(jointvalues); + Eigen::Matrix3d rot_mat_orig = fk_orig.topLeftCorner(3, 3); + Eigen::Quaterniond rot_orig(rot_mat_orig); calibration.correctChain(); Eigen::Matrix4d fk_corrected = calibration.calcForwardKinematics(jointvalues); + Eigen::Matrix3d rot_mat_corrected = fk_corrected.topLeftCorner(3, 3); + Eigen::Quaterniond rot_corrected(rot_mat_corrected); + double angle_error = std::abs(rot_orig.angularDistance(rot_corrected)); EXPECT_PRED3(isApproximately, fk_orig(0, 3), fk_corrected(0, 3), 1e-12); EXPECT_PRED3(isApproximately, fk_orig(1, 3), fk_corrected(1, 3), 1e-12); EXPECT_PRED3(isApproximately, fk_orig(2, 3), fk_corrected(2, 3), 1e-12); + EXPECT_PRED3(isApproximately, angle_error, 0.0, 1e-12); } } } // namespace @@ -132,4 +121,3 @@ int main(int argc, char* argv[]) return RUN_ALL_TESTS(); } -