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

Use plain Eigen for kinematics

Browse Source
This commit is contained in:
Felix Mauch
2019-03-20 18:33:04 +01:00
parent 8a9192aeba
commit bc15944fa1
4 changed files with 161 additions and 351 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
test/calibration_test.cpp
View File

@@ -17,6 +17,11 @@
namespace
{
bool isApproximately(const double val1, const double val2, const double precision)
{
return std::abs(val1 - val2) < precision;
}
TEST(UrRtdeDriver, ur10_ideal)
{
DHRobot my_robot;
@@ -34,18 +39,26 @@ TEST(UrRtdeDriver, ur10_ideal)
Calibration calibration(my_robot);
KDL::Chain robot_chain = calibration.getChain();
uint32_t num_jts = robot_chain.getNrOfJoints();
Eigen::Matrix<double, 6, 1> jointvalues;
{
jointvalues << 0, 0, 0, 0, 0, 0;
Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues);
EXPECT_DOUBLE_EQ(fk(0, 3), my_robot.segments_[1].a_ + my_robot.segments_[2].a_);
EXPECT_DOUBLE_EQ(fk(1, 3), -1 * (my_robot.segments_[3].d_ + my_robot.segments_[5].d_));
EXPECT_DOUBLE_EQ(fk(2, 3), my_robot.segments_[0].d_ - my_robot.segments_[4].d_);
}
KDL::ChainFkSolverPos_recursive fk_solver(robot_chain);
KDL::JntArray jointpositions = KDL::JntArray(num_jts);
KDL::Frame result;
fk_solver.JntToCart(jointpositions, result);
// Check whether our internal KDL representation gives correct values
EXPECT_DOUBLE_EQ(result.p.x(), my_robot.segments_[1].a_ + my_robot.segments_[2].a_);
EXPECT_DOUBLE_EQ(result.p.y(), -1 * (my_robot.segments_[3].d_ + my_robot.segments_[5].d_));
EXPECT_DOUBLE_EQ(result.p.z(), my_robot.segments_[0].d_ - my_robot.segments_[4].d_);
{
jointvalues << M_PI_2, -M_PI_4, M_PI_2, -M_PI_4, 0, 0;
Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues);
EXPECT_DOUBLE_EQ(fk(0, 3), my_robot.segments_[3].d_ + my_robot.segments_[5].d_);
EXPECT_DOUBLE_EQ(fk(1, 3), my_robot.segments_[1].a_ / std::sqrt(2) + my_robot.segments_[2].a_ / std::sqrt(2));
// Because of the sqrt calculations a DOUBLE_EQ does not work here.
EXPECT_PRED3(isApproximately, fk(2, 3),
my_robot.segments_[0].d_ - my_robot.segments_[1].a_ / std::sqrt(2) +
my_robot.segments_[2].a_ / std::sqrt(2) - my_robot.segments_[4].d_,
1e-16);
}
}
TEST(UrRtdeDriver, calibration)
@@ -65,34 +78,36 @@ TEST(UrRtdeDriver, calibration)
// clang-format on
DHRobot my_robot_calibration;
// clang-format off
// d, a, theta, alpha
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 ));
// 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));
// clang-format on
Calibration calibration(my_robot + my_robot_calibration);
KDL::Chain robot_chain = calibration.getChain();
uint32_t num_jts = robot_chain.getNrOfJoints();
// First let's see, whether our calibration input does make sense.
{
Eigen::Matrix<double, 6, 1> 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);
}
KDL::ChainFkSolverPos_recursive fk_solver(robot_chain);
KDL::JntArray jointpositions = KDL::JntArray(num_jts);
KDL::Frame result_original;
fk_solver.JntToCart(jointpositions, result_original);
KDL::Chain robot_chain_corrected = calibration.correctChain();
KDL::ChainFkSolverPos_recursive fk_solver_corrected(robot_chain_corrected);
KDL::Frame result_corrected;
fk_solver.JntToCart(jointpositions, result_corrected);
//
// Check whether our internal KDL representation gives correct values
EXPECT_DOUBLE_EQ(result_original.p.x(), result_corrected.p.x());
EXPECT_DOUBLE_EQ(result_original.p.y(), result_corrected.p.y());
EXPECT_DOUBLE_EQ(result_original.p.z(), result_corrected.p.z());
calibration.correctChain();
{
Eigen::Matrix<double, 6, 1> 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);
}
}
} // namespace