Guide - How to use robot_calibration with UR5

CMakeLists.txt

@@ -60,11 +60,14 @@ add_subdirectory(doc/examples/moveit_cpp)
 add_subdirectory(doc/examples/planning_scene_ros_api)
 add_subdirectory(doc/examples/planning_scene)
 add_subdirectory(doc/examples/realtime_servo)
-add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning)
 add_subdirectory(doc/examples/robot_model_and_robot_state)
+
 add_subdirectory(doc/tutorials/pick_and_place_with_moveit_task_constructor)
 add_subdirectory(doc/tutorials/quickstart_in_rviz)
 
+add_subdirectory(doc/how_to_guides/robot_calibration)
+add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning)
+
 ament_export_dependencies(
   ${THIS_PACKAGE_INCLUDE_DEPENDS}
 )

doc/how_to_guides/robot_calibration/CMakeLists.txt

@@ -0,0 +1,9 @@
+# cmake_minimum_required(VERSION 3.5)
+# project(ur5_calibration)
+
+# find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY config launch
+  DESTINATION share/${PROJECT_NAME}/
+)

doc/how_to_guides/robot_calibration/config/calibrate.yaml

@@ -0,0 +1,83 @@
+robot_calibration:
+  ros__parameters:
+    verbose: true
+    base_link: base_link
+    calibration_steps:
+    - single_calibration_step
+    single_calibration_step:
+      models:
+      - ur_manipulator
+      - camera
+      # - ground_camera
+      ur_manipulator:
+        type: chain3d
+        frame: wrist_3_link
+      camera:
+        type: camera3d
+        frame: head_camera_rgb_optical_frame
+        topic: /camera/depth/color/points
+      # ground_camera:
+      #   type: camera3d
+      #   param_name: camera  # this is the same physical camera as above
+      #   frame: head_camera_rgb_optical_frame
+      #   topic: /head_camera/depth_registered/points
+      free_params:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+      - camera_fx
+      - camera_fy
+      - camera_cx
+      - camera_cy
+      - camera_z_offset
+      - camera_z_scaling
+      free_frames:
+      # - head_camera_rgb_joint
+      # - head_pan_joint
+      - checkerboard
+      # head_camera_rgb_joint:
+      #   x: true
+      #   y: true
+      #   z: true
+      #   roll: true
+      #   pitch: true
+      #   yaw: true
+      # head_pan_joint:
+      #   x: true
+      #   y: true
+      #   z: true
+      #   roll: true
+      #   pitch: true
+      #   yaw: true
+      checkerboard:
+        x: true
+        y: false
+        z: true
+        roll: false
+        pitch: true
+        yaw: false
+      error_blocks:
+      - hand_eye
+      - restrict_camera
+      hand_eye:
+        type: chain3d_to_chain3d
+        model_a: camera
+        model_b: ur_manipulator
+      # ground_plane:
+      #   type: chain3d_to_plane
+      #   model: ground_camera
+      #   a: 0.0
+      #   b: 0.0
+      #   c: 1.0
+      #   d: 0.0
+      #   scale: 1.0
+      restrict_camera:
+        type: outrageous
+        param: head_camera_rgb_joint
+        joint_scale: 0.0
+        position_scale: 0.1
+        rotation_scale: 0.1
+

doc/how_to_guides/robot_calibration/config/calibration_poses.yaml

@@ -0,0 +1,49 @@
+- features:
+  - checkerboard_finder
+  joints:
+  - shoulder_pan_joint
+  - shoulder_lift_joint
+  - elbow_joint
+  - wrist_1_joint
+  - wrist_2_joint
+  - wrist_3_joint
+  positions:
+  - 0.0
+  - -1.5708
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+- features:
+  - checkerboard_finder
+  joints:
+  - shoulder_pan_joint
+  - shoulder_lift_joint
+  - elbow_joint
+  - wrist_1_joint
+  - wrist_2_joint
+  - wrist_3_joint
+  positions:
+  - 0.0
+  - -2.04204
+  - 1.15192
+  - 0.401426
+  - -0.0
+  - 1.06465
+- features:
+  - checkerboard_finder
+  joints:
+  - shoulder_pan_joint
+  - shoulder_lift_joint
+  - elbow_joint
+  - wrist_1_joint
+  - wrist_2_joint
+  - wrist_3_joint
+  positions:
+  - 0.0
+  - -1.5708
+  - 0.0
+  - -1.5708
+  - 0.0
+  - 0.0
+

doc/how_to_guides/robot_calibration/config/capture.yaml

@@ -0,0 +1,30 @@
+robot_calibration:
+  ros__parameters:
+    chains:
+    - ur_manipulator
+    ur_manipulator:
+      topic: /scaled_joint_trajectory_controller/follow_joint_trajectory
+      joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+      #planning_group: arm TODO: revert when moveit is setup
+    features:
+    - checkerboard_finder
+    # - ground_plane_finder
+    checkerboard_finder:
+      type: robot_calibration::CheckerboardFinder
+      topic: /camera/depth/color/points
+      camera_sensor_name: camera
+      chain_sensor_name: ur_manipulator
+      camera_driver: /camera/depth/camera_info
+
+    # ground_plane_finder:
+    #   type: robot_calibration::PlaneFinder
+    #   topic: /head_camera/depth_registered/points
+    #   camera_sensor_name: ground_camera
+    #   points_max: 60
+    #   debug: false

doc/how_to_guides/robot_calibration/launch/calibrate.launch.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2020-2022, Michael Ferguson
+# All rights reserved.
+#
+# Software License Agreement (BSD License 2.0)
+#
+# 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 OWNER 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.
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription, LaunchService
+from launch.actions import ExecuteProcess
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # Load the capture config
+    capture_config = os.path.join(
+        get_package_share_directory('ur5_calibration'),
+        'config',
+        'capture.yaml'
+    )
+
+    # Load the calibration config
+    calibration_config = os.path.join(
+        get_package_share_directory('ur5_calibration'),
+        'config',
+        'calibrate.yaml'
+    )
+
+    # Load the calibration poses YAML
+    calibration_poses = os.path.join(
+        get_package_share_directory('ur5_calibration'),
+        'config',
+        'calibration_poses.yaml'
+    )
+
+    # Make a directory for bagfiles to be located
+    # try:
+    #     os.mkdir("/tmp/ubr1_calibration")
+    # except FileExistsError:
+    #     pass
+
+    return LaunchDescription([
+        # Calibration
+        Node(
+            name='robot_calibration',
+            package='robot_calibration',
+            executable='calibrate',
+            arguments=[calibration_poses],
+            parameters=[capture_config,
+                        calibration_config],
+            output='screen',
+        ),
+        # Record bagfile for debugging
+        # ExecuteProcess(
+        #     cmd=["ros2", "bag", "record", "/calibration_data", "/robot_description"],
+        #     cwd="/tmp/ur5_calibration"
+        # )
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return ls.run()
+
+
+if __name__ == '__main__':
+    main()