Add ur_calibration package (#451)

Co-authored-by: Mads Holm Peters <[email protected]>

Author: Felix Exner

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.

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()

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:=<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 <colcon_ws>/src
+$ ros2 pkg  create <organization_name>_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 <organization_name>_ur_launch/etc
+$ mkdir -p <organization_name>_ur_launch/launch
+$ echo 'install(DIRECTORY etc launch DESTINATION share/${PROJECT_NAME})' >> <organization_name>_ur_launch/CMakeLists.txt
+$ colcon build --packages-select <organization_name>_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:=<robot_ip> \
+target_filename:="$(ros2 pkg prefix <organization_name>_ur_launch)/share/<organization_name>_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 <colcon_ws>/src/<organization_name>_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("<organization_name>_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_ws>
+$ colcon build --packages-select <organization_name>_ur_launch
+$ ros2 launch <organization_name>_ur_launch ex-ur10-1.launch.py
+robot_ip:=xxx.yyy.zzz.www ur_type:=ur5e  use_fake_hardware:=false launch_rviz:=true
+```