Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00
Code Releases Activity

test 1000 random joint states in fk test

Browse Source
This commit is contained in:
Felix Mauch
2019-03-28 16:02:54 +01:00
parent f373eca92b
commit 281d4c3955
1 changed files with 36 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
test/calibration_test.cpp
View File

@@ -63,6 +63,11 @@ TEST(UrRtdeDriver, ur10_ideal)
TEST(UrRtdeDriver, calibration) 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; DHRobot my_robot;
const double pi = std::atan(1) * 4; const double pi = std::atan(1) * 4;
@@ -79,34 +84,44 @@ TEST(UrRtdeDriver, calibration)
DHRobot my_robot_calibration; DHRobot my_robot_calibration;
// clang-format off // clang-format off
// d, a, theta, alpha // d, a, theta, alpha
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.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(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(-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 , 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 // clang-format on
Calibration calibration(my_robot + my_robot_calibration); Calibration calibration(my_robot + my_robot_calibration);
Eigen::Matrix<double, 6, 1> jointvalues;
jointvalues << 0, 0, 0, 0, 0, 0;
// First let's see, whether our calibration input does make sense. // First let's see, whether our calibration input does make sense.
{ //{
Eigen::Matrix<double, 6, 1> jointvalues; // Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues);
jointvalues << 0, 0, 0, 0, 0, 0; // EXPECT_PRED3(isApproximately, -1.25672673098643, fk(0, 3), 1e-11);
Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); // EXPECT_PRED3(isApproximately, -0.320928557210126, fk(1, 3), 1e-11);
EXPECT_PRED3(isApproximately, -1.25672673098643, fk(0, 3), 1e-11); // EXPECT_PRED3(isApproximately, 0.158086917698569, fk(2, 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<double, 6, 1> jointvalues; Calibration calibration(my_robot + my_robot_calibration);
jointvalues << 0, 0, 0, 0, 0, 0; jointvalues = 2 * pi * Eigen::Matrix<double, 6, 1>::Random();
Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); // TODO: Remove this output
EXPECT_PRED3(isApproximately, -1.25672673098643, fk(0, 3), 1e-11); std::cout << "Testing with jointvalues: [" << jointvalues.transpose() << "]" << std::endl;
EXPECT_PRED3(isApproximately, -0.320928557210126, fk(1, 3), 1e-11); Eigen::Matrix4d fk_orig = calibration.calcForwardKinematics(jointvalues);
EXPECT_PRED3(isApproximately, 0.158086917698569, fk(2, 3), 1e-11); 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 } // namespace