.. _program_listing_file__tmp_ws_src_ur_robot_driver_ur_calibration_include_ur_calibration_calibration.hpp:

Program Listing for File calibration.hpp
========================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_ur_robot_driver_ur_calibration_include_ur_calibration_calibration.hpp>` (``/tmp/ws/src/ur_robot_driver/ur_calibration/include/ur_calibration/calibration.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // -- BEGIN LICENSE BLOCK ----------------------------------------------
   // Created on behalf of Universal Robots A/S
   // Copyright 2019 FZI Forschungszentrum Informatik
   //
   // Redistribution and use in source and binary forms, with or without
   // modification, are permitted provided that the following conditions are met:
   //
   //    * Redistributions of source code must retain the above copyright
   //      notice, this list of conditions and the following disclaimer.
   //
   //    * Redistributions in binary form must reproduce the above copyright
   //      notice, this list of conditions and the following disclaimer in the
   //      documentation and/or other materials provided with the distribution.
   //
   //    * Neither the name of the {copyright_holder} nor the names of its
   //      contributors may be used to endorse or promote products derived from
   //      this software without specific prior written permission.
   //
   // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
   // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
   // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
   // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
   // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
   // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
   // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
   // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
   // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   // POSSIBILITY OF SUCH DAMAGE.
   
   // -- END LICENSE BLOCK ------------------------------------------------
   
   //----------------------------------------------------------------------
   //----------------------------------------------------------------------
   
   #ifndef UR_CALIBRATION__CALIBRATION_HPP_
   #define UR_CALIBRATION__CALIBRATION_HPP_
   
   #include <Eigen/Dense>
   #include <fstream>
   #include <string>
   #include <vector>
   
   #include "rclcpp/rclcpp.hpp"
   #include "yaml-cpp/yaml.h"
   
   namespace ur_calibration
   {
   struct DHSegment
   {
     double d_;
     double a_;
     double theta_;
     double alpha_;
   
     DHSegment(const double d, const double a, const double theta, const double alpha)
       : d_(d), a_(a), theta_(theta), alpha_(alpha)
     {
     }
   
     DHSegment() : d_(0), a_(0), theta_(0), alpha_(0)
     {
     }
   
     DHSegment operator+(const DHSegment& other)
     {
       return DHSegment(this->d_ + other.d_, this->a_ + other.a_, this->theta_ + other.theta_,
                        this->alpha_ + other.alpha_);
     }
   };
   
   struct DHRobot
   {
     std::vector<DHSegment> segments_;
     double delta_theta_correction2_;
     double delta_theta_correction3_;
   
     explicit DHRobot(const std::vector<DHSegment>& segments)
     {
       delta_theta_correction2_ = 0;
       delta_theta_correction3_ = 0;
     }
   
     DHRobot() = default;
   
     DHRobot operator+(const DHRobot& other)
     {
       assert(this->segments_.size() == other.segments_.size());
       DHRobot ret;
       for (size_t i = 0; i < this->segments_.size(); ++i) {
         ret.segments_.push_back(this->segments_[i] + other.segments_[i]);
       }
       return ret;
     }
   };
   
   class Calibration
   {
   public:
     explicit Calibration(const DHRobot& robot);
     virtual ~Calibration();
   
     void correctChain();
   
     std::vector<Eigen::Matrix4d> getChain()
     {
       return chain_;
     }
   
     std::vector<Eigen::Matrix4d> getSimplified() const;
   
     YAML::Node toYaml() const;
   
     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_;
     std::vector<std::string> link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" };
   
     std::vector<Eigen::Matrix4d> chain_;
   };
   }  // namespace ur_calibration
   #endif  // UR_CALIBRATION__CALIBRATION_HPP_