diff --git a/README.md b/README.md index ad8c47c47..d885ed425 100644 --- a/README.md +++ b/README.md @@ -132,6 +132,7 @@ The driver is compatible across the entire line of UR robots -- from 3 kg payloa ## Packages in the Repository: - `ur_bringup` - launch file and run-time configurations, e.g. controllers. + - `ur_calibration` - tool for extracting calibration information from a real robot. - `ur_controllers` - implementations of controllers specific for UR robots. - `ur_dashboard_msgs` - package defining messages used by dashboard node. - `ur_description` - description files for the UR robots: meshes, URDF/XACRO files, etc. @@ -175,6 +176,17 @@ The driver is compatible across the entire line of UR robots -- from 3 kg payloa 4. In the Program tab of the teach pendant, navigate to the URCaps section on the left and add the external control to the robot program by clicking on it. The program can then be executed by pressing the play button. Make sure the robot is turned on. The robot power status will be displayed on the bottom left. +### Extract calibration information + +Each UR robot is calibrated inside the factory giving exact forward and inverse kinematics. To also +make use of this in ROS, you first have to extract the calibration information from the robot. + +Though this step is not necessary to control the robot using this driver, it is highly recommended +to do so, as otherwise endeffector positions might be off in the magnitude of centimeters. + +See the [`ur_calibration`](ur_calibration/README.md) package's documentation for details on +calibration extraction and handling. + ## Usage For starting the driver there are three main launch files in the `ur_bringup` package. diff --git a/ur_calibration/CMakeLists.txt b/ur_calibration/CMakeLists.txt new file mode 100644 index 000000000..9e151fefb --- /dev/null +++ b/ur_calibration/CMakeLists.txt @@ -0,0 +1,90 @@ +cmake_minimum_required(VERSION 3.5) +project(ur_calibration) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +add_compile_options(-Wno-unused-parameter) + +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + message("${PROJECT_NAME}: You did not request a specific build type: selecting 'RelWithDebInfo'.") + set(CMAKE_BUILD_TYPE RelWithDebInfo) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ur_robot_driver REQUIRED) + +find_package(Eigen3 REQUIRED) +find_package(yaml-cpp REQUIRED) +find_package(ur_client_library REQUIRED) + +set(YAML_CPP_INCLUDE_DIRS ${YAML_CPP_INCLUDE_DIR}) + +########### +## Build ## +########### + +include_directories( + include + ${EIGEN3_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIRS} +) + +add_executable(calibration_correction + src/calibration.cpp + src/calibration_consumer.cpp + src/calibration_correction.cpp +) +target_include_directories(calibration_correction PRIVATE include) +target_link_libraries(calibration_correction + ${YAML_CPP_LIBRARIES} + ur_client_library::urcl +) +ament_target_dependencies( + calibration_correction + rclcpp + ur_robot_driver +) + +install(TARGETS calibration_correction + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +## Add gtest based cpp test target and link libraries +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + + # Get the first item (it will be the build space version of the build path). + list(GET ament_index_build_path 0 ament_index_build_path) + if(WIN32) + # On Windows prevent CMake errors and prevent it being evaluated as a list. + string(REPLACE "\\" "/" ament_index_build_path "${ament_index_build_path}") + endif() + + ament_add_gmock( + calibration_test + test/calibration_test.cpp + src/calibration.cpp + ) + target_include_directories(calibration_test PRIVATE include ${EIGEN3_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIRS}) + target_link_libraries(calibration_test ${rclcpp_LIBRARIES} ${EIGEN3_LIBRARIES} ${YAML_CPP_LIBRARIES} ur_client_library::urcl) +endif() + +ament_export_include_directories( + include +) +ament_export_dependencies( + ur_robot_driver + rclcpp +) +ament_package() diff --git a/ur_calibration/README.md b/ur_calibration/README.md new file mode 100644 index 000000000..b3a7f8c89 --- /dev/null +++ b/ur_calibration/README.md @@ -0,0 +1,78 @@ +# ur_calibration + +Package for extracting the factory calibration from a UR robot and changing it to be used by `ur_description` to gain a correct URDF model. + +Each UR robot is calibrated inside the factory giving exact forward and inverse kinematics. To also +make use of this in ROS, you first have to extract the calibration information from the robot. + +Though this step is not necessary, to control the robot using this driver, it is highly recommended +to do so, as end effector positions might be off in the magnitude of centimeters. + +## Nodes +### calibration_correction +This node extracts calibration information directly from a robot, calculates the URDF correction and +saves it into a .yaml file. + +In the launch folder of the ur_calibration package is a helper script: + +```bash +$ ros2 launch ur_calibration calibration_correction.launch.py \ +robot_ip:= target_filename:="${HOME}/my_robot_calibration.yaml" +``` + +For the parameter `robot_ip` insert the IP address on which the ROS pc can reach the robot. As +`target_filename` provide an absolute path where the result will be saved to. + +## Creating a calibration / launch package for all local robots +When dealing with multiple robots in one organization it might make sense to store calibration data +into a package dedicated to that purpose only. To do so, create a new package (if it doesn't already +exist) + +```bash +# Replace your actual colcon_ws folder +$ cd /src +$ ros2 pkg create _ur_launch --build-type ament_cmake --dependencies ur_client_library \ +--description "Package containing calibrations and launch files for our UR robots." +# Create a skeleton package +$ mkdir -p _ur_launch/etc +$ mkdir -p _ur_launch/launch +$ echo 'install(DIRECTORY etc launch DESTINATION share/${PROJECT_NAME})' >> _ur_launch/CMakeLists.txt +$ colcon build --packages-select _ur_launch +``` + +We can use the new package to store the calibration data in that package. We recommend naming each +robot individually, e.g. *ex-ur10-1*. + +```bash +$ ros2 launch ur_calibration calibration_correction.launch.py \ +robot_ip:= \ +target_filename:="$(ros2 pkg prefix _ur_launch)/share/_ur_launch/etc/ex-ur10-1_calibration.yaml" +``` + +To make life easier, we create a launchfile for this particular robot. We base it upon the +respective launchfile in the driver: + +```bash +# Replace your actual colcon_ws folder +$ cd /src/_ur_launch/launch +$ cp $(ros2 pkg prefix ur_bringup)/share/ur_bringup/launch/ur_control.launch.py ex-ur10-1.launch.py +``` + +Next, modify the parameter section of the new launchfile to match your actual calibration: + +```py +kinematics_params = PathJoinSubstitution( + [FindPackageShare("_ur_launch"), "etc", "", "ex-ur10-1_calibration.yaml"] + ) + +``` + +Then, anybody cloning this repository can startup the robot simply by launching + +```bash +# Replace your actual colcon_ws folder +$ cd +$ colcon build --packages-select _ur_launch +$ ros2 launch _ur_launch ex-ur10-1.launch.py +robot_ip:=xxx.yyy.zzz.www ur_type:=ur5e use_fake_hardware:=false launch_rviz:=true +``` diff --git a/ur_calibration/include/ur_calibration/calibration.hpp b/ur_calibration/include/ur_calibration/calibration.hpp new file mode 100644 index 000000000..f6fdec8f0 --- /dev/null +++ b/ur_calibration/include/ur_calibration/calibration.hpp @@ -0,0 +1,217 @@ +// -- 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 ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2019-01-10 + * \author Lovro Ivanov lovro.ivanov@gmail.com + * \date 2021-09-22 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CALIBRATION__CALIBRATION_HPP_ +#define UR_CALIBRATION__CALIBRATION_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "yaml-cpp/yaml.h" + +namespace ur_calibration +{ +/*! + * \brief An internal representation of a DH-parametrized link. + * + * Each segment consists of parameters a, d, alpha and theta. + */ +struct DHSegment +{ + double d_; + double a_; + double theta_; + double alpha_; + + /*! + * \brief Creates an element with defined elements. + */ + DHSegment(const double d, const double a, const double theta, const double alpha) + : d_(d), a_(a), theta_(theta), alpha_(alpha) + { + } + + /*! + * \brief Creates a Segment with all elements equal to zero. + */ + DHSegment() : d_(0), a_(0), theta_(0), alpha_(0) + { + } + + /*! + * \brief Adds another segment element-wise (a+a, d+d, alpha+alpha, theta+theta) + * + * \param other Other segment to add + * + * \returns Segment consisting of element-wise addition of \p this and \p other + */ + DHSegment operator+(const DHSegment& other) + { + return DHSegment(this->d_ + other.d_, this->a_ + other.a_, this->theta_ + other.theta_, + this->alpha_ + other.alpha_); + } +}; + +/*! + * \brief Internal representation of a robot based on DH parameters. + * + * Note that this representation doesn't contain a real DH parameter representation, but is used for + * a corrected model of calibrated UR robots. Shoulder and elbow axes are artificially shortened in + * the final representation, requiring a correction parameter in \p theta_2 and \p theta_3. + */ +struct DHRobot +{ + std::vector segments_; + double delta_theta_correction2_; + double delta_theta_correction3_; + + /*! + * \brief Create a new robot representation giving a set of \ref DHSegment objects + */ + explicit DHRobot(const std::vector& segments) + { + delta_theta_correction2_ = 0; + delta_theta_correction3_ = 0; + } + + DHRobot() = default; + + /*! + * \brief Adds another robot representation, by adding their segments element-wise. See \ref + * DHSegment::operator+ for details. + */ + 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; + } +}; + +/*! + * \brief Class that handles the calibration correction for Universal Robots + * + * Universal robots provide a factory calibration of their DH parameters to exactly estimate their + * TCP pose using forward kinematics. However, those DH parameters construct a kinematic chain that + * can be very long, as upper arm and lower arm segments can be drawn out of their physical position + * by multiple meters (even above 100m can occur). + * + * This class helps creating a kinematic chain, that is as close as possible to the physical model, + * by dragging the upper and lower arm segments back to their zero position. + */ +class Calibration +{ +public: + explicit Calibration(const DHRobot& robot); + virtual ~Calibration(); + + /*! + * \brief Corrects a UR kinematic chain in such a way that shoulder and elbow offsets are 0. + */ + 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 getChain() + { + return chain_; + } + + /*! + * \brief Get the transformation matrix representation of the chain, where each joint is + * represented by one matrix. + * + * \returns Vector of 4x4 transformation matrices, one for each joint going from the base to the + * tcp. + */ + std::vector getSimplified() const; + + /*! + * \brief Generates a yaml representation of all transformation matrices as returned by + * getSimplified() + * + * \returns A YAML tree representing all transformation 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& 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 link_names_ = { "shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3" }; + + std::vector chain_; +}; +} // namespace ur_calibration +#endif // UR_CALIBRATION__CALIBRATION_HPP_ diff --git a/ur_calibration/include/ur_calibration/calibration_consumer.hpp b/ur_calibration/include/ur_calibration/calibration_consumer.hpp new file mode 100644 index 000000000..9acadc9a8 --- /dev/null +++ b/ur_calibration/include/ur_calibration/calibration_consumer.hpp @@ -0,0 +1,76 @@ +// -- 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 ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2019-05-28 + * \author Lovro Ivanov lovro.ivanov@gmail.com + * \date 2021-09-22 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CALIBRATION__CALIBRATION_CONSUMER_HPP_ +#define UR_CALIBRATION__CALIBRATION_CONSUMER_HPP_ + +#include + +#include "ur_client_library/comm/pipeline.h" +#include "ur_client_library/primary/robot_state/kinematics_info.h" + +#include "ur_calibration/calibration.hpp" + +namespace ur_calibration +{ +class CalibrationConsumer : public urcl::comm::IConsumer +{ +public: + CalibrationConsumer(); + virtual ~CalibrationConsumer() = default; + + virtual bool consume(std::shared_ptr product); + + bool isCalibrated() const + { + return calibrated_; + } + + YAML::Node getCalibrationParameters() const; + +private: + bool calibrated_; + YAML::Node calibration_parameters_; +}; +} // namespace ur_calibration +#endif // UR_CALIBRATION__CALIBRATION_CONSUMER_HPP_ diff --git a/ur_calibration/launch/calibration_correction.launch.py b/ur_calibration/launch/calibration_correction.launch.py new file mode 100644 index 000000000..31d920a8e --- /dev/null +++ b/ur_calibration/launch/calibration_correction.launch.py @@ -0,0 +1,73 @@ +# Copyright (c) 2021 PickNik, Inc. +# +# 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. + +# +# Author: Lovro Ivanov + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", description="The IP address at which the robot is reachable." + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "target_filename", + default_value="robot_calibration.yaml", + description="The extracted calibration information " + "will be written to this target file.", + ) + ) + + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + output_filename = LaunchConfiguration("target_filename") + + calibration_correction = Node( + package="ur_calibration", + executable="calibration_correction", + parameters=[{"robot_ip": robot_ip}, {"output_filename": output_filename}], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + nodes_to_start = [ + calibration_correction, + ] + + return LaunchDescription(declared_arguments + nodes_to_start) diff --git a/ur_calibration/package.xml b/ur_calibration/package.xml new file mode 100644 index 000000000..7419973fd --- /dev/null +++ b/ur_calibration/package.xml @@ -0,0 +1,29 @@ + + + ur_calibration + 2.0.0 + Package for extracting the factory calibration from a UR robot and change it such that it can be used by ur_description to gain a correct URDF + + Felix Exner + Tristan Schnell + + BSD-3-Clause + + ament_cmake + + rclcpp + ur_client_library + ur_robot_driver + + eigen + yaml-cpp + + ament_cmake_gmock + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ur_calibration/src/calibration.cpp b/ur_calibration/src/calibration.cpp new file mode 100644 index 000000000..0c9108b35 --- /dev/null +++ b/ur_calibration/src/calibration.cpp @@ -0,0 +1,224 @@ +// -- 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 ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2019-01-10 + * \author Lovro Ivanov lovro.ivanov@gmail.com + * \date 2021-09-22 + * + */ +//---------------------------------------------------------------------- + +#include "ur_calibration/calibration.hpp" + +#include + +namespace ur_calibration +{ +Calibration::Calibration(const DHRobot& robot_parameters) : robot_parameters_(robot_parameters) +{ + buildChain(); +} + +Calibration::~Calibration() +{ +} + +void Calibration::correctChain() +{ + 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::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); + + Eigen::Vector3d eigen_next = (fk_next_passive * chain_[(link_index + 1) * 2]).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); + + 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()); + + // 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(); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_calibration"), "Wrist line intersecting at " << std::endl << intersection); + 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"), "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 + chain_[2 * link_index](2, 3) = 0.0; + chain_[2 * link_index].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) = + 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(); + + // Correct next joint + chain_[2 * link_index + 2](2, 3) -= distance_correction; +} + +Eigen::Matrix4d Calibration::calcForwardKinematics(const Eigen::Matrix& joint_values, + const size_t 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; + } + + 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.emplace_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); + + Eigen::Matrix3d rot_b = chain_[i + 1].topLeftCorner(3, 3); + Eigen::Vector3d rpy_b = rot_b.eulerAngles(0, 1, 2); + + Eigen::Matrix3d rot = simplified_chain.back().topLeftCorner(3, 3); + Eigen::Vector3d rpy = rot.eulerAngles(0, 1, 2); + Eigen::Quaterniond quat(rot); + */ + } + simplified_chain.push_back(chain_.back()); + return simplified_chain; +} + +YAML::Node Calibration::toYaml() 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; + } + + return node; +} +} // namespace ur_calibration diff --git a/ur_calibration/src/calibration_consumer.cpp b/ur_calibration/src/calibration_consumer.cpp new file mode 100644 index 000000000..13ed1885f --- /dev/null +++ b/ur_calibration/src/calibration_consumer.cpp @@ -0,0 +1,81 @@ +// -- 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 ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2019-05-28 + * \author Lovro Ivanov lovro.ivanov@gmail.com + * \date 2021-09-22 + * + */ +//---------------------------------------------------------------------- + +#include "ur_calibration/calibration_consumer.hpp" + +#include + +namespace ur_calibration +{ +CalibrationConsumer::CalibrationConsumer() : calibrated_(false) +{ +} + +bool CalibrationConsumer::consume(std::shared_ptr product) +{ + auto kin_info = std::dynamic_pointer_cast(product); + if (kin_info != nullptr) { + RCLCPP_INFO(rclcpp::get_logger("ur_calibration"), "%s", product->toString().c_str()); + DHRobot my_robot; + for (size_t i = 0; i < kin_info->dh_a_.size(); ++i) { + my_robot.segments_.push_back( + DHSegment(kin_info->dh_d_[i], kin_info->dh_a_[i], kin_info->dh_theta_[i], kin_info->dh_alpha_[i])); + } + Calibration calibration(my_robot); + calibration.correctChain(); + + calibration_parameters_ = calibration.toYaml(); + calibration_parameters_["kinematics"]["hash"] = kin_info->toHash(); + calibrated_ = true; + } + return true; +} + +YAML::Node CalibrationConsumer::getCalibrationParameters() const +{ + if (!calibrated_) { + throw(std::runtime_error("Cannot get calibration, as no calibration data received yet")); + } + return calibration_parameters_; +} +} // namespace ur_calibration diff --git a/ur_calibration/src/calibration_correction.cpp b/ur_calibration/src/calibration_correction.cpp new file mode 100644 index 000000000..8b2a5fa7c --- /dev/null +++ b/ur_calibration/src/calibration_correction.cpp @@ -0,0 +1,177 @@ +// -- 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 ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2019-01-10 + * \author Lovro Ivanov lovro.ivanov@gmail.com + * \date 2021-09-22 + * + */ +//---------------------------------------------------------------------- + +#include "ur_calibration/calibration_consumer.hpp" + +#include +#include +#include + +#include "ur_client_library/comm/parser.h" +#include "ur_client_library/comm/pipeline.h" +#include "ur_client_library/comm/producer.h" +#include "ur_client_library/comm/stream.h" +#include "ur_client_library/primary/package_header.h" +#include "ur_client_library/primary/primary_parser.h" + +#include "ur_robot_driver/urcl_log_handler.hpp" + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "rclcpp/exceptions/exceptions.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "tf2_ros/transform_listener.h" + +namespace fs = std::filesystem; + +using urcl::UrException; +using urcl::comm::INotifier; +using urcl::comm::Pipeline; +using urcl::comm::URProducer; +using urcl::comm::URStream; +using urcl::primary_interface::PrimaryPackage; +using urcl::primary_interface::PrimaryParser; +using urcl::primary_interface::UR_PRIMARY_PORT; + +using ur_calibration::CalibrationConsumer; + +class CalibrationCorrection : public rclcpp::Node +{ +public: + CalibrationCorrection() + : Node( + "ur_calibration", + rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) + { + std::string output_package_name; + + try { + // The robot's IP address + robot_ip_ = this->get_parameter("robot_ip").as_string(); + // The target file where the calibration data is written to + output_filename_ = this->get_parameter("output_filename").as_string(); + } catch (rclcpp::exceptions::ParameterNotDeclaredException& e) { + RCLCPP_FATAL_STREAM(this->get_logger(), e.what()); + exit(1); + } + } + + virtual ~CalibrationCorrection() = default; + + void run() + { + URStream stream(robot_ip_, UR_PRIMARY_PORT); + PrimaryParser parser; + URProducer prod(stream, parser); + CalibrationConsumer consumer; + + INotifier notifier; + + Pipeline pipeline(prod, &consumer, "Pipeline", notifier); + pipeline.run(); + while (!consumer.isCalibrated()) { + rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.1).to_chrono()); + } + calibration_data_.reset(new YAML::Node); + *calibration_data_ = consumer.getCalibrationParameters(); + } + + bool writeCalibrationData() + { + if (calibration_data_ == nullptr) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Calibration data not yet set."); + return false; + } + + fs::path out_path = fs::absolute(output_filename_); + + fs::path dst_path = out_path.parent_path(); + if (!fs::exists(dst_path)) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Parent folder " << dst_path << " does not exist."); + return false; + } + RCLCPP_INFO_STREAM(this->get_logger(), "Writing calibration data to " << out_path); + if (fs::exists(output_filename_)) { + RCLCPP_WARN_STREAM(this->get_logger(), "Output file " << output_filename_ << " already exists. Overwriting."); + } + std::ofstream file(output_filename_); + if (file.is_open()) { + file << *calibration_data_; + } else { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed writing the file. Do you have the correct rights?"); + return false; + } + RCLCPP_INFO_STREAM(this->get_logger(), "Wrote output."); + + return true; + } + +private: + std::string robot_ip_; + std::string output_filename_; + std::shared_ptr calibration_data_; +}; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + ur_robot_driver::registerUrclLogHandler(); + + try { + auto calib_node = std::make_shared(); + calib_node->run(); + if (!calib_node->writeCalibrationData()) { + RCLCPP_ERROR_STREAM(calib_node->get_logger(), "Failed writing calibration data. See errors above for details."); + return -1; + } + RCLCPP_INFO_STREAM(calib_node->get_logger(), "Calibration correction done"); + } catch (const UrException& e) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("ur_calibration"), e.what()); + } catch (const std::exception& e) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("ur_calibration"), e.what()); + } + + rclcpp::shutdown(); + + return 0; +} diff --git a/ur_calibration/test/calibration_test.cpp b/ur_calibration/test/calibration_test.cpp new file mode 100644 index 000000000..f6910fe0f --- /dev/null +++ b/ur_calibration/test/calibration_test.cpp @@ -0,0 +1,224 @@ +// -- 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 ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2019-03-11 + * \author Lovro Ivanov lovro.ivanov@gmail.com + * \date 2021-09-27 + * + */ +//---------------------------------------------------------------------- + +#include +#include + +using ur_calibration::Calibration; +using ur_calibration::DHRobot; +using ur_calibration::DHSegment; + +namespace +{ +/* +bool isApproximately(const double val1, const double val2, const double precision) +{ + return std::abs(val1 - val2) < precision; +} +*/ +template +void doubleEqVec(const Eigen::Matrix vec1, const Eigen::Matrix vec2, + const double precision) +{ + for (size_t i = 0; i < dim_; ++i) { + EXPECT_NEAR(vec1[i], vec2[i], precision); + } +} + +} // namespace + +class CalibrationTest : public ::testing::Test +{ +public: + void SetUp() + { + Test::SetUp(); + } + void TearDown() + { + Test::TearDown(); + } + +protected: + const double pi = std::atan(1) * 4; + const double pi_2 = 1.570796327; // This is what the simulated robot reports as pi/2 + DHRobot my_robot_; + DHRobot my_robot_calibration_; +}; + +TEST_F(CalibrationTest, ur10_fw_kinematics_ideal) +{ + my_robot_.segments_.clear(); + // This is an ideal UR10 + // clang-format off + my_robot_.segments_.emplace_back(DHSegment(0.1273 , 0 , 0 , pi / 2)); + my_robot_.segments_.emplace_back(DHSegment(0 , -0.612 , 0 , 0)); + my_robot_.segments_.emplace_back(DHSegment(0 , -0.5723, 0 , 0.0)); + my_robot_.segments_.emplace_back(DHSegment(0.163941, 0 , 0 , pi / 2)); + my_robot_.segments_.emplace_back(DHSegment(0.1157 , 0 , 0 , -pi / 2)); + my_robot_.segments_.emplace_back(DHSegment(0.0922 , 0 , 0 , 0)); + // clang-format on + + Calibration calibration(my_robot_); + + Eigen::Matrix jointvalues; + Eigen::Vector3d expected_position; + Eigen::Quaterniond expected_orientation; + { + jointvalues << 0, 0, 0, 0, 0, 0; + Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); + EXPECT_DOUBLE_EQ(fk(0, 3), my_robot_.segments_[1].a_ + my_robot_.segments_[2].a_); + EXPECT_DOUBLE_EQ(fk(1, 3), -1 * (my_robot_.segments_[3].d_ + my_robot_.segments_[5].d_)); + EXPECT_DOUBLE_EQ(fk(2, 3), my_robot_.segments_[0].d_ - my_robot_.segments_[4].d_); + } + + { + jointvalues << M_PI_2, -M_PI_4, M_PI_2, -M_PI_4, 0, 0; + Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); + + expected_position << my_robot_.segments_[3].d_ + my_robot_.segments_[5].d_, + my_robot_.segments_[1].a_ / std::sqrt(2) + my_robot_.segments_[2].a_ / std::sqrt(2), + my_robot_.segments_[0].d_ - my_robot_.segments_[1].a_ / std::sqrt(2) + + my_robot_.segments_[2].a_ / std::sqrt(2) - my_robot_.segments_[4].d_; + doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-16); + } +} + +TEST_F(CalibrationTest, ur10_fw_kinematics_real) +{ + // This test compares a corrected ideal model against positions taken from a simulated robot. + my_robot_.segments_.clear(); + // This is an ideal UR10 + // clang-format off + my_robot_.segments_.emplace_back(DHSegment(0.1273 , 0 , 0 , pi_2)); + my_robot_.segments_.emplace_back(DHSegment(0 , -0.612 , 0 , 0)); + my_robot_.segments_.emplace_back(DHSegment(0 , -0.5723, 0 , 0.0)); + my_robot_.segments_.emplace_back(DHSegment(0.163941, 0 , 0 , pi_2)); + my_robot_.segments_.emplace_back(DHSegment(0.1157 , 0 , 0 , -pi_2)); + my_robot_.segments_.emplace_back(DHSegment(0.0922 , 0 , 0 , 0)); + // clang-format on + + Calibration calibration(my_robot_); + + Eigen::Matrix jointvalues; + Eigen::Vector3d expected_position; + Eigen::Quaterniond expected_orientation; + { + jointvalues << -1.6007002035724084976209269370884, -1.7271001974688928726209269370884, + -2.2029998938189905288709269370884, -0.80799991289247685699592693708837, 1.59510004520416259765625, + -0.03099996248354131012092693708837; + expected_position << -0.179925914147547, -0.606869755162764, 0.230789102067257; + + Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); + + doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-15); + } + { + jointvalues << 1.32645022869110107421875, 2.426007747650146484375, 5.951572895050048828125, + 1.27409040927886962890625, -0.54105216661562138824592693708837, 0.122173048555850982666015625; + expected_position << 0.39922988003280424074148413637886, 0.59688365069340565405298093537567, + -0.6677819040276375961440180617501; + + Eigen::Matrix4d fk = calibration.calcForwardKinematics(jointvalues); + + doubleEqVec(expected_position, fk.topRightCorner(3, 1), 1e-15); + } + + TearDown(); +} + +TEST_F(CalibrationTest, calibration) +{ + // This test compares the forward kinematics of the model constructed from uncorrected + // parameters with the one from the corrected parameters. They are tested against random + // joint values and should be equal (in a numeric sense). + + my_robot_.segments_.clear(); + + // This is an ideal UR10 + // clang-format off + // d, a, theta, alpha + my_robot_.segments_.emplace_back(DHSegment(0.1273 , 0 , 0 , pi / 2)); + my_robot_.segments_.emplace_back(DHSegment(0 , -0.612 , 0 , 0)); + my_robot_.segments_.emplace_back(DHSegment(0 , -0.5723, 0 , 0.0)); + my_robot_.segments_.emplace_back(DHSegment(0.163941, 0 , 0 , pi / 2)); + my_robot_.segments_.emplace_back(DHSegment(0.1157 , 0 , 0 , -pi / 2)); + my_robot_.segments_.emplace_back(DHSegment(0.0922 , 0 , 0 , 0)); + // clang-format on + my_robot_calibration_.segments_.clear(); + // clang-format off + // d, a, theta, alpha + my_robot_calibration_.segments_.emplace_back(DHSegment( 0.00065609212979853 , 4.6311376834935676e-05 , + -7.290070070824746e-05 , 0.000211987863869334 )); + my_robot_calibration_.segments_.emplace_back(DHSegment( 1.4442162376284788 , -0.00012568315331862312 , + -0.01713897289704999 , -0.0072553625957652995)); + my_robot_calibration_.segments_.emplace_back(DHSegment( 0.854147723854608 , 0.00186216581161458 , + -0.03707159413492756 , -0.013483226769541364 )); + my_robot_calibration_.segments_.emplace_back(DHSegment(-2.2989425877563705 , 9.918593870679266e-05 , + 0.054279462160583214 , 0.0013495820227329425 )); + my_robot_calibration_.segments_.emplace_back(DHSegment(-1.573498686836816e-05 , 4.215462720453189e-06 , + 1.488984257025741e-07 , -0.001263136163679901 )); + my_robot_calibration_.segments_.emplace_back(DHSegment( 1.9072435590711256e-05 , 0 , + 1.551499479707493e-05 , 0 )); + // clang-format on + + Eigen::Matrix jointvalues; + jointvalues << 0, 0, 0, 0, 0, 0; + + for (size_t i = 0; i < 1000; ++i) { + Calibration calibration(my_robot_ + my_robot_calibration_); + jointvalues = 2 * pi * Eigen::Matrix::Random(); + Eigen::Matrix4d fk_orig = calibration.calcForwardKinematics(jointvalues); + Eigen::Matrix3d rot_mat_orig = fk_orig.topLeftCorner(3, 3); + Eigen::Quaterniond rot_orig(rot_mat_orig); + calibration.correctChain(); + Eigen::Matrix4d fk_corrected = calibration.calcForwardKinematics(jointvalues); + Eigen::Matrix3d rot_mat_corrected = fk_corrected.topLeftCorner(3, 3); + Eigen::Quaterniond rot_corrected(rot_mat_corrected); + double angle_error = std::abs(rot_orig.angularDistance(rot_corrected)); + EXPECT_NEAR(fk_orig(0, 3), fk_corrected(0, 3), 1e-12); + EXPECT_NEAR(fk_orig(1, 3), fk_corrected(1, 3), 1e-12); + EXPECT_NEAR(fk_orig(2, 3), fk_corrected(2, 3), 1e-12); + EXPECT_NEAR(angle_error, 0.0, 1e-12); + } +}