From d2eb7a86831d6f0ed6622fd159b8850e19d93e15 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 10 Jan 2019 16:28:45 +0100 Subject: [PATCH 01/29] added first version of calibration --- CMakeLists.txt | 30 ++- include/ur_rtde_driver/calibration.h | 207 ++++++++++++++++ package.xml | 3 + src/calibration.cpp | 351 +++++++++++++++++++++++++++ src/calibration_correction.cpp | 25 ++ 5 files changed, 612 insertions(+), 4 deletions(-) create mode 100644 include/ur_rtde_driver/calibration.h create mode 100644 src/calibration.cpp create mode 100644 src/calibration_correction.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9fe8476..e083bf6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,17 +13,21 @@ find_package(catkin REQUIRED actionlib control_msgs controller_manager + eigen_conversions geometry_msgs hardware_interface + kdl_parser industrial_msgs roscpp sensor_msgs std_srvs tf + tf_conversions trajectory_msgs ur_msgs ) find_package(Boost REQUIRED) +find_package(Eigen3 REQUIRED) catkin_package( INCLUDE_DIRS @@ -34,13 +38,18 @@ catkin_package( actionlib control_msgs controller_manager + eigen_conversions geometry_msgs hardware_interface + kdl_parser industrial_msgs roscpp sensor_msgs trajectory_msgs ur_msgs + std_srvs + tf + tf_conversions DEPENDS Boost ) @@ -67,6 +76,7 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} ) add_library(ur_rtde_driver @@ -110,12 +120,19 @@ add_executable(plain_driver target_link_libraries(plain_driver ${catkin_LIBRARIES} ur_rtde_driver) add_dependencies(plain_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -install(TARGETS ur_rtde_driver ur_rtde_driver_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + +add_executable(calibration_correction + src/calibration.cpp + src/calibration_correction.cpp ) +add_dependencies(calibration_correction ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(calibration_correction + ${catkin_LIBRARIES} + yaml-cpp +) install(DIRECTORY config launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) @@ -136,3 +153,8 @@ install(DIRECTORY include/${PROJECT_NAME}/ #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}) #endif() +install(TARGETS calibration_correction RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install(TARGETS ur_rtde_driver ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(FILES Apache-2.0_ur_modern_driver.txt DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/LICENSES) +install(FILES manifest.yaml rosdoc.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/doc) diff --git a/include/ur_rtde_driver/calibration.h b/include/ur_rtde_driver/calibration.h new file mode 100644 index 0000000..ef99cd4 --- /dev/null +++ b/include/ur_rtde_driver/calibration.h @@ -0,0 +1,207 @@ +// 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-01-10 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED +#define UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED + +#include + +#include +#include + +/*! + * \brief An internal representation of a DH-parametrized link. + * + * Each segment consists of parameters a, d, alpha and theta. + */ +struct DHSegment +{ + double d_; + double a_; + double theta_; + double alpha_; + + /*! + * \brief Creates an element with defined elements. + */ + DHSegment(const double d, const double a, const double theta, const double alpha) + : d_(d), a_(a), theta_(theta), alpha_(alpha) + { + } + + /*! + * \brief Creates a Segment with all elements equal to zero. + */ + DHSegment() : d_(0), a_(0), theta_(0), alpha_(0) + { + } + + /*! + * \brief Adds another segment element-wise (a+a, d+d, alpha+alpha, theta+theta) + * + * \param other Other segment to add + * + * \returns Segment consisting of element-wise addition of \p this and \p other + */ + DHSegment operator+(const DHSegment& other) + { + return DHSegment(this->d_ + other.d_, this->a_ + other.a_, this->theta_ + other.theta_, + this->alpha_ + other.alpha_); + } +}; + +/*! + * \brief Internal representation of a robot based on DH parameters. + * + * Note that this representation doesn't contain a real DH parameter representation, but is used for + * a corrected model of calibrated UR robots. Shoulder and elbow axes are artificially shortened in + * the final representation, requiring a correction parameter in \p theta_2 and \p theta_3. + */ +struct DHRobot +{ + std::vector segments_; + double delta_theta_correction2_; + double delta_theta_correction3_; + + /*! + * \brief Create a new robot representation giving a set of \ref DHSegment objects + */ + DHRobot(const std::vector& segments) + { + } + + DHRobot() = default; + + /*! + * \brief Adds another robot representation, by adding their segments element-wise. See \ref + * DHSegment::operator+ for details. + */ + DHRobot operator+(const DHRobot& other) + { + assert(this->segments_.size() == other.segments_.size()); + DHRobot ret; + for (size_t i = 0; i < this->segments_.size(); ++i) + { + ret.segments_.push_back(this->segments_[i] + other.segments_[i]); + } + return ret; + } + + /*! + * \brief Generates a string representation of this robot representation. + * + * This string can be directly included into the xacro file used for a calibrated robot. + */ + std::string toXacroProperties() + { + std::stringstream ss; + for (size_t i = 0; i < segments_.size(); ++i) + { + ss << "\n"; + ss << "\n"; + ss << "\n"; + ss << "\n"; + } + ss << "\n"; + ss << "\n"; + return ss.str(); + } +}; + + +/*! + * \brief Class that handles the calibration correction for Universal Robots + * + * Universal robots provide a factory calibration of their DH parameters to exactly estimate their + * TCP pose using forward kinematics. However, those DH parameters construct a kinematic chain that + * can be very long, as upper arm and lower arm segments can be drawn out of their physical position + * by multiple meters (even above 100m can occur). + * + * This class helps creating a kinematic chain, that is as close as possible to the physical model, + * by dragging the upper and lower arm segments back to their zero position. + */ +class Calibration +{ +public: + Calibration(); + virtual ~Calibration(); + + /*! + * \brief Corrects a UR kinematic chain in such a way that shoulder and elbow offsets are 0. + */ + KDL::Chain correctChain(); + + /*! + * \brief Print information about every segment in the chain + * + */ + static void debugChain(const KDL::Chain& chain); + +private: + /*! + * \brief Splits the given chain \p in into a subchain before \p split_index and after. + * + * \param[in,out] in Complete chain that will be split. Will be only the part before the \p + * split_index after the function returns. + * \param[out] second Will contain the subchain after \p split_index after the function returns. + * \param[in] split_index Index at which the chain should be split. The segment with this index + * will be the first segment in \p second. + * + * \returns true on success, false if \p split_index is larger than the chain size. In the latter + * case, \p in will not be touched. + */ + bool splitChain(KDL::Chain& in, KDL::Chain& second, const size_t split_index); + + KDL::Chain correctAxis(KDL::Chain& chain); + + /*! + * \brief Converts a \p KDL::Chain representation into a \ref DHRobot representation + */ + 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 + * + * Correction will set the \p d parameter at the segment to 0 and adapt the rest of the chain + * accordingly + * + * \param[in,out] robot_chain The kinemtatic chain to correct + * \param[in] correction_index Index at which the correction step should be performed. + * + * \returns true on success, false if the chain is smaller than the \p correction_index + */ + bool correctChainAt(KDL::Chain& robot_chain, const size_t correction_index); + + /*! + * \brief Given all parameters to correct the chain and a chain, create a corrected chain. + * + * \param[in] robot_chain Uncorrected chain from which the correction parameters were calculated. + * \param[in] new_length New segment length (\p a) of the corrected segment + * \param[in] new_theta New rotation offset (\p theta) of the corrected segment + * \param[in] distance_correction Distance by which the next segment has to be adapted due to the + * correction + * + * \returns The corrected chain, where the first segment's \p d parameter is 0 + */ + static KDL::Chain buildCorrectedChain(const KDL::Chain& robot_chain, const double new_length, const double new_theta, const double distance_correction); + + DHRobot robot_parameters_; + std::vector link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" }; +}; +#endif // ifndef UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED diff --git a/package.xml b/package.xml index f722e9b..a3255a2 100644 --- a/package.xml +++ b/package.xml @@ -24,13 +24,16 @@ controller_manager actionlib control_msgs + eigen_conversions geometry_msgs + kdl_parser industrial_msgs roscpp sensor_msgs trajectory_msgs ur_msgs tf + tf_conversions std_srvs force_torque_sensor_controller diff --git a/src/calibration.cpp b/src/calibration.cpp new file mode 100644 index 0000000..f215bf3 --- /dev/null +++ b/src/calibration.cpp @@ -0,0 +1,351 @@ +// 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-01-10 + * + */ +//---------------------------------------------------------------------- + +#include + +#include + +Calibration::Calibration() +{ + // 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() +{ +} + +KDL::Chain Calibration::correctChain() +{ + KDL::Chain robot_chain = getChain(); + + correctChainAt(robot_chain, 2); + correctChainAt(robot_chain, 4); + + return robot_chain; +} + +bool Calibration::correctChainAt(KDL::Chain& robot_chain, const size_t correction_index) +{ + bool success = true; + KDL::Chain correction_chain, corrected_subchain; + + // Split chain into part before the correction joint and after. We then change the first link on + // the second chain. This splitting is necessary as KDL doesn't support forward kinematics + // relative to another link than the first. + success = splitChain(robot_chain, correction_chain, correction_index); + + // Correct the chain after the correction joint + corrected_subchain = correctAxis(correction_chain); + + // Add the corrected second part to the robot chain to get a full robot model again + robot_chain.addChain(corrected_subchain); + + return success; +} + +bool Calibration::splitChain(KDL::Chain& robot_chain, KDL::Chain& second_chain, const size_t split_index) +{ + if (split_index >= robot_chain.segments.size()) + { + return false; + } + + second_chain = KDL::Chain(); + for (auto it = robot_chain.segments.begin() + split_index; it != robot_chain.segments.end();) + { + KDL::Segment new_segment = *it; + robot_chain.segments.erase(it); + second_chain.addSegment(new_segment); + } + + return true; +} + +KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain) +{ + // Each DH-Segment is split into two KDL segments. One representing the d and theta parameters and + // one with the a and alpha parameters. If we start from the first segment (which represents d and + // theta), there follows a passive segment (with a and alpha) and the next d/theta-segment after + // that. + // + // In principle, the d parameter of the first segment gets set to zero, first. With this change, + // the kinematic structure gets destroyed, which has to be corrected: + // - With setting d to 0, both the start and end points of the passive segment move along the + // rotational axis of the start segment. Instead, the end point of the passive segment has to + // move along the rotational axis of the next segment. This creates a change in a and and theta, if + // the two rotational axes are not parallel. + // + // - The length of moving along the next segment's rotational axis is calculated by intersecting + // the rotational axis with the XY-plane of the first segment. + // + // - + KDL::Frame next_segment_frame; + KDL::Frame passive_frame; + + KDL::ChainFkSolverPos_recursive fk_solver(robot_chain); + uint32_t num_jts = robot_chain.getNrOfJoints(); + KDL::JntArray jointpositions = KDL::JntArray(num_jts); + + fk_solver.JntToCart(jointpositions, passive_frame, 2); + ROS_INFO_STREAM(passive_frame.p.x() << ", " << passive_frame.p.y() << ", " << passive_frame.p.z()); + fk_solver.JntToCart(jointpositions, next_segment_frame, 3); + ROS_INFO_STREAM(next_segment_frame.p.x() << ", " << next_segment_frame.p.y() << ", " << next_segment_frame.p.z()); + + Eigen::Vector3d eigen_passive; + tf::vectorKDLToEigen(passive_frame.p, eigen_passive); + Eigen::Vector3d eigen_next; + tf::vectorKDLToEigen(next_segment_frame.p, eigen_next); + + // Construct a representation of the next segment's rotational axis + Eigen::ParametrizedLine next_line; + next_line = Eigen::ParametrizedLine::Through(eigen_passive, eigen_next); + + // ROS_INFO_STREAM("next_line:" << std::endl + //<< "Base:" << std::endl + //<< next_line.origin() << std::endl + //<< "Direction:" << std::endl + //<< next_line.direction()); + + // XY-Plane of first segment's start + Eigen::Hyperplane plane(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, 0)); + + // Intersect the rotation axis with the XY-Plane. We use both notations, the length and + // intersection point, as we will need both. The intersection_param is the length moving along the + // plane (change in the next segment's d param), while the intersection point will be used for + // calculating the new angle theta. + double intersection_param = next_line.intersectionParameter(plane); + Eigen::Vector3d intersection = next_line.intersectionPoint(plane); + double new_theta = std::atan(intersection.y() / intersection.x()); + // Upper and lower arm segments on URs all have negative length due to dh params + double new_length = -1 * intersection.norm(); + // ROS_INFO_STREAM("Wrist line intersecting at " << std::endl << intersection); + // ROS_INFO_STREAM("Angle is " << new_theta); + // ROS_INFO_STREAM("Length is " << new_length); + // ROS_INFO_STREAM("Intersection param is " << intersection_param); + + // as we move the passive segment towards the first segment, we have to move away the next segment + // again, to keep the same kinematic structure. + double sign_dir = next_line.direction().z() > 0 ? 1.0 : -1.0; + double distance_correction = intersection_param * sign_dir; + + ROS_INFO_STREAM("Corrected chain at " << robot_chain.segments[0].getName()); + 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 corrected_chain; + + // Set d parameter of the first segment to 0 and theta to the calculated new angle + { + KDL::Frame frame = KDL::Frame::DH(0, 0, 0, new_theta); + KDL::Joint joint = KDL::Joint(KDL::Vector(0, 0, 0), KDL::Vector(0, 0, 1), KDL::Joint::RotAxis); + KDL::Segment segment = KDL::Segment(robot_chain.segments[0].getName(), joint, frame); + corrected_chain.addSegment(segment); + } + + // Correct arm segment length and angle + { + // Get the original theta from the first link + double roll_a, pitch_a, yaw_a; + robot_chain.segments[0].getFrameToTip().M.GetRPY(roll_a, pitch_a, yaw_a); + + double roll, pitch, yaw; + robot_chain.segments[1].getFrameToTip().M.GetRPY(roll, pitch, yaw); + + // Since we changed the kinematic structure, we have to make sure to rotate around the correct + // axis, so we revert the theta change in this joint, again. + KDL::Rotation rotation = KDL::Rotation::EulerZYX(yaw_a - new_theta, 0, roll); + KDL::Vector position(new_length, 0, 0); + KDL::Frame frame(rotation, position); + + // KDL::Frame frame = KDL::Frame::DH(new_length, roll, 0, yaw_a - new_theta); + + KDL::Segment segment = KDL::Segment(robot_chain.segments[1].getName(), KDL::Joint(KDL::Joint::None), frame); + corrected_chain.addSegment(segment); + } + + // Correct next joint + { + KDL::Frame new_frame = robot_chain.getSegment(2).getFrameToTip(); + 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); + + ROS_INFO_STREAM("Corrected segment i+2 length to " << new_frame.p.z()); + KDL::Joint new_joint = robot_chain.getSegment(2).getJoint(); + + KDL::Segment segment = KDL::Segment(robot_chain.getSegment(2).getName(), new_joint, new_frame); + corrected_chain.addSegment(segment); + } + + for (size_t i = 3; i < robot_chain.segments.size(); ++i) + { + corrected_chain.addSegment(robot_chain.segments[i]); + } + + return corrected_chain; +} + +DHRobot Calibration::chainToDH(const KDL::Chain& chain) +{ + KDL::ChainFkSolverPos_recursive fk_solver(chain); + uint32_t num_jts = chain.getNrOfJoints(); + KDL::JntArray jointpositions = KDL::JntArray(num_jts); + // Assign some values to the joint positions + for (unsigned int i = 0; i < num_jts; i++) + { + jointpositions(i) = 0; + } + + DHRobot robot; + + for (size_t i = 0; i < chain.segments.size(); i += 2) + { + ROS_INFO_STREAM("Extracting DH parameters for joint " << i); + for (size_t j = i; j < i + 2; ++j) + { + // ROS_INFO_STREAM(j << " < " << i << "+2"); + KDL::Frame result; + result = chain.segments[j].getFrameToTip(); + ROS_INFO_STREAM("Relative position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z()); + double roll, pitch, yaw; + result.M.GetRPY(roll, pitch, yaw); + ROS_INFO_STREAM("Relative rotation: " << roll << ", " << pitch << ", " << yaw); + fk_solver.JntToCart(jointpositions, result, j); + KDL::Joint joint = chain.segments[j].getJoint(); + } + + DHSegment seg; + { + KDL::Frame result = chain.segments[i].getFrameToTip(); + double roll, pitch, yaw; + result.M.GetRPY(roll, pitch, yaw); + seg.theta_ = yaw; + seg.d_ = result.p.z(); + } + { + KDL::Frame result = chain.segments[i + 1].getFrameToTip(); + double roll, pitch, yaw; + result.M.GetRPY(roll, pitch, yaw); + seg.alpha_ = roll; + seg.a_ = result.p.x(); + } + + robot.segments_.push_back(seg); + } + + { + double roll, pitch, yaw; + chain.segments[3].getFrameToTip().M.GetRPY(roll, pitch, yaw); + robot.delta_theta_correction2_ = yaw; + chain.segments[5].getFrameToTip().M.GetRPY(roll, pitch, yaw); + robot.delta_theta_correction3_ = yaw; + } + + return robot; +} + +KDL::Chain Calibration::getChain() +{ + KDL::Chain robot_chain; + for (size_t i = 0; i < robot_parameters_.segments_.size(); ++i) + { + KDL::Frame frame = KDL::Frame::DH(0, 0, robot_parameters_.segments_[i].d_, robot_parameters_.segments_[i].theta_); + KDL::Joint joint = KDL::Joint(link_names_[i], KDL::Vector(0, 0, robot_parameters_.segments_[i].d_), + KDL::Vector(0, 0, 1), KDL::Joint::RotAxis); + + KDL::Segment segment = KDL::Segment(link_names_[i], joint, frame); + robot_chain.addSegment(segment); + 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; +} + +void Calibration::debugChain(const KDL::Chain& robot_chain) +{ + uint32_t num_jts = robot_chain.getNrOfJoints(); + + KDL::ChainFkSolverPos_recursive fk_solver(robot_chain); + KDL::JntArray jointpositions = KDL::JntArray(num_jts); + // Assign some values to the joint positions + for (unsigned int i = 0; i < num_jts; i++) + { + jointpositions(i) = 0; + } + + for (size_t i = 0; i < robot_chain.segments.size(); ++i) + { + ROS_INFO_STREAM("Segment " << i << ": " << robot_chain.segments[i].getName()); + KDL::Frame result; + result = robot_chain.segments[i].getFrameToTip(); + ROS_INFO_STREAM("Relative position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z()); + double roll, pitch, yaw; + 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()); + 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() << ", " + << joint.pose(0).p.z()); + ROS_INFO_STREAM("Joint Axis: " << joint.JointAxis().x() << "," << joint.JointAxis().y() << ", " + << joint.JointAxis().z()); + } + + DHRobot robot_out = Calibration::chainToDH(robot_chain); + ROS_INFO_STREAM("Robot data:\n" << robot_out.toXacroProperties()); +} diff --git a/src/calibration_correction.cpp b/src/calibration_correction.cpp new file mode 100644 index 0000000..5acbf65 --- /dev/null +++ b/src/calibration_correction.cpp @@ -0,0 +1,25 @@ +// 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-01-10 + * + */ +//---------------------------------------------------------------------- + + +#include + +int main(int argc, char* argv[]) +{ + Calibration calibration; + KDL::Chain corrected_chain = calibration.correctChain(); + calibration.debugChain(corrected_chain); + + return 0; +} From 792cdc3a08c07eff922e6f63ae052290285c38c8 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 11 Mar 2019 22:15:49 +0100 Subject: [PATCH 02/29] added unit tests for calibration --- CMakeLists.txt | 17 +++-- include/ur_rtde_driver/calibration.h | 12 +-- package.xml | 2 + src/calibration.cpp | 46 ++--------- src/calibration_correction.cpp | 21 +++++- test/calibration_test.cpp | 109 +++++++++++++++++++++++++++ 6 files changed, 154 insertions(+), 53 deletions(-) create mode 100644 test/calibration_test.cpp 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(); +} + From eaae5234843781870ab16eb3405edee836820a42 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Tue, 12 Mar 2019 03:04:25 +0100 Subject: [PATCH 03/29] implemented a prototype for an integration test for calibration We should put this into a proper calibration test. --- src/calibration_correction.cpp | 105 ++++++++++++++++++++++++++------- 1 file changed, 85 insertions(+), 20 deletions(-) diff --git a/src/calibration_correction.cpp b/src/calibration_correction.cpp index 3026a8d..6935361 100644 --- a/src/calibration_correction.cpp +++ b/src/calibration_correction.cpp @@ -14,29 +14,94 @@ #include +#include +#include + +std::unique_ptr fk_solver; +sensor_msgs::JointStatePtr joint_state; +std::unique_ptr tf_listener; + +void jointStateCallback(sensor_msgs::JointStatePtr msg) +{ + tf::StampedTransform stamped_transform; + try + { + tf_listener->lookupTransform("base", "tool0_controller", ros::Time(0), stamped_transform); + } + catch (tf::TransformException ex) + { + ROS_ERROR("TF lookup error error: %s", ex.what()); + return; + } + + geometry_msgs::TransformStamped tf_msg; + tf::transformStampedTFToMsg(stamped_transform, tf_msg); + + // ROS_INFO_STREAM(tf_msg); + + KDL::Frame result_original; + KDL::JntArray jointpositions = KDL::JntArray(6); + for (size_t i = 0; i < 6; ++i) + { + jointpositions.data[i] = msg->position[i]; + } + fk_solver->JntToCart(jointpositions, result_original); + + Eigen::Matrix4d mat; + result_original.Make4x4(mat.data()); + + Eigen::Vector3d error; + error << mat.transpose().topRightCorner(3, 1) - Eigen::Vector3d(stamped_transform.getOrigin()); + + ROS_INFO_STREAM(error); +} + int main(int argc, char* argv[]) { - 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 + ros::init(argc, argv, "ur_calibration"); + ros::NodeHandle nh; - Calibration calibration(my_robot + my_robot_calibration); - KDL::Chain corrected_chain = calibration.correctChain(); - calibration.debugChain(corrected_chain); + DHRobot my_robot; + // This is an ideal UR10 + // clang-format off + // d, a, theta, alpha + 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.163941, 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)); + // 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 )); + // Calibration calibration(my_robot + my_robot_calibration); + Calibration calibration(my_robot); + KDL::Chain corrected_chain = calibration.getChain(); + // calibration.debugChain(corrected_chain); + + uint32_t num_jts = corrected_chain.getNrOfJoints(); + + fk_solver.reset(new KDL::ChainFkSolverPos_recursive(corrected_chain)); + KDL::JntArray jointpositions = KDL::JntArray(num_jts); + KDL::Frame result_original; + + tf_listener.reset(new tf::TransformListener); + + ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback); + + ros::spin(); return 0; } From 8a9192aebabf909ce98cc1b3d491d75a0fe6a7c3 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 20 Mar 2019 09:59:00 +0100 Subject: [PATCH 04/29] reduce output during testing --- src/calibration.cpp | 10 +++++----- test/calibration_test.cpp | 16 ++++++---------- 2 files changed, 11 insertions(+), 15 deletions(-) diff --git a/src/calibration.cpp b/src/calibration.cpp index d690484..3fbeef5 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -97,9 +97,9 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain) KDL::JntArray jointpositions = KDL::JntArray(num_jts); fk_solver.JntToCart(jointpositions, passive_frame, 2); - ROS_INFO_STREAM(passive_frame.p.x() << ", " << passive_frame.p.y() << ", " << passive_frame.p.z()); + //ROS_INFO_STREAM(passive_frame.p.x() << ", " << passive_frame.p.y() << ", " << passive_frame.p.z()); fk_solver.JntToCart(jointpositions, next_segment_frame, 3); - ROS_INFO_STREAM(next_segment_frame.p.x() << ", " << next_segment_frame.p.y() << ", " << next_segment_frame.p.z()); + //ROS_INFO_STREAM(next_segment_frame.p.x() << ", " << next_segment_frame.p.y() << ", " << next_segment_frame.p.z()); Eigen::Vector3d eigen_passive; tf::vectorKDLToEigen(passive_frame.p, eigen_passive); @@ -138,7 +138,7 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain) double sign_dir = next_line.direction().z() > 0 ? 1.0 : -1.0; double distance_correction = intersection_param * sign_dir; - ROS_INFO_STREAM("Corrected chain at " << robot_chain.segments[0].getName()); + //ROS_INFO_STREAM("Corrected chain at " << robot_chain.segments[0].getName()); return buildCorrectedChain(robot_chain, new_length, new_theta, distance_correction); } @@ -180,13 +180,13 @@ KDL::Chain Calibration::buildCorrectedChain(const KDL::Chain& robot_chain, const { KDL::Frame new_frame = robot_chain.getSegment(2).getFrameToTip(); 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 // along the rotation axis new_frame.p = KDL::Vector(0, 0, new_frame.p.z() - distance_correction); - ROS_INFO_STREAM("Corrected segment i+2 length to " << new_frame.p.z()); + //ROS_INFO_STREAM("Corrected segment i+2 length to " << new_frame.p.z()); KDL::Joint new_joint = robot_chain.getSegment(2).getJoint(); KDL::Segment segment = KDL::Segment(robot_chain.getSegment(2).getName(), new_joint, new_frame); diff --git a/test/calibration_test.cpp b/test/calibration_test.cpp index f706e8c..395903c 100644 --- a/test/calibration_test.cpp +++ b/test/calibration_test.cpp @@ -15,14 +15,15 @@ #include #include +namespace +{ TEST(UrRtdeDriver, ur10_ideal) { DHRobot my_robot; - const double pi = std::atan(1)*4; + 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)); @@ -50,7 +51,7 @@ TEST(UrRtdeDriver, ur10_ideal) TEST(UrRtdeDriver, calibration) { DHRobot my_robot; - const double pi = std::atan(1)*4; + const double pi = std::atan(1) * 4; // This is an ideal UR10 // clang-format off @@ -89,20 +90,15 @@ TEST(UrRtdeDriver, calibration) 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()); } +} // namespace int main(int argc, char* argv[]) { - testing::InitGoogleTest(&argc, argv); + ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } From bc15944fa1f656db723a0544a5ffd182c9ca19e5 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 20 Mar 2019 18:33:04 +0100 Subject: [PATCH 05/29] Use plain Eigen for kinematics --- include/ur_rtde_driver/calibration.h | 70 +------ src/calibration.cpp | 290 +++++++-------------------- src/calibration_correction.cpp | 69 +++---- test/calibration_test.cpp | 83 ++++---- 4 files changed, 161 insertions(+), 351 deletions(-) diff --git a/include/ur_rtde_driver/calibration.h b/include/ur_rtde_driver/calibration.h index 7068515..6bf7d84 100644 --- a/include/ur_rtde_driver/calibration.h +++ b/include/ur_rtde_driver/calibration.h @@ -16,9 +16,7 @@ #define UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED #include - -#include -#include +#include /*! * \brief An internal representation of a DH-parametrized link. @@ -119,7 +117,6 @@ struct DHRobot } }; - /*! * \brief Class that handles the calibration correction for Universal Robots * @@ -140,68 +137,23 @@ public: /*! * \brief Corrects a UR kinematic chain in such a way that shoulder and elbow offsets are 0. */ - KDL::Chain correctChain(); + void correctChain(); - /*! - * \brief Print information about every segment in the chain - * - */ - static void debugChain(const KDL::Chain& chain); + std::vector getChain() + { + return chain_; + } - /*! - * \brief Creates a \p KDL::Chain representation of the member \p robot_parameters_ - */ - KDL::Chain getChain(); + Eigen::Matrix4d calcForwardKinematics(const Eigen::Matrix& joint_values, const size_t link_nr = 6); private: - /*! - * \brief Splits the given chain \p in into a subchain before \p split_index and after. - * - * \param[in,out] in Complete chain that will be split. Will be only the part before the \p - * split_index after the function returns. - * \param[out] second Will contain the subchain after \p split_index after the function returns. - * \param[in] split_index Index at which the chain should be split. The segment with this index - * will be the first segment in \p second. - * - * \returns true on success, false if \p split_index is larger than the chain size. In the latter - * case, \p in will not be touched. - */ - bool splitChain(KDL::Chain& in, KDL::Chain& second, const size_t split_index); + void correctAxis(const size_t correction_index); - KDL::Chain correctAxis(KDL::Chain& chain); - - /*! - * \brief Converts a \p KDL::Chain representation into a \ref DHRobot representation - */ - static DHRobot chainToDH(const KDL::Chain& chain); - - /*! - * \brief Modifies the robot chain at segment \p correction_index - * - * Correction will set the \p d parameter at the segment to 0 and adapt the rest of the chain - * accordingly - * - * \param[in,out] robot_chain The kinemtatic chain to correct - * \param[in] correction_index Index at which the correction step should be performed. - * - * \returns true on success, false if the chain is smaller than the \p correction_index - */ - bool correctChainAt(KDL::Chain& robot_chain, const size_t correction_index); - - /*! - * \brief Given all parameters to correct the chain and a chain, create a corrected chain. - * - * \param[in] robot_chain Uncorrected chain from which the correction parameters were calculated. - * \param[in] new_length New segment length (\p a) of the corrected segment - * \param[in] new_theta New rotation offset (\p theta) of the corrected segment - * \param[in] distance_correction Distance by which the next segment has to be adapted due to the - * correction - * - * \returns The corrected chain, where the first segment's \p d parameter is 0 - */ - static KDL::Chain buildCorrectedChain(const KDL::Chain& robot_chain, const double new_length, const double new_theta, const double distance_correction); + void buildChain(); DHRobot robot_parameters_; std::vector link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" }; + + std::vector chain_; }; #endif // ifndef UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED diff --git a/src/calibration.cpp b/src/calibration.cpp index 3fbeef5..6bc704a 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -14,66 +14,24 @@ #include -#include - Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters) { + buildChain(); } Calibration::~Calibration() { } -KDL::Chain Calibration::correctChain() +void Calibration::correctChain() { - KDL::Chain robot_chain = getChain(); - - correctChainAt(robot_chain, 2); - correctChainAt(robot_chain, 4); - - return robot_chain; + correctAxis(1); + correctAxis(2); } -bool Calibration::correctChainAt(KDL::Chain& robot_chain, const size_t correction_index) +void Calibration::correctAxis(const size_t link_index) { - bool success = true; - KDL::Chain correction_chain, corrected_subchain; - - // Split chain into part before the correction joint and after. We then change the first link on - // the second chain. This splitting is necessary as KDL doesn't support forward kinematics - // relative to another link than the first. - success = splitChain(robot_chain, correction_chain, correction_index); - - // Correct the chain after the correction joint - corrected_subchain = correctAxis(correction_chain); - - // Add the corrected second part to the robot chain to get a full robot model again - robot_chain.addChain(corrected_subchain); - - return success; -} - -bool Calibration::splitChain(KDL::Chain& robot_chain, KDL::Chain& second_chain, const size_t split_index) -{ - if (split_index >= robot_chain.segments.size()) - { - return false; - } - - second_chain = KDL::Chain(); - for (auto it = robot_chain.segments.begin() + split_index; it != robot_chain.segments.end();) - { - KDL::Segment new_segment = *it; - robot_chain.segments.erase(it); - second_chain.addSegment(new_segment); - } - - return true; -} - -KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain) -{ - // Each DH-Segment is split into two KDL segments. One representing the d and theta parameters and + // Each DH-Segment is split into two chain segments. One representing the d and theta parameters and // one with the a and alpha parameters. If we start from the first segment (which represents d and // theta), there follows a passive segment (with a and alpha) and the next d/theta-segment after // that. @@ -87,24 +45,28 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain) // // - The length of moving along the next segment's rotational axis is calculated by intersecting // the rotational axis with the XY-plane of the first segment. - // - // - - KDL::Frame next_segment_frame; - KDL::Frame passive_frame; - KDL::ChainFkSolverPos_recursive fk_solver(robot_chain); - uint32_t num_jts = robot_chain.getNrOfJoints(); - KDL::JntArray jointpositions = KDL::JntArray(num_jts); + if (chain_[2 * link_index](2, 3) == 0.0) + { + // Nothing to do here. + return; + } - fk_solver.JntToCart(jointpositions, passive_frame, 2); - //ROS_INFO_STREAM(passive_frame.p.x() << ", " << passive_frame.p.y() << ", " << passive_frame.p.z()); - fk_solver.JntToCart(jointpositions, next_segment_frame, 3); - //ROS_INFO_STREAM(next_segment_frame.p.x() << ", " << next_segment_frame.p.y() << ", " << next_segment_frame.p.z()); + Eigen::Matrix jointvalues; + jointvalues << 0, 0, 0, 0, 0, 0; + // Eigen::Matrix4d fk_current = calcForwardKinematics(jointvalues, link_index); + // Eigen::Vector3d current_passive = fk_current.topRightCorner(3, 1); + // ROS_INFO_STREAM("current passive:\n" << current_passive); + Eigen::Matrix4d fk_current = Eigen::Matrix4d::Identity(); + Eigen::Vector3d current_passive = Eigen::Vector3d::Zero(); - Eigen::Vector3d eigen_passive; - tf::vectorKDLToEigen(passive_frame.p, eigen_passive); - Eigen::Vector3d eigen_next; - tf::vectorKDLToEigen(next_segment_frame.p, eigen_next); + Eigen::Matrix4d fk_next_passive = Eigen::Matrix4d::Identity(); + fk_next_passive *= chain_[link_index * 2] * chain_[link_index * 2 + 1]; + Eigen::Vector3d eigen_passive = fk_next_passive.topRightCorner(3, 1); + // ROS_INFO_STREAM("Eigen passive:\n" << eigen_passive); + + Eigen::Vector3d eigen_next = (fk_next_passive * chain_[(link_index + 1) * 2]).topRightCorner(3, 1); + // ROS_INFO_STREAM("Eigen next:\n" << eigen_next); // Construct a representation of the next segment's rotational axis Eigen::ParametrizedLine next_line; @@ -117,14 +79,14 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain) //<< next_line.direction()); // XY-Plane of first segment's start - Eigen::Hyperplane plane(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, 0)); + Eigen::Hyperplane plane(fk_current.topLeftCorner(3, 3) * Eigen::Vector3d(0, 0, 1), current_passive); // Intersect the rotation axis with the XY-Plane. We use both notations, the length and // intersection point, as we will need both. The intersection_param is the length moving along the // plane (change in the next segment's d param), while the intersection point will be used for // calculating the new angle theta. double intersection_param = next_line.intersectionParameter(plane); - Eigen::Vector3d intersection = next_line.intersectionPoint(plane); + Eigen::Vector3d intersection = next_line.intersectionPoint(plane) - current_passive; double new_theta = std::atan(intersection.y() / intersection.x()); // Upper and lower arm segments on URs all have negative length due to dh params double new_length = -1 * intersection.norm(); @@ -138,180 +100,66 @@ KDL::Chain Calibration::correctAxis(KDL::Chain& robot_chain) double sign_dir = next_line.direction().z() > 0 ? 1.0 : -1.0; double distance_correction = intersection_param * sign_dir; - //ROS_INFO_STREAM("Corrected chain at " << robot_chain.segments[0].getName()); - 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 corrected_chain; - // Set d parameter of the first segment to 0 and theta to the calculated new angle - { - KDL::Frame frame = KDL::Frame::DH(0, 0, 0, new_theta); - KDL::Joint joint = KDL::Joint(KDL::Vector(0, 0, 0), KDL::Vector(0, 0, 1), KDL::Joint::RotAxis); - KDL::Segment segment = KDL::Segment(robot_chain.segments[0].getName(), joint, frame); - corrected_chain.addSegment(segment); - } + // Correct arm segment length and angle + // ROS_INFO_STREAM("Passive old:\n" << chain_[2 * link_index]); + chain_[2 * link_index](2, 3) = 0.0; + chain_[2 * link_index].topLeftCorner(3, 3) = + Eigen::AngleAxisd(new_theta, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + // ROS_INFO_STREAM("Passive new:\n" << chain_[2 * link_index]); // Correct arm segment length and angle - { - // Get the original theta from the first link - double roll_a, pitch_a, yaw_a; - robot_chain.segments[0].getFrameToTip().M.GetRPY(roll_a, pitch_a, yaw_a); - - double roll, pitch, yaw; - robot_chain.segments[1].getFrameToTip().M.GetRPY(roll, pitch, yaw); - - // Since we changed the kinematic structure, we have to make sure to rotate around the correct - // axis, so we revert the theta change in this joint, again. - KDL::Rotation rotation = KDL::Rotation::EulerZYX(yaw_a - new_theta, 0, roll); - KDL::Vector position(new_length, 0, 0); - KDL::Frame frame(rotation, position); - - // KDL::Frame frame = KDL::Frame::DH(new_length, roll, 0, yaw_a - new_theta); - - KDL::Segment segment = KDL::Segment(robot_chain.segments[1].getName(), KDL::Joint(KDL::Joint::None), frame); - corrected_chain.addSegment(segment); - } + // ROS_INFO_STREAM("Next old:\n" << chain_[2 * link_index + 1]); + // ROS_INFO_STREAM("Theta correction: " << robot_parameters_.segments_[link_index].theta_ - new_theta); + // ROS_INFO_STREAM("Alpha correction: " << robot_parameters_.segments_[link_index].alpha_); + chain_[2 * link_index + 1](0, 3) = new_length; + chain_[2 * link_index + 1].topLeftCorner(3, 3) = + Eigen::AngleAxisd(robot_parameters_.segments_[link_index].theta_ - new_theta, Eigen::Vector3d::UnitZ()) + .toRotationMatrix() * + Eigen::AngleAxisd(robot_parameters_.segments_[link_index].alpha_, Eigen::Vector3d::UnitX()).toRotationMatrix(); + // ROS_INFO_STREAM("Next new:\n" << chain_[2 * link_index + 1]); // Correct next joint - { - KDL::Frame new_frame = robot_chain.getSegment(2).getFrameToTip(); - 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); - - //ROS_INFO_STREAM("Corrected segment i+2 length to " << new_frame.p.z()); - KDL::Joint new_joint = robot_chain.getSegment(2).getJoint(); - - KDL::Segment segment = KDL::Segment(robot_chain.getSegment(2).getName(), new_joint, new_frame); - corrected_chain.addSegment(segment); - } - - for (size_t i = 3; i < robot_chain.segments.size(); ++i) - { - corrected_chain.addSegment(robot_chain.segments[i]); - } - - return corrected_chain; + // ROS_INFO_STREAM("Second Next old:\n" << chain_[2 * link_index + 2]); + chain_[2 * link_index + 2](2, 3) -= distance_correction; + // ROS_INFO_STREAM("Second Next new:\n" << chain_[2 * link_index + 2]); } -DHRobot Calibration::chainToDH(const KDL::Chain& chain) +Eigen::Matrix4d Calibration::calcForwardKinematics(const Eigen::Matrix& joint_values, + const size_t link_nr) { - KDL::ChainFkSolverPos_recursive fk_solver(chain); - uint32_t num_jts = chain.getNrOfJoints(); - KDL::JntArray jointpositions = KDL::JntArray(num_jts); - // Assign some values to the joint positions - for (unsigned int i = 0; i < num_jts; i++) + // ROS_INFO_STREAM("Calculating forward kinematics at link " << link_nr); + // Currently ignore input and calculate for zero vector input + Eigen::Matrix4d output = Eigen::Matrix4d::Identity(); + for (size_t i = 0; i < link_nr; ++i) { - jointpositions(i) = 0; + Eigen::Matrix4d rotation = Eigen::Matrix4d::Identity(); + rotation.topLeftCorner(3, 3) = Eigen::AngleAxisd(joint_values(i), Eigen::Vector3d::UnitZ()).toRotationMatrix(); + output *= chain_[2 * i] * rotation * chain_[2 * i + 1]; } - DHRobot robot; + // ROS_INFO_STREAM("forward_kinematics at " << link_nr << std::endl << output); - for (size_t i = 0; i < chain.segments.size(); i += 2) - { - ROS_INFO_STREAM("Extracting DH parameters for joint " << i); - for (size_t j = i; j < i + 2; ++j) - { - // ROS_INFO_STREAM(j << " < " << i << "+2"); - KDL::Frame result; - result = chain.segments[j].getFrameToTip(); - ROS_INFO_STREAM("Relative position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z()); - double roll, pitch, yaw; - result.M.GetRPY(roll, pitch, yaw); - ROS_INFO_STREAM("Relative rotation: " << roll << ", " << pitch << ", " << yaw); - fk_solver.JntToCart(jointpositions, result, j); - KDL::Joint joint = chain.segments[j].getJoint(); - } - - DHSegment seg; - { - KDL::Frame result = chain.segments[i].getFrameToTip(); - double roll, pitch, yaw; - result.M.GetRPY(roll, pitch, yaw); - seg.theta_ = yaw; - seg.d_ = result.p.z(); - } - { - KDL::Frame result = chain.segments[i + 1].getFrameToTip(); - double roll, pitch, yaw; - result.M.GetRPY(roll, pitch, yaw); - seg.alpha_ = roll; - seg.a_ = result.p.x(); - } - - robot.segments_.push_back(seg); - } - - { - double roll, pitch, yaw; - chain.segments[3].getFrameToTip().M.GetRPY(roll, pitch, yaw); - robot.delta_theta_correction2_ = yaw; - chain.segments[5].getFrameToTip().M.GetRPY(roll, pitch, yaw); - robot.delta_theta_correction3_ = yaw; - } - - return robot; + return output; } -KDL::Chain Calibration::getChain() +void Calibration::buildChain() { - KDL::Chain robot_chain; + chain_.clear(); for (size_t i = 0; i < robot_parameters_.segments_.size(); ++i) { - KDL::Frame frame = KDL::Frame::DH(0, 0, robot_parameters_.segments_[i].d_, robot_parameters_.segments_[i].theta_); - KDL::Joint joint = KDL::Joint(link_names_[i], KDL::Vector(0, 0, robot_parameters_.segments_[i].d_), - KDL::Vector(0, 0, 1), KDL::Joint::RotAxis); + Eigen::Matrix4d seg1_mat = Eigen::Matrix4d::Identity(); + seg1_mat.topLeftCorner(3, 3) = + Eigen::AngleAxisd(robot_parameters_.segments_[i].theta_, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + seg1_mat(2, 3) = robot_parameters_.segments_[i].d_; - KDL::Segment segment = KDL::Segment(link_names_[i], joint, frame); - robot_chain.addSegment(segment); - 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); + chain_.push_back(seg1_mat); - robot_chain.addSegment(segment2); + Eigen::Matrix4d seg2_mat = Eigen::Matrix4d::Identity(); + seg2_mat.topLeftCorner(3, 3) = + Eigen::AngleAxisd(robot_parameters_.segments_[i].alpha_, Eigen::Vector3d::UnitX()).toRotationMatrix(); + seg2_mat(0, 3) = robot_parameters_.segments_[i].a_; + + chain_.push_back(seg2_mat); } - return robot_chain; -} - -void Calibration::debugChain(const KDL::Chain& robot_chain) -{ - uint32_t num_jts = robot_chain.getNrOfJoints(); - - KDL::ChainFkSolverPos_recursive fk_solver(robot_chain); - KDL::JntArray jointpositions = KDL::JntArray(num_jts); - // Assign some values to the joint positions - for (unsigned int i = 0; i < num_jts; i++) - { - jointpositions(i) = 0; - } - - for (size_t i = 0; i < robot_chain.segments.size(); ++i) - { - ROS_INFO_STREAM("Segment " << i << ": " << robot_chain.segments[i].getName()); - KDL::Frame result; - result = robot_chain.segments[i].getFrameToTip(); - ROS_INFO_STREAM("Relative position: " << result.p.x() << ", " << result.p.y() << ", " << result.p.z()); - double roll, pitch, yaw; - 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()); - 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() << ", " - << joint.pose(0).p.z()); - ROS_INFO_STREAM("Joint Axis: " << joint.JointAxis().x() << "," << joint.JointAxis().y() << ", " - << joint.JointAxis().z()); - } - - DHRobot robot_out = Calibration::chainToDH(robot_chain); - ROS_INFO_STREAM("Robot data:\n" << robot_out.toXacroProperties()); } diff --git a/src/calibration_correction.cpp b/src/calibration_correction.cpp index 6935361..a4dc374 100644 --- a/src/calibration_correction.cpp +++ b/src/calibration_correction.cpp @@ -17,16 +17,18 @@ #include #include -std::unique_ptr fk_solver; sensor_msgs::JointStatePtr joint_state; +std::unique_ptr my_calibration; std::unique_ptr tf_listener; void jointStateCallback(sensor_msgs::JointStatePtr msg) { + ROS_INFO("Callback"); tf::StampedTransform stamped_transform; try { - tf_listener->lookupTransform("base", "tool0_controller", ros::Time(0), stamped_transform); + tf_listener->waitForTransform("base", "tool0_controller", msg->header.stamp, ros::Duration(0.5)); + tf_listener->lookupTransform("base", "tool0_controller", msg->header.stamp, stamped_transform); } catch (tf::TransformException ex) { @@ -37,23 +39,16 @@ void jointStateCallback(sensor_msgs::JointStatePtr msg) geometry_msgs::TransformStamped tf_msg; tf::transformStampedTFToMsg(stamped_transform, tf_msg); - // ROS_INFO_STREAM(tf_msg); + Eigen::Matrix jointvalues = Eigen::Matrix(msg->position.data()); - KDL::Frame result_original; - KDL::JntArray jointpositions = KDL::JntArray(6); - for (size_t i = 0; i < 6; ++i) - { - jointpositions.data[i] = msg->position[i]; - } - fk_solver->JntToCart(jointpositions, result_original); + Eigen::Matrix4d mat = my_calibration->calcForwardKinematics(jointvalues); - Eigen::Matrix4d mat; - result_original.Make4x4(mat.data()); Eigen::Vector3d error; - error << mat.transpose().topRightCorner(3, 1) - Eigen::Vector3d(stamped_transform.getOrigin()); + error << mat.topRightCorner(3, 1) - Eigen::Vector3d(stamped_transform.getOrigin()); - ROS_INFO_STREAM(error); + ROS_INFO_STREAM("Error(abs): " << error.norm()); + ROS_INFO_STREAM("Error per axis:\n" << error); } int main(int argc, char* argv[]) @@ -65,41 +60,41 @@ int main(int argc, char* argv[]) // This is an ideal UR10 // clang-format off // d, a, theta, alpha - my_robot.segments_.push_back(DHSegment(0.1273 , 0 , 0 , M_PI / 2)); + 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.163941, 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.163941, 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)); // 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.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); - Calibration calibration(my_robot); - KDL::Chain corrected_chain = calibration.getChain(); - // calibration.debugChain(corrected_chain); + my_calibration.reset(new Calibration(my_robot)); - uint32_t num_jts = corrected_chain.getNrOfJoints(); + my_calibration->correctChain(); - fk_solver.reset(new KDL::ChainFkSolverPos_recursive(corrected_chain)); - KDL::JntArray jointpositions = KDL::JntArray(num_jts); - KDL::Frame result_original; + Eigen::Matrix jointvalues; + jointvalues << 0, 0, 0, 0, 0, 0; + Eigen::Matrix4d fk = my_calibration->calcForwardKinematics(jointvalues); + ROS_INFO_STREAM("Zero transform:\n" << fk); tf_listener.reset(new tf::TransformListener); - ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback); + ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback); ros::spin(); diff --git a/test/calibration_test.cpp b/test/calibration_test.cpp index 395903c..9aab691 100644 --- a/test/calibration_test.cpp +++ b/test/calibration_test.cpp @@ -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 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 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 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 From c96338c9e7d0ad39eac5e5f1094aa5d6d804b796 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 28 Mar 2019 10:31:08 +0100 Subject: [PATCH 06/29] create xacro properties again --- include/ur_rtde_driver/calibration.h | 3 ++ src/calibration.cpp | 13 ++++++++ src/calibration_correction.cpp | 50 ++++++++++++++-------------- 3 files changed, 41 insertions(+), 25 deletions(-) diff --git a/include/ur_rtde_driver/calibration.h b/include/ur_rtde_driver/calibration.h index 6bf7d84..31504bf 100644 --- a/include/ur_rtde_driver/calibration.h +++ b/include/ur_rtde_driver/calibration.h @@ -144,6 +144,8 @@ public: return chain_; } + std::string toXacroProperties() {return robot_parameters_corrected_.toXacroProperties();} + Eigen::Matrix4d calcForwardKinematics(const Eigen::Matrix& joint_values, const size_t link_nr = 6); private: @@ -152,6 +154,7 @@ private: void buildChain(); DHRobot robot_parameters_; + DHRobot robot_parameters_corrected_; std::vector link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" }; std::vector chain_; diff --git a/src/calibration.cpp b/src/calibration.cpp index 6bc704a..d1e7dc9 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -25,6 +25,7 @@ Calibration::~Calibration() void Calibration::correctChain() { + robot_parameters_corrected_ = robot_parameters_; correctAxis(1); correctAxis(2); } @@ -104,8 +105,10 @@ void Calibration::correctAxis(const size_t link_index) // Correct arm segment length and angle // ROS_INFO_STREAM("Passive old:\n" << chain_[2 * link_index]); chain_[2 * link_index](2, 3) = 0.0; + robot_parameters_corrected_.segments_[link_index].d_ = 0.0; chain_[2 * link_index].topLeftCorner(3, 3) = Eigen::AngleAxisd(new_theta, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + robot_parameters_corrected_.segments_[link_index].theta_ = new_theta; // ROS_INFO_STREAM("Passive new:\n" << chain_[2 * link_index]); // Correct arm segment length and angle @@ -113,10 +116,20 @@ void Calibration::correctAxis(const size_t link_index) // ROS_INFO_STREAM("Theta correction: " << robot_parameters_.segments_[link_index].theta_ - new_theta); // ROS_INFO_STREAM("Alpha correction: " << robot_parameters_.segments_[link_index].alpha_); chain_[2 * link_index + 1](0, 3) = new_length; + robot_parameters_corrected_.segments_[link_index].a_ = new_length; chain_[2 * link_index + 1].topLeftCorner(3, 3) = Eigen::AngleAxisd(robot_parameters_.segments_[link_index].theta_ - new_theta, Eigen::Vector3d::UnitZ()) .toRotationMatrix() * Eigen::AngleAxisd(robot_parameters_.segments_[link_index].alpha_, Eigen::Vector3d::UnitX()).toRotationMatrix(); + if (link_index == 1) + { + robot_parameters_corrected_.delta_theta_correction2_ = robot_parameters_.segments_[link_index].theta_ - new_theta; + } + else if (link_index == 2) + { + robot_parameters_corrected_.delta_theta_correction3_ = robot_parameters_.segments_[link_index].theta_ - new_theta; + } + // ROS_INFO_STREAM("Next new:\n" << chain_[2 * link_index + 1]); // Correct next joint diff --git a/src/calibration_correction.cpp b/src/calibration_correction.cpp index a4dc374..cee20d5 100644 --- a/src/calibration_correction.cpp +++ b/src/calibration_correction.cpp @@ -43,7 +43,6 @@ void jointStateCallback(sensor_msgs::JointStatePtr msg) Eigen::Matrix4d mat = my_calibration->calcForwardKinematics(jointvalues); - Eigen::Vector3d error; error << mat.topRightCorner(3, 1) - Eigen::Vector3d(stamped_transform.getOrigin()); @@ -61,40 +60,41 @@ int main(int argc, char* argv[]) // clang-format off // d, a, theta, alpha 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.0 , -0.612 , 0.0 , 0.0)); + my_robot.segments_.push_back(DHSegment(0.0 , -0.5723, 0 , 0.0)); my_robot.segments_.push_back(DHSegment(0.163941, 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)); // 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); - my_calibration.reset(new Calibration(my_robot)); + //DHRobot my_robot_calibration; + // clang-format off + // d, a, theta, alpha + // ids-ur10-2 + //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 )); + // + // ids-ur10-3 + //my_robot_calibration.segments_.push_back(DHSegment(0.000489427369583322891 , -0.000157659251888300598 , 7.12144160891142552e-06 , -0.000181484221894123721)); + //my_robot_calibration.segments_.push_back(DHSegment(2.15820897072427709 , 0.000797598154419598693 , -0.0597679909195674153 , -0.0169424433153391799 )); + //my_robot_calibration.segments_.push_back(DHSegment(-0.200070560336579328 , 0.00141079051860837357 , 0.0276740817239430857 , -0.00936706021457411366 )); + //my_robot_calibration.segments_.push_back(DHSegment(-1.95850825255269623 , 8.02528233902343753e-05 , 0.0321549453426039911 , 0.000705308984090935454 )); + //my_robot_calibration.segments_.push_back(DHSegment(-2.27384815544295904e-05, 2.49305433182637798e-06 , -7.91131358362739777e-05, -0.000129247903554619015)); + //my_robot_calibration.segments_.push_back(DHSegment(1.40058148229704749e-05 , 0 , 2.43093497590859384e-05 , 0 )); + // clang-format on + //my_calibration.reset(new Calibration(my_robot + my_robot_calibration)); + my_calibration.reset(new Calibration(my_robot)); my_calibration->correctChain(); - Eigen::Matrix jointvalues; - jointvalues << 0, 0, 0, 0, 0, 0; - Eigen::Matrix4d fk = my_calibration->calcForwardKinematics(jointvalues); - ROS_INFO_STREAM("Zero transform:\n" << fk); + ROS_INFO_STREAM("Corrected calibration: " << std::endl << my_calibration->toXacroProperties()); tf_listener.reset(new tf::TransformListener); - ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback); + ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback); ros::spin(); From f373eca92bb650acc813e58f0726f4210e9e642f Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 28 Mar 2019 16:02:18 +0100 Subject: [PATCH 07/29] adding changed d parameter of next element to output --- src/calibration.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/calibration.cpp b/src/calibration.cpp index d1e7dc9..24844b0 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -135,6 +135,7 @@ void Calibration::correctAxis(const size_t link_index) // Correct next joint // ROS_INFO_STREAM("Second Next old:\n" << chain_[2 * link_index + 2]); chain_[2 * link_index + 2](2, 3) -= distance_correction; + robot_parameters_corrected_.segments_[link_index+1].d_ -= distance_correction; // ROS_INFO_STREAM("Second Next new:\n" << chain_[2 * link_index + 2]); } From 281d4c39551ecbf2560f285cec31e5739cc464be Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 28 Mar 2019 16:02:54 +0100 Subject: [PATCH 08/29] test 1000 random joint states in fk test --- test/calibration_test.cpp | 57 ++++++++++++++++++++++++--------------- 1 file changed, 36 insertions(+), 21 deletions(-) diff --git a/test/calibration_test.cpp b/test/calibration_test.cpp index 9aab691..d30c266 100644 --- a/test/calibration_test.cpp +++ b/test/calibration_test.cpp @@ -63,6 +63,11 @@ TEST(UrRtdeDriver, ur10_ideal) 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; const double pi = std::atan(1) * 4; @@ -79,34 +84,44 @@ TEST(UrRtdeDriver, calibration) DHRobot my_robot_calibration; // clang-format off // 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)); + //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)); + 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); + Eigen::Matrix jointvalues; + jointvalues << 0, 0, 0, 0, 0, 0; // First let's see, whether our calibration input does make sense. - { - Eigen::Matrix 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); - } + //{ + // 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); + //} - calibration.correctChain(); + for (size_t i = 0; i < 1000; ++i) { - Eigen::Matrix 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); + Calibration calibration(my_robot + my_robot_calibration); + jointvalues = 2 * pi * Eigen::Matrix::Random(); + // TODO: Remove this output + std::cout << "Testing with jointvalues: [" << jointvalues.transpose() << "]" << std::endl; + Eigen::Matrix4d fk_orig = calibration.calcForwardKinematics(jointvalues); + 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 From 579a39a7df00b22d451f126b6d236369472df0c3 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 28 Mar 2019 16:03:47 +0100 Subject: [PATCH 09/29] check multiple calibrations in one run --- src/calibration_correction.cpp | 92 +++++++++++++++++++++++++--------- 1 file changed, 67 insertions(+), 25 deletions(-) diff --git a/src/calibration_correction.cpp b/src/calibration_correction.cpp index cee20d5..624547e 100644 --- a/src/calibration_correction.cpp +++ b/src/calibration_correction.cpp @@ -66,37 +66,79 @@ int main(int argc, char* argv[]) my_robot.segments_.push_back(DHSegment(0.1157 , 0 , 0 , -M_PI_2)); my_robot.segments_.push_back(DHSegment(0.0922 , 0 , 0 , 0)); // clang-format on - //DHRobot my_robot_calibration; - // clang-format off + std::map calibrations; // d, a, theta, alpha - // ids-ur10-2 - //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 )); - // - // ids-ur10-3 - //my_robot_calibration.segments_.push_back(DHSegment(0.000489427369583322891 , -0.000157659251888300598 , 7.12144160891142552e-06 , -0.000181484221894123721)); - //my_robot_calibration.segments_.push_back(DHSegment(2.15820897072427709 , 0.000797598154419598693 , -0.0597679909195674153 , -0.0169424433153391799 )); - //my_robot_calibration.segments_.push_back(DHSegment(-0.200070560336579328 , 0.00141079051860837357 , 0.0276740817239430857 , -0.00936706021457411366 )); - //my_robot_calibration.segments_.push_back(DHSegment(-1.95850825255269623 , 8.02528233902343753e-05 , 0.0321549453426039911 , 0.000705308984090935454 )); - //my_robot_calibration.segments_.push_back(DHSegment(-2.27384815544295904e-05, 2.49305433182637798e-06 , -7.91131358362739777e-05, -0.000129247903554619015)); - //my_robot_calibration.segments_.push_back(DHSegment(1.40058148229704749e-05 , 0 , 2.43093497590859384e-05 , 0 )); - // clang-format on - //my_calibration.reset(new Calibration(my_robot + my_robot_calibration)); - my_calibration.reset(new Calibration(my_robot)); + { + DHRobot robot_calibration; + // clang-format off + robot_calibration.segments_.push_back(DHSegment( 0.00065609212979853 , 4.6311376834935676e-05 , -7.290070070824746e-05 , 0.000211987863869334 )); + robot_calibration.segments_.push_back(DHSegment( 1.4442162376284788 , -0.00012568315331862312 , -0.01713897289704999 , -0.0072553625957652995)); + robot_calibration.segments_.push_back(DHSegment( 0.854147723854608 , 0.00186216581161458 , -0.03707159413492756 , -0.013483226769541364 )); + robot_calibration.segments_.push_back(DHSegment(-2.2989425877563705 , 9.918593870679266e-05 , 0.054279462160583214 , 0.0013495820227329425 )); + robot_calibration.segments_.push_back(DHSegment(-1.573498686836816e-05 , 4.215462720453189e-06 , 1.488984257025741e-07 , -0.001263136163679901 )); + robot_calibration.segments_.push_back(DHSegment( 1.9072435590711256e-05 , 0 , 1.551499479707493e-05 , 0 )); + // clang-format on + calibrations.insert(std::make_pair("ids-ur10-2", robot_calibration)); + } + { + DHRobot robot_calibration; + // clang-format off + robot_calibration.segments_.push_back(DHSegment(0.000489427369583322891 , -0.000157659251888300598 , 7.12144160891142552e-06 , -0.000181484221894123721)); + robot_calibration.segments_.push_back(DHSegment(2.15820897072427709 , 0.000797598154419598693 , -0.0597679909195674153 , -0.0169424433153391799 )); + robot_calibration.segments_.push_back(DHSegment(-0.200070560336579328 , 0.00141079051860837357 , 0.0276740817239430857 , -0.00936706021457411366 )); + robot_calibration.segments_.push_back(DHSegment(-1.95850825255269623 , 8.02528233902343753e-05 , 0.0321549453426039911 , 0.000705308984090935454 )); + robot_calibration.segments_.push_back(DHSegment(-2.27384815544295904e-05, 2.49305433182637798e-06 , -7.91131358362739777e-05, -0.000129247903554619015)); + robot_calibration.segments_.push_back(DHSegment(1.40058148229704749e-05 , 0 , 2.43093497590859384e-05 , 0 )); + // clang-format on + calibrations.insert(std::make_pair("ids-ur10-3", robot_calibration)); + } + // my_calibration.reset(new Calibration(my_robot)); - my_calibration->correctChain(); + for (auto& calib : calibrations) + { + Eigen::Matrix jointvalues; - ROS_INFO_STREAM("Corrected calibration: " << std::endl << my_calibration->toXacroProperties()); + double min_error = 9999; + double max_error = 0; + double mean_error = 0; + const size_t num_runs = 100000; + for (size_t i = 0; i < num_runs; ++i) + { + Calibration calibration(my_robot + calib.second); + jointvalues = 2 * M_PI * Eigen::Matrix::Random(); + Eigen::Matrix4d fk_orig = calibration.calcForwardKinematics(jointvalues); + calibration.correctChain(); + Eigen::Matrix4d fk_corrected = calibration.calcForwardKinematics(jointvalues); - tf_listener.reset(new tf::TransformListener); + Eigen::Vector3d error; + error << fk_orig.topRightCorner(3, 1) - fk_corrected.topRightCorner(3, 1); - ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback); + if (error.norm() < min_error) + { + min_error = error.norm(); + } + if (error.norm() > max_error) + { + max_error = error.norm(); + } + mean_error += error.norm() / static_cast(num_runs); + } - ros::spin(); + ROS_INFO_STREAM(calib.first << ": Error over " << num_runs << " joint angles (mean, min, max): (" << mean_error + << ", " << min_error << ", " << max_error << ")"); + // Calibration calibration(my_robot + calib.second); + // calibration.correctChain(); + // ROS_INFO_STREAM("Corrected calibration: " << std::endl << calibration.toXacroProperties()); + } + // my_calibration->correctChain(); + + // ROS_INFO_STREAM("Corrected calibration: " << std::endl << my_calibration->toXacroProperties()); + + // tf_listener.reset(new tf::TransformListener); + + // ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback); + + // ros::spin(); return 0; } From 8d3b787b27258f58af9603e3ff1fad115a80ef0f Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Fri, 29 Mar 2019 12:57:14 +0100 Subject: [PATCH 10/29] also check for rotation errors --- src/calibration_correction.cpp | 44 ++++++++++++++++++++++++---------- 1 file changed, 32 insertions(+), 12 deletions(-) diff --git a/src/calibration_correction.cpp b/src/calibration_correction.cpp index 624547e..9fc588c 100644 --- a/src/calibration_correction.cpp +++ b/src/calibration_correction.cpp @@ -98,9 +98,12 @@ int main(int argc, char* argv[]) { Eigen::Matrix jointvalues; - double min_error = 9999; - double max_error = 0; - double mean_error = 0; + double pos_min_error = 9999; + double pos_max_error = 0; + double pos_mean_error = 0; + double angle_min_error = 9999; + double angle_max_error = 0; + double angle_mean_error = 0; const size_t num_runs = 100000; for (size_t i = 0; i < num_runs; ++i) { @@ -110,22 +113,39 @@ int main(int argc, char* argv[]) calibration.correctChain(); Eigen::Matrix4d fk_corrected = calibration.calcForwardKinematics(jointvalues); - Eigen::Vector3d error; - error << fk_orig.topRightCorner(3, 1) - fk_corrected.topRightCorner(3, 1); + Eigen::Vector3d pos_error; + pos_error << fk_orig.topRightCorner(3, 1) - fk_corrected.topRightCorner(3, 1); - if (error.norm() < min_error) + if (pos_error.norm() < pos_min_error) { - min_error = error.norm(); + pos_min_error = pos_error.norm(); } - if (error.norm() > max_error) + if (pos_error.norm() > pos_max_error) { - max_error = error.norm(); + pos_max_error = pos_error.norm(); } - mean_error += error.norm() / static_cast(num_runs); + pos_mean_error += pos_error.norm() / static_cast(num_runs); + + Eigen::Matrix3d rot_mat_orig = fk_orig.topLeftCorner(3, 3); + Eigen::Matrix3d rot_mat_corrected = fk_corrected.topLeftCorner(3, 3); + Eigen::Quaterniond rot_orig(rot_mat_orig); + Eigen::Quaterniond rot_corrected(rot_mat_corrected); + double angle_error = std::abs(rot_orig.angularDistance(rot_corrected)); + if (angle_error < angle_min_error) + { + angle_min_error = angle_error; + } + if (angle_error > angle_max_error) + { + angle_max_error = angle_error; + } + angle_mean_error += angle_error / static_cast(num_runs); } - ROS_INFO_STREAM(calib.first << ": Error over " << num_runs << " joint angles (mean, min, max): (" << mean_error - << ", " << min_error << ", " << max_error << ")"); + ROS_INFO_STREAM(calib.first << ": Position Error over " << num_runs << " joint angles (mean, min, max): (" + << pos_mean_error << ", " << pos_min_error << ", " << pos_max_error << ")"); + ROS_INFO_STREAM(calib.first << ": Angle Error over " << num_runs << " joint angles (mean, min, max): (" + << angle_mean_error << ", " << angle_min_error << ", " << angle_max_error << ")"); // Calibration calibration(my_robot + calib.second); // calibration.correctChain(); // ROS_INFO_STREAM("Corrected calibration: " << std::endl << calibration.toXacroProperties()); From 3d93dd422670e35e2370285d494dca35872f95cd Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 1 Apr 2019 10:07:18 +0200 Subject: [PATCH 11/29] manually set corrections to 0 --- include/ur_rtde_driver/calibration.h | 2 ++ src/calibration.cpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/include/ur_rtde_driver/calibration.h b/include/ur_rtde_driver/calibration.h index 31504bf..9f228f6 100644 --- a/include/ur_rtde_driver/calibration.h +++ b/include/ur_rtde_driver/calibration.h @@ -77,6 +77,8 @@ struct DHRobot */ DHRobot(const std::vector& segments) { + delta_theta_correction2_ = 0; + delta_theta_correction3_ = 0; } DHRobot() = default; diff --git a/src/calibration.cpp b/src/calibration.cpp index 24844b0..d6d1735 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -26,6 +26,8 @@ Calibration::~Calibration() void Calibration::correctChain() { robot_parameters_corrected_ = robot_parameters_; + robot_parameters_corrected_.delta_theta_correction2_ = 0.0; + robot_parameters_corrected_.delta_theta_correction3_ = 0.0; correctAxis(1); correctAxis(2); } From bab303e83c33e9e1f7c9119e919da5e77e20dbcf Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 1 Apr 2019 10:08:29 +0200 Subject: [PATCH 12/29] Cleaned up tests --- test/calibration_test.cpp | 36 ++++++++++++------------------------ 1 file changed, 12 insertions(+), 24 deletions(-) diff --git a/test/calibration_test.cpp b/test/calibration_test.cpp index d30c266..2bdaf51 100644 --- a/test/calibration_test.cpp +++ b/test/calibration_test.cpp @@ -84,44 +84,33 @@ TEST(UrRtdeDriver, calibration) DHRobot my_robot_calibration; // clang-format off // 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)); - 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 )); + 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); - Eigen::Matrix jointvalues; jointvalues << 0, 0, 0, 0, 0, 0; - // First let's see, whether our calibration input does make sense. - //{ - // 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); - //} for (size_t i = 0; i < 1000; ++i) { Calibration calibration(my_robot + my_robot_calibration); jointvalues = 2 * pi * Eigen::Matrix::Random(); - // TODO: Remove this output - std::cout << "Testing with jointvalues: [" << jointvalues.transpose() << "]" << std::endl; Eigen::Matrix4d fk_orig = calibration.calcForwardKinematics(jointvalues); + Eigen::Matrix3d rot_mat_orig = fk_orig.topLeftCorner(3, 3); + Eigen::Quaterniond rot_orig(rot_mat_orig); calibration.correctChain(); Eigen::Matrix4d fk_corrected = calibration.calcForwardKinematics(jointvalues); + Eigen::Matrix3d rot_mat_corrected = fk_corrected.topLeftCorner(3, 3); + Eigen::Quaterniond rot_corrected(rot_mat_corrected); + double angle_error = std::abs(rot_orig.angularDistance(rot_corrected)); 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); + EXPECT_PRED3(isApproximately, angle_error, 0.0, 1e-12); } } } // namespace @@ -132,4 +121,3 @@ int main(int argc, char* argv[]) return RUN_ALL_TESTS(); } - From 4113f3ab2a5d142b9348704086d5bfb4353626ce Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 6 May 2019 08:47:10 +0200 Subject: [PATCH 13/29] Result in 6-jointed chain --- CMakeLists.txt | 1 - include/ur_rtde_driver/calibration.h | 5 +++ src/calibration.cpp | 51 +++++++++++++++++++++- test/calibration_test.cpp | 64 +++++++++++++++++++++------- 4 files changed, 103 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ebd26ab..7d75787 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -127,7 +127,6 @@ add_executable(calibration_correction ) add_dependencies(calibration_correction ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - ## Specify libraries to link a library or executable target against target_link_libraries(calibration_correction ${catkin_LIBRARIES} diff --git a/include/ur_rtde_driver/calibration.h b/include/ur_rtde_driver/calibration.h index 9f228f6..55e00d5 100644 --- a/include/ur_rtde_driver/calibration.h +++ b/include/ur_rtde_driver/calibration.h @@ -17,6 +17,8 @@ #include #include +#include +#include /*! * \brief An internal representation of a DH-parametrized link. @@ -147,6 +149,9 @@ public: } std::string toXacroProperties() {return robot_parameters_corrected_.toXacroProperties();} + void writeToYaml(std::ofstream& ofstream) const; + + std::vector getSimplified() const; Eigen::Matrix4d calcForwardKinematics(const Eigen::Matrix& joint_values, const size_t link_nr = 6); diff --git a/src/calibration.cpp b/src/calibration.cpp index d6d1735..1d44819 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -147,11 +147,13 @@ Eigen::Matrix4d Calibration::calcForwardKinematics(const Eigen::Matrix simplified_chain = getSimplified(); for (size_t i = 0; i < link_nr; ++i) { Eigen::Matrix4d rotation = Eigen::Matrix4d::Identity(); rotation.topLeftCorner(3, 3) = Eigen::AngleAxisd(joint_values(i), Eigen::Vector3d::UnitZ()).toRotationMatrix(); - output *= chain_[2 * i] * rotation * chain_[2 * i + 1]; + output *= simplified_chain[i] * rotation; } // ROS_INFO_STREAM("forward_kinematics at " << link_nr << std::endl << output); @@ -179,3 +181,50 @@ void Calibration::buildChain() chain_.push_back(seg2_mat); } } + +std::vector Calibration::getSimplified() const +{ + std::vector simplified_chain; + simplified_chain.push_back(chain_[0]); + for (size_t i = 1; i < chain_.size() - 1; i += 2) + { + simplified_chain.push_back(chain_[i] * chain_[i + 1]); + Eigen::Matrix3d rot_a = chain_[i].topLeftCorner(3, 3); + Eigen::Vector3d rpy_a = rot_a.eulerAngles(0, 1, 2); + // ROS_INFO_STREAM("Rotation " << i / 2 << " a: [" << rpy_a.transpose() << "]"); + Eigen::Matrix3d rot_b = chain_[i + 1].topLeftCorner(3, 3); + Eigen::Vector3d rpy_b = rot_b.eulerAngles(0, 1, 2); + // ROS_INFO_STREAM("Rotation " << i / 2 << " b: [" << rpy_b.transpose() << "]"); + // ROS_INFO_STREAM("Matrix " << i / 2 << ":\n" << simplified_chain.back()); + Eigen::Matrix3d rot = simplified_chain.back().topLeftCorner(3, 3); + Eigen::Vector3d rpy = rot.eulerAngles(0, 1, 2); + Eigen::Quaterniond quat(rot); + // ROS_INFO_STREAM("Rotation (rpy) " << i / 2 << ": [" << rpy.transpose() << "]"); + // ROS_INFO_STREAM("Rotation (quat, [xyz], w)" << i / 2 << ": [" << quat.vec().transpose() << "], " << quat.w()); + } + simplified_chain.push_back(chain_.back()); + return simplified_chain; +} + +void Calibration::writeToYaml(std::ofstream& ofstream) const +{ + YAML::Node node; + + std::vector chain = getSimplified(); + + for (std::size_t i = 0; i < link_names_.size(); ++i) + { + YAML::Node link; + link["x"] = chain[i](0, 3); + link["y"] = chain[i](1, 3); + link["z"] = chain[i](2, 3); + Eigen::Matrix3d rot = chain[i].topLeftCorner(3, 3); + Eigen::Vector3d rpy = rot.eulerAngles(0, 1, 2); + link["roll"] = rpy[0]; + link["pitch"] = rpy[1]; + link["yaw"] = rpy[2]; + node["kinematics"][link_names_[i]] = link; + } + + ofstream << node; +} diff --git a/test/calibration_test.cpp b/test/calibration_test.cpp index 2bdaf51..130a1c7 100644 --- a/test/calibration_test.cpp +++ b/test/calibration_test.cpp @@ -22,10 +22,22 @@ bool isApproximately(const double val1, const double val2, const double precisio return std::abs(val1 - val2) < precision; } -TEST(UrRtdeDriver, ur10_ideal) +template +void doubleEqVec(const Eigen::Matrix vec1, const Eigen::Matrix vec2, + const double precision) +{ + for (size_t i = 0; i < dim_; ++i) + { + EXPECT_NEAR(vec1[i], vec2[i], precision); + } +} + +TEST(UrRtdeDriver, ur10_fw_kinematics) { DHRobot my_robot; const double pi = std::atan(1) * 4; + // const double pi = 1.570796327 * 2.0; + // const double pi = M_PI; // This is an ideal UR10 // clang-format off @@ -40,6 +52,8 @@ TEST(UrRtdeDriver, ur10_ideal) Calibration calibration(my_robot); Eigen::Matrix jointvalues; + Eigen::Vector3d expected_position; + Eigen::Quaterniond expected_orientation; { jointvalues << 0, 0, 0, 0, 0, 0; Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); @@ -51,22 +65,40 @@ TEST(UrRtdeDriver, ur10_ideal) { 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); + + 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(expected_position, fk.topRightCorner(3, 1), 1e-16); + } + { + jointvalues << -1.6007002035724084976209269370884, -1.7271001974688928726209269370884, + -2.2029998938189905288709269370884, -0.80799991289247685699592693708837, 1.59510004520416259765625, + -0.03099996248354131012092693708837; + expected_position << -0.179925914147547, -0.606869755162764, 0.230789102067257; + + Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); + + doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-10); + } + { + jointvalues << 1.32645022869110107421875, 2.426007747650146484375, 5.951572895050048828125, + 1.27409040927886962890625, -0.54105216661562138824592693708837, 0.122173048555850982666015625; + expected_position << 0.39922988003280424074148413637886, 0.59688365069340565405298093537567, + -0.6677819040276375961440180617501; + + Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); + + doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-9); } } 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). - */ + // 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; const double pi = std::atan(1) * 4; @@ -107,10 +139,10 @@ TEST(UrRtdeDriver, calibration) Eigen::Matrix3d rot_mat_corrected = fk_corrected.topLeftCorner(3, 3); Eigen::Quaterniond rot_corrected(rot_mat_corrected); double angle_error = std::abs(rot_orig.angularDistance(rot_corrected)); - 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); - EXPECT_PRED3(isApproximately, angle_error, 0.0, 1e-12); + EXPECT_NEAR(fk_orig(0, 3), fk_corrected(0, 3), 1e-12); + EXPECT_NEAR(fk_orig(1, 3), fk_corrected(1, 3), 1e-12); + EXPECT_NEAR(fk_orig(2, 3), fk_corrected(2, 3), 1e-12); + EXPECT_NEAR(angle_error, 0.0, 1e-12); } } } // namespace From f00f76d5085ae038d17c2c46823cf200827277f0 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 6 May 2019 15:11:02 +0200 Subject: [PATCH 14/29] code formatting --- src/calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/calibration.cpp b/src/calibration.cpp index 1d44819..818b10e 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -137,7 +137,7 @@ void Calibration::correctAxis(const size_t link_index) // Correct next joint // ROS_INFO_STREAM("Second Next old:\n" << chain_[2 * link_index + 2]); chain_[2 * link_index + 2](2, 3) -= distance_correction; - robot_parameters_corrected_.segments_[link_index+1].d_ -= distance_correction; + robot_parameters_corrected_.segments_[link_index + 1].d_ -= distance_correction; // ROS_INFO_STREAM("Second Next new:\n" << chain_[2 * link_index + 2]); } From ceb00d8d6d7dd6650345e235cbfbd46ac152a9e3 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 6 May 2019 15:11:38 +0200 Subject: [PATCH 15/29] output simplified chain --- src/calibration_correction.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/calibration_correction.cpp b/src/calibration_correction.cpp index 9fc588c..dee695e 100644 --- a/src/calibration_correction.cpp +++ b/src/calibration_correction.cpp @@ -94,6 +94,7 @@ int main(int argc, char* argv[]) } // my_calibration.reset(new Calibration(my_robot)); + /* for (auto& calib : calibrations) { Eigen::Matrix jointvalues; @@ -150,9 +151,25 @@ int main(int argc, char* argv[]) // calibration.correctChain(); // ROS_INFO_STREAM("Corrected calibration: " << std::endl << calibration.toXacroProperties()); } - // my_calibration->correctChain(); + */ + my_calibration->correctChain(); + std::vector corrected_chain = my_calibration->getSimplified(); + + for (auto& it : corrected_chain) + { + Eigen::Matrix3d rot_a = it.topLeftCorner(3, 3); + Eigen::Vector3d rpy_a = rot_a.eulerAngles(0, 1, 2); + ROS_INFO_STREAM("Translation: [" << it.topRightCorner(3, 1).transpose() << "]"); + ROS_INFO_STREAM("Rotation: [" << rpy_a.transpose() << "]"); + } + + std::ofstream file; + file.open("test.yaml"); + my_calibration->writeToYaml(file); + file.close(); // ROS_INFO_STREAM("Corrected calibration: " << std::endl << my_calibration->toXacroProperties()); + std::cout << my_calibration->toXacroProperties() << std::endl; // tf_listener.reset(new tf::TransformListener); From 312fe8b1b7931fdcd156b39d0922528059e42e20 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 27 May 2019 16:30:21 +0200 Subject: [PATCH 16/29] moved calibration to own repository --- ur_calibration/CMakeLists.txt | 186 ++++++++++++++++++ .../include/ur_calibration}/calibration.h | 5 +- ur_calibration/package.xml | 57 ++++++ {src => ur_calibration/src}/calibration.cpp | 2 +- .../src}/calibration_correction.cpp | 2 +- .../test}/calibration_test.cpp | 2 +- .../CMakeLists.txt | 39 ---- LICENSE => ur_rtde_driver/LICENSE | 0 README.md => ur_rtde_driver/README.md | 0 .../config}/ur10_controllers.yaml | 0 .../config}/ur3_controllers.yaml | 0 .../config}/ur5_controllers.yaml | 0 .../include}/ur_rtde_driver/comm/bin_parser.h | 0 .../include}/ur_rtde_driver/comm/package.h | 0 .../ur_rtde_driver/comm/package_serializer.h | 0 .../include}/ur_rtde_driver/comm/parser.h | 0 .../include}/ur_rtde_driver/comm/pipeline.h | 0 .../include}/ur_rtde_driver/comm/producer.h | 0 .../ur_rtde_driver/comm/reverse_interface.h | 0 .../include}/ur_rtde_driver/comm/server.h | 0 .../ur_rtde_driver/comm/shell_consumer.h | 0 .../include}/ur_rtde_driver/comm/stream.h | 0 .../include}/ur_rtde_driver/comm/tcp_socket.h | 0 .../include}/ur_rtde_driver/event_counter.h | 0 .../include}/ur_rtde_driver/log.h | 0 .../ur_rtde_driver/primary/package_header.h | 0 .../ur_rtde_driver/primary/primary_package.h | 0 .../ur_rtde_driver/primary/primary_parser.h | 0 .../ur_rtde_driver/primary/robot_message.h | 0 .../primary/robot_message/version_message.h | 0 .../ur_rtde_driver/primary/robot_state.h | 0 .../primary/robot_state/kinematics_info.h | 0 .../primary/robot_state/master_board.h | 0 .../primary/robot_state/robot_mode_data.h | 0 .../include}/ur_rtde_driver/queue/LICENSE.md | 0 .../include}/ur_rtde_driver/queue/atomicops.h | 0 .../ur_rtde_driver/queue/readerwriterqueue.h | 0 .../ur_rtde_driver/ros/hardware_interface.h | 0 .../include}/ur_rtde_driver/ros/io_service.h | 0 .../ur_rtde_driver/ros/service_stopper.h | 0 .../rtde/control_package_pause.h | 0 .../rtde/control_package_setup_inputs.h | 0 .../rtde/control_package_setup_outputs.h | 0 .../rtde/control_package_start.h | 0 .../ur_rtde_driver/rtde/data_package.h | 0 .../rtde/get_urcontrol_version.h | 0 .../ur_rtde_driver/rtde/package_header.h | 0 .../rtde/request_protocol_version.h | 0 .../ur_rtde_driver/rtde/rtde_client.h | 0 .../ur_rtde_driver/rtde/rtde_package.h | 0 .../ur_rtde_driver/rtde/rtde_parser.h | 0 .../ur_rtde_driver/rtde/text_message.h | 0 .../ur_rtde_driver/test/random_data.h | 0 .../include}/ur_rtde_driver/test/utils.h | 0 .../include}/ur_rtde_driver/types.h | 0 .../include}/ur_rtde_driver/ur/commander.h | 0 .../include}/ur_rtde_driver/ur/consumer.h | 0 .../include}/ur_rtde_driver/ur/factory.h | 0 .../include}/ur_rtde_driver/ur/messages.h | 0 .../ur_rtde_driver/ur/messages_parser.h | 0 .../include}/ur_rtde_driver/ur/robot_mode.h | 0 .../include}/ur_rtde_driver/ur/rt_parser.h | 0 .../include}/ur_rtde_driver/ur/rt_state.h | 0 .../include}/ur_rtde_driver/ur/state.h | 0 .../include}/ur_rtde_driver/ur/state_parser.h | 0 .../include}/ur_rtde_driver/ur/ur_driver.h | 0 .../launch}/ur10_bringup.launch | 0 .../launch}/ur10_bringup_compatible.launch | 0 .../launch}/ur10_bringup_joint_limited.launch | 0 .../launch}/ur10_ros_control.launch | 0 .../launch}/ur3_bringup.launch | 0 .../launch}/ur3_bringup_joint_limited.launch | 0 .../launch}/ur3_ros_control.launch | 0 .../launch}/ur5_bringup.launch | 0 .../launch}/ur5_bringup_compatible.launch | 0 .../launch}/ur5_bringup_joint_limited.launch | 0 .../launch}/ur5_ros_control.launch | 0 .../launch}/ur_common.launch | 0 package.xml => ur_rtde_driver/package.xml | 3 - {src => ur_rtde_driver/src}/comm/server.cpp | 0 .../src}/comm/tcp_socket.cpp | 0 .../src}/main_plain_driver.cpp | 0 .../src}/primary/primary_package.cpp | 0 .../src}/primary/robot_message.cpp | 0 .../primary/robot_message/version_message.cpp | 0 .../primary/robot_state/kinematics_info.cpp | 0 {src => ur_rtde_driver/src}/producer.cpp | 0 .../src}/ros/hardware_interface.cpp | 0 .../src}/ros/hardware_interface_node.cpp | 0 .../src}/ros/service_stopper.cpp | 0 .../src}/rtde/control_package_pause.cpp | 0 .../rtde/control_package_setup_inputs.cpp | 0 .../rtde/control_package_setup_outputs.cpp | 0 .../src}/rtde/control_package_start.cpp | 0 .../src}/rtde/data_package.cpp | 0 .../src}/rtde/get_urcontrol_version.cpp | 0 .../src}/rtde/request_protocol_version.cpp | 0 .../src}/rtde/rtde_client.cpp | 0 .../src}/rtde/rtde_package.cpp | 0 .../src}/rtde/text_message.cpp | 0 {src => ur_rtde_driver/src}/ur/commander.cpp | 0 .../src}/ur/master_board.cpp | 0 {src => ur_rtde_driver/src}/ur/messages.cpp | 0 {src => ur_rtde_driver/src}/ur/robot_mode.cpp | 0 {src => ur_rtde_driver/src}/ur/rt_state.cpp | 0 {src => ur_rtde_driver/src}/ur/ur_driver.cpp | 0 {tests => ur_rtde_driver/tests}/main.cpp | 0 .../tests}/ur/master_board.cpp | 0 .../tests}/ur/robot_mode.cpp | 0 .../tests}/ur/rt_state.cpp | 0 110 files changed, 250 insertions(+), 46 deletions(-) create mode 100644 ur_calibration/CMakeLists.txt rename {include/ur_rtde_driver => ur_calibration/include/ur_calibration}/calibration.h (98%) create mode 100644 ur_calibration/package.xml rename {src => ur_calibration/src}/calibration.cpp (99%) rename {src => ur_calibration/src}/calibration_correction.cpp (99%) rename {test => ur_calibration/test}/calibration_test.cpp (99%) rename CMakeLists.txt => ur_rtde_driver/CMakeLists.txt (73%) rename LICENSE => ur_rtde_driver/LICENSE (100%) rename README.md => ur_rtde_driver/README.md (100%) rename {config => ur_rtde_driver/config}/ur10_controllers.yaml (100%) rename {config => ur_rtde_driver/config}/ur3_controllers.yaml (100%) rename {config => ur_rtde_driver/config}/ur5_controllers.yaml (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/comm/bin_parser.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/comm/package.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/comm/package_serializer.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/comm/parser.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/comm/pipeline.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/comm/producer.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/comm/reverse_interface.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/comm/server.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/comm/shell_consumer.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/comm/stream.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/comm/tcp_socket.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/event_counter.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/log.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/primary/package_header.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/primary/primary_package.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/primary/primary_parser.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/primary/robot_message.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/primary/robot_message/version_message.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/primary/robot_state.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/primary/robot_state/kinematics_info.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/primary/robot_state/master_board.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/primary/robot_state/robot_mode_data.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/queue/LICENSE.md (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/queue/atomicops.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/queue/readerwriterqueue.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ros/hardware_interface.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ros/io_service.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ros/service_stopper.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/rtde/control_package_pause.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/rtde/control_package_setup_inputs.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/rtde/control_package_setup_outputs.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/rtde/control_package_start.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/rtde/data_package.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/rtde/get_urcontrol_version.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/rtde/package_header.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/rtde/request_protocol_version.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/rtde/rtde_client.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/rtde/rtde_package.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/rtde/rtde_parser.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/rtde/text_message.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/test/random_data.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/test/utils.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/types.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ur/commander.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ur/consumer.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ur/factory.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ur/messages.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ur/messages_parser.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ur/robot_mode.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ur/rt_parser.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ur/rt_state.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ur/state.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ur/state_parser.h (100%) rename {include => ur_rtde_driver/include}/ur_rtde_driver/ur/ur_driver.h (100%) rename {launch => ur_rtde_driver/launch}/ur10_bringup.launch (100%) rename {launch => ur_rtde_driver/launch}/ur10_bringup_compatible.launch (100%) rename {launch => ur_rtde_driver/launch}/ur10_bringup_joint_limited.launch (100%) rename {launch => ur_rtde_driver/launch}/ur10_ros_control.launch (100%) rename {launch => ur_rtde_driver/launch}/ur3_bringup.launch (100%) rename {launch => ur_rtde_driver/launch}/ur3_bringup_joint_limited.launch (100%) rename {launch => ur_rtde_driver/launch}/ur3_ros_control.launch (100%) rename {launch => ur_rtde_driver/launch}/ur5_bringup.launch (100%) rename {launch => ur_rtde_driver/launch}/ur5_bringup_compatible.launch (100%) rename {launch => ur_rtde_driver/launch}/ur5_bringup_joint_limited.launch (100%) rename {launch => ur_rtde_driver/launch}/ur5_ros_control.launch (100%) rename {launch => ur_rtde_driver/launch}/ur_common.launch (100%) rename package.xml => ur_rtde_driver/package.xml (94%) rename {src => ur_rtde_driver/src}/comm/server.cpp (100%) rename {src => ur_rtde_driver/src}/comm/tcp_socket.cpp (100%) rename {src => ur_rtde_driver/src}/main_plain_driver.cpp (100%) rename {src => ur_rtde_driver/src}/primary/primary_package.cpp (100%) rename {src => ur_rtde_driver/src}/primary/robot_message.cpp (100%) rename {src => ur_rtde_driver/src}/primary/robot_message/version_message.cpp (100%) rename {src => ur_rtde_driver/src}/primary/robot_state/kinematics_info.cpp (100%) rename {src => ur_rtde_driver/src}/producer.cpp (100%) rename {src => ur_rtde_driver/src}/ros/hardware_interface.cpp (100%) rename {src => ur_rtde_driver/src}/ros/hardware_interface_node.cpp (100%) rename {src => ur_rtde_driver/src}/ros/service_stopper.cpp (100%) rename {src => ur_rtde_driver/src}/rtde/control_package_pause.cpp (100%) rename {src => ur_rtde_driver/src}/rtde/control_package_setup_inputs.cpp (100%) rename {src => ur_rtde_driver/src}/rtde/control_package_setup_outputs.cpp (100%) rename {src => ur_rtde_driver/src}/rtde/control_package_start.cpp (100%) rename {src => ur_rtde_driver/src}/rtde/data_package.cpp (100%) rename {src => ur_rtde_driver/src}/rtde/get_urcontrol_version.cpp (100%) rename {src => ur_rtde_driver/src}/rtde/request_protocol_version.cpp (100%) rename {src => ur_rtde_driver/src}/rtde/rtde_client.cpp (100%) rename {src => ur_rtde_driver/src}/rtde/rtde_package.cpp (100%) rename {src => ur_rtde_driver/src}/rtde/text_message.cpp (100%) rename {src => ur_rtde_driver/src}/ur/commander.cpp (100%) rename {src => ur_rtde_driver/src}/ur/master_board.cpp (100%) rename {src => ur_rtde_driver/src}/ur/messages.cpp (100%) rename {src => ur_rtde_driver/src}/ur/robot_mode.cpp (100%) rename {src => ur_rtde_driver/src}/ur/rt_state.cpp (100%) rename {src => ur_rtde_driver/src}/ur/ur_driver.cpp (100%) rename {tests => ur_rtde_driver/tests}/main.cpp (100%) rename {tests => ur_rtde_driver/tests}/ur/master_board.cpp (100%) rename {tests => ur_rtde_driver/tests}/ur/robot_mode.cpp (100%) rename {tests => ur_rtde_driver/tests}/ur/rt_state.cpp (100%) diff --git a/ur_calibration/CMakeLists.txt b/ur_calibration/CMakeLists.txt new file mode 100644 index 0000000..7da1dd6 --- /dev/null +++ b/ur_calibration/CMakeLists.txt @@ -0,0 +1,186 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ur_calibration) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + ur_rtde_driver +) +find_package(Eigen3 REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES ur_calibration + CATKIN_DEPENDS + roscpp + ur_rtde_driver +# DEPENDS Eigen3 +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +add_executable(calibration_correction + src/calibration.cpp + src/calibration_correction.cpp +) + +add_dependencies(calibration_correction ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(calibration_correction + ${catkin_LIBRARIES} + yaml-cpp +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +if (CATKIN_ENABLE_TESTING) + catkin_add_gtest(calibration_test + test/calibration_test.cpp + src/calibration.cpp) + target_link_libraries(calibration_test ${catkin_LIBRARIES}) +endif() + +install(TARGETS calibration_correction RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/include/ur_rtde_driver/calibration.h b/ur_calibration/include/ur_calibration/calibration.h similarity index 98% rename from include/ur_rtde_driver/calibration.h rename to ur_calibration/include/ur_calibration/calibration.h index 55e00d5..6175286 100644 --- a/include/ur_rtde_driver/calibration.h +++ b/ur_calibration/include/ur_calibration/calibration.h @@ -148,7 +148,10 @@ public: return chain_; } - std::string toXacroProperties() {return robot_parameters_corrected_.toXacroProperties();} + std::string toXacroProperties() + { + return robot_parameters_corrected_.toXacroProperties(); + } void writeToYaml(std::ofstream& ofstream) const; std::vector getSimplified() const; diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml new file mode 100644 index 0000000..698e83f --- /dev/null +++ b/ur_calibration/package.xml @@ -0,0 +1,57 @@ + + + ur_calibration + 0.0.1 + Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF + + + + + Felix Mauch + + + + + + Apache 2.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + ur_rtde_driver + + + rosunit + diff --git a/src/calibration.cpp b/ur_calibration/src/calibration.cpp similarity index 99% rename from src/calibration.cpp rename to ur_calibration/src/calibration.cpp index 818b10e..cdff728 100644 --- a/src/calibration.cpp +++ b/ur_calibration/src/calibration.cpp @@ -12,7 +12,7 @@ */ //---------------------------------------------------------------------- -#include +#include Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters) { diff --git a/src/calibration_correction.cpp b/ur_calibration/src/calibration_correction.cpp similarity index 99% rename from src/calibration_correction.cpp rename to ur_calibration/src/calibration_correction.cpp index dee695e..68f6424 100644 --- a/src/calibration_correction.cpp +++ b/ur_calibration/src/calibration_correction.cpp @@ -12,7 +12,7 @@ */ //---------------------------------------------------------------------- -#include +#include #include #include diff --git a/test/calibration_test.cpp b/ur_calibration/test/calibration_test.cpp similarity index 99% rename from test/calibration_test.cpp rename to ur_calibration/test/calibration_test.cpp index 130a1c7..1fb5028 100644 --- a/test/calibration_test.cpp +++ b/ur_calibration/test/calibration_test.cpp @@ -13,7 +13,7 @@ //---------------------------------------------------------------------- #include -#include +#include namespace { diff --git a/CMakeLists.txt b/ur_rtde_driver/CMakeLists.txt similarity index 73% rename from CMakeLists.txt rename to ur_rtde_driver/CMakeLists.txt index 7d75787..0dec3ab 100644 --- a/CMakeLists.txt +++ b/ur_rtde_driver/CMakeLists.txt @@ -13,21 +13,17 @@ find_package(catkin REQUIRED actionlib control_msgs controller_manager - eigen_conversions geometry_msgs hardware_interface - kdl_parser industrial_msgs roscpp sensor_msgs std_srvs tf - tf_conversions trajectory_msgs ur_msgs ) find_package(Boost REQUIRED) -find_package(Eigen3 REQUIRED) catkin_package( INCLUDE_DIRS @@ -38,10 +34,8 @@ catkin_package( actionlib control_msgs controller_manager - eigen_conversions geometry_msgs hardware_interface - kdl_parser industrial_msgs roscpp sensor_msgs @@ -49,7 +43,6 @@ catkin_package( ur_msgs std_srvs tf - tf_conversions DEPENDS Boost ) @@ -121,18 +114,6 @@ target_link_libraries(plain_driver ${catkin_LIBRARIES} ur_rtde_driver) add_dependencies(plain_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_executable(calibration_correction - src/calibration.cpp - src/calibration_correction.cpp -) - -add_dependencies(calibration_correction ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Specify libraries to link a library or executable target against -target_link_libraries(calibration_correction - ${catkin_LIBRARIES} - yaml-cpp -) - install(DIRECTORY config launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) @@ -142,25 +123,5 @@ install(DIRECTORY include/${PROJECT_NAME}/ FILES_MATCHING PATTERN "*.h" ) -############# -## Testing ## -############# - -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}) -#endif() -install(TARGETS calibration_correction RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(TARGETS ur_rtde_driver ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(FILES Apache-2.0_ur_modern_driver.txt DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/LICENSES) -install(FILES manifest.yaml rosdoc.yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/doc) diff --git a/LICENSE b/ur_rtde_driver/LICENSE similarity index 100% rename from LICENSE rename to ur_rtde_driver/LICENSE diff --git a/README.md b/ur_rtde_driver/README.md similarity index 100% rename from README.md rename to ur_rtde_driver/README.md diff --git a/config/ur10_controllers.yaml b/ur_rtde_driver/config/ur10_controllers.yaml similarity index 100% rename from config/ur10_controllers.yaml rename to ur_rtde_driver/config/ur10_controllers.yaml diff --git a/config/ur3_controllers.yaml b/ur_rtde_driver/config/ur3_controllers.yaml similarity index 100% rename from config/ur3_controllers.yaml rename to ur_rtde_driver/config/ur3_controllers.yaml diff --git a/config/ur5_controllers.yaml b/ur_rtde_driver/config/ur5_controllers.yaml similarity index 100% rename from config/ur5_controllers.yaml rename to ur_rtde_driver/config/ur5_controllers.yaml diff --git a/include/ur_rtde_driver/comm/bin_parser.h b/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h similarity index 100% rename from include/ur_rtde_driver/comm/bin_parser.h rename to ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h diff --git a/include/ur_rtde_driver/comm/package.h b/ur_rtde_driver/include/ur_rtde_driver/comm/package.h similarity index 100% rename from include/ur_rtde_driver/comm/package.h rename to ur_rtde_driver/include/ur_rtde_driver/comm/package.h diff --git a/include/ur_rtde_driver/comm/package_serializer.h b/ur_rtde_driver/include/ur_rtde_driver/comm/package_serializer.h similarity index 100% rename from include/ur_rtde_driver/comm/package_serializer.h rename to ur_rtde_driver/include/ur_rtde_driver/comm/package_serializer.h diff --git a/include/ur_rtde_driver/comm/parser.h b/ur_rtde_driver/include/ur_rtde_driver/comm/parser.h similarity index 100% rename from include/ur_rtde_driver/comm/parser.h rename to ur_rtde_driver/include/ur_rtde_driver/comm/parser.h diff --git a/include/ur_rtde_driver/comm/pipeline.h b/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h similarity index 100% rename from include/ur_rtde_driver/comm/pipeline.h rename to ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h diff --git a/include/ur_rtde_driver/comm/producer.h b/ur_rtde_driver/include/ur_rtde_driver/comm/producer.h similarity index 100% rename from include/ur_rtde_driver/comm/producer.h rename to ur_rtde_driver/include/ur_rtde_driver/comm/producer.h diff --git a/include/ur_rtde_driver/comm/reverse_interface.h b/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h similarity index 100% rename from include/ur_rtde_driver/comm/reverse_interface.h rename to ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h diff --git a/include/ur_rtde_driver/comm/server.h b/ur_rtde_driver/include/ur_rtde_driver/comm/server.h similarity index 100% rename from include/ur_rtde_driver/comm/server.h rename to ur_rtde_driver/include/ur_rtde_driver/comm/server.h diff --git a/include/ur_rtde_driver/comm/shell_consumer.h b/ur_rtde_driver/include/ur_rtde_driver/comm/shell_consumer.h similarity index 100% rename from include/ur_rtde_driver/comm/shell_consumer.h rename to ur_rtde_driver/include/ur_rtde_driver/comm/shell_consumer.h diff --git a/include/ur_rtde_driver/comm/stream.h b/ur_rtde_driver/include/ur_rtde_driver/comm/stream.h similarity index 100% rename from include/ur_rtde_driver/comm/stream.h rename to ur_rtde_driver/include/ur_rtde_driver/comm/stream.h diff --git a/include/ur_rtde_driver/comm/tcp_socket.h b/ur_rtde_driver/include/ur_rtde_driver/comm/tcp_socket.h similarity index 100% rename from include/ur_rtde_driver/comm/tcp_socket.h rename to ur_rtde_driver/include/ur_rtde_driver/comm/tcp_socket.h diff --git a/include/ur_rtde_driver/event_counter.h b/ur_rtde_driver/include/ur_rtde_driver/event_counter.h similarity index 100% rename from include/ur_rtde_driver/event_counter.h rename to ur_rtde_driver/include/ur_rtde_driver/event_counter.h diff --git a/include/ur_rtde_driver/log.h b/ur_rtde_driver/include/ur_rtde_driver/log.h similarity index 100% rename from include/ur_rtde_driver/log.h rename to ur_rtde_driver/include/ur_rtde_driver/log.h diff --git a/include/ur_rtde_driver/primary/package_header.h b/ur_rtde_driver/include/ur_rtde_driver/primary/package_header.h similarity index 100% rename from include/ur_rtde_driver/primary/package_header.h rename to ur_rtde_driver/include/ur_rtde_driver/primary/package_header.h diff --git a/include/ur_rtde_driver/primary/primary_package.h b/ur_rtde_driver/include/ur_rtde_driver/primary/primary_package.h similarity index 100% rename from include/ur_rtde_driver/primary/primary_package.h rename to ur_rtde_driver/include/ur_rtde_driver/primary/primary_package.h diff --git a/include/ur_rtde_driver/primary/primary_parser.h b/ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h similarity index 100% rename from include/ur_rtde_driver/primary/primary_parser.h rename to ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h diff --git a/include/ur_rtde_driver/primary/robot_message.h b/ur_rtde_driver/include/ur_rtde_driver/primary/robot_message.h similarity index 100% rename from include/ur_rtde_driver/primary/robot_message.h rename to ur_rtde_driver/include/ur_rtde_driver/primary/robot_message.h diff --git a/include/ur_rtde_driver/primary/robot_message/version_message.h b/ur_rtde_driver/include/ur_rtde_driver/primary/robot_message/version_message.h similarity index 100% rename from include/ur_rtde_driver/primary/robot_message/version_message.h rename to ur_rtde_driver/include/ur_rtde_driver/primary/robot_message/version_message.h diff --git a/include/ur_rtde_driver/primary/robot_state.h b/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state.h similarity index 100% rename from include/ur_rtde_driver/primary/robot_state.h rename to ur_rtde_driver/include/ur_rtde_driver/primary/robot_state.h diff --git a/include/ur_rtde_driver/primary/robot_state/kinematics_info.h b/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/kinematics_info.h similarity index 100% rename from include/ur_rtde_driver/primary/robot_state/kinematics_info.h rename to ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/kinematics_info.h diff --git a/include/ur_rtde_driver/primary/robot_state/master_board.h b/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/master_board.h similarity index 100% rename from include/ur_rtde_driver/primary/robot_state/master_board.h rename to ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/master_board.h diff --git a/include/ur_rtde_driver/primary/robot_state/robot_mode_data.h b/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/robot_mode_data.h similarity index 100% rename from include/ur_rtde_driver/primary/robot_state/robot_mode_data.h rename to ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/robot_mode_data.h diff --git a/include/ur_rtde_driver/queue/LICENSE.md b/ur_rtde_driver/include/ur_rtde_driver/queue/LICENSE.md similarity index 100% rename from include/ur_rtde_driver/queue/LICENSE.md rename to ur_rtde_driver/include/ur_rtde_driver/queue/LICENSE.md diff --git a/include/ur_rtde_driver/queue/atomicops.h b/ur_rtde_driver/include/ur_rtde_driver/queue/atomicops.h similarity index 100% rename from include/ur_rtde_driver/queue/atomicops.h rename to ur_rtde_driver/include/ur_rtde_driver/queue/atomicops.h diff --git a/include/ur_rtde_driver/queue/readerwriterqueue.h b/ur_rtde_driver/include/ur_rtde_driver/queue/readerwriterqueue.h similarity index 100% rename from include/ur_rtde_driver/queue/readerwriterqueue.h rename to ur_rtde_driver/include/ur_rtde_driver/queue/readerwriterqueue.h diff --git a/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h similarity index 100% rename from include/ur_rtde_driver/ros/hardware_interface.h rename to ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h diff --git a/include/ur_rtde_driver/ros/io_service.h b/ur_rtde_driver/include/ur_rtde_driver/ros/io_service.h similarity index 100% rename from include/ur_rtde_driver/ros/io_service.h rename to ur_rtde_driver/include/ur_rtde_driver/ros/io_service.h diff --git a/include/ur_rtde_driver/ros/service_stopper.h b/ur_rtde_driver/include/ur_rtde_driver/ros/service_stopper.h similarity index 100% rename from include/ur_rtde_driver/ros/service_stopper.h rename to ur_rtde_driver/include/ur_rtde_driver/ros/service_stopper.h diff --git a/include/ur_rtde_driver/rtde/control_package_pause.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_pause.h similarity index 100% rename from include/ur_rtde_driver/rtde/control_package_pause.h rename to ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_pause.h diff --git a/include/ur_rtde_driver/rtde/control_package_setup_inputs.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_setup_inputs.h similarity index 100% rename from include/ur_rtde_driver/rtde/control_package_setup_inputs.h rename to ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_setup_inputs.h diff --git a/include/ur_rtde_driver/rtde/control_package_setup_outputs.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_setup_outputs.h similarity index 100% rename from include/ur_rtde_driver/rtde/control_package_setup_outputs.h rename to ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_setup_outputs.h diff --git a/include/ur_rtde_driver/rtde/control_package_start.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_start.h similarity index 100% rename from include/ur_rtde_driver/rtde/control_package_start.h rename to ur_rtde_driver/include/ur_rtde_driver/rtde/control_package_start.h diff --git a/include/ur_rtde_driver/rtde/data_package.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/data_package.h similarity index 100% rename from include/ur_rtde_driver/rtde/data_package.h rename to ur_rtde_driver/include/ur_rtde_driver/rtde/data_package.h diff --git a/include/ur_rtde_driver/rtde/get_urcontrol_version.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/get_urcontrol_version.h similarity index 100% rename from include/ur_rtde_driver/rtde/get_urcontrol_version.h rename to ur_rtde_driver/include/ur_rtde_driver/rtde/get_urcontrol_version.h diff --git a/include/ur_rtde_driver/rtde/package_header.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/package_header.h similarity index 100% rename from include/ur_rtde_driver/rtde/package_header.h rename to ur_rtde_driver/include/ur_rtde_driver/rtde/package_header.h diff --git a/include/ur_rtde_driver/rtde/request_protocol_version.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/request_protocol_version.h similarity index 100% rename from include/ur_rtde_driver/rtde/request_protocol_version.h rename to ur_rtde_driver/include/ur_rtde_driver/rtde/request_protocol_version.h diff --git a/include/ur_rtde_driver/rtde/rtde_client.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h similarity index 100% rename from include/ur_rtde_driver/rtde/rtde_client.h rename to ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h diff --git a/include/ur_rtde_driver/rtde/rtde_package.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_package.h similarity index 100% rename from include/ur_rtde_driver/rtde/rtde_package.h rename to ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_package.h diff --git a/include/ur_rtde_driver/rtde/rtde_parser.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_parser.h similarity index 100% rename from include/ur_rtde_driver/rtde/rtde_parser.h rename to ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_parser.h diff --git a/include/ur_rtde_driver/rtde/text_message.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/text_message.h similarity index 100% rename from include/ur_rtde_driver/rtde/text_message.h rename to ur_rtde_driver/include/ur_rtde_driver/rtde/text_message.h diff --git a/include/ur_rtde_driver/test/random_data.h b/ur_rtde_driver/include/ur_rtde_driver/test/random_data.h similarity index 100% rename from include/ur_rtde_driver/test/random_data.h rename to ur_rtde_driver/include/ur_rtde_driver/test/random_data.h diff --git a/include/ur_rtde_driver/test/utils.h b/ur_rtde_driver/include/ur_rtde_driver/test/utils.h similarity index 100% rename from include/ur_rtde_driver/test/utils.h rename to ur_rtde_driver/include/ur_rtde_driver/test/utils.h diff --git a/include/ur_rtde_driver/types.h b/ur_rtde_driver/include/ur_rtde_driver/types.h similarity index 100% rename from include/ur_rtde_driver/types.h rename to ur_rtde_driver/include/ur_rtde_driver/types.h diff --git a/include/ur_rtde_driver/ur/commander.h b/ur_rtde_driver/include/ur_rtde_driver/ur/commander.h similarity index 100% rename from include/ur_rtde_driver/ur/commander.h rename to ur_rtde_driver/include/ur_rtde_driver/ur/commander.h diff --git a/include/ur_rtde_driver/ur/consumer.h b/ur_rtde_driver/include/ur_rtde_driver/ur/consumer.h similarity index 100% rename from include/ur_rtde_driver/ur/consumer.h rename to ur_rtde_driver/include/ur_rtde_driver/ur/consumer.h diff --git a/include/ur_rtde_driver/ur/factory.h b/ur_rtde_driver/include/ur_rtde_driver/ur/factory.h similarity index 100% rename from include/ur_rtde_driver/ur/factory.h rename to ur_rtde_driver/include/ur_rtde_driver/ur/factory.h diff --git a/include/ur_rtde_driver/ur/messages.h b/ur_rtde_driver/include/ur_rtde_driver/ur/messages.h similarity index 100% rename from include/ur_rtde_driver/ur/messages.h rename to ur_rtde_driver/include/ur_rtde_driver/ur/messages.h diff --git a/include/ur_rtde_driver/ur/messages_parser.h b/ur_rtde_driver/include/ur_rtde_driver/ur/messages_parser.h similarity index 100% rename from include/ur_rtde_driver/ur/messages_parser.h rename to ur_rtde_driver/include/ur_rtde_driver/ur/messages_parser.h diff --git a/include/ur_rtde_driver/ur/robot_mode.h b/ur_rtde_driver/include/ur_rtde_driver/ur/robot_mode.h similarity index 100% rename from include/ur_rtde_driver/ur/robot_mode.h rename to ur_rtde_driver/include/ur_rtde_driver/ur/robot_mode.h diff --git a/include/ur_rtde_driver/ur/rt_parser.h b/ur_rtde_driver/include/ur_rtde_driver/ur/rt_parser.h similarity index 100% rename from include/ur_rtde_driver/ur/rt_parser.h rename to ur_rtde_driver/include/ur_rtde_driver/ur/rt_parser.h diff --git a/include/ur_rtde_driver/ur/rt_state.h b/ur_rtde_driver/include/ur_rtde_driver/ur/rt_state.h similarity index 100% rename from include/ur_rtde_driver/ur/rt_state.h rename to ur_rtde_driver/include/ur_rtde_driver/ur/rt_state.h diff --git a/include/ur_rtde_driver/ur/state.h b/ur_rtde_driver/include/ur_rtde_driver/ur/state.h similarity index 100% rename from include/ur_rtde_driver/ur/state.h rename to ur_rtde_driver/include/ur_rtde_driver/ur/state.h diff --git a/include/ur_rtde_driver/ur/state_parser.h b/ur_rtde_driver/include/ur_rtde_driver/ur/state_parser.h similarity index 100% rename from include/ur_rtde_driver/ur/state_parser.h rename to ur_rtde_driver/include/ur_rtde_driver/ur/state_parser.h diff --git a/include/ur_rtde_driver/ur/ur_driver.h b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h similarity index 100% rename from include/ur_rtde_driver/ur/ur_driver.h rename to ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h diff --git a/launch/ur10_bringup.launch b/ur_rtde_driver/launch/ur10_bringup.launch similarity index 100% rename from launch/ur10_bringup.launch rename to ur_rtde_driver/launch/ur10_bringup.launch diff --git a/launch/ur10_bringup_compatible.launch b/ur_rtde_driver/launch/ur10_bringup_compatible.launch similarity index 100% rename from launch/ur10_bringup_compatible.launch rename to ur_rtde_driver/launch/ur10_bringup_compatible.launch diff --git a/launch/ur10_bringup_joint_limited.launch b/ur_rtde_driver/launch/ur10_bringup_joint_limited.launch similarity index 100% rename from launch/ur10_bringup_joint_limited.launch rename to ur_rtde_driver/launch/ur10_bringup_joint_limited.launch diff --git a/launch/ur10_ros_control.launch b/ur_rtde_driver/launch/ur10_ros_control.launch similarity index 100% rename from launch/ur10_ros_control.launch rename to ur_rtde_driver/launch/ur10_ros_control.launch diff --git a/launch/ur3_bringup.launch b/ur_rtde_driver/launch/ur3_bringup.launch similarity index 100% rename from launch/ur3_bringup.launch rename to ur_rtde_driver/launch/ur3_bringup.launch diff --git a/launch/ur3_bringup_joint_limited.launch b/ur_rtde_driver/launch/ur3_bringup_joint_limited.launch similarity index 100% rename from launch/ur3_bringup_joint_limited.launch rename to ur_rtde_driver/launch/ur3_bringup_joint_limited.launch diff --git a/launch/ur3_ros_control.launch b/ur_rtde_driver/launch/ur3_ros_control.launch similarity index 100% rename from launch/ur3_ros_control.launch rename to ur_rtde_driver/launch/ur3_ros_control.launch diff --git a/launch/ur5_bringup.launch b/ur_rtde_driver/launch/ur5_bringup.launch similarity index 100% rename from launch/ur5_bringup.launch rename to ur_rtde_driver/launch/ur5_bringup.launch diff --git a/launch/ur5_bringup_compatible.launch b/ur_rtde_driver/launch/ur5_bringup_compatible.launch similarity index 100% rename from launch/ur5_bringup_compatible.launch rename to ur_rtde_driver/launch/ur5_bringup_compatible.launch diff --git a/launch/ur5_bringup_joint_limited.launch b/ur_rtde_driver/launch/ur5_bringup_joint_limited.launch similarity index 100% rename from launch/ur5_bringup_joint_limited.launch rename to ur_rtde_driver/launch/ur5_bringup_joint_limited.launch diff --git a/launch/ur5_ros_control.launch b/ur_rtde_driver/launch/ur5_ros_control.launch similarity index 100% rename from launch/ur5_ros_control.launch rename to ur_rtde_driver/launch/ur5_ros_control.launch diff --git a/launch/ur_common.launch b/ur_rtde_driver/launch/ur_common.launch similarity index 100% rename from launch/ur_common.launch rename to ur_rtde_driver/launch/ur_common.launch diff --git a/package.xml b/ur_rtde_driver/package.xml similarity index 94% rename from package.xml rename to ur_rtde_driver/package.xml index f1e6b57..15af7a3 100644 --- a/package.xml +++ b/ur_rtde_driver/package.xml @@ -24,16 +24,13 @@ controller_manager actionlib control_msgs - eigen_conversions geometry_msgs - kdl_parser industrial_msgs roscpp sensor_msgs trajectory_msgs ur_msgs tf - tf_conversions std_srvs gtest diff --git a/src/comm/server.cpp b/ur_rtde_driver/src/comm/server.cpp similarity index 100% rename from src/comm/server.cpp rename to ur_rtde_driver/src/comm/server.cpp diff --git a/src/comm/tcp_socket.cpp b/ur_rtde_driver/src/comm/tcp_socket.cpp similarity index 100% rename from src/comm/tcp_socket.cpp rename to ur_rtde_driver/src/comm/tcp_socket.cpp diff --git a/src/main_plain_driver.cpp b/ur_rtde_driver/src/main_plain_driver.cpp similarity index 100% rename from src/main_plain_driver.cpp rename to ur_rtde_driver/src/main_plain_driver.cpp diff --git a/src/primary/primary_package.cpp b/ur_rtde_driver/src/primary/primary_package.cpp similarity index 100% rename from src/primary/primary_package.cpp rename to ur_rtde_driver/src/primary/primary_package.cpp diff --git a/src/primary/robot_message.cpp b/ur_rtde_driver/src/primary/robot_message.cpp similarity index 100% rename from src/primary/robot_message.cpp rename to ur_rtde_driver/src/primary/robot_message.cpp diff --git a/src/primary/robot_message/version_message.cpp b/ur_rtde_driver/src/primary/robot_message/version_message.cpp similarity index 100% rename from src/primary/robot_message/version_message.cpp rename to ur_rtde_driver/src/primary/robot_message/version_message.cpp diff --git a/src/primary/robot_state/kinematics_info.cpp b/ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp similarity index 100% rename from src/primary/robot_state/kinematics_info.cpp rename to ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp diff --git a/src/producer.cpp b/ur_rtde_driver/src/producer.cpp similarity index 100% rename from src/producer.cpp rename to ur_rtde_driver/src/producer.cpp diff --git a/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp similarity index 100% rename from src/ros/hardware_interface.cpp rename to ur_rtde_driver/src/ros/hardware_interface.cpp diff --git a/src/ros/hardware_interface_node.cpp b/ur_rtde_driver/src/ros/hardware_interface_node.cpp similarity index 100% rename from src/ros/hardware_interface_node.cpp rename to ur_rtde_driver/src/ros/hardware_interface_node.cpp diff --git a/src/ros/service_stopper.cpp b/ur_rtde_driver/src/ros/service_stopper.cpp similarity index 100% rename from src/ros/service_stopper.cpp rename to ur_rtde_driver/src/ros/service_stopper.cpp diff --git a/src/rtde/control_package_pause.cpp b/ur_rtde_driver/src/rtde/control_package_pause.cpp similarity index 100% rename from src/rtde/control_package_pause.cpp rename to ur_rtde_driver/src/rtde/control_package_pause.cpp diff --git a/src/rtde/control_package_setup_inputs.cpp b/ur_rtde_driver/src/rtde/control_package_setup_inputs.cpp similarity index 100% rename from src/rtde/control_package_setup_inputs.cpp rename to ur_rtde_driver/src/rtde/control_package_setup_inputs.cpp diff --git a/src/rtde/control_package_setup_outputs.cpp b/ur_rtde_driver/src/rtde/control_package_setup_outputs.cpp similarity index 100% rename from src/rtde/control_package_setup_outputs.cpp rename to ur_rtde_driver/src/rtde/control_package_setup_outputs.cpp diff --git a/src/rtde/control_package_start.cpp b/ur_rtde_driver/src/rtde/control_package_start.cpp similarity index 100% rename from src/rtde/control_package_start.cpp rename to ur_rtde_driver/src/rtde/control_package_start.cpp diff --git a/src/rtde/data_package.cpp b/ur_rtde_driver/src/rtde/data_package.cpp similarity index 100% rename from src/rtde/data_package.cpp rename to ur_rtde_driver/src/rtde/data_package.cpp diff --git a/src/rtde/get_urcontrol_version.cpp b/ur_rtde_driver/src/rtde/get_urcontrol_version.cpp similarity index 100% rename from src/rtde/get_urcontrol_version.cpp rename to ur_rtde_driver/src/rtde/get_urcontrol_version.cpp diff --git a/src/rtde/request_protocol_version.cpp b/ur_rtde_driver/src/rtde/request_protocol_version.cpp similarity index 100% rename from src/rtde/request_protocol_version.cpp rename to ur_rtde_driver/src/rtde/request_protocol_version.cpp diff --git a/src/rtde/rtde_client.cpp b/ur_rtde_driver/src/rtde/rtde_client.cpp similarity index 100% rename from src/rtde/rtde_client.cpp rename to ur_rtde_driver/src/rtde/rtde_client.cpp diff --git a/src/rtde/rtde_package.cpp b/ur_rtde_driver/src/rtde/rtde_package.cpp similarity index 100% rename from src/rtde/rtde_package.cpp rename to ur_rtde_driver/src/rtde/rtde_package.cpp diff --git a/src/rtde/text_message.cpp b/ur_rtde_driver/src/rtde/text_message.cpp similarity index 100% rename from src/rtde/text_message.cpp rename to ur_rtde_driver/src/rtde/text_message.cpp diff --git a/src/ur/commander.cpp b/ur_rtde_driver/src/ur/commander.cpp similarity index 100% rename from src/ur/commander.cpp rename to ur_rtde_driver/src/ur/commander.cpp diff --git a/src/ur/master_board.cpp b/ur_rtde_driver/src/ur/master_board.cpp similarity index 100% rename from src/ur/master_board.cpp rename to ur_rtde_driver/src/ur/master_board.cpp diff --git a/src/ur/messages.cpp b/ur_rtde_driver/src/ur/messages.cpp similarity index 100% rename from src/ur/messages.cpp rename to ur_rtde_driver/src/ur/messages.cpp diff --git a/src/ur/robot_mode.cpp b/ur_rtde_driver/src/ur/robot_mode.cpp similarity index 100% rename from src/ur/robot_mode.cpp rename to ur_rtde_driver/src/ur/robot_mode.cpp diff --git a/src/ur/rt_state.cpp b/ur_rtde_driver/src/ur/rt_state.cpp similarity index 100% rename from src/ur/rt_state.cpp rename to ur_rtde_driver/src/ur/rt_state.cpp diff --git a/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp similarity index 100% rename from src/ur/ur_driver.cpp rename to ur_rtde_driver/src/ur/ur_driver.cpp diff --git a/tests/main.cpp b/ur_rtde_driver/tests/main.cpp similarity index 100% rename from tests/main.cpp rename to ur_rtde_driver/tests/main.cpp diff --git a/tests/ur/master_board.cpp b/ur_rtde_driver/tests/ur/master_board.cpp similarity index 100% rename from tests/ur/master_board.cpp rename to ur_rtde_driver/tests/ur/master_board.cpp diff --git a/tests/ur/robot_mode.cpp b/ur_rtde_driver/tests/ur/robot_mode.cpp similarity index 100% rename from tests/ur/robot_mode.cpp rename to ur_rtde_driver/tests/ur/robot_mode.cpp diff --git a/tests/ur/rt_state.cpp b/ur_rtde_driver/tests/ur/rt_state.cpp similarity index 100% rename from tests/ur/rt_state.cpp rename to ur_rtde_driver/tests/ur/rt_state.cpp From afc1161b1deb61be730b2120c45b695b8e7294b6 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 27 May 2019 18:05:40 +0200 Subject: [PATCH 17/29] correctly depend on yaml-cpp --- ur_calibration/CMakeLists.txt | 6 ++++-- ur_calibration/package.xml | 1 + 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/ur_calibration/CMakeLists.txt b/ur_calibration/CMakeLists.txt index 7da1dd6..59508cd 100644 --- a/ur_calibration/CMakeLists.txt +++ b/ur_calibration/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS ur_rtde_driver ) find_package(Eigen3 REQUIRED) +find_package(yaml-cpp REQUIRED) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -108,7 +109,8 @@ catkin_package( CATKIN_DEPENDS roscpp ur_rtde_driver -# DEPENDS Eigen3 + DEPENDS + yaml-cpp ) ########### @@ -180,7 +182,7 @@ if (CATKIN_ENABLE_TESTING) catkin_add_gtest(calibration_test test/calibration_test.cpp src/calibration.cpp) - target_link_libraries(calibration_test ${catkin_LIBRARIES}) + target_link_libraries(calibration_test ${catkin_LIBRARIES} yaml-cpp) endif() install(TARGETS calibration_correction RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml index 698e83f..e15e130 100644 --- a/ur_calibration/package.xml +++ b/ur_calibration/package.xml @@ -51,6 +51,7 @@ catkin roscpp ur_rtde_driver + yaml-cpp rosunit From 2df2c4176f0fc1e25b9577e2a0a9a81382cf3f1e Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 27 May 2019 19:04:24 +0200 Subject: [PATCH 18/29] corrected default accuracy --- ur_calibration/test/calibration_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ur_calibration/test/calibration_test.cpp b/ur_calibration/test/calibration_test.cpp index 1fb5028..d2caa95 100644 --- a/ur_calibration/test/calibration_test.cpp +++ b/ur_calibration/test/calibration_test.cpp @@ -44,7 +44,7 @@ TEST(UrRtdeDriver, ur10_fw_kinematics) 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.163941, 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 @@ -109,7 +109,7 @@ TEST(UrRtdeDriver, calibration) 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.163941, 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 From 73624533b9cbcc90789788a41b18068f3a83e1c8 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Tue, 28 May 2019 09:48:59 +0200 Subject: [PATCH 19/29] Fixed a couple of includes --- ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h | 1 + ur_rtde_driver/include/ur_rtde_driver/primary/robot_state.h | 3 +-- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h b/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h index d10989c..7bf825d 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h @@ -26,6 +26,7 @@ #include #include #include +#include #include "ur_rtde_driver/log.h" #include "ur_rtde_driver/types.h" diff --git a/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state.h b/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state.h index ab23846..5cd812e 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state.h +++ b/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state.h @@ -28,8 +28,7 @@ #ifndef UR_RTDE_DRIVER_ROBOT_STATE_H_INCLUDED #define UR_RTDE_DRIVER_ROBOT_STATE_H_INCLUDED -#include -#include +#include #include "ur_rtde_driver/primary/primary_package.h" #include "ur_rtde_driver/primary/package_header.h" From 22f392222e6384229fe2227ef5c81855d0887819 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Tue, 28 May 2019 09:49:33 +0200 Subject: [PATCH 20/29] Pipeline: Stop on destruction explicitly --- ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h b/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h index 315bf6a..3312611 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h @@ -151,6 +151,12 @@ public: { } + ~Pipeline() + { + LOG_INFO("Destructing pipeline"); + stop(); + } + void run() { if (running_) From 157b5a696cc0d1fc31b37af69d8d2224fc4d9260 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Tue, 28 May 2019 10:03:23 +0200 Subject: [PATCH 21/29] Made calibration extraction a working node. --- ur_calibration/CMakeLists.txt | 1 + .../include/ur_calibration/calibration.h | 5 +- .../ur_calibration/calibration_consumer.h | 60 ++++++ ur_calibration/src/calibration.cpp | 7 +- ur_calibration/src/calibration_consumer.cpp | 53 ++++++ ur_calibration/src/calibration_correction.cpp | 175 +++--------------- .../ur_rtde_driver/primary/primary_parser.h | 2 +- 7 files changed, 153 insertions(+), 150 deletions(-) create mode 100644 ur_calibration/include/ur_calibration/calibration_consumer.h create mode 100644 ur_calibration/src/calibration_consumer.cpp diff --git a/ur_calibration/CMakeLists.txt b/ur_calibration/CMakeLists.txt index 59508cd..6b8c026 100644 --- a/ur_calibration/CMakeLists.txt +++ b/ur_calibration/CMakeLists.txt @@ -127,6 +127,7 @@ include_directories( add_executable(calibration_correction src/calibration.cpp + src/calibration_consumer.cpp src/calibration_correction.cpp ) diff --git a/ur_calibration/include/ur_calibration/calibration.h b/ur_calibration/include/ur_calibration/calibration.h index 6175286..3c98f58 100644 --- a/ur_calibration/include/ur_calibration/calibration.h +++ b/ur_calibration/include/ur_calibration/calibration.h @@ -20,6 +20,8 @@ #include #include +namespace ur_calibration +{ /*! * \brief An internal representation of a DH-parametrized link. * @@ -152,7 +154,7 @@ public: { return robot_parameters_corrected_.toXacroProperties(); } - void writeToYaml(std::ofstream& ofstream) const; + YAML::Node toYaml() const; std::vector getSimplified() const; @@ -169,4 +171,5 @@ private: std::vector chain_; }; +} // namespace ur_calibration #endif // ifndef UR_RTDE_DRIVER_CALIBRATION_H_INCLUDED diff --git a/ur_calibration/include/ur_calibration/calibration_consumer.h b/ur_calibration/include/ur_calibration/calibration_consumer.h new file mode 100644 index 0000000..e871cf8 --- /dev/null +++ b/ur_calibration/include/ur_calibration/calibration_consumer.h @@ -0,0 +1,60 @@ +// 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-05-28 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CALIBRATION_CALIBRATION_CONSUMER_H_INCLUDED +#define UR_CALIBRATION_CALIBRATION_CONSUMER_H_INCLUDED +#include + +#include + +#include + +namespace ur_calibration +{ +class CalibrationConsumer + : public ur_driver::comm::IConsumer> +{ +public: + CalibrationConsumer(); + virtual ~CalibrationConsumer() = default; + + virtual void setupConsumer() + { + } + virtual void teardownConsumer() + { + } + virtual void stopConsumer() + { + } + virtual void onTimeout() + { + } + + virtual bool + consume(std::shared_ptr> product); + + bool isCalibrated() const + { + return calibrated_; + } + + YAML::Node getCalibrationParameters() const; + +private: + bool calibrated_; + YAML::Node calibration_parameters_; +}; +} // namespace ur_calibration +#endif // ifndef UR_CALIBRATION_CALIBRATION_CONSUMER_H_INCLUDED diff --git a/ur_calibration/src/calibration.cpp b/ur_calibration/src/calibration.cpp index cdff728..ca98ccd 100644 --- a/ur_calibration/src/calibration.cpp +++ b/ur_calibration/src/calibration.cpp @@ -14,6 +14,8 @@ #include +namespace ur_calibration +{ Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters) { buildChain(); @@ -206,7 +208,7 @@ std::vector Calibration::getSimplified() const return simplified_chain; } -void Calibration::writeToYaml(std::ofstream& ofstream) const +YAML::Node Calibration::toYaml() const { YAML::Node node; @@ -226,5 +228,6 @@ void Calibration::writeToYaml(std::ofstream& ofstream) const node["kinematics"][link_names_[i]] = link; } - ofstream << node; + return node; } +} // namespace ur_calibration diff --git a/ur_calibration/src/calibration_consumer.cpp b/ur_calibration/src/calibration_consumer.cpp new file mode 100644 index 0000000..12ef2a7 --- /dev/null +++ b/ur_calibration/src/calibration_consumer.cpp @@ -0,0 +1,53 @@ +// 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-05-28 + * + */ +//---------------------------------------------------------------------- + +#include + +namespace ur_calibration +{ +CalibrationConsumer::CalibrationConsumer() : calibrated_(false) +{ +} + +bool CalibrationConsumer::consume( + std::shared_ptr> product) +{ + auto kin_info = std::dynamic_pointer_cast(product); + if (kin_info != nullptr) + { + LOG_INFO("%s", product->toString().c_str()); + DHRobot my_robot; + for (size_t i = 0; i < kin_info->dh_a_.size(); ++i) + { + my_robot.segments_.push_back( + DHSegment(kin_info->dh_d_[i], kin_info->dh_a_[i], kin_info->dh_theta_[i], kin_info->dh_alpha_[i])); + } + Calibration calibration(my_robot); + calibration.correctChain(); + + calibration_parameters_ = calibration.toYaml(); + calibrated_ = true; + } + return true; +} + +YAML::Node CalibrationConsumer::getCalibrationParameters() const +{ + if (!calibrated_) + { + throw(std::runtime_error("Cannot get calibration, as no calibration data received yet")); + } + return calibration_parameters_; +} +} // namespace ur_calibration diff --git a/ur_calibration/src/calibration_correction.cpp b/ur_calibration/src/calibration_correction.cpp index 68f6424..75663ec 100644 --- a/ur_calibration/src/calibration_correction.cpp +++ b/ur_calibration/src/calibration_correction.cpp @@ -12,170 +12,53 @@ */ //---------------------------------------------------------------------- -#include +#include + +#include +#include +#include +#include +#include +#include #include #include -sensor_msgs::JointStatePtr joint_state; -std::unique_ptr my_calibration; -std::unique_ptr tf_listener; +using namespace ur_driver; +using namespace primary_interface; +using namespace ur_calibration; -void jointStateCallback(sensor_msgs::JointStatePtr msg) -{ - ROS_INFO("Callback"); - tf::StampedTransform stamped_transform; - try - { - tf_listener->waitForTransform("base", "tool0_controller", msg->header.stamp, ros::Duration(0.5)); - tf_listener->lookupTransform("base", "tool0_controller", msg->header.stamp, stamped_transform); - } - catch (tf::TransformException ex) - { - ROS_ERROR("TF lookup error error: %s", ex.what()); - return; - } - - geometry_msgs::TransformStamped tf_msg; - tf::transformStampedTFToMsg(stamped_transform, tf_msg); - - Eigen::Matrix jointvalues = Eigen::Matrix(msg->position.data()); - - Eigen::Matrix4d mat = my_calibration->calcForwardKinematics(jointvalues); - - Eigen::Vector3d error; - error << mat.topRightCorner(3, 1) - Eigen::Vector3d(stamped_transform.getOrigin()); - - ROS_INFO_STREAM("Error(abs): " << error.norm()); - ROS_INFO_STREAM("Error per axis:\n" << error); -} +static const int UR_PRIMARY_PORT = 30001; int main(int argc, char* argv[]) { ros::init(argc, argv, "ur_calibration"); ros::NodeHandle nh; - DHRobot my_robot; - // This is an ideal UR10 - // clang-format off - // d, a, theta, alpha - my_robot.segments_.push_back(DHSegment(0.1273 , 0 , 0 , M_PI_2)); - my_robot.segments_.push_back(DHSegment(0.0 , -0.612 , 0.0 , 0.0)); - my_robot.segments_.push_back(DHSegment(0.0 , -0.5723, 0 , 0.0)); - my_robot.segments_.push_back(DHSegment(0.163941, 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)); - // clang-format on - std::map calibrations; - // d, a, theta, alpha + std::string robot_ip = "192.168.56.101"; + comm::URStream stream(robot_ip, UR_PRIMARY_PORT); + primary_interface::PrimaryParser parser; + comm::URProducer prod(stream, parser); + CalibrationConsumer consumer; + + comm::INotifier notifier; + + comm::Pipeline pipeline(prod, consumer, "Pipeline", notifier); + pipeline.run(); + while (!consumer.isCalibrated()) { - DHRobot robot_calibration; - // clang-format off - robot_calibration.segments_.push_back(DHSegment( 0.00065609212979853 , 4.6311376834935676e-05 , -7.290070070824746e-05 , 0.000211987863869334 )); - robot_calibration.segments_.push_back(DHSegment( 1.4442162376284788 , -0.00012568315331862312 , -0.01713897289704999 , -0.0072553625957652995)); - robot_calibration.segments_.push_back(DHSegment( 0.854147723854608 , 0.00186216581161458 , -0.03707159413492756 , -0.013483226769541364 )); - robot_calibration.segments_.push_back(DHSegment(-2.2989425877563705 , 9.918593870679266e-05 , 0.054279462160583214 , 0.0013495820227329425 )); - robot_calibration.segments_.push_back(DHSegment(-1.573498686836816e-05 , 4.215462720453189e-06 , 1.488984257025741e-07 , -0.001263136163679901 )); - robot_calibration.segments_.push_back(DHSegment( 1.9072435590711256e-05 , 0 , 1.551499479707493e-05 , 0 )); - // clang-format on - calibrations.insert(std::make_pair("ids-ur10-2", robot_calibration)); - } - { - DHRobot robot_calibration; - // clang-format off - robot_calibration.segments_.push_back(DHSegment(0.000489427369583322891 , -0.000157659251888300598 , 7.12144160891142552e-06 , -0.000181484221894123721)); - robot_calibration.segments_.push_back(DHSegment(2.15820897072427709 , 0.000797598154419598693 , -0.0597679909195674153 , -0.0169424433153391799 )); - robot_calibration.segments_.push_back(DHSegment(-0.200070560336579328 , 0.00141079051860837357 , 0.0276740817239430857 , -0.00936706021457411366 )); - robot_calibration.segments_.push_back(DHSegment(-1.95850825255269623 , 8.02528233902343753e-05 , 0.0321549453426039911 , 0.000705308984090935454 )); - robot_calibration.segments_.push_back(DHSegment(-2.27384815544295904e-05, 2.49305433182637798e-06 , -7.91131358362739777e-05, -0.000129247903554619015)); - robot_calibration.segments_.push_back(DHSegment(1.40058148229704749e-05 , 0 , 2.43093497590859384e-05 , 0 )); - // clang-format on - calibrations.insert(std::make_pair("ids-ur10-3", robot_calibration)); - } - // my_calibration.reset(new Calibration(my_robot)); - - /* - for (auto& calib : calibrations) - { - Eigen::Matrix jointvalues; - - double pos_min_error = 9999; - double pos_max_error = 0; - double pos_mean_error = 0; - double angle_min_error = 9999; - double angle_max_error = 0; - double angle_mean_error = 0; - const size_t num_runs = 100000; - for (size_t i = 0; i < num_runs; ++i) - { - Calibration calibration(my_robot + calib.second); - jointvalues = 2 * M_PI * Eigen::Matrix::Random(); - Eigen::Matrix4d fk_orig = calibration.calcForwardKinematics(jointvalues); - calibration.correctChain(); - Eigen::Matrix4d fk_corrected = calibration.calcForwardKinematics(jointvalues); - - Eigen::Vector3d pos_error; - pos_error << fk_orig.topRightCorner(3, 1) - fk_corrected.topRightCorner(3, 1); - - if (pos_error.norm() < pos_min_error) - { - pos_min_error = pos_error.norm(); - } - if (pos_error.norm() > pos_max_error) - { - pos_max_error = pos_error.norm(); - } - pos_mean_error += pos_error.norm() / static_cast(num_runs); - - Eigen::Matrix3d rot_mat_orig = fk_orig.topLeftCorner(3, 3); - Eigen::Matrix3d rot_mat_corrected = fk_corrected.topLeftCorner(3, 3); - Eigen::Quaterniond rot_orig(rot_mat_orig); - Eigen::Quaterniond rot_corrected(rot_mat_corrected); - double angle_error = std::abs(rot_orig.angularDistance(rot_corrected)); - if (angle_error < angle_min_error) - { - angle_min_error = angle_error; - } - if (angle_error > angle_max_error) - { - angle_max_error = angle_error; - } - angle_mean_error += angle_error / static_cast(num_runs); - } - - ROS_INFO_STREAM(calib.first << ": Position Error over " << num_runs << " joint angles (mean, min, max): (" - << pos_mean_error << ", " << pos_min_error << ", " << pos_max_error << ")"); - ROS_INFO_STREAM(calib.first << ": Angle Error over " << num_runs << " joint angles (mean, min, max): (" - << angle_mean_error << ", " << angle_min_error << ", " << angle_max_error << ")"); - // Calibration calibration(my_robot + calib.second); - // calibration.correctChain(); - // ROS_INFO_STREAM("Corrected calibration: " << std::endl << calibration.toXacroProperties()); - } - */ - my_calibration->correctChain(); - std::vector corrected_chain = my_calibration->getSimplified(); - - for (auto& it : corrected_chain) - { - Eigen::Matrix3d rot_a = it.topLeftCorner(3, 3); - Eigen::Vector3d rpy_a = rot_a.eulerAngles(0, 1, 2); - ROS_INFO_STREAM("Translation: [" << it.topRightCorner(3, 1).transpose() << "]"); - ROS_INFO_STREAM("Rotation: [" << rpy_a.transpose() << "]"); + sleep(1); + // LOG_INFO("Still running"); } + pipeline.stop(); std::ofstream file; file.open("test.yaml"); - my_calibration->writeToYaml(file); + YAML::Node calibration_parameters = consumer.getCalibrationParameters(); + file << calibration_parameters; file.close(); - // ROS_INFO_STREAM("Corrected calibration: " << std::endl << my_calibration->toXacroProperties()); - std::cout << my_calibration->toXacroProperties() << std::endl; - - // tf_listener.reset(new tf::TransformListener); - - // ros::Subscriber joint_state_sub = nh.subscribe("joint_states", 1, jointStateCallback); - - // ros::spin(); + ROS_INFO_STREAM("Corrected calibration: " << std::endl << calibration_parameters); return 0; } diff --git a/ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h b/ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h index 3510342..6805643 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h +++ b/ur_rtde_driver/include/ur_rtde_driver/primary/primary_parser.h @@ -123,7 +123,7 @@ public: default: { - LOG_WARN("Invalid robot package type recieved: %u", static_cast(type)); + LOG_DEBUG("Invalid robot package type recieved: %u", static_cast(type)); bp.consume(); return true; } From a61788f69422493e1b0a4649428620d097901387 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Tue, 28 May 2019 11:26:27 +0200 Subject: [PATCH 22/29] use double precision in output of kinematics info --- .../src/primary/robot_state/kinematics_info.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp b/ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp index 3a22bcb..459641c 100644 --- a/ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp +++ b/ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp @@ -15,6 +15,8 @@ #include "ur_rtde_driver/log.h" #include "ur_rtde_driver/primary/robot_state/kinematics_info.h" +#include + namespace ur_driver { namespace primary_interface @@ -43,28 +45,28 @@ std::string KinematicsInfo::toString() const os << "dh_theta: ["; for (size_t i = 0; i < dh_theta_.size(); ++i) { - os << dh_theta_[i] << " "; + os << std::setprecision(15) << dh_theta_[i] << " "; } os << "]" << std::endl; os << "dh_a: ["; for (size_t i = 0; i < dh_a_.size(); ++i) { - os << dh_a_[i] << " "; + os << std::setprecision(15) << dh_a_[i] << " "; } os << "]" << std::endl; os << "dh_d: ["; for (size_t i = 0; i < dh_d_.size(); ++i) { - os << dh_d_[i] << " "; + os << std::setprecision(15) << dh_d_[i] << " "; } os << "]" << std::endl; os << "dh_alpha: ["; for (size_t i = 0; i < dh_alpha_.size(); ++i) { - os << dh_alpha_[i] << " "; + os << std::setprecision(15) << dh_alpha_[i] << " "; } os << "]" << std::endl; From be0cb7f5312faaafad3435f33d30b4ea5fa392c6 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Tue, 28 May 2019 11:45:19 +0200 Subject: [PATCH 23/29] fix namespace move --- ur_calibration/test/calibration_test.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ur_calibration/test/calibration_test.cpp b/ur_calibration/test/calibration_test.cpp index d2caa95..9e7dd93 100644 --- a/ur_calibration/test/calibration_test.cpp +++ b/ur_calibration/test/calibration_test.cpp @@ -15,6 +15,8 @@ #include #include +using namespace ur_calibration; + namespace { bool isApproximately(const double val1, const double val2, const double precision) From c50556cc8a84c92345bc4fc8b23ac859106d5f41 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Tue, 28 May 2019 11:46:39 +0200 Subject: [PATCH 24/29] move real-value examples to other tests The ideal robot uses a non-perfect approximation for pi, which results in larger errors (magnitude 10e-8) when comparing against a model with a more exact pi. Therefore, we use the "UR-pi" for real-value test model. --- ur_calibration/test/calibration_test.cpp | 31 ++++++++++++++++++++---- 1 file changed, 26 insertions(+), 5 deletions(-) diff --git a/ur_calibration/test/calibration_test.cpp b/ur_calibration/test/calibration_test.cpp index 9e7dd93..8b97adb 100644 --- a/ur_calibration/test/calibration_test.cpp +++ b/ur_calibration/test/calibration_test.cpp @@ -34,12 +34,10 @@ void doubleEqVec(const Eigen::Matrix vec1, const Eigen::Matrix } } -TEST(UrRtdeDriver, ur10_fw_kinematics) +TEST(UrRtdeDriver, ur10_fw_kinematics_ideal) { DHRobot my_robot; const double pi = std::atan(1) * 4; - // const double pi = 1.570796327 * 2.0; - // const double pi = M_PI; // This is an ideal UR10 // clang-format off @@ -74,6 +72,29 @@ TEST(UrRtdeDriver, ur10_fw_kinematics) my_robot.segments_[4].d_; doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-16); } +} + +TEST(UrRtdeDriver, ur10_fw_kinematics_real) +{ + // This test compares a corrected ideal model against positions taken from a simulated robot. + DHRobot my_robot; + const double pi_2 = 1.570796327; // This is what the simulated robot reports as pi/2 + + // This is an ideal UR10 + // clang-format off + 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.163941, 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); + + Eigen::Matrix jointvalues; + Eigen::Vector3d expected_position; + Eigen::Quaterniond expected_orientation; { jointvalues << -1.6007002035724084976209269370884, -1.7271001974688928726209269370884, -2.2029998938189905288709269370884, -0.80799991289247685699592693708837, 1.59510004520416259765625, @@ -82,7 +103,7 @@ TEST(UrRtdeDriver, ur10_fw_kinematics) Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); - doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-10); + doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-15); } { jointvalues << 1.32645022869110107421875, 2.426007747650146484375, 5.951572895050048828125, @@ -92,7 +113,7 @@ TEST(UrRtdeDriver, ur10_fw_kinematics) Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); - doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-9); + doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-15); } } From 118b083ce86220cbdf310ade4cf3ff6bcef303fe Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Tue, 28 May 2019 12:52:35 +0200 Subject: [PATCH 25/29] sleep less before checking for calibration --- ur_calibration/src/calibration_correction.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ur_calibration/src/calibration_correction.cpp b/ur_calibration/src/calibration_correction.cpp index 75663ec..7f77b2a 100644 --- a/ur_calibration/src/calibration_correction.cpp +++ b/ur_calibration/src/calibration_correction.cpp @@ -47,8 +47,7 @@ int main(int argc, char* argv[]) pipeline.run(); while (!consumer.isCalibrated()) { - sleep(1); - // LOG_INFO("Still running"); + ros::Duration(0.1).sleep(); } pipeline.stop(); From 0cced4a6c3bdc7844068b970a577521828efff46 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Tue, 28 May 2019 14:18:10 +0200 Subject: [PATCH 26/29] Define where the output file should go by a parameter --- ur_calibration/src/calibration_correction.cpp | 74 ++++++++++++++++++- 1 file changed, 71 insertions(+), 3 deletions(-) diff --git a/ur_calibration/src/calibration_correction.cpp b/ur_calibration/src/calibration_correction.cpp index 7f77b2a..33670db 100644 --- a/ur_calibration/src/calibration_correction.cpp +++ b/ur_calibration/src/calibration_correction.cpp @@ -23,6 +23,11 @@ #include #include +#include + +#include + +namespace fs = boost::filesystem; using namespace ur_driver; using namespace primary_interface; @@ -30,12 +35,65 @@ using namespace ur_calibration; static const int UR_PRIMARY_PORT = 30001; +class ParamaterMissingException : public ros::Exception +{ +public: + ParamaterMissingException(const std::string& name) + : Exception("Cannot find required parameter " + name + " on the parameter server.") + { + } +}; + +template +T getRequiredParameter(const ros::NodeHandle& nh, const std::string& param_name) +{ + T ret_val; + if (nh.hasParam(param_name)) + { + nh.getParam(param_name, ret_val); + } + else + { + throw ParamaterMissingException(nh.resolveName(param_name)); + } + + return ret_val; +} + int main(int argc, char* argv[]) { ros::init(argc, argv, "ur_calibration"); - ros::NodeHandle nh; + ros::NodeHandle nh("~"); + + std::string subfolder = nh.param("subfolder_name", "etc"); + std::string robot_ip, robot_name, output_package_name; + try + { + // The robot's IP address + robot_ip = getRequiredParameter(nh, "robot_ip"); + + // Name with which the robot will be referenced. Will be used for the filename the calibration + // data is stored in. This can be any arbitrary name. If left empty, the robot's serial number + // will be used. + robot_name = getRequiredParameter(nh, "robot_name"); + + // The resulting parameter file will be stored inside + // /subfolder/_calibration.yaml + output_package_name = getRequiredParameter(nh, "output_package_name"); + } + catch (const ParamaterMissingException& e) + { + ROS_FATAL_STREAM(e.what()); + return -1; + } + + std::string package_path = ros::package::getPath(output_package_name); + if (package_path == "") + { + ROS_FATAL_STREAM("Could not find package " << output_package_name << ". Cannot save output file there."); + return -1; + } - std::string robot_ip = "192.168.56.101"; comm::URStream stream(robot_ip, UR_PRIMARY_PORT); primary_interface::PrimaryParser parser; comm::URProducer prod(stream, parser); @@ -52,7 +110,17 @@ int main(int argc, char* argv[]) pipeline.stop(); std::ofstream file; - file.open("test.yaml"); + fs::path dst_path = fs::path(package_path) / fs::path(subfolder); + if (!fs::exists(dst_path)) + { + fs::create_directory(dst_path); + } + fs::path output_filename = dst_path / fs::path(robot_name + "_calibration.yaml"); + if (fs::exists(output_filename)) + { + ROS_WARN_STREAM("Output file " << output_filename << " already exists. Overwriting."); + } + file.open(output_filename.string()); YAML::Node calibration_parameters = consumer.getCalibrationParameters(); file << calibration_parameters; file.close(); From a603e9bb9735add7967b22ce6e8912f29b561d10 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Tue, 28 May 2019 16:51:44 +0200 Subject: [PATCH 27/29] Improved handling of invalid save paths --- ur_calibration/src/calibration_correction.cpp | 193 +++++++++++------- 1 file changed, 123 insertions(+), 70 deletions(-) diff --git a/ur_calibration/src/calibration_correction.cpp b/ur_calibration/src/calibration_correction.cpp index 33670db..eee1eb5 100644 --- a/ur_calibration/src/calibration_correction.cpp +++ b/ur_calibration/src/calibration_correction.cpp @@ -44,88 +44,141 @@ public: } }; -template -T getRequiredParameter(const ros::NodeHandle& nh, const std::string& param_name) +class CalibrationCorrection { - T ret_val; - if (nh.hasParam(param_name)) +public: + CalibrationCorrection(const ros::NodeHandle& nh) : nh_(nh) { - nh.getParam(param_name, ret_val); - } - else - { - throw ParamaterMissingException(nh.resolveName(param_name)); + subfolder_ = nh_.param("subfolder_name", "etc"); + std::string output_package_name; + try + { + // The robot's IP address + robot_ip_ = getRequiredParameter("robot_ip"); + + // Name with which the robot will be referenced. Will be used for the filename the calibration + // data is stored in. This can be any arbitrary name. If left empty, the robot's serial number + // will be used. + robot_name_ = getRequiredParameter("robot_name"); + + // The resulting parameter file will be stored inside + // /subfolder/_calibration.yaml + output_package_name = getRequiredParameter("output_package_name"); + } + catch (const ParamaterMissingException& e) + { + ROS_FATAL_STREAM(e.what()); + exit(1); + } + package_path_ = ros::package::getPath(output_package_name); + if (package_path_ == "") + { + ROS_FATAL_STREAM("Could not find package " << output_package_name << ". Cannot save output file there."); + exit(1); + } } - return ret_val; -} + virtual ~CalibrationCorrection() = default; + + void run() + { + comm::URStream stream(robot_ip_, UR_PRIMARY_PORT); + primary_interface::PrimaryParser parser; + comm::URProducer prod(stream, parser); + CalibrationConsumer consumer; + + comm::INotifier notifier; + + comm::Pipeline pipeline(prod, consumer, "Pipeline", notifier); + pipeline.run(); + while (!consumer.isCalibrated()) + { + ros::Duration(0.1).sleep(); + } + calibration_data_.reset(new YAML::Node); + *calibration_data_ = consumer.getCalibrationParameters(); + } + + bool writeCalibrationData() + { + if (calibration_data_ == nullptr) + { + ROS_ERROR_STREAM("Calibration data not yet set."); + return false; + } + + ROS_INFO_STREAM("Writing calibration data"); + fs::path dst_path = fs::path(package_path_) / fs::path(subfolder_); + if (!fs::exists(dst_path)) + { + try + { + fs::create_directory(dst_path); + } + catch (const boost::filesystem::filesystem_error& e) + { + ROS_ERROR_STREAM("Could not create " << dst_path << ". Reason: " << e.what()); + return false; + } + } + fs::path output_filename = dst_path / fs::path(robot_name_ + "_calibration.yaml"); + if (fs::exists(output_filename)) + { + ROS_WARN_STREAM("Output file " << output_filename << " already exists. Overwriting."); + } + std::ofstream file(output_filename.string()); + if (file.is_open()) + { + file << *calibration_data_; + } + else + { + ROS_ERROR_STREAM("Failed writing the file. Do you have the correct rights?"); + return false; + } + ROS_INFO_STREAM("Wrote output to " << output_filename); + + return true; + } + +private: + template + T getRequiredParameter(const std::string& param_name) const + { + T ret_val; + if (nh_.hasParam(param_name)) + { + nh_.getParam(param_name, ret_val); + } + else + { + throw ParamaterMissingException(nh_.resolveName(param_name)); + } + + return ret_val; + } + + ros::NodeHandle nh_; + std::string robot_ip_; + std::string robot_name_; + std::string package_path_; + std::string subfolder_; + + std::unique_ptr calibration_data_; +}; int main(int argc, char* argv[]) { ros::init(argc, argv, "ur_calibration"); ros::NodeHandle nh("~"); - std::string subfolder = nh.param("subfolder_name", "etc"); - std::string robot_ip, robot_name, output_package_name; - try + CalibrationCorrection my_calibration_correction(nh); + my_calibration_correction.run(); + if (!my_calibration_correction.writeCalibrationData()) { - // The robot's IP address - robot_ip = getRequiredParameter(nh, "robot_ip"); - - // Name with which the robot will be referenced. Will be used for the filename the calibration - // data is stored in. This can be any arbitrary name. If left empty, the robot's serial number - // will be used. - robot_name = getRequiredParameter(nh, "robot_name"); - - // The resulting parameter file will be stored inside - // /subfolder/_calibration.yaml - output_package_name = getRequiredParameter(nh, "output_package_name"); - } - catch (const ParamaterMissingException& e) - { - ROS_FATAL_STREAM(e.what()); + ROS_ERROR_STREAM("Failed writing calibration data. See errors above for details."); return -1; } - - std::string package_path = ros::package::getPath(output_package_name); - if (package_path == "") - { - ROS_FATAL_STREAM("Could not find package " << output_package_name << ". Cannot save output file there."); - return -1; - } - - comm::URStream stream(robot_ip, UR_PRIMARY_PORT); - primary_interface::PrimaryParser parser; - comm::URProducer prod(stream, parser); - CalibrationConsumer consumer; - - comm::INotifier notifier; - - comm::Pipeline pipeline(prod, consumer, "Pipeline", notifier); - pipeline.run(); - while (!consumer.isCalibrated()) - { - ros::Duration(0.1).sleep(); - } - pipeline.stop(); - - std::ofstream file; - fs::path dst_path = fs::path(package_path) / fs::path(subfolder); - if (!fs::exists(dst_path)) - { - fs::create_directory(dst_path); - } - fs::path output_filename = dst_path / fs::path(robot_name + "_calibration.yaml"); - if (fs::exists(output_filename)) - { - ROS_WARN_STREAM("Output file " << output_filename << " already exists. Overwriting."); - } - file.open(output_filename.string()); - YAML::Node calibration_parameters = consumer.getCalibrationParameters(); - file << calibration_parameters; - file.close(); - - ROS_INFO_STREAM("Corrected calibration: " << std::endl << calibration_parameters); - + ROS_INFO("Calibration correction done"); return 0; } From 6adda40a3f7b44b267780b97713fc6f8e6632ae1 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 29 May 2019 08:52:39 +0200 Subject: [PATCH 28/29] Cleaned up legacy code and updated documentation. --- .../include/ur_calibration/calibration.h | 65 +++++++++++-------- ur_calibration/src/calibration.cpp | 59 ++++------------- 2 files changed, 49 insertions(+), 75 deletions(-) diff --git a/ur_calibration/include/ur_calibration/calibration.h b/ur_calibration/include/ur_calibration/calibration.h index 3c98f58..3283749 100644 --- a/ur_calibration/include/ur_calibration/calibration.h +++ b/ur_calibration/include/ur_calibration/calibration.h @@ -101,26 +101,6 @@ struct DHRobot } return ret; } - - /*! - * \brief Generates a string representation of this robot representation. - * - * This string can be directly included into the xacro file used for a calibrated robot. - */ - std::string toXacroProperties() - { - std::stringstream ss; - for (size_t i = 0; i < segments_.size(); ++i) - { - ss << "\n"; - ss << "\n"; - ss << "\n"; - ss << "\n"; - } - ss << "\n"; - ss << "\n"; - return ss.str(); - } }; /*! @@ -145,28 +125,59 @@ public: */ void correctChain(); + /*! + * \brief Get the transformation matrix representation of the chain as constructed by the + * DH parameters. + * + * This will contain twice as many transformation matrices as joints, as for each set of DH + * parameters two matrices are generated. If you'd like to receive one matrix per joint instead, + * use the getSimplified() function instead. + * + * \returns A vector of 4x4 transformation matrices, two for each joint going from the base to the + * tcp. + */ std::vector getChain() { return chain_; } - std::string toXacroProperties() - { - return robot_parameters_corrected_.toXacroProperties(); - } - YAML::Node toYaml() const; - + /*! + * \brief Get the transformation matrix representation of the chain, where each joint is + * represented by one matrix. + * + * \returns Vector of 4x4 transformation matrices, one for each joint going from the base to the + * tcp. + */ std::vector getSimplified() const; + /*! + * \brief Generates a yaml representation of all transformation matrices as returned by + * getSimplified() + * + * \returns A YAML tree representing all tranformation matrices. + */ + YAML::Node toYaml() const; + + /*! + * \brief Calculates the forwart kinematics given a joint configuration with respect to the base + * link. + * + * \param joint_values Joint values for which the forward kinematics should be calculated. + * \param link_nr If given, the cartesian position for this joint (starting at 1) is returned. By + * default the 6th joint is used. + * + * \returns Transformation matrix giving the full pose of the requested link in base coordinates. + */ Eigen::Matrix4d calcForwardKinematics(const Eigen::Matrix& joint_values, const size_t link_nr = 6); private: + // Corrects a single axis void correctAxis(const size_t correction_index); + // Builds the chain from robot_parameters_ void buildChain(); DHRobot robot_parameters_; - DHRobot robot_parameters_corrected_; std::vector link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" }; std::vector chain_; diff --git a/ur_calibration/src/calibration.cpp b/ur_calibration/src/calibration.cpp index ca98ccd..1233c91 100644 --- a/ur_calibration/src/calibration.cpp +++ b/ur_calibration/src/calibration.cpp @@ -27,9 +27,6 @@ Calibration::~Calibration() void Calibration::correctChain() { - robot_parameters_corrected_ = robot_parameters_; - robot_parameters_corrected_.delta_theta_correction2_ = 0.0; - robot_parameters_corrected_.delta_theta_correction3_ = 0.0; correctAxis(1); correctAxis(2); } @@ -57,31 +54,24 @@ void Calibration::correctAxis(const size_t link_index) return; } - Eigen::Matrix jointvalues; - jointvalues << 0, 0, 0, 0, 0, 0; - // Eigen::Matrix4d fk_current = calcForwardKinematics(jointvalues, link_index); - // Eigen::Vector3d current_passive = fk_current.topRightCorner(3, 1); - // ROS_INFO_STREAM("current passive:\n" << current_passive); Eigen::Matrix4d fk_current = Eigen::Matrix4d::Identity(); Eigen::Vector3d current_passive = Eigen::Vector3d::Zero(); Eigen::Matrix4d fk_next_passive = Eigen::Matrix4d::Identity(); fk_next_passive *= chain_[link_index * 2] * chain_[link_index * 2 + 1]; Eigen::Vector3d eigen_passive = fk_next_passive.topRightCorner(3, 1); - // ROS_INFO_STREAM("Eigen passive:\n" << eigen_passive); Eigen::Vector3d eigen_next = (fk_next_passive * chain_[(link_index + 1) * 2]).topRightCorner(3, 1); - // ROS_INFO_STREAM("Eigen next:\n" << eigen_next); // Construct a representation of the next segment's rotational axis Eigen::ParametrizedLine next_line; next_line = Eigen::ParametrizedLine::Through(eigen_passive, eigen_next); - // ROS_INFO_STREAM("next_line:" << std::endl - //<< "Base:" << std::endl - //<< next_line.origin() << std::endl - //<< "Direction:" << std::endl - //<< next_line.direction()); + ROS_DEBUG_STREAM("next_line:" << std::endl + << "Base:" << std::endl + << next_line.origin() << std::endl + << "Direction:" << std::endl + << next_line.direction()); // XY-Plane of first segment's start Eigen::Hyperplane plane(fk_current.topLeftCorner(3, 3) * Eigen::Vector3d(0, 0, 1), current_passive); @@ -95,10 +85,10 @@ void Calibration::correctAxis(const size_t link_index) double new_theta = std::atan(intersection.y() / intersection.x()); // Upper and lower arm segments on URs all have negative length due to dh params double new_length = -1 * intersection.norm(); - // ROS_INFO_STREAM("Wrist line intersecting at " << std::endl << intersection); - // ROS_INFO_STREAM("Angle is " << new_theta); - // ROS_INFO_STREAM("Length is " << new_length); - // ROS_INFO_STREAM("Intersection param is " << intersection_param); + ROS_DEBUG_STREAM("Wrist line intersecting at " << std::endl << intersection); + ROS_DEBUG_STREAM("Angle is " << new_theta); + ROS_DEBUG_STREAM("Length is " << new_length); + ROS_DEBUG_STREAM("Intersection param is " << intersection_param); // as we move the passive segment towards the first segment, we have to move away the next segment // again, to keep the same kinematic structure. @@ -107,46 +97,24 @@ void Calibration::correctAxis(const size_t link_index) // Set d parameter of the first segment to 0 and theta to the calculated new angle // Correct arm segment length and angle - // ROS_INFO_STREAM("Passive old:\n" << chain_[2 * link_index]); chain_[2 * link_index](2, 3) = 0.0; - robot_parameters_corrected_.segments_[link_index].d_ = 0.0; chain_[2 * link_index].topLeftCorner(3, 3) = Eigen::AngleAxisd(new_theta, Eigen::Vector3d::UnitZ()).toRotationMatrix(); - robot_parameters_corrected_.segments_[link_index].theta_ = new_theta; - // ROS_INFO_STREAM("Passive new:\n" << chain_[2 * link_index]); // Correct arm segment length and angle - // ROS_INFO_STREAM("Next old:\n" << chain_[2 * link_index + 1]); - // ROS_INFO_STREAM("Theta correction: " << robot_parameters_.segments_[link_index].theta_ - new_theta); - // ROS_INFO_STREAM("Alpha correction: " << robot_parameters_.segments_[link_index].alpha_); chain_[2 * link_index + 1](0, 3) = new_length; - robot_parameters_corrected_.segments_[link_index].a_ = new_length; chain_[2 * link_index + 1].topLeftCorner(3, 3) = Eigen::AngleAxisd(robot_parameters_.segments_[link_index].theta_ - new_theta, Eigen::Vector3d::UnitZ()) .toRotationMatrix() * Eigen::AngleAxisd(robot_parameters_.segments_[link_index].alpha_, Eigen::Vector3d::UnitX()).toRotationMatrix(); - if (link_index == 1) - { - robot_parameters_corrected_.delta_theta_correction2_ = robot_parameters_.segments_[link_index].theta_ - new_theta; - } - else if (link_index == 2) - { - robot_parameters_corrected_.delta_theta_correction3_ = robot_parameters_.segments_[link_index].theta_ - new_theta; - } - - // ROS_INFO_STREAM("Next new:\n" << chain_[2 * link_index + 1]); // Correct next joint - // ROS_INFO_STREAM("Second Next old:\n" << chain_[2 * link_index + 2]); chain_[2 * link_index + 2](2, 3) -= distance_correction; - robot_parameters_corrected_.segments_[link_index + 1].d_ -= distance_correction; - // ROS_INFO_STREAM("Second Next new:\n" << chain_[2 * link_index + 2]); } Eigen::Matrix4d Calibration::calcForwardKinematics(const Eigen::Matrix& joint_values, const size_t link_nr) { - // ROS_INFO_STREAM("Calculating forward kinematics at link " << link_nr); // Currently ignore input and calculate for zero vector input Eigen::Matrix4d output = Eigen::Matrix4d::Identity(); @@ -158,8 +126,6 @@ Eigen::Matrix4d Calibration::calcForwardKinematics(const Eigen::Matrix Calibration::getSimplified() const simplified_chain.push_back(chain_[i] * chain_[i + 1]); Eigen::Matrix3d rot_a = chain_[i].topLeftCorner(3, 3); Eigen::Vector3d rpy_a = rot_a.eulerAngles(0, 1, 2); - // ROS_INFO_STREAM("Rotation " << i / 2 << " a: [" << rpy_a.transpose() << "]"); + Eigen::Matrix3d rot_b = chain_[i + 1].topLeftCorner(3, 3); Eigen::Vector3d rpy_b = rot_b.eulerAngles(0, 1, 2); - // ROS_INFO_STREAM("Rotation " << i / 2 << " b: [" << rpy_b.transpose() << "]"); - // ROS_INFO_STREAM("Matrix " << i / 2 << ":\n" << simplified_chain.back()); + Eigen::Matrix3d rot = simplified_chain.back().topLeftCorner(3, 3); Eigen::Vector3d rpy = rot.eulerAngles(0, 1, 2); Eigen::Quaterniond quat(rot); - // ROS_INFO_STREAM("Rotation (rpy) " << i / 2 << ": [" << rpy.transpose() << "]"); - // ROS_INFO_STREAM("Rotation (quat, [xyz], w)" << i / 2 << ": [" << quat.vec().transpose() << "], " << quat.w()); } simplified_chain.push_back(chain_.back()); return simplified_chain; From db8bf458eef632d7b3a493e89a73018964a044c3 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 29 May 2019 09:04:00 +0200 Subject: [PATCH 29/29] Added README file --- ur_calibration/README.md | 39 +++++++++++++++++++ ur_calibration/src/calibration_correction.cpp | 2 +- 2 files changed, 40 insertions(+), 1 deletion(-) create mode 100644 ur_calibration/README.md diff --git a/ur_calibration/README.md b/ur_calibration/README.md new file mode 100644 index 0000000..bbae9ff --- /dev/null +++ b/ur_calibration/README.md @@ -0,0 +1,39 @@ +# ur_calibration + +Package for extracting the factory calibration from a UR robot and change it such that it can be used by `ur_description` to gain a correct URDF + +## Nodes +### calibration_correction +This node extracts calibration information directly from a robot, calculates the URDF correction and +saves it into a yaml file. + +With the parameters explained below calibration will be saved inside +```bash +//_calibration.yaml +``` + +#### Example usage +```bash +rosrun ur_calibration calibration_correction _robot_ip:=192.168.56.101 _robot_name:=ur10_ideal _output_package_name:=ur_calibration +``` + +#### Parameters + * **"~subfolder_name"** (default: "etc") + + Given a package where the output should be saved, the calibration file will be saved in this + subfolder relative to the package root. + + * **"~robot_ip"** (required) + + IP address of the robot. The robot has to be reachable with this ip from the machine running + this node. + + * **"~robot_name"** (required) + + Arbitrary name that will be used for generating the calibration file's filename (see node + description). + + * **"~output_package_name"** (required) + + Package inside which the calibration data will be stored in. This package has to exist and has + to be writable. Otherwise execution will fail and calibration data won't be saved. diff --git a/ur_calibration/src/calibration_correction.cpp b/ur_calibration/src/calibration_correction.cpp index eee1eb5..ff4ad37 100644 --- a/ur_calibration/src/calibration_correction.cpp +++ b/ur_calibration/src/calibration_correction.cpp @@ -62,7 +62,7 @@ public: robot_name_ = getRequiredParameter("robot_name"); // The resulting parameter file will be stored inside - // /subfolder/_calibration.yaml + // //_calibration.yaml output_package_name = getRequiredParameter("output_package_name"); } catch (const ParamaterMissingException& e)