From 281d4c39551ecbf2560f285cec31e5739cc464be Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 28 Mar 2019 16:02:54 +0100 Subject: [PATCH] test 1000 random joint states in fk test --- test/calibration_test.cpp | 57 ++++++++++++++++++++++++--------------- 1 file changed, 36 insertions(+), 21 deletions(-) diff --git a/test/calibration_test.cpp b/test/calibration_test.cpp index 9aab691..d30c266 100644 --- a/test/calibration_test.cpp +++ b/test/calibration_test.cpp @@ -63,6 +63,11 @@ TEST(UrRtdeDriver, ur10_ideal) TEST(UrRtdeDriver, calibration) { + /* This test compares the forward kinematics of the model constructed from uncorrected + * parameters with the one from the corrected parameters. They are tested against random + * joint values and should be equal (in a numeric sense). + */ + DHRobot my_robot; const double pi = std::atan(1) * 4; @@ -79,34 +84,44 @@ 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 , 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 )); // 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::Matrix jointvalues; - jointvalues << 0, 0, 0, 0, 0, 0; - 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); - } + //{ + // 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); + //} - calibration.correctChain(); + for (size_t i = 0; i < 1000; ++i) { - Eigen::Matrix jointvalues; - jointvalues << 0, 0, 0, 0, 0, 0; - 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); + 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); + calibration.correctChain(); + Eigen::Matrix4d fk_corrected = calibration.calcForwardKinematics(jointvalues); + 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); } } } // namespace