diff --git a/CMakeLists.txt b/CMakeLists.txt
index e083bf6..ebd26ab 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -143,12 +143,19 @@ install(DIRECTORY include/${PROJECT_NAME}/
FILES_MATCHING PATTERN "*.h"
)
+#############
+## Testing ##
+#############
-#if (CATKIN_ENABLE_TESTING)
- #set(${PROJECT_NAME}_TEST_SOURCES
- #tests/ur/master_board.cpp
- #tests/ur/robot_mode.cpp
- #tests/ur/rt_state.cpp)
+if (CATKIN_ENABLE_TESTING)
+ catkin_add_gtest(calibration_test
+ test/calibration_test.cpp
+ src/calibration.cpp)
+ target_link_libraries(calibration_test ${catkin_LIBRARIES})
+endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
#catkin_add_gtest(ur_rtde_driver_test ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_TEST_SOURCES} tests/main.cpp)
#target_link_libraries(ur_rtde_driver_test ur_rtde_driver ${catkin_LIBRARIES})
diff --git a/include/ur_rtde_driver/calibration.h b/include/ur_rtde_driver/calibration.h
index ef99cd4..7068515 100644
--- a/include/ur_rtde_driver/calibration.h
+++ b/include/ur_rtde_driver/calibration.h
@@ -134,7 +134,7 @@ struct DHRobot
class Calibration
{
public:
- Calibration();
+ Calibration(const DHRobot& robot);
virtual ~Calibration();
/*!
@@ -148,6 +148,11 @@ public:
*/
static void debugChain(const KDL::Chain& chain);
+ /*!
+ * \brief Creates a \p KDL::Chain representation of the member \p robot_parameters_
+ */
+ KDL::Chain getChain();
+
private:
/*!
* \brief Splits the given chain \p in into a subchain before \p split_index and after.
@@ -170,11 +175,6 @@ private:
*/
static DHRobot chainToDH(const KDL::Chain& chain);
- /*!
- * \brief Creates a \p KDL::Chain representation of the member \p robot_parameters_
- */
- KDL::Chain getChain();
-
/*!
* \brief Modifies the robot chain at segment \p correction_index
*
diff --git a/package.xml b/package.xml
index a3255a2..f1e6b57 100644
--- a/package.xml
+++ b/package.xml
@@ -36,6 +36,8 @@
tf_conversions
std_srvs
+ gtest
+
force_torque_sensor_controller
joint_state_controller
joint_trajectory_controller
diff --git a/src/calibration.cpp b/src/calibration.cpp
index f215bf3..d690484 100644
--- a/src/calibration.cpp
+++ b/src/calibration.cpp
@@ -16,43 +16,8 @@
#include
-Calibration::Calibration()
+Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters)
{
- // This is the data of our ids-ur10-2 Later, dh parameters will be given using another
- // constructor.
-
- DHRobot my_robot;
- my_robot.segments_.push_back(DHSegment(0.1273, 0, 0, M_PI / 2));
- my_robot.segments_.push_back(DHSegment(0, -0.612, 0, 0));
- my_robot.segments_.push_back(DHSegment(0, -0.5723, 0, 0.0));
- my_robot.segments_.push_back(DHSegment(0.163841, 0, 0.0, M_PI / 2));
- my_robot.segments_.push_back(DHSegment(0.1157, 0, 0, -M_PI / 2));
- my_robot.segments_.push_back(DHSegment(0.0922, 0, 0, 0));
- // my_robot.segments_.push_back(DHSegment(0.1, 0.2, 0.3, 0.4));
-
- // DHRobot my_robot_calibration;
- //// clang-format off
- //// d, a, theta, alpha
- // my_robot_calibration.segments_.push_back(DHSegment(0 ,0 ,0 ,0 ));
- // my_robot_calibration.segments_.push_back(DHSegment(1.5 ,0 ,0.0 ,0.0 ));
- // my_robot_calibration.segments_.push_back(DHSegment(0.5 ,0.0 ,0.2 ,0.0 ));
- // my_robot_calibration.segments_.push_back(DHSegment(-2 ,0 ,0 ,0 ));
- // my_robot_calibration.segments_.push_back(DHSegment(0 ,0 ,0 ,0 ));
- // my_robot_calibration.segments_.push_back(DHSegment(0 ,0 ,0 ,0 ));
- //// 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 ));
- // clang-format on
-
- robot_parameters_ = my_robot + my_robot_calibration;
}
Calibration::~Calibration()
@@ -177,8 +142,8 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain)
return buildCorrectedChain(robot_chain, new_length, new_theta, distance_correction);
}
-KDL::Chain Calibration::buildCorrectedChain(const KDL::Chain& robot_chain, const double new_length, const double new_theta,
- const double distance_correction)
+KDL::Chain Calibration::buildCorrectedChain(const KDL::Chain& robot_chain, const double new_length,
+ const double new_theta, const double distance_correction)
{
KDL::Chain corrected_chain;
@@ -217,7 +182,6 @@ KDL::Chain Calibration::buildCorrectedChain(const KDL::Chain& robot_chain, const
new_frame.p = robot_chain.getSegment(2).getFrameToTip().p;
ROS_INFO_STREAM("Correcting segment i+2 length from " << new_frame.p.z());
-
// the d-parameter can be modified by the intersection_parameter which is the distance traveled
// along the rotation axis
new_frame.p = KDL::Vector(0, 0, new_frame.p.z() - distance_correction);
@@ -310,6 +274,7 @@ KDL::Chain Calibration::getChain()
KDL::Frame frame2 = KDL::Frame::DH(robot_parameters_.segments_[i].a_, robot_parameters_.segments_[i].alpha_, 0, 0);
KDL::Segment segment2 =
KDL::Segment(link_names_[i] + "_passive", KDL::Joint(link_names_[i] + "_passive", KDL::Joint::None), frame2);
+
robot_chain.addSegment(segment2);
}
return robot_chain;
@@ -337,7 +302,8 @@ void Calibration::debugChain(const KDL::Chain& robot_chain)
result.M.GetRPY(roll, pitch, yaw);
ROS_INFO_STREAM("Relative rotation: " << roll << ", " << pitch << ", " << yaw);
fk_solver.JntToCart(jointpositions, result, i);
- ROS_INFO_STREAM(std::setprecision(15) << "Absolute position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z());
+ ROS_INFO_STREAM(std::setprecision(15)
+ << "Absolute position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z());
KDL::Joint joint = robot_chain.segments[i].getJoint();
ROS_INFO_STREAM("Joint type: " << joint.getTypeName());
ROS_INFO_STREAM("Joint position: " << joint.pose(0).p.x() << ", " << joint.pose(0).p.y() << ", "
diff --git a/src/calibration_correction.cpp b/src/calibration_correction.cpp
index 5acbf65..3026a8d 100644
--- a/src/calibration_correction.cpp
+++ b/src/calibration_correction.cpp
@@ -12,12 +12,29 @@
*/
//----------------------------------------------------------------------
-
#include
int main(int argc, char* argv[])
{
- Calibration calibration;
+ DHRobot my_robot;
+ my_robot.segments_.push_back(DHSegment(0.1273, 0, 0, M_PI / 2));
+ my_robot.segments_.push_back(DHSegment(0, -0.612, 0, 0));
+ my_robot.segments_.push_back(DHSegment(0, -0.5723, 0, 0.0));
+ my_robot.segments_.push_back(DHSegment(0.163841, 0, 0.0, M_PI / 2));
+ my_robot.segments_.push_back(DHSegment(0.1157, 0, 0, -M_PI / 2));
+ my_robot.segments_.push_back(DHSegment(0.0922, 0, 0, 0));
+ 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 ));
+ // clang-format on
+
+ Calibration calibration(my_robot + my_robot_calibration);
KDL::Chain corrected_chain = calibration.correctChain();
calibration.debugChain(corrected_chain);
diff --git a/test/calibration_test.cpp b/test/calibration_test.cpp
new file mode 100644
index 0000000..f706e8c
--- /dev/null
+++ b/test/calibration_test.cpp
@@ -0,0 +1,109 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author Felix Mauch mauch@fzi.de
+ * \date 2019-03-11
+ *
+ */
+//----------------------------------------------------------------------
+
+#include
+#include
+
+TEST(UrRtdeDriver, ur10_ideal)
+{
+ DHRobot my_robot;
+ const double pi = std::atan(1)*4;
+
+ // This is an ideal UR10
+ // clang-format off
+ // d, a, theta, alpha
+ my_robot.segments_.push_back(DHSegment(0.1273 , 0 , 0 , pi / 2));
+ my_robot.segments_.push_back(DHSegment(0 , -0.612 , 0 , 0));
+ my_robot.segments_.push_back(DHSegment(0 , -0.5723, 0 , 0.0));
+ my_robot.segments_.push_back(DHSegment(0.163841, 0 , 0 , pi / 2));
+ my_robot.segments_.push_back(DHSegment(0.1157 , 0 , 0 , -pi / 2));
+ my_robot.segments_.push_back(DHSegment(0.0922 , 0 , 0 , 0));
+ // clang-format on
+
+ Calibration calibration(my_robot);
+
+ KDL::Chain robot_chain = calibration.getChain();
+ uint32_t num_jts = robot_chain.getNrOfJoints();
+
+ 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_);
+}
+
+TEST(UrRtdeDriver, calibration)
+{
+ DHRobot my_robot;
+ const double pi = std::atan(1)*4;
+
+ // This is an ideal UR10
+ // clang-format off
+ // d, a, theta, alpha
+ my_robot.segments_.push_back(DHSegment(0.1273 , 0 , 0 , pi / 2));
+ my_robot.segments_.push_back(DHSegment(0 , -0.612 , 0 , 0));
+ my_robot.segments_.push_back(DHSegment(0 , -0.5723, 0 , 0.0));
+ my_robot.segments_.push_back(DHSegment(0.163841, 0 , 0 , pi / 2));
+ my_robot.segments_.push_back(DHSegment(0.1157 , 0 , 0 , -pi / 2));
+ my_robot.segments_.push_back(DHSegment(0.0922 , 0 , 0 , 0));
+ // 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 ));
+ // clang-format on
+
+ Calibration calibration(my_robot + my_robot_calibration);
+
+ KDL::Chain robot_chain = calibration.getChain();
+ uint32_t num_jts = robot_chain.getNrOfJoints();
+
+ 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
+ std::cout << result_original.p.x() << std::endl;
+ std::cout << result_corrected.p.x() << std::endl;
+ std::cout << result_original.p.y() << std::endl;
+ std::cout << result_corrected.p.y() << std::endl;
+ std::cout << result_original.p.z() << std::endl;
+ std::cout << result_corrected.p.z() << std::endl;
+ 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());
+}
+
+int main(int argc, char* argv[])
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ return RUN_ALL_TESTS();
+}
+