diff --git a/CMakeLists.txt b/CMakeLists.txt index 829905f9ef..c02817523b 100644 --- a/CMakeLists.txt +++ b/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} ) diff --git a/doc/how_to_guides/robot_calibration/CMakeLists.txt b/doc/how_to_guides/robot_calibration/CMakeLists.txt new file mode 100644 index 0000000000..5bf6a7f19d --- /dev/null +++ b/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}/ +) diff --git a/doc/how_to_guides/robot_calibration/config/calibrate.yaml b/doc/how_to_guides/robot_calibration/config/calibrate.yaml new file mode 100644 index 0000000000..f0dde02ae7 --- /dev/null +++ b/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 + \ No newline at end of file diff --git a/doc/how_to_guides/robot_calibration/config/calibration_poses.yaml b/doc/how_to_guides/robot_calibration/config/calibration_poses.yaml new file mode 100644 index 0000000000..a32d3b7314 --- /dev/null +++ b/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 + \ No newline at end of file diff --git a/doc/how_to_guides/robot_calibration/config/capture.yaml b/doc/how_to_guides/robot_calibration/config/capture.yaml new file mode 100644 index 0000000000..8a134deb79 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/doc/how_to_guides/robot_calibration/launch/calibrate.launch.py b/doc/how_to_guides/robot_calibration/launch/calibrate.launch.py new file mode 100644 index 0000000000..b403e98346 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/doc/how_to_guides/robot_calibration/launch/ur5.launch.py b/doc/how_to_guides/robot_calibration/launch/ur5.launch.py new file mode 100644 index 0000000000..b8d3af17ed --- /dev/null +++ b/doc/how_to_guides/robot_calibration/launch/ur5.launch.py @@ -0,0 +1,375 @@ +# -*- coding: utf-8 -*- +import os +import yaml +import math +from launch import LaunchDescription +from launch.actions import ( + RegisterEventHandler, + DeclareLaunchArgument, +) +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.actions import ExecuteProcess +from launch.event_handlers import OnProcessExit +from ament_index_python.packages import get_package_share_directory + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + + +def generate_launch_description(): + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + default_value="", + description="IP address of the robot" + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="true", + description="Use real hardware or work in sim" + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "fake_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + + robot_ip = LaunchConfiguration("robot_ip") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + prefix = LaunchConfiguration("prefix") + + joint_limit_params = PathJoinSubstitution( + [FindPackageShare("ur_description"), "config", "ur5", "joint_limits.yaml"] + ) + kinematics_params = PathJoinSubstitution( + [FindPackageShare("ur_description"), "config", "ur5", "default_kinematics.yaml"] + ) + physical_params = PathJoinSubstitution( + [FindPackageShare("ur_description"), "config", "ur5", "physical_parameters.yaml"] + ) + visual_params = PathJoinSubstitution( + [FindPackageShare("ur_description"), "config", "ur5", "visual_parameters.yaml"] + ) + script_filename = PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "resources", "ros_control.urscript"] + ) + input_recipe_filename = PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "resources", "rtde_input_recipe.txt"] + ) + output_recipe_filename = PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "resources", "rtde_output_recipe.txt"] + ) + + # planning context + # robot_description_content = Command( + # [ + # PathJoinSubstitution([FindExecutable(name="xacro")]), + # " ", + # PathJoinSubstitution([FindPackageShare("ur_description"), "urdf", "ur.urdf.xacro"]), + # " ", + # "robot_ip:=", + # robot_ip, + # " ", + # "joint_limit_params:=", + # joint_limit_params, + # " ", + # "kinematics_params:=", + # kinematics_params, + # " ", + # "physical_params:=", + # physical_params, + # " ", + # "visual_params:=", + # visual_params, + # " ", + # "name:=", + # "ur5", + # " ", + # "script_filename:=", + # script_filename, + # " ", + # "input_recipe_filename:=", + # input_recipe_filename, + # " ", + # "output_recipe_filename:=", + # output_recipe_filename, + # " ", + # "prefix:=", + # prefix, + # " ", + # "use_fake_hardware:=", + # use_fake_hardware, + # " ", + # "fake_sensor_commands:=", + # fake_sensor_commands, + # " ", + # ] + # ) + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution([FindPackageShare("ur_description"), "urdf", "ur.urdf.xacro"]), + " ", + "robot_ip:=xxx.yyy.zzz.www", + " ", + "joint_limit_params:=", + joint_limit_params, + " ", + "kinematics_params:=", + kinematics_params, + " ", + "physical_params:=", + physical_params, + " ", + "visual_params:=", + visual_params, + # " ", + # "safety_limits:=", + # safety_limits, + # " ", + # "safety_pos_margin:=", + # safety_pos_margin, + # " ", + # "safety_k_position:=", + # safety_k_position, + " ", + "name:=", + "ur", + " ", + "script_filename:=", + script_filename, + " ", + "input_recipe_filename:=", + input_recipe_filename, + " ", + "output_recipe_filename:=", + output_recipe_filename, + " ", + "use_fake_hardware:=", + use_fake_hardware, + " ", + ] + ) + robot_description = {"robot_description": robot_description_content} + + # MoveIt Configuration + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("ur_moveit_config"), "srdf", "ur.srdf.xacro"] + ), + " ", + "name:=", + # Also ur_type parameter could be used but then the planning group names in yaml + # configs has to be updated! + "ur", + " ", + "prefix:=", + prefix, + " ", + ] + ) + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content} + + joint_limits_yaml = load_yaml("ur_description", "config/ur5/joint_limits.yaml") + robot_description_planning = {"robot_description_planning": joint_limits_yaml} + + kinematics_yaml = PathJoinSubstitution( + [FindPackageShare("ur_moveit_config"), "config", "kinematics.yaml"] + ) + + # Planning Functionality + ompl_planning_yaml = load_yaml( + "ur_moveit_config", "config/ompl_planning.yaml" + ) + ompl_planning_pipeline_config = {"move_group": ompl_planning_yaml} + + # Trajectory Execution Functionality + moveit_simple_controllers_yaml = load_yaml( + "ur_moveit_config", "config/controllers.yaml" + ) + moveit_controllers = { + "moveit_simple_controller_manager": moveit_simple_controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } + + trajectory_execution = { + "moveit_manage_controllers": False, + "trajectory_execution.allowed_execution_duration_scaling": 1.2, + "trajectory_execution.allowed_goal_duration_margin": 0.5, + "trajectory_execution.allowed_start_tolerance": 0.01, + } + + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + } + + # Start the actual move_group node/action server + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + robot_description_planning, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + {"use_sim_time": True} + ], + ) + + # RViz + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("ur_moveit_config"), "rviz", "view_robot.rviz"] + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + robot_description, + robot_description_semantic, + robot_description_planning, + ompl_planning_pipeline_config, + kinematics_yaml, + ], + ) + + # Static TF to publish dummy world-base_link transform + # static_tf_odom = Node( + # package="tf2_ros", + # executable="static_transform_publisher", + # name="static_transform_publisher", + # output="log", + # arguments=[ + # "0.0", + # "0.0", + # "0.0", + # "0.0", + # "0.0", + # "0.0", + # "world", + # "base_link", + # ], + # ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + ros2_controllers_path = PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "config", "ur_controllers.yaml"] + ) + + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_controllers_path], + prefix='xterm -e gdb --ex=run --args', + output="both", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager", + ], + ) + + speed_scaling_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["speed_scaling_state_broadcaster", "-c", "/controller_manager"], + ) + + scaled_joint_trajectory_contoller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "scaled_joint_trajectory_controller", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager"], + ) + + nodes_to_start = [ + rviz_node, + # static_tf_odom, + robot_state_publisher, + run_move_group_node, + ros2_control_node, + joint_state_broadcaster_spawner, + speed_scaling_state_broadcaster_spawner, + scaled_joint_trajectory_contoller_spawner, + + ] + return LaunchDescription(declared_arguments + nodes_to_start) \ No newline at end of file diff --git a/doc/how_to_guides/robot_calibration/package.xml b/doc/how_to_guides/robot_calibration/package.xml new file mode 100644 index 0000000000..988c755ff7 --- /dev/null +++ b/doc/how_to_guides/robot_calibration/package.xml @@ -0,0 +1,23 @@ + + + + ur5_calibration + 0.1.0 + + Launch and configuration files for calibrating UR5 using the 'robot_calibration' package. + + + Abi Sivaraman + Michael Ferguson + + BSD + + ament_cmake + + robot_calibration + + + ament_cmake + + + \ No newline at end of file