Skip to content

Commit

Permalink
Make calibration tests parametrizable
Browse files Browse the repository at this point in the history
  • Loading branch information
@fmauch
fmauch committed Jun 6, 2024
1 parent 9da8109 commit b2bf704
Showing 1 changed file with 63 additions and 66 deletions.
129 changes: 63 additions & 66 deletions ur_calibration/test/calibration_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,12 +42,16 @@
//----------------------------------------------------------------------

#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <map>
#include <ur_calibration/calibration.hpp>

using ur_calibration::Calibration;
using ur_calibration::DHRobot;
using ur_calibration::DHSegment;

using CalibrationMat = Eigen::Matrix<double, 6, 4>;

namespace
{
/*
Expand All @@ -67,12 +71,36 @@ void doubleEqVec(const Eigen::Matrix<Scalar_, dim_, 1> vec1, const Eigen::Matrix

} // namespace

class CalibrationTest : public ::testing::Test
DHRobot model_from_dh(std::array<double, 6> d, std::array<double, 6> a, std::array<double, 6> theta,
std::array<double, 6> alpha)
{
DHRobot robot;
for (size_t i = 0; i < 6; ++i) {
robot.segments_.emplace_back(DHSegment(d[i], a[i], theta[i], alpha[i]));
}
return robot;
}

class CalibrationTest : public ::testing::TestWithParam<DHRobot>
{
public:
void SetUp()
{
Test::SetUp();
robot_models_.insert({ "ur10_ideal", model_from_dh({ 0.1273, 0, 0, 0.163941, 0.1157, 0.0922 }, // d
{ 0, -0.612, -0.5723, 0, 0, 0 }, // a
{ 0, 0, 0, 0, 0, 0 }, // theta
{ pi / 2, 0, 0, pi / 2, -pi / 2, 0 } // alpha
) });
robot_models_.insert({ "ur10", model_from_dh({ 0.1273, 0, 0, 0.163941, 0.1157, 0.0922 }, // d
{ 0, -0.612, -0.5723, 0, 0, 0 }, // a
{ 0, 0, 0, 0, 0, 0 }, // theta
{ pi_2, 0, 0, pi_2, -pi_2, 0 } // alpha
) });
robot_models_.insert({ "ur10e", model_from_dh({ 0.1807, 0, 0, 0.17415, 0.11985, 0.11655 }, // d
{ 0, -0.6127, -0.57155, 0, 0, 0 }, // a
{ 0, 0, 0, 0, 0, 0 }, // theta
{ pi_2, 0, 0, pi_2, -pi_2, 0 } // alpha
) });
}
void TearDown()
{
Expand All @@ -82,63 +110,42 @@ class CalibrationTest : public ::testing::Test
protected:
const double pi = std::atan(1) * 4;
const double pi_2 = 1.570796327; // This is what the simulated robot reports as pi/2
DHRobot my_robot_;
DHRobot my_robot_calibration_;
std::map<std::string, DHRobot> robot_models_;
};

TEST_F(CalibrationTest, ur10_fw_kinematics_ideal)
{
my_robot_.segments_.clear();
// This is an ideal UR10
// clang-format off
my_robot_.segments_.emplace_back(DHSegment(0.1273 , 0 , 0 , pi / 2));
my_robot_.segments_.emplace_back(DHSegment(0 , -0.612 , 0 , 0));
my_robot_.segments_.emplace_back(DHSegment(0 , -0.5723, 0 , 0.0));
my_robot_.segments_.emplace_back(DHSegment(0.163941, 0 , 0 , pi / 2));
my_robot_.segments_.emplace_back(DHSegment(0.1157 , 0 , 0 , -pi / 2));
my_robot_.segments_.emplace_back(DHSegment(0.0922 , 0 , 0 , 0));
// clang-format on

Calibration calibration(my_robot_);
const auto& my_robot = robot_models_["ur10_ideal"];
Calibration calibration(my_robot);

Eigen::Matrix<double, 6, 1> jointvalues;
Eigen::Vector3d expected_position;
Eigen::Quaterniond expected_orientation;
{
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_);
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_);
}

{
jointvalues << M_PI_2, -M_PI_4, M_PI_2, -M_PI_4, 0, 0;
Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues);

expected_position << my_robot_.segments_[3].d_ + my_robot_.segments_[5].d_,
my_robot_.segments_[1].a_ / std::sqrt(2) + my_robot_.segments_[2].a_ / std::sqrt(2),
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_;
doubleEqVec<double, 3>(expected_position, fk.topRightCorner(3, 1), 1e-16);
expected_position << my_robot.segments_[3].d_ + my_robot.segments_[5].d_,
my_robot.segments_[1].a_ / std::sqrt(2) + my_robot.segments_[2].a_ / std::sqrt(2),
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_;
doubleEqVec<double, 3>(expected_position, fk.topRightCorner(3, 1), 1e-8);
}
}

TEST_F(CalibrationTest, ur10_fw_kinematics_real)
{
// This test compares a corrected ideal model against positions taken from a simulated robot.
my_robot_.segments_.clear();
// This is an ideal UR10
// clang-format off
my_robot_.segments_.emplace_back(DHSegment(0.1273 , 0 , 0 , pi_2));
my_robot_.segments_.emplace_back(DHSegment(0 , -0.612 , 0 , 0));
my_robot_.segments_.emplace_back(DHSegment(0 , -0.5723, 0 , 0.0));
my_robot_.segments_.emplace_back(DHSegment(0.163941, 0 , 0 , pi_2));
my_robot_.segments_.emplace_back(DHSegment(0.1157 , 0 , 0 , -pi_2));
my_robot_.segments_.emplace_back(DHSegment(0.0922 , 0 , 0 , 0));
// clang-format on

Calibration calibration(my_robot_);
const auto& my_robot = robot_models_["ur10"];
Calibration calibration(my_robot);

Eigen::Matrix<double, 6, 1> jointvalues;
Eigen::Vector3d expected_position;
Expand Down Expand Up @@ -167,46 +174,22 @@ TEST_F(CalibrationTest, ur10_fw_kinematics_real)
TearDown();
}

TEST_F(CalibrationTest, calibration)
TEST_P(CalibrationTest, 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).

my_robot_.segments_.clear();

// This is an ideal UR10
// clang-format off
// d, a, theta, alpha
my_robot_.segments_.emplace_back(DHSegment(0.1273 , 0 , 0 , pi / 2));
my_robot_.segments_.emplace_back(DHSegment(0 , -0.612 , 0 , 0));
my_robot_.segments_.emplace_back(DHSegment(0 , -0.5723, 0 , 0.0));
my_robot_.segments_.emplace_back(DHSegment(0.163941, 0 , 0 , pi / 2));
my_robot_.segments_.emplace_back(DHSegment(0.1157 , 0 , 0 , -pi / 2));
my_robot_.segments_.emplace_back(DHSegment(0.0922 , 0 , 0 , 0));
// clang-format on
my_robot_calibration_.segments_.clear();
// clang-format off
// d, a, theta, alpha
my_robot_calibration_.segments_.emplace_back(DHSegment( 0.00065609212979853 , 4.6311376834935676e-05 ,
-7.290070070824746e-05 , 0.000211987863869334 ));
my_robot_calibration_.segments_.emplace_back(DHSegment( 1.4442162376284788 , -0.00012568315331862312 ,
-0.01713897289704999 , -0.0072553625957652995));
my_robot_calibration_.segments_.emplace_back(DHSegment( 0.854147723854608 , 0.00186216581161458 ,
-0.03707159413492756 , -0.013483226769541364 ));
my_robot_calibration_.segments_.emplace_back(DHSegment(-2.2989425877563705 , 9.918593870679266e-05 ,
0.054279462160583214 , 0.0013495820227329425 ));
my_robot_calibration_.segments_.emplace_back(DHSegment(-1.573498686836816e-05 , 4.215462720453189e-06 ,
1.488984257025741e-07 , -0.001263136163679901 ));
my_robot_calibration_.segments_.emplace_back(DHSegment( 1.9072435590711256e-05 , 0 ,
1.551499479707493e-05 , 0 ));
// clang-format on
auto my_robot = robot_models_["ur10"];
Calibration calibration(my_robot);

auto robot_calibration = GetParam();

Eigen::Matrix<double, 6, 1> jointvalues;
jointvalues << 0, 0, 0, 0, 0, 0;

for (size_t i = 0; i < 1000; ++i) {
Calibration calibration(my_robot_ + my_robot_calibration_);
Calibration calibration(my_robot + robot_calibration);
jointvalues = 2 * pi * Eigen::Matrix<double, 6, 1>::Random();
Eigen::Matrix4d fk_orig = calibration.calcForwardKinematics(jointvalues);
Eigen::Matrix3d rot_mat_orig = fk_orig.topLeftCorner(3, 3);
Expand All @@ -222,3 +205,17 @@ TEST_F(CalibrationTest, calibration)
EXPECT_NEAR(angle_error, 0.0, 1e-12);
}
}

INSTANTIATE_TEST_SUITE_P(
CalibrationTests, CalibrationTest,
::testing::Values(model_from_dh({ 0.00065609212979853, 1.4442162376284788, 0.854147723854608, -2.2989425877563705,
-1.573498686836816e-05, 1.9072435590711256e-05 }, // d
{ 4.6311376834935676e-05, -0.00012568315331862312, 0.00186216581161458,
9.918593870679266e-05, 4.215462720453189e-06, 0 }, // a
{ -7.290070070824746e-05, -0.01713897289704999, -0.03707159413492756,
0.054279462160583214, 1.488984257025741e-07, 1.551499479707493e-05 }, // theta
{ 0.000211987863869334, -0.0072553625957652995, -0.013483226769541364,
0.0013495820227329425, -0.001263136163679901, 0 } // alpha
)

));

0 comments on commit b2bf704

Please sign in to comment.