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

added unit tests for calibration

Browse Source
This commit is contained in:
Felix Mauch
2019-03-11 22:15:49 +01:00
parent d2eb7a8683
commit 792cdc3a08
6 changed files with 154 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
CMakeLists.txt
View File

@@ -143,12 +143,19 @@ install(DIRECTORY include/${PROJECT_NAME}/
FILES_MATCHING PATTERN "*.h" FILES_MATCHING PATTERN "*.h"
) )
#############
## Testing ##
#############
#if (CATKIN_ENABLE_TESTING) if (CATKIN_ENABLE_TESTING)
#set(${PROJECT_NAME}_TEST_SOURCES catkin_add_gtest(calibration_test
#tests/ur/master_board.cpp test/calibration_test.cpp
#tests/ur/robot_mode.cpp src/calibration.cpp)
#tests/ur/rt_state.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) #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}) #target_link_libraries(ur_rtde_driver_test ur_rtde_driver ${catkin_LIBRARIES})

12
include/ur_rtde_driver/calibration.h
View File

@@ -134,7 +134,7 @@ struct DHRobot
class Calibration class Calibration
{ {
public: public:
Calibration(); Calibration(const DHRobot& robot);
virtual ~Calibration(); virtual ~Calibration();
/*! /*!
@@ -148,6 +148,11 @@ public:
*/ */
static void debugChain(const KDL::Chain& chain); static void debugChain(const KDL::Chain& chain);
/*!
* \brief Creates a \p KDL::Chain representation of the member \p robot_parameters_
*/
KDL::Chain getChain();
private: private:
/*! /*!
* \brief Splits the given chain \p in into a subchain before \p split_index and after. * \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); 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 * \brief Modifies the robot chain at segment \p correction_index
* *

2
package.xml
View File

@@ -36,6 +36,8 @@
<depend>tf_conversions</depend> <depend>tf_conversions</depend>
<depend>std_srvs</depend> <depend>std_srvs</depend>
<test_depend>gtest</test_depend>
<exec_depend>force_torque_sensor_controller</exec_depend> <exec_depend>force_torque_sensor_controller</exec_depend>
<exec_depend>joint_state_controller</exec_depend> <exec_depend>joint_state_controller</exec_depend>
<exec_depend>joint_trajectory_controller</exec_depend> <exec_depend>joint_trajectory_controller</exec_depend>

46
src/calibration.cpp
View File

@@ -16,43 +16,8 @@
#include <eigen_conversions/eigen_kdl.h> #include <eigen_conversions/eigen_kdl.h>
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() Calibration::~Calibration()
@@ -177,8 +142,8 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain)
return buildCorrectedChain(robot_chain, new_length, new_theta, distance_correction); 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, KDL::Chain Calibration::buildCorrectedChain(const KDL::Chain& robot_chain, const double new_length,
const double distance_correction) const double new_theta, const double distance_correction)
{ {
KDL::Chain corrected_chain; 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; new_frame.p = robot_chain.getSegment(2).getFrameToTip().p;
ROS_INFO_STREAM("Correcting segment i+2 length from " << new_frame.p.z()); 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 // the d-parameter can be modified by the intersection_parameter which is the distance traveled
// along the rotation axis // along the rotation axis
new_frame.p = KDL::Vector(0, 0, new_frame.p.z() - distance_correction); 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::Frame frame2 = KDL::Frame::DH(robot_parameters_.segments_[i].a_, robot_parameters_.segments_[i].alpha_, 0, 0);
KDL::Segment segment2 = KDL::Segment segment2 =
KDL::Segment(link_names_[i] + "_passive", KDL::Joint(link_names_[i] + "_passive", KDL::Joint::None), frame2); KDL::Segment(link_names_[i] + "_passive", KDL::Joint(link_names_[i] + "_passive", KDL::Joint::None), frame2);
robot_chain.addSegment(segment2); robot_chain.addSegment(segment2);
} }
return robot_chain; return robot_chain;
@@ -337,7 +302,8 @@ void Calibration::debugChain(const KDL::Chain& robot_chain)
result.M.GetRPY(roll, pitch, yaw); result.M.GetRPY(roll, pitch, yaw);
ROS_INFO_STREAM("Relative rotation: " << roll << ", " << pitch << ", " << yaw); ROS_INFO_STREAM("Relative rotation: " << roll << ", " << pitch << ", " << yaw);
fk_solver.JntToCart(jointpositions, result, i); 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(); KDL::Joint joint = robot_chain.segments[i].getJoint();
ROS_INFO_STREAM("Joint type: " << joint.getTypeName()); ROS_INFO_STREAM("Joint type: " << joint.getTypeName());
ROS_INFO_STREAM("Joint position: " << joint.pose(0).p.x() << ", " << joint.pose(0).p.y() << ", " ROS_INFO_STREAM("Joint position: " << joint.pose(0).p.x() << ", " << joint.pose(0).p.y() << ", "

21
src/calibration_correction.cpp
View File

@@ -12,12 +12,29 @@
*/ */
//---------------------------------------------------------------------- //----------------------------------------------------------------------
#include <ur_rtde_driver/calibration.h> #include <ur_rtde_driver/calibration.h>
int main(int argc, char* argv[]) 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(); KDL::Chain corrected_chain = calibration.correctChain();
calibration.debugChain(corrected_chain); calibration.debugChain(corrected_chain);

109
test/calibration_test.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include <ur_rtde_driver/calibration.h>
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();
}