From d2eb7a86831d6f0ed6622fd159b8850e19d93e15 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 10 Jan 2019 16:28:45 +0100 Subject: [PATCH] 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; +}