From 89381ab0d2bd500e80cb58882a01912fcea40146 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 21 Oct 2021 17:51:37 +0200 Subject: [PATCH 1/7] Add repos file for admittance controller --- admittance_controller.repos | 45 +++++++++++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 admittance_controller.repos diff --git a/admittance_controller.repos b/admittance_controller.repos new file mode 100644 index 000000000..dda81cad3 --- /dev/null +++ b/admittance_controller.repos @@ -0,0 +1,45 @@ +repositories: + control_msgs: # Branch: rolling/add-addmittance-controller-state + type: git + url: https://github.com/nbbrooks/control_msgs.git + version: 81419078f8544e9e320f3405a4fa5d3a7ba3dc1e # Add calculation values of the admittance rule + + geometry2: # Branch: twist_wrench_transform_foxy + type: git + url: https://github.com/JafarAbdi/geometry2.git + version: 2888c23b97cb76674129314cc648b5b355bda3f4 # Add tomsg conversion for geometry_msgs/Transform + + iirob_filters: # Branch: ros2 + type: git + url: https://github.com/nbbrooks/iirob_filters.git + version: d0c9a21672264ae9506686de9ff928e6ada96f15 # Changes for Galactic/Rolling + + ros2_control: + type: git + url: https://github.com/destogl/ros2_control.git + version: admittance-controller-development + + ros2_controllers: + type: git + url: https://github.com/destogl/ros2_controllers.git + version: add-admittance-controller + + teleop_twist_keyboard: # Branch: wrench + type: git + url: https://github.com/nbbrooks/teleop_twist_keyboard.git + version: 63de1c17c4c5d0724018f3b5fd8139c472cd15ab # Add Float64MultiArray publisher + + Universal_Robots_Client_Library: # Branch: master + type: git + url: https://github.com/UniversalRobots/Universal_Robots_Client_Library.git + version: 90338a76d65d86240710c8f976fd40296a49cd84 # Merge pull request #85 from jornb/feat/target_frequency + + Universal_Robots_ROS2_Driver: # Branch: main + type: git + url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git + version: 07690bfa3167a58df6e8ba94623f92153d158043 # Driver to headless. (#217) + + ur_msgs: Branch: ros2 + type: git + url: https://github.com/destogl/ur_msgs.git + version: d72e82719b72deef6a59ea50e5295817d0b4f9f6 # Corrected/updated dependecies From 877814066a9885cb1556cc0828f22584bdd71a20 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 21 Oct 2021 19:14:30 +0200 Subject: [PATCH 2/7] Adding initial configuration for admittance_controller demo. --- admittance_controller.repos | 8 +- .../config/admittance_demo_controllers.yaml | 231 +++++++++++ .../admittance_controller_demo.launch.py | 374 ++++++++++++++++++ .../joint_limits_admittance.yaml | 54 +++ using_admittance_controller.md | 25 ++ 5 files changed, 691 insertions(+), 1 deletion(-) create mode 100644 ros2_control_demo_bringup/config/admittance_demo_controllers.yaml create mode 100644 ros2_control_demo_bringup/launch/admittance_controller_demo.launch.py create mode 100644 ros2_control_demo_description/diffbot_description/admittance_demo/joint_limits_admittance.yaml create mode 100644 using_admittance_controller.md diff --git a/admittance_controller.repos b/admittance_controller.repos index dda81cad3..ea6c03293 100644 --- a/admittance_controller.repos +++ b/admittance_controller.repos @@ -1,4 +1,5 @@ repositories: + control_msgs: # Branch: rolling/add-addmittance-controller-state type: git url: https://github.com/nbbrooks/control_msgs.git @@ -14,6 +15,11 @@ repositories: url: https://github.com/nbbrooks/iirob_filters.git version: d0c9a21672264ae9506686de9ff928e6ada96f15 # Changes for Galactic/Rolling + rosparam_shortcuts: # ros2 + type: git + url: https://github.com/PickNikRobotics/rosparam_shortcuts.git + version: b3ffe9061cf01d0acff07967d10b80c10447a618 # Create CI job for ros2 branch (#24) + ros2_control: type: git url: https://github.com/destogl/ros2_control.git @@ -39,7 +45,7 @@ repositories: url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git version: 07690bfa3167a58df6e8ba94623f92153d158043 # Driver to headless. (#217) - ur_msgs: Branch: ros2 + ur_msgs: # Branch: ros2 type: git url: https://github.com/destogl/ur_msgs.git version: d72e82719b72deef6a59ea50e5295817d0b4f9f6 # Corrected/updated dependecies diff --git a/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml b/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml new file mode 100644 index 000000000..702d9d60d --- /dev/null +++ b/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml @@ -0,0 +1,231 @@ +controller_manager: + ros__parameters: + update_rate: 600 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + io_and_status_controller: + type: ur_controllers/GPIOController + + speed_scaling_state_broadcaster: + type: ur_controllers/SpeedScalingStateBroadcaster + + force_torque_sensor_broadcaster: + type: ur_controllers/ForceTorqueStateBroadcaster + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + scaled_joint_trajectory_controller: + type: ur_controllers/ScaledJointTrajectoryController + + forward_velocity_controller: + type: velocity_controllers/JointGroupVelocityController + + admittance_controller: + type: admittance_controller/AdmittanceController + + faked_forces_controller: + type: forward_command_controller/MultiInterfaceForwardController + + +speed_scaling_state_broadcaster: + ros__parameters: + state_publish_rate: 100.0 + + +force_torque_sensor_broadcaster: + ros__parameters: + sensor_name: tcp_fts_sensor + state_interface_names: + - force.x + - force.y + - force.z + - torque.x + - torque.y + - torque.z + frame_id: tool0 + topic_name: ft_data + + +joint_trajectory_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.2 + goal_time: 0.0 + shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + elbow_joint: { trajectory: 0.2, goal: 0.1 } + wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + + +scaled_joint_trajectory_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.2 + goal_time: 0.0 + shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + elbow_joint: { trajectory: 0.2, goal: 0.1 } + wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + + +forward_velocity_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + interface_name: velocity + + +admittance_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + + command_interfaces: + - position + - velocity + + state_interfaces: + - position + + ft_sensor_name: gemini_6dof + use_joint_commands_as_input: true + joint_limiter_type: "joint_limits/SimpleJointLimiter" + + state_publish_rate: 200.0 # Defaults to 50 + action_monitor_rate: 20.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) + + IK: + base: base_link # Assumed to be stationary + tip: tool0 + group_name: gemini + + endeffector_frame: tool0 + control_frame: end_effector # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + fixed_world_frame: world # Gravity points down (neg. Z) in this frame + sensor_frame: force_sensor # Wrench measurements are in this frame + + admittance: + selected_axes: + x: true + y: true + z: true + rx: true + ry: true + rz: true + + # Having ".0" at the end is MUST, otherwise there is a loading error + # F = M*a + D*v + S*(x - x_d) + mass: + x: 100.0 + y: 100.0 + z: 100.0 + rx: 100.0 + ry: 100.0 + rz: 100.0 + + damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S )) + x: 2.828427 + y: 2.828427 + z: 2.828427 + rx: 2.23607 + ry: 2.23607 + rz: 2.23607 + + stiffness: + x: 5000.0 + y: 5000.0 + z: 5000.0 + rx: 500.0 + ry: 500.0 + rz: 500.0 + + input_wrench_filter_chain: + filter1: + type: iirob_filters/GravityCompensatorWrench + name: wrist_gravity_compensation + params: + CoG_x: 0.0 + CoG_y: 0.0 + CoG_z: 0.112 + force: 232.26 # mass * 9.81 + force_frame: force_sensor + sensor_frame: force_sensor + world_frame: world + + filter2: + type: iirob_filters/GravityCompensatorWrench + name: tool_gravity_compensation + params: + CoG_x: 0.0 + CoG_y: 0.0 + CoG_z: 0.2 + force: 0.0 # mass * 9.81 + force_frame: tool0 + sensor_frame: force_sensor + world_frame: world + + +faked_forces_controller: + ros__parameters: + joint: tcp_fts_sensor + interface_names: + - force.x + - force.y + - force.z + - torque.x + - torque.y + - torque.z diff --git a/ros2_control_demo_bringup/launch/admittance_controller_demo.launch.py b/ros2_control_demo_bringup/launch/admittance_controller_demo.launch.py new file mode 100644 index 000000000..d10bc914f --- /dev/null +++ b/ros2_control_demo_bringup/launch/admittance_controller_demo.launch.py @@ -0,0 +1,374 @@ + +# Copyright (c) 2021 PickNik, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Denis Stogl + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + declared_arguments = [] + # UR specific arguments + declared_arguments.append( + DeclareLaunchArgument( + "ur_type", + description="Type/series of used UR robot.", + choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"], + default_value="ur5e", + ) + ) + # TODO(anyone): enable this when added into ROS2-foxy + # choices=['ur3', 'ur3e', 'ur5', 'ur5e', 'ur10', 'ur10e', 'ur16e'])) + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", + description="IP address by which the robot can be reached.", + default_value: "xxx.xxx.xxx.xxx", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_limits", + default_value="true", + description="Enables the safety limits controller if true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_pos_margin", + default_value="0.15", + description="The margin to lower and upper limits in the safety controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_k_position", + default_value="20", + description="k-position factor in the safety controller.", + ) + ) + # General arguments + declared_arguments.append( + DeclareLaunchArgument( + "runtime_config_package", + default_value="ur_bringup", + description='Package with the controller\'s configuration in "config" folder. \ + Usually the argument is not set, it enables use of a custom setup.', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="ur_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="ur_description", + description="Description package with robot URDF/XACRO files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="ur.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + 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.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="true", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "fake_sensor_commands", + default_value="true", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "headless_mode", + default_value="false", + description="Enable headless mode for robot control", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "start_joint_controller", + default_value="true", + description="Enable headless mode for robot control", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="joint_trajectory_controller", + description="Robot controller to start.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") + ) + declared_arguments.append( + DeclareLaunchArgument( + "launch_dashboard_client", default_value="true", description="Launch RViz?" + ) + ) + + # Initialize Arguments + ur_type = LaunchConfiguration("ur_type") + robot_ip = LaunchConfiguration("robot_ip") + safety_limits = LaunchConfiguration("safety_limits") + safety_pos_margin = LaunchConfiguration("safety_pos_margin") + safety_k_position = LaunchConfiguration("safety_k_position") + # General arguments + runtime_config_package = LaunchConfiguration("runtime_config_package") + controllers_file = LaunchConfiguration("controllers_file") + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + prefix = LaunchConfiguration("prefix") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + start_joint_controller = LaunchConfiguration("start_joint_controller") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + launch_rviz = LaunchConfiguration("launch_rviz") + headless_mode = LaunchConfiguration("headless_mode") + launch_dashboard_client = LaunchConfiguration("launch_dashboard_client") + + joint_limit_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "joint_limits.yaml"] + ) + kinematics_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "default_kinematics.yaml"] + ) + physical_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "physical_parameters.yaml"] + ) + visual_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "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"] + ) + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]), + " ", + "robot_ip:=", + robot_ip, + " ", + "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_type, + " ", + "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, + " ", + "headless_mode:=", + headless_mode, + " ", + ] + ) + robot_description = {"robot_description": robot_description_content} + + initial_joint_controllers = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", controllers_file] + ) + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rviz", "view_robot.rviz"] + ) + + joint_limits_admittance = PathJoinSubstitution( + [FindPackageShare("diffbot_description"), "admittance_demo", "joint_limits_admittance.yaml"] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, initial_joint_controllers, joint_limits_admittance], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + dashboard_client_node = Node( + package="ur_robot_driver", + condition=IfCondition(launch_dashboard_client), + executable="dashboard_client", + name="dashboard_client", + output="screen", + emulate_tty=True, + parameters=[{"robot_ip": robot_ip}], + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + rviz_node = Node( + package="rviz2", + condition=IfCondition(launch_rviz), + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + io_and_status_controller_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["io_and_status_controller", "-c", "/controller_manager"], + ) + + speed_scaling_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=[ + "speed_scaling_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + force_torque_sensor_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=[ + "force_torque_sensor_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + admittance_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["admittance_controller", "-c", "/controller_manager", "--stopped"], + ) + + faked_forces_controller_spawner = Node( + package="controller_manager", + executable="spawner", + condition=IfCondition(fake_sensor_commands), + arguments=["faked_forces_controller", "-c", "/controller_manager"], + ) + + # There may be other controllers of the joints, but this is the initially-started one + initial_joint_controller_spawner_started = Node( + package="controller_manager", + executable="spawner.py", + arguments=[initial_joint_controller, "-c", "/controller_manager"], + condition=IfCondition(start_joint_controller), + ) + initial_joint_controller_spawner_stopped = Node( + package="controller_manager", + executable="spawner.py", + arguments=[initial_joint_controller, "-c", "/controller_manager", "--stopped"], + condition=UnlessCondition(start_joint_controller), + ) + + nodes_to_start = [ + control_node, + dashboard_client_node, + robot_state_publisher_node, + rviz_node, + joint_state_broadcaster_spawner, + io_and_status_controller_spawner, + speed_scaling_state_broadcaster_spawner, + force_torque_sensor_broadcaster_spawner, + admittance_controller_spawner, + faked_forces_controller_spawner, + initial_joint_controller_spawner_stopped, + initial_joint_controller_spawner_started, + ] + + return LaunchDescription(declared_arguments + nodes_to_start) diff --git a/ros2_control_demo_description/diffbot_description/admittance_demo/joint_limits_admittance.yaml b/ros2_control_demo_description/diffbot_description/admittance_demo/joint_limits_admittance.yaml new file mode 100644 index 000000000..8588baf49 --- /dev/null +++ b/ros2_control_demo_description/diffbot_description/admittance_demo/joint_limits_admittance.yaml @@ -0,0 +1,54 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +/admittance_controller: + ros__parameters: + joint_limits: + shoulder_pan_joint: + has_position_limits: true + min_position: -1.0471975511965976 + max_position: 1.0471975511965976 + has_velocity_limits: true + max_velocity: 0.35 + has_acceleration_limits: true + max_acceleration: 1.0 + shoulder_lift_joint: + has_position_limits: true + min_position: -0.5916666164260777 + max_position: 1.7819811662862104 + has_velocity_limits: true + max_velocity: 0.35 + has_acceleration_limits: true + max_acceleration: 1.0 + elbow_joint: + has_position_limits: true + min_position: -2.3125612588924866 + max_position: 0.0 + has_velocity_limits: true + max_velocity: 0.35 + has_acceleration_limits: true + max_acceleration: 4.0 + wrist_1_joint: + has_position_limits: true + min_position: -1.0471975511965976 + max_position: 1.0471975511965976 + has_velocity_limits: true + max_velocity: 1.0 + has_acceleration_limits: true + max_acceleration: 4.0 + wrist_2_joint: + has_position_limits: true + min_position: -0.5235987755982988 + max_position: 1.5707963267948966 + has_velocity_limits: true + max_velocity: 1.0 + has_acceleration_limits: true + max_acceleration: 4.0 + wrist_3_joint: + has_position_limits: false + min_position: -6.283185307179586 + max_position: 6.283185307179586 + has_velocity_limits: true + max_velocity: 8.0 + has_acceleration_limits: true + max_acceleration: 8.0 diff --git a/using_admittance_controller.md b/using_admittance_controller.md new file mode 100644 index 000000000..fc9232157 --- /dev/null +++ b/using_admittance_controller.md @@ -0,0 +1,25 @@ +# Using admittance controller + +This manual targets ROS2 rolling. + +1. Create a new ROS workspace and compile it with ROS2 rolling. +1. Install some dependencies + ``` + sudo apt-get install python3-colcon-common-extensions python3-rosdep python3-vcstool + sudo rosdep init + rosdep update + ``` +1. Checkout the repositories from `admittance_controller.repos` file: + ``` + wget https://raw.githubusercontent.com/destogl/ros2_control_demos/admittance-controller-setup/admittance_controller.repos + vcs import --input admittance_controller.repos . + rosdep install --from-paths . -y -i + ``` + +1. Compile your workspace using `colcon build`. + NOTE: Maybe you get some error message in output. Please ignore those until we fix the dependencies. + +1. Start the demo using: + ``` + TBD + ``` From 059cd0915720712401b7064fc4db192212a34841 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 25 Oct 2021 19:13:03 +0200 Subject: [PATCH 3/7] Using fake system is working --- admittance_controller.repos | 21 +- .../config/admittance_demo_controllers.yaml | 58 ++-- .../admittance_controller_demo.launch.py | 91 ++++-- .../rrbot_description/CMakeLists.txt | 2 +- .../admittance_demo/admittance_demo.rviz | 269 ++++++++++++++++++ .../admittance_demo/initial_positions.yaml | 6 + .../joint_limits_admittance.yaml | 34 +-- using_admittance_controller.md | 22 +- 8 files changed, 416 insertions(+), 87 deletions(-) create mode 100644 ros2_control_demo_description/rrbot_description/admittance_demo/admittance_demo.rviz create mode 100644 ros2_control_demo_description/rrbot_description/admittance_demo/initial_positions.yaml rename ros2_control_demo_description/{diffbot_description => rrbot_description}/admittance_demo/joint_limits_admittance.yaml (68%) diff --git a/admittance_controller.repos b/admittance_controller.repos index ea6c03293..dea9d4a4c 100644 --- a/admittance_controller.repos +++ b/admittance_controller.repos @@ -15,20 +15,25 @@ repositories: url: https://github.com/nbbrooks/iirob_filters.git version: d0c9a21672264ae9506686de9ff928e6ada96f15 # Changes for Galactic/Rolling - rosparam_shortcuts: # ros2 - type: git - url: https://github.com/PickNikRobotics/rosparam_shortcuts.git - version: b3ffe9061cf01d0acff07967d10b80c10447a618 # Create CI job for ros2 branch (#24) - ros2_control: type: git url: https://github.com/destogl/ros2_control.git version: admittance-controller-development + ros2_control_demos: + type: git + url: https://github.com/destogl/ros2_control_demos.git + version: admittance-controller-setup + ros2_controllers: type: git url: https://github.com/destogl/ros2_controllers.git - version: add-admittance-controller + version: admittance-controller-demo + + rosparam_shortcuts: # ros2 + type: git + url: https://github.com/PickNikRobotics/rosparam_shortcuts.git + version: b3ffe9061cf01d0acff07967d10b80c10447a618 # Create CI job for ros2 branch (#24) teleop_twist_keyboard: # Branch: wrench type: git @@ -43,9 +48,9 @@ repositories: Universal_Robots_ROS2_Driver: # Branch: main type: git url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git - version: 07690bfa3167a58df6e8ba94623f92153d158043 # Driver to headless. (#217) + version: ebe29b1a270544a44765a3750d3747576ec6d598 # Add missing command and state interfaces to get everything working with the fake hardware and add some comment into xacro file to be clearer. ur_msgs: # Branch: ros2 type: git url: https://github.com/destogl/ur_msgs.git - version: d72e82719b72deef6a59ea50e5295817d0b4f9f6 # Corrected/updated dependecies + version: d72e82719b72deef6a59ea50e5295817d0b4f9f6 # Corrected/updated dependencies diff --git a/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml b/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml index 702d9d60d..c35505459 100644 --- a/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml +++ b/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml @@ -27,7 +27,7 @@ controller_manager: type: admittance_controller/AdmittanceController faked_forces_controller: - type: forward_command_controller/MultiInterfaceForwardController + type: forward_command_controller/MultiInterfaceForwardCommandController speed_scaling_state_broadcaster: @@ -129,14 +129,14 @@ admittance_controller: command_interfaces: - position - - velocity state_interfaces: - position + - velocity - ft_sensor_name: gemini_6dof + ft_sensor_name: tcp_fts_sensor use_joint_commands_as_input: true - joint_limiter_type: "joint_limits/SimpleJointLimiter" + #joint_limiter_type: "joint_limits/SimpleJointLimiter" state_publish_rate: 200.0 # Defaults to 50 action_monitor_rate: 20.0 # Defaults to 20 @@ -151,12 +151,12 @@ admittance_controller: IK: base: base_link # Assumed to be stationary tip: tool0 - group_name: gemini + group_name: ur5e_manipulator endeffector_frame: tool0 - control_frame: end_effector # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector - fixed_world_frame: world # Gravity points down (neg. Z) in this frame - sensor_frame: force_sensor # Wrench measurements are in this frame + control_frame: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector + fixed_world_frame: base_link # Gravity points down (neg. Z) in this frame (Usually: world or base_link) + sensor_frame: tool0 # Wrench measurements are in this frame admittance: selected_axes: @@ -170,12 +170,12 @@ admittance_controller: # Having ".0" at the end is MUST, otherwise there is a loading error # F = M*a + D*v + S*(x - x_d) mass: - x: 100.0 - y: 100.0 - z: 100.0 - rx: 100.0 - ry: 100.0 - rz: 100.0 + x: 10.0 + y: 10.0 + z: 10.0 + rx: 10.0 + ry: 10.0 + rz: 10.0 damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S )) x: 2.828427 @@ -186,12 +186,12 @@ admittance_controller: rz: 2.23607 stiffness: - x: 5000.0 - y: 5000.0 - z: 5000.0 - rx: 500.0 - ry: 500.0 - rz: 500.0 + x: 50.0 + y: 50.0 + z: 50.0 + rx: 50.0 + ry: 50.0 + rz: 50.0 input_wrench_filter_chain: filter1: @@ -200,11 +200,11 @@ admittance_controller: params: CoG_x: 0.0 CoG_y: 0.0 - CoG_z: 0.112 - force: 232.26 # mass * 9.81 - force_frame: force_sensor - sensor_frame: force_sensor - world_frame: world + CoG_z: 0.0 + force: 0.0 # mass * 9.81 + force_frame: wrist_3_link + sensor_frame: tool0 + world_frame: base_link filter2: type: iirob_filters/GravityCompensatorWrench @@ -212,11 +212,11 @@ admittance_controller: params: CoG_x: 0.0 CoG_y: 0.0 - CoG_z: 0.2 + CoG_z: 0.0 force: 0.0 # mass * 9.81 - force_frame: tool0 - sensor_frame: force_sensor - world_frame: world + force_frame: wrist_3_link + sensor_frame: tool0 + world_frame: base_link faked_forces_controller: diff --git a/ros2_control_demo_bringup/launch/admittance_controller_demo.launch.py b/ros2_control_demo_bringup/launch/admittance_controller_demo.launch.py index d10bc914f..5498bb4ff 100644 --- a/ros2_control_demo_bringup/launch/admittance_controller_demo.launch.py +++ b/ros2_control_demo_bringup/launch/admittance_controller_demo.launch.py @@ -1,4 +1,3 @@ - # Copyright (c) 2021 PickNik, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -17,7 +16,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import IfCondition from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -40,7 +39,7 @@ def generate_launch_description(): DeclareLaunchArgument( "robot_ip", description="IP address by which the robot can be reached.", - default_value: "xxx.xxx.xxx.xxx", + default_value="xxx.xxx.xxx.xxx", ) ) declared_arguments.append( @@ -68,7 +67,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "runtime_config_package", - default_value="ur_bringup", + default_value="ros2_control_demo_bringup", description='Package with the controller\'s configuration in "config" folder. \ Usually the argument is not set, it enables use of a custom setup.', ) @@ -76,7 +75,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="ur_controllers.yaml", + default_value="admittance_demo_controllers.yaml", description="YAML file with the controllers configuration.", ) ) @@ -104,6 +103,14 @@ def generate_launch_description(): have to be updated.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_config_package", + default_value="ur_moveit_config", + description="Description package with robot moveit config files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) declared_arguments.append( DeclareLaunchArgument( "use_fake_hardware", @@ -160,11 +167,12 @@ def generate_launch_description(): controllers_file = LaunchConfiguration("controllers_file") description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") + moveit_config_package = LaunchConfiguration("moveit_config_package") prefix = LaunchConfiguration("prefix") use_fake_hardware = LaunchConfiguration("use_fake_hardware") fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") - start_joint_controller = LaunchConfiguration("start_joint_controller") - initial_joint_controller = LaunchConfiguration("initial_joint_controller") + # start_joint_controller = LaunchConfiguration("start_joint_controller") + # initial_joint_controller = LaunchConfiguration("initial_joint_controller") launch_rviz = LaunchConfiguration("launch_rviz") headless_mode = LaunchConfiguration("headless_mode") launch_dashboard_client = LaunchConfiguration("launch_dashboard_client") @@ -195,7 +203,9 @@ def generate_launch_description(): [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", - PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]), + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", description_file] + ), " ", "robot_ip:=", robot_ip, @@ -245,26 +255,61 @@ def generate_launch_description(): "headless_mode:=", headless_mode, " ", + "initial_positions_file:=", + PathJoinSubstitution( + [ + FindPackageShare("rrbot_description"), + "admittance_demo", + "initial_positions.yaml", + ] + ), + " ", ] ) robot_description = {"robot_description": robot_description_content} + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(moveit_config_package), "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! + "ur5e", + " ", + "prefix:=", + prefix, + " ", + ] + ) + + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content} + initial_joint_controllers = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", controllers_file] ) rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "view_robot.rviz"] + [FindPackageShare("rrbot_description"), "admittance_demo", "admittance_demo.rviz"] ) joint_limits_admittance = PathJoinSubstitution( - [FindPackageShare("diffbot_description"), "admittance_demo", "joint_limits_admittance.yaml"] + [FindPackageShare("rrbot_description"), "admittance_demo", "joint_limits_admittance.yaml"] ) control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, initial_joint_controllers, joint_limits_admittance], + parameters=[ + robot_description, + initial_joint_controllers, + robot_description_semantic, + joint_limits_admittance, + ], output={ "stdout": "screen", "stderr": "screen", @@ -299,19 +344,19 @@ def generate_launch_description(): joint_state_broadcaster_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) io_and_status_controller_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=["io_and_status_controller", "-c", "/controller_manager"], ) speed_scaling_state_broadcaster_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=[ "speed_scaling_state_broadcaster", "--controller-manager", @@ -321,7 +366,7 @@ def generate_launch_description(): force_torque_sensor_broadcaster_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=[ "force_torque_sensor_broadcaster", "--controller-manager", @@ -342,20 +387,6 @@ def generate_launch_description(): arguments=["faked_forces_controller", "-c", "/controller_manager"], ) - # There may be other controllers of the joints, but this is the initially-started one - initial_joint_controller_spawner_started = Node( - package="controller_manager", - executable="spawner.py", - arguments=[initial_joint_controller, "-c", "/controller_manager"], - condition=IfCondition(start_joint_controller), - ) - initial_joint_controller_spawner_stopped = Node( - package="controller_manager", - executable="spawner.py", - arguments=[initial_joint_controller, "-c", "/controller_manager", "--stopped"], - condition=UnlessCondition(start_joint_controller), - ) - nodes_to_start = [ control_node, dashboard_client_node, @@ -367,8 +398,6 @@ def generate_launch_description(): force_torque_sensor_broadcaster_spawner, admittance_controller_spawner, faked_forces_controller_spawner, - initial_joint_controller_spawner_stopped, - initial_joint_controller_spawner_started, ] return LaunchDescription(declared_arguments + nodes_to_start) diff --git a/ros2_control_demo_description/rrbot_description/CMakeLists.txt b/ros2_control_demo_description/rrbot_description/CMakeLists.txt index 65835b764..1b106116f 100644 --- a/ros2_control_demo_description/rrbot_description/CMakeLists.txt +++ b/ros2_control_demo_description/rrbot_description/CMakeLists.txt @@ -4,7 +4,7 @@ project(rrbot_description) find_package(ament_cmake REQUIRED) install( - DIRECTORY config gazebo meshes launch ros2_control urdf + DIRECTORY admittance_demo config gazebo meshes launch ros2_control urdf DESTINATION share/${PROJECT_NAME} ) diff --git a/ros2_control_demo_description/rrbot_description/admittance_demo/admittance_demo.rviz b/ros2_control_demo_description/rrbot_description/admittance_demo/admittance_demo.rviz new file mode 100644 index 000000000..26da8b5de --- /dev/null +++ b/ros2_control_demo_description/rrbot_description/admittance_demo/admittance_demo.rviz @@ -0,0 +1,269 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 1104 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fts_broadcaster/wrench + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base: + Value: false + base_link: + Value: true + base_link_inertia: + Value: false + flange: + Value: false + forearm_link: + Value: false + shoulder_link: + Value: false + tool0: + Value: true + upper_arm_link: + Value: false + wrist_1_link: + Value: false + wrist_2_link: + Value: false + wrist_3_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_link: + base: + {} + base_link_inertia: + shoulder_link: + upper_arm_link: + forearm_link: + wrist_1_link: + wrist_2_link: + wrist_3_link: + flange: + tool0: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 1.8979061841964722 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.11539864540100098 + Target Frame: + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 0.3453986942768097 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1381 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004f6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000004f6000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004f6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000046000004f6000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004f600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 1280 + Y: 30 diff --git a/ros2_control_demo_description/rrbot_description/admittance_demo/initial_positions.yaml b/ros2_control_demo_description/rrbot_description/admittance_demo/initial_positions.yaml new file mode 100644 index 000000000..e95a6399a --- /dev/null +++ b/ros2_control_demo_description/rrbot_description/admittance_demo/initial_positions.yaml @@ -0,0 +1,6 @@ +shoulder_pan_joint: 0.0 +shoulder_lift_joint: -1.57 +elbow_joint: 1.57 +wrist_1_joint: -1.57 +wrist_2_joint: -1.57 +wrist_3_joint: 0.0 diff --git a/ros2_control_demo_description/diffbot_description/admittance_demo/joint_limits_admittance.yaml b/ros2_control_demo_description/rrbot_description/admittance_demo/joint_limits_admittance.yaml similarity index 68% rename from ros2_control_demo_description/diffbot_description/admittance_demo/joint_limits_admittance.yaml rename to ros2_control_demo_description/rrbot_description/admittance_demo/joint_limits_admittance.yaml index 8588baf49..031562c11 100644 --- a/ros2_control_demo_description/diffbot_description/admittance_demo/joint_limits_admittance.yaml +++ b/ros2_control_demo_description/rrbot_description/admittance_demo/joint_limits_admittance.yaml @@ -5,50 +5,50 @@ ros__parameters: joint_limits: shoulder_pan_joint: - has_position_limits: true + has_position_limits: false min_position: -1.0471975511965976 max_position: 1.0471975511965976 - has_velocity_limits: true + has_velocity_limits: false max_velocity: 0.35 - has_acceleration_limits: true + has_acceleration_limits: false max_acceleration: 1.0 shoulder_lift_joint: - has_position_limits: true + has_position_limits: false min_position: -0.5916666164260777 max_position: 1.7819811662862104 - has_velocity_limits: true + has_velocity_limits: false max_velocity: 0.35 - has_acceleration_limits: true + has_acceleration_limits: false max_acceleration: 1.0 elbow_joint: - has_position_limits: true + has_position_limits: false min_position: -2.3125612588924866 max_position: 0.0 - has_velocity_limits: true + has_velocity_limits: false max_velocity: 0.35 - has_acceleration_limits: true + has_acceleration_limits: false max_acceleration: 4.0 wrist_1_joint: - has_position_limits: true + has_position_limits: false min_position: -1.0471975511965976 max_position: 1.0471975511965976 - has_velocity_limits: true + has_velocity_limits: false max_velocity: 1.0 - has_acceleration_limits: true + has_acceleration_limits: false max_acceleration: 4.0 wrist_2_joint: - has_position_limits: true + has_position_limits: false min_position: -0.5235987755982988 max_position: 1.5707963267948966 - has_velocity_limits: true + has_velocity_limits: false max_velocity: 1.0 - has_acceleration_limits: true + has_acceleration_limits: false max_acceleration: 4.0 wrist_3_joint: has_position_limits: false min_position: -6.283185307179586 max_position: 6.283185307179586 - has_velocity_limits: true + has_velocity_limits: false max_velocity: 8.0 - has_acceleration_limits: true + has_acceleration_limits: false max_acceleration: 8.0 diff --git a/using_admittance_controller.md b/using_admittance_controller.md index fc9232157..841df757b 100644 --- a/using_admittance_controller.md +++ b/using_admittance_controller.md @@ -21,5 +21,25 @@ This manual targets ROS2 rolling. 1. Start the demo using: ``` - TBD + ros2 launch ros2_control_demo_bringup admittance_controller_demo.launch.py + ``` + +1. Listing controller with `ros2 control list_controllers` you should see admittance controller in the state `inactive`. + +1. Activate the admittance controller: + ``` + ros2 control switch_controllers --start admittance_controller + ``` + +1. Start custom version of `teleop_twist_keyboard` to publish fake forces: + ``` + ros2 run teleop_twist_keyboard teleop_twist_keyboard + ``` + +1. Use keyboard keys to impose fake forces on the robot. + TIP: set maximal "speed" values (using "q" key) around `10` to get visible force/torque influence. To influence the robot with separate forces and torques use "holonomic"-mode (hold key or turn on Caps Lock). For further adjustment follow the notes in terminal. + + To see the faked forces use: + ``` + ros2 topic echo /faked_forces_controller/commands ``` From 2bb1cff8f8fce55cdf506bcc00d4f3c0fa0a6498 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 26 Oct 2021 14:53:21 +0200 Subject: [PATCH 4/7] Update manual with running real UR driver. --- using_admittance_controller.md | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/using_admittance_controller.md b/using_admittance_controller.md index 841df757b..5dd38122e 100644 --- a/using_admittance_controller.md +++ b/using_admittance_controller.md @@ -43,3 +43,20 @@ This manual targets ROS2 rolling. ``` ros2 topic echo /faked_forces_controller/commands ``` + + +## Test with URSim (not finished!) + +1. Build and run URSim as described [here](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver#usage-with-official-ur-simulator) + NOTE: First time docker container has to be build and everything has to be compiled, so take a cup of coffee or your favorite tea and start reading your emails :D + +1. Stop `ursim_driver_driver_*` docker with `docker stop ...` + +1. Start demo setup with the following command: + ``` + ros2 launch ros2_control_demo_bringup admittance_controller_demo.launch.py use_fake_hardware:=false fake_sensor_commands:=false headless_mode:=true robot_ip:=192.168.56.101 + ``` + +1. Now everything should start normally + +1. Simulate forces on FTS... TBD... (maybe not possible) From dffa6c637ae60d6e5c4dd4aa052645dff8bd4cef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 26 Oct 2021 15:53:06 +0200 Subject: [PATCH 5/7] Update UR driver hash. --- admittance_controller.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/admittance_controller.repos b/admittance_controller.repos index dea9d4a4c..0ac3b0c65 100644 --- a/admittance_controller.repos +++ b/admittance_controller.repos @@ -48,7 +48,7 @@ repositories: Universal_Robots_ROS2_Driver: # Branch: main type: git url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git - version: ebe29b1a270544a44765a3750d3747576ec6d598 # Add missing command and state interfaces to get everything working with the fake hardware and add some comment into xacro file to be clearer. + version: f0fc36c04065f609cec5b99c3cc88c363d25fd41 # Correct format ur_msgs: # Branch: ros2 type: git From 658ff0c95a607e335acaa2501db220e1d4887f09 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 28 Oct 2021 19:42:39 +0200 Subject: [PATCH 6/7] Extend showing admittance controller with filters and parameters update. --- admittance_controller.repos | 17 +++----- .../config/admittance_demo_controllers.yaml | 26 +++++++----- .../admittance_controller_demo.launch.py | 4 +- using_admittance_controller.md | 41 ++++++++++++++++++- 4 files changed, 64 insertions(+), 24 deletions(-) diff --git a/admittance_controller.repos b/admittance_controller.repos index 0ac3b0c65..6456b1929 100644 --- a/admittance_controller.repos +++ b/admittance_controller.repos @@ -1,5 +1,10 @@ repositories: + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + verstion: demo-admittance-controller-delete-when-paramhandler-gravity-comp-merged + control_msgs: # Branch: rolling/add-addmittance-controller-state type: git url: https://github.com/nbbrooks/control_msgs.git @@ -10,11 +15,6 @@ repositories: url: https://github.com/JafarAbdi/geometry2.git version: 2888c23b97cb76674129314cc648b5b355bda3f4 # Add tomsg conversion for geometry_msgs/Transform - iirob_filters: # Branch: ros2 - type: git - url: https://github.com/nbbrooks/iirob_filters.git - version: d0c9a21672264ae9506686de9ff928e6ada96f15 # Changes for Galactic/Rolling - ros2_control: type: git url: https://github.com/destogl/ros2_control.git @@ -30,11 +30,6 @@ repositories: url: https://github.com/destogl/ros2_controllers.git version: admittance-controller-demo - rosparam_shortcuts: # ros2 - type: git - url: https://github.com/PickNikRobotics/rosparam_shortcuts.git - version: b3ffe9061cf01d0acff07967d10b80c10447a618 # Create CI job for ros2 branch (#24) - teleop_twist_keyboard: # Branch: wrench type: git url: https://github.com/nbbrooks/teleop_twist_keyboard.git @@ -48,7 +43,7 @@ repositories: Universal_Robots_ROS2_Driver: # Branch: main type: git url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git - version: f0fc36c04065f609cec5b99c3cc88c363d25fd41 # Correct format + version: 10f16b8869bb47aa82daf7bc05e0b62406ae8c5d # Add ft_sensor frame. ur_msgs: # Branch: ros2 type: git diff --git a/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml b/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml index c35505459..f553d98be 100644 --- a/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml +++ b/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml @@ -134,6 +134,8 @@ admittance_controller: - position - velocity + enable_parameter_update_without_reactivation: false + ft_sensor_name: tcp_fts_sensor use_joint_commands_as_input: true #joint_limiter_type: "joint_limits/SimpleJointLimiter" @@ -156,7 +158,7 @@ admittance_controller: endeffector_frame: tool0 control_frame: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector fixed_world_frame: base_link # Gravity points down (neg. Z) in this frame (Usually: world or base_link) - sensor_frame: tool0 # Wrench measurements are in this frame + sensor_frame: ft_frame # Wrench measurements are in this frame admittance: selected_axes: @@ -195,27 +197,29 @@ admittance_controller: input_wrench_filter_chain: filter1: - type: iirob_filters/GravityCompensatorWrench + type: control_filters/GravityCompensationWrench name: wrist_gravity_compensation params: - CoG_x: 0.0 - CoG_y: 0.0 - CoG_z: 0.0 + CoG: + x: 0.0 + y: 0.0 + z: 0.1 force: 0.0 # mass * 9.81 force_frame: wrist_3_link - sensor_frame: tool0 + sensor_frame: ft_frame world_frame: base_link filter2: - type: iirob_filters/GravityCompensatorWrench + type: control_filters/GravityCompensationWrench name: tool_gravity_compensation params: - CoG_x: 0.0 - CoG_y: 0.0 - CoG_z: 0.0 + CoG: + x: 0.0 + y: 0.0 + z: 0.1 force: 0.0 # mass * 9.81 force_frame: wrist_3_link - sensor_frame: tool0 + sensor_frame: ft_frame world_frame: base_link diff --git a/ros2_control_demo_bringup/launch/admittance_controller_demo.launch.py b/ros2_control_demo_bringup/launch/admittance_controller_demo.launch.py index 5498bb4ff..03eca3707 100644 --- a/ros2_control_demo_bringup/launch/admittance_controller_demo.launch.py +++ b/ros2_control_demo_bringup/launch/admittance_controller_demo.launch.py @@ -152,7 +152,9 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - "launch_dashboard_client", default_value="true", description="Launch RViz?" + "launch_dashboard_client", + default_value="false", + description="Launch dashboard client?", ) ) diff --git a/using_admittance_controller.md b/using_admittance_controller.md index 5dd38122e..5114c56b5 100644 --- a/using_admittance_controller.md +++ b/using_admittance_controller.md @@ -44,6 +44,45 @@ This manual targets ROS2 rolling. ros2 topic echo /faked_forces_controller/commands ``` +## Features + +### Dynamic parameters + +Admittance controller provides a possibility to set and change its parameters during run-time. +This is useful for tuning its parameters or using different set of parameters for different scenarios. +It is recommended to reactivate (deactivate then activate) the controller each time when parameters are adjusted. +Otherwise, the controller could become unstable when updating multiple parameters. +Nevertheless, you can configure updating parameters without reactivation using `enable_parameter_update_without_reactivation`. + +1. To test dynamic parameters set first check how the robot behaves in 'X'-direction using "+I" and "+K" keys in the `teleop_twist_keyboard` node. + +1. Change a parameter of admittance controller, e.g., damping ratio: + ``` + ros2 param set /admittance_controller admittance.damping_ratio.y 20 + ``` + +1. Then restart controller: + ``` + ros2 control switch_controllers --stop admittance_controller --start admittance_controller + ``` + +1. Then you should see how the controllers dynamic has changed when executing the same movements. + +### Using Filters + +Admittance controller uses filter chain to filter input force data. +The usual example is to compensate the gravitational influence of a tool. +In the example files there are already two filters for Gravity compensation. + +The GravityCompensator filter compensate the gravitational influence to force measurements. (TBD: add link to the filter's documentation). + +To test the filter, start the admittance controller and then set filter's force parameter: + ``` + ros2 param set /admittance_controller input_wrench_filter_chain.filter1.params.force 5.0 + + ``` +NOTE: if using fake hardware you should first publish some force using `teleop_twist_keyboard` and then will this work (this will reset values from NaN). + ## Test with URSim (not finished!) @@ -54,7 +93,7 @@ This manual targets ROS2 rolling. 1. Start demo setup with the following command: ``` - ros2 launch ros2_control_demo_bringup admittance_controller_demo.launch.py use_fake_hardware:=false fake_sensor_commands:=false headless_mode:=true robot_ip:=192.168.56.101 + ros2 launch ros2_control_demo_bringup admittance_controller_demo.launch.py use_fake_hardware:=false fake_sensor_commands:=false headless_mode:=true robot_ip:=192.168.56.101 launch_dashboard_client:=true ``` 1. Now everything should start normally From 39f84108e4d1167478e64b30bbe97476571dd640 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 16 Nov 2021 15:51:11 +0100 Subject: [PATCH 7/7] Extend controller list with FTS-broadcaster from ros2_controllers. --- .../config/admittance_demo_controllers.yaml | 44 ++++++++++++++++++- 1 file changed, 43 insertions(+), 1 deletion(-) diff --git a/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml b/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml index f553d98be..e43a5447e 100644 --- a/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml +++ b/ros2_control_demo_bringup/config/admittance_demo_controllers.yaml @@ -14,6 +14,9 @@ controller_manager: force_torque_sensor_broadcaster: type: ur_controllers/ForceTorqueStateBroadcaster + force_sensor_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController @@ -49,6 +52,36 @@ force_torque_sensor_broadcaster: topic_name: ft_data +force_sensor_broadcaster: + ros__parameters: + sensor_name: tcp_fts_sensor + frame_id: tool0 + additional_frames_to_publish: [base_link] + + sensor_filter_chain: + filter1: + type: control_filters/LowPassFilterWrench + name: low_pass_filter + params: + sampling_frequency: 200.0 + damping_frequency: 50.0 + damping_intensity: 1.0 + divider: 1 + + filter2: + type: control_filters/GravityCompensationWrench + name: tool_gravity_compensation + params: + CoG: + x: 0.0 + y: 0.0 + z: 0.1 + force: 0.0 # mass * 9.81 + force_frame: wrist_3_link + sensor_frame: ft_frame + world_frame: base_link + + joint_trajectory_controller: ros__parameters: joints: @@ -197,6 +230,15 @@ admittance_controller: input_wrench_filter_chain: filter1: + type: control_filters/LowPassFilterWrench + name: low_pass_filter + params: + sampling_frequency: 200.0 + damping_frequency: 50.0 + damping_intensity: 1.0 + divider: 1 + + filter2: type: control_filters/GravityCompensationWrench name: wrist_gravity_compensation params: @@ -209,7 +251,7 @@ admittance_controller: sensor_frame: ft_frame world_frame: base_link - filter2: + filter3: type: control_filters/GravityCompensationWrench name: tool_gravity_compensation params: