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