From 8b05ea2fa39df6948b44f1b2de065acee99821f4 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 7 Jun 2024 08:21:30 +0200 Subject: [PATCH] Fix calibration correction There was an error from using std::atan instead of std::atan2. In most cases that seemed to work fine, but with certain calibrations the calculated angle could end up in another quadrant, so atan was returning the wrong angle. We renamed a lot of variables and changed some of the docstrings in order to hopefully make things more understandable in the future. --- ur_calibration/src/calibration.cpp | 81 ++++++++++++++++++------------ 1 file changed, 48 insertions(+), 33 deletions(-) diff --git a/ur_calibration/src/calibration.cpp b/ur_calibration/src/calibration.cpp index 0c9108b35..cb6cb6ad5 100644 --- a/ur_calibration/src/calibration.cpp +++ b/ur_calibration/src/calibration.cpp @@ -73,67 +73,82 @@ void Calibration::correctAxis(const size_t link_index) // 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 + // move along the rotational axis of the next segment. This creates a change in a 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. + // + auto& d_theta_segment = chain_[2 * link_index]; + auto& a_alpha_segment = chain_[2 * link_index + 1]; + + auto& d = d_theta_segment(2, 3); + auto& a = a_alpha_segment(0, 3); - if (chain_[2 * link_index](2, 3) == 0.0) { + if (d == 0.0) { // Nothing to do here. return; } - Eigen::Matrix4d fk_current = Eigen::Matrix4d::Identity(); - Eigen::Vector3d current_passive = Eigen::Vector3d::Zero(); + // Start of the next joint's d_theta segment relative to the joint before the current one + Eigen::Matrix4d next_joint_root = Eigen::Matrix4d::Identity(); + next_joint_root *= d_theta_segment * a_alpha_segment; - 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); + Eigen::Vector3d next_root_position = next_joint_root.topRightCorner(3, 1); - Eigen::Vector3d eigen_next = (fk_next_passive * chain_[(link_index + 1) * 2]).topRightCorner(3, 1); + const auto& next_d_theta_segment = chain_[(link_index + 1) * 2]; + Eigen::Vector3d next_d_theta_end = (next_joint_root * next_d_theta_segment).topRightCorner(3, 1); // Construct a representation of the next segment's rotational axis - Eigen::ParametrizedLine next_line; - next_line = Eigen::ParametrizedLine::Through(eigen_passive, eigen_next); + Eigen::ParametrizedLine next_rotation_axis; + next_rotation_axis = Eigen::ParametrizedLine::Through(next_root_position, next_d_theta_end); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "next_line:" << std::endl - << "Base:" << std::endl - << next_line.origin() << std::endl - << "Direction:" << std::endl - << next_line.direction()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "next rotation axis:" << std::endl + << "Base:" << std::endl + << next_rotation_axis.origin() + << std::endl + << "Direction:" << std::endl + << next_rotation_axis.direction()); // XY-Plane of first segment's start - 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) - current_passive; - double new_theta = std::atan(intersection.y() / intersection.x()); + Eigen::Hyperplane plane(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d::Zero()); + + // Intersect the rotation axis of the next joint with the XY-Plane. + // * The intersection_param is the length moving along the rotation axis until intersecting the plane. + // * The intersection point will be used for calculating the new angle theta. + double intersection_param = next_rotation_axis.intersectionParameter(plane); + Eigen::Vector3d intersection_point = next_rotation_axis.intersectionPoint(plane); + + // A non-zero a parameter will result in an intersection point at (a, 0) even without any + // additional rotations. This effect has to be subtracted from the resulting theta value. + double subtraction_angle = 0.0; + if (std::abs(a) > 0) { + // This is pi + subtraction_angle = atan(1) * 4; + } + double new_theta = std::atan2(intersection_point.y(), intersection_point.x()) - subtraction_angle; // Upper and lower arm segments on URs all have negative length due to dh params - double new_length = -1 * intersection.norm(); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Wrist line intersecting at " << std::endl << intersection); + double new_link_length = -1 * intersection_point.norm(); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Next joint's rotation axis intersecting at " + << std::endl + << intersection_point); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Angle is " << new_theta); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Length is " << new_length); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Length is " << new_link_length); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "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 sign_dir = next_rotation_axis.direction().z() > 0 ? 1.0 : -1.0; double distance_correction = intersection_param * sign_dir; // Set d parameter of the first segment to 0 and theta to the calculated new angle // Correct arm segment length and angle - chain_[2 * link_index](2, 3) = 0.0; - chain_[2 * link_index].topLeftCorner(3, 3) = - Eigen::AngleAxisd(new_theta, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + d = 0.0; + d_theta_segment.topLeftCorner(3, 3) = Eigen::AngleAxisd(new_theta, Eigen::Vector3d::UnitZ()).toRotationMatrix(); // Correct arm segment length and angle - chain_[2 * link_index + 1](0, 3) = new_length; - chain_[2 * link_index + 1].topLeftCorner(3, 3) = + a = new_link_length; + a_alpha_segment.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();