<commit_before>// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-

// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------

//----------------------------------------------------------------------
/*!\file
 *
 * \author  <NAME> <EMAIL>
 * \date    2019-01-10
 *
 */
//----------------------------------------------------------------------

#include <ur_rtde_driver/calibration.h>

Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters)
{
  buildChain();
}

Calibration::~Calibration()
{
}

void Calibration::correctChain()
{
  robot_parameters_corrected_ = robot_parameters_;
  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<double, 6, 1> 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<double, 3> next_line;
  next_line = Eigen::ParametrizedLine<double, 3>::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<double, 3> 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;
  // ROS_INFO_STREAM("Second Next new:\n" << chain_[2 * link_index + 2]);<commit_msg>adding changed d parameter of next element to output<commit_after>// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-

// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------

//----------------------------------------------------------------------
/*!\file
 *
 * \author  <NAME> <EMAIL>
 * \date    2019-01-10
 *
 */
//----------------------------------------------------------------------

#include <ur_rtde_driver/calibration.h>

Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters)
{
  buildChain();
}

Calibration::~Calibration()
{
}

void Calibration::correctChain()
{
  robot_parameters_corrected_ = robot_parameters_;
  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<double, 6, 1> 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<double, 3> next_line;
  next_line = Eigen::ParametrizedLine<double, 3>::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<double, 3> 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]);
{'fraction_non_alphanumeric': 0.1570769230769231, 'fraction_numerical': 0.015615384615384616, 'mean_word_length': 3.518943343760862, 'pattern_counts': {'":': 0, '<': 117, '<?xml version=': 0, '>': 19, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4781585', 'n_tokens_mistral': 4200, 'n_tokens_neox': 3934, 'n_words': 1111}