// 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 Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters) { buildChain(); } 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); } void Calibration::correctAxis(const size_t link_index) { // 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. // // 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. if (chain_[2 * link_index](2, 3) == 0.0) { // Nothing to do here. 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()); // 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()); // 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; // 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(); std::vector 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 *= simplified_chain[i] * rotation; } // ROS_INFO_STREAM("forward_kinematics at " << link_nr << std::endl << output); return output; } void Calibration::buildChain() { chain_.clear(); for (size_t i = 0; i < robot_parameters_.segments_.size(); ++i) { 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_; chain_.push_back(seg1_mat); 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); } } 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; }