Cleaned up legacy code and updated documentation.

ur_calibration/include/ur_calibration/calibration.h

@@ -101,26 +101,6 @@ struct DHRobot
     }
     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 << "<xacro:property name=\"d" << i + 1 << "\" value=\"" << segments_[i].d_ << "\" />\n";
-      ss << "<xacro:property name=\"a" << i + 1 << "\" value=\"" << segments_[i].a_ << "\" />\n";
-      ss << "<xacro:property name=\"theta" << i + 1 << "\" value=\"" << segments_[i].theta_ << "\" />\n";
-      ss << "<xacro:property name=\"alpha" << i + 1 << "\" value=\"" << segments_[i].alpha_ << "\" />\n";
-    }
-    ss << "<xacro:property name=\"delta_theta_correction2\" value=\"" << delta_theta_correction2_ << "\" />\n";
-    ss << "<xacro:property name=\"delta_theta_correction3\" value=\"" << delta_theta_correction3_ << "\" />\n";
-    return ss.str();
-  }
 };
 
 /*!
@@ -145,28 +125,59 @@ public:
    */
   void correctChain();
 
+  /*!
+   * \brief Get the transformation matrix representation of the chain as constructed by the
+   * DH parameters.
+   *
+   * This will contain twice as many transformation matrices as joints, as for each set of DH
+   * parameters two matrices are generated. If you'd like to receive one matrix per joint instead,
+   * use the getSimplified() function instead.
+   *
+   * \returns A vector of 4x4 transformation matrices, two for each joint going from the base to the
+   * tcp.
+   */
   std::vector<Eigen::Matrix4d> getChain()
   {
     return chain_;
   }
 
-  std::string toXacroProperties()
+  /*!
-  {
+   * \brief Get the transformation matrix representation of the chain, where each joint is
-    return robot_parameters_corrected_.toXacroProperties();
+   * represented by one matrix.
-  }
+   *
-  YAML::Node toYaml() const;
+   * \returns Vector of 4x4 transformation matrices, one for each joint going from the base to the
-
+   * tcp.
+   */
   std::vector<Eigen::Matrix4d> getSimplified() const;
 
+  /*!
+   * \brief Generates a yaml representation of all transformation matrices as returned by
+   * getSimplified()
+   *
+   * \returns A YAML tree representing all tranformation matrices.
+   */
+  YAML::Node toYaml() const;
+
+  /*!
+   * \brief Calculates the forwart kinematics given a joint configuration with respect to the base
+   * link.
+   *
+   * \param joint_values Joint values for which the forward kinematics should be calculated.
+   * \param link_nr If given, the cartesian position for this joint (starting at 1) is returned. By
+   * default the 6th joint is used.
+   *
+   * \returns Transformation matrix giving the full pose of the requested link in base coordinates.
+   */
   Eigen::Matrix4d calcForwardKinematics(const Eigen::Matrix<double, 6, 1>& joint_values, const size_t link_nr = 6);
 
 private:
+  // Corrects a single axis
   void correctAxis(const size_t correction_index);
 
+  // Builds the chain from robot_parameters_
   void buildChain();
 
   DHRobot robot_parameters_;
-  DHRobot robot_parameters_corrected_;
   std::vector<std::string> link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" };
 
   std::vector<Eigen::Matrix4d> chain_;

ur_calibration/src/calibration.cpp

@@ -27,9 +27,6 @@ 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);
 }
@@ -57,31 +54,24 @@ void Calibration::correctAxis(const size_t link_index)
     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
+  ROS_DEBUG_STREAM("next_line:" << std::endl
-  //<< "Base:" << std::endl
+                                << "Base:" << std::endl
-  //<< next_line.origin() << std::endl
+                                << next_line.origin() << std::endl
-  //<< "Direction:" << std::endl
+                                << "Direction:" << std::endl
-  //<< next_line.direction());
+                                << 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);
@@ -95,10 +85,10 @@ void Calibration::correctAxis(const size_t link_index)
   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_DEBUG_STREAM("Wrist line intersecting at " << std::endl << intersection);
-  // ROS_INFO_STREAM("Angle is " << new_theta);
+  ROS_DEBUG_STREAM("Angle is " << new_theta);
-  // ROS_INFO_STREAM("Length is " << new_length);
+  ROS_DEBUG_STREAM("Length is " << new_length);
-  // ROS_INFO_STREAM("Intersection param is " << intersection_param);
+  ROS_DEBUG_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.
@@ -107,46 +97,24 @@ void Calibration::correctAxis(const size_t link_index)
 
   // 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<double, 6, 1>& 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();
 
@@ -158,8 +126,6 @@ Eigen::Matrix4d Calibration::calcForwardKinematics(const Eigen::Matrix<double, 6
     output *= simplified_chain[i] * rotation;
   }
 
-  // ROS_INFO_STREAM("forward_kinematics at " << link_nr << std::endl << output);
-
   return output;
 }
 
@@ -193,16 +159,13 @@ std::vector<Eigen::Matrix4d> Calibration::getSimplified() const
     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;