From 8d1f4642dcf3e7ac23041fab5f2918ee72bf9161 Mon Sep 17 00:00:00 2001 From: Niko Date: Thu, 21 Mar 2024 15:28:24 +0100 Subject: [PATCH] issue with Gripper controllers --- src/ur_robotiq_description/CMakeLists.txt | 7 +- .../config/ur_robotiq_controllers.yaml | 144 ++++ .../launch/ur_robotiq_controller.launch.py | 614 ++++++++++++++++++ src/ur_robotiq_description/package.xml | 20 + .../rviz/view_robot.rviz | 117 +--- .../urdf/ur_robotiq.urdf.xacro | 2 +- .../urdf/ur_to_robotiq_adapter.urdf.xacro | 39 ++ src/ur_robotiq_moveit_config/CMakeLists.txt | 3 +- .../config/controllers.yaml | 15 + .../config/initial_positions.yaml | 3 +- .../config/robotiq_update_rate.yaml | 3 + 11 files changed, 857 insertions(+), 110 deletions(-) create mode 100644 src/ur_robotiq_description/config/ur_robotiq_controllers.yaml create mode 100644 src/ur_robotiq_description/launch/ur_robotiq_controller.launch.py rename src/{ur_robotiq_moveit_config => ur_robotiq_description}/rviz/view_robot.rviz (63%) create mode 100644 src/ur_robotiq_description/urdf/ur_to_robotiq_adapter.urdf.xacro create mode 100644 src/ur_robotiq_moveit_config/config/robotiq_update_rate.yaml diff --git a/src/ur_robotiq_description/CMakeLists.txt b/src/ur_robotiq_description/CMakeLists.txt index 5fd7cc4..6ada202 100644 --- a/src/ur_robotiq_description/CMakeLists.txt +++ b/src/ur_robotiq_description/CMakeLists.txt @@ -5,9 +5,10 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -install(DIRECTORY urdf/ - DESTINATION share/${PROJECT_NAME}/urdf) - +install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) # find dependencies find_package(ament_cmake REQUIRED) diff --git a/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml b/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml new file mode 100644 index 0000000..6ce1ef9 --- /dev/null +++ b/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml @@ -0,0 +1,144 @@ +controller_manager: + ros__parameters: + 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: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + scaled_joint_trajectory_controller: + type: ur_controllers/ScaledJointTrajectoryController + + forward_velocity_controller: + type: velocity_controllers/JointGroupVelocityController + + forward_position_controller: + type: position_controllers/JointGroupPositionController + + robotiq_gripper_controller: + type: position_controllers/GripperActionController + + robotiq_activation_controller: + type: robotiq_controllers/RobotiqActivationController + + +speed_scaling_state_broadcaster: + ros__parameters: + state_publish_rate: 100.0 + tf_prefix: "$(var tf_prefix)" + +io_and_status_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" + +force_torque_sensor_broadcaster: + ros__parameters: + sensor_name: $(var tf_prefix)tcp_fts_sensor + state_interface_names: + - force.x + - force.y + - force.z + - torque.x + - torque.y + - torque.z + frame_id: $(var tf_prefix)tool0 + topic_name: ft_data + + +joint_trajectory_controller: + ros__parameters: + joints: + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)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 + $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + + +scaled_joint_trajectory_controller: + ros__parameters: + joints: + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)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 + $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor + +forward_velocity_controller: + ros__parameters: + joints: + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint + interface_name: velocity + +forward_position_controller: + ros__parameters: + joints: + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint + +robotiq_gripper_controller: + ros__parameters: + default: true + joint: $(var tf_prefix)robotiq_140_left_knuckle_joint + use_effort_interface: true + use_speed_interface: true + +robotiq_activation_controller: + ros__parameters: + default: true + diff --git a/src/ur_robotiq_description/launch/ur_robotiq_controller.launch.py b/src/ur_robotiq_description/launch/ur_robotiq_controller.launch.py new file mode 100644 index 0000000..7629a81 --- /dev/null +++ b/src/ur_robotiq_description/launch/ur_robotiq_controller.launch.py @@ -0,0 +1,614 @@ +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution + +def launch_setup(context, *args, **kwargs): + # 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") + ur_description_package = LaunchConfiguration("ur_description_package") + description_file = LaunchConfiguration("description_file") + tf_prefix = LaunchConfiguration("tf_prefix") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + controller_spawner_timeout = LaunchConfiguration("controller_spawner_timeout") + initial_joint_controller = LaunchConfiguration("initial_joint_controller") + activate_joint_controller = LaunchConfiguration("activate_joint_controller") + launch_rviz = LaunchConfiguration("launch_rviz") + headless_mode = LaunchConfiguration("headless_mode") + launch_dashboard_client = LaunchConfiguration("launch_dashboard_client") + use_tool_communication = LaunchConfiguration("use_tool_communication") + tool_parity = LaunchConfiguration("tool_parity") + tool_baud_rate = LaunchConfiguration("tool_baud_rate") + tool_stop_bits = LaunchConfiguration("tool_stop_bits") + tool_rx_idle_chars = LaunchConfiguration("tool_rx_idle_chars") + tool_tx_idle_chars = LaunchConfiguration("tool_tx_idle_chars") + tool_device_name = LaunchConfiguration("tool_device_name") + tool_tcp_port = LaunchConfiguration("tool_tcp_port") + tool_voltage = LaunchConfiguration("tool_voltage") + reverse_ip = LaunchConfiguration("reverse_ip") + script_command_port = LaunchConfiguration("script_command_port") + reverse_port = LaunchConfiguration("reverse_port") + script_sender_port = LaunchConfiguration("script_sender_port") + trajectory_port = LaunchConfiguration("trajectory_port") + + + + # Config Paths UR + joint_limit_params = PathJoinSubstitution( + [FindPackageShare(ur_description_package), "config", ur_type, "joint_limits.yaml"] + ) + kinematics_params = PathJoinSubstitution( + [FindPackageShare(ur_description_package), "config", ur_type, "default_kinematics.yaml"] + ) + physical_params = PathJoinSubstitution( + [FindPackageShare(ur_description_package), "config", ur_type, "physical_parameters.yaml"] + ) + visual_params = PathJoinSubstitution( + [FindPackageShare(ur_description_package), "config", ur_type, "visual_parameters.yaml"] + ) + script_filename = PathJoinSubstitution( + [FindPackageShare("ur_client_library"), "resources", "external_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"] + ) + + # URDF + 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, + " ", + "tf_prefix:=", + tf_prefix, + " ", + "use_fake_hardware:=", + use_fake_hardware, + " ", + "fake_sensor_commands:=", + fake_sensor_commands, + " ", + "headless_mode:=", + headless_mode, + " ", + "use_tool_communication:=", + use_tool_communication, + " ", + "tool_parity:=", + tool_parity, + " ", + "tool_baud_rate:=", + tool_baud_rate, + " ", + "tool_stop_bits:=", + tool_stop_bits, + " ", + "tool_rx_idle_chars:=", + tool_rx_idle_chars, + " ", + "tool_tx_idle_chars:=", + tool_tx_idle_chars, + " ", + "tool_device_name:=", + tool_device_name, + " ", + "tool_tcp_port:=", + tool_tcp_port, + " ", + "tool_voltage:=", + tool_voltage, + " ", + "reverse_ip:=", + reverse_ip, + " ", + "script_command_port:=", + script_command_port, + " ", + "reverse_port:=", + reverse_port, + " ", + "script_sender_port:=", + script_sender_port, + " ", + "trajectory_port:=", + trajectory_port, + " ", + ] + ) + + robot_description = {"robot_description": robot_description_content} + + initial_joint_controllers = PathJoinSubstitution( + [FindPackageShare(description_package), "config", controllers_file] + ) + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rviz", "view_robot.rviz"] + ) + + # define update rate + update_rate_config_file = PathJoinSubstitution( + [ + FindPackageShare(runtime_config_package), + "config", + ur_type.perform(context) + "_update_rate.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + robot_description, + update_rate_config_file, + ParameterFile(initial_joint_controllers, allow_substs=True), + ], + output="screen", + condition=IfCondition(use_fake_hardware), + ) + + ur_control_node = Node( + package="ur_robot_driver", + executable="ur_ros2_control_node", + parameters=[ + robot_description, + update_rate_config_file, + ParameterFile(initial_joint_controllers, allow_substs=True), + ], + output="screen", + condition=UnlessCondition(use_fake_hardware), + ) + + robotiq_gripper_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["robotiq_gripper_controller", "-c", "/controller_manager"], + ) + + robotiq_activation_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["robotiq_activation_controller", "-c", "/controller_manager"], + ) + + dashboard_client_node = Node( + package="ur_robot_driver", + condition=IfCondition(launch_dashboard_client) and UnlessCondition(use_fake_hardware), + executable="dashboard_client", + name="dashboard_client", + output="screen", + emulate_tty=True, + parameters=[{"robot_ip": robot_ip}], + ) + + tool_communication_node = Node( + package="ur_robot_driver", + condition=IfCondition(use_tool_communication), + executable="tool_communication.py", + name="ur_tool_comm", + output="screen", + parameters=[ + { + "robot_ip": robot_ip, + "tcp_port": tool_tcp_port, + "device_name": tool_device_name, + } + ], + ) + + urscript_interface = Node( + package="ur_robot_driver", + executable="urscript_interface", + parameters=[{"robot_ip": robot_ip}], + output="screen", + ) + + controller_stopper_node = Node( + package="ur_robot_driver", + executable="controller_stopper_node", + name="controller_stopper", + output="screen", + emulate_tty=True, + condition=UnlessCondition(use_fake_hardware), + parameters=[ + {"headless_mode": headless_mode}, + {"joint_controller_active": activate_joint_controller}, + { + "consistent_controllers": [ + "io_and_status_controller", + "force_torque_sensor_broadcaster", + "joint_state_broadcaster", + "speed_scaling_state_broadcaster", + ] + }, + ], + ) + + 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], + ) + + # Spawn controllers + def controller_spawner(name, active=True): + inactive_flags = ["--inactive"] if not active else [] + return Node( + package="controller_manager", + executable="spawner", + arguments=[ + name, + "--controller-manager", + "/controller_manager", + "--controller-manager-timeout", + controller_spawner_timeout, + ] + + inactive_flags, + ) + + controller_spawner_names = [ + "joint_state_broadcaster", + "io_and_status_controller", + "speed_scaling_state_broadcaster", + "force_torque_sensor_broadcaster", + ] + controller_spawner_inactive_names = ["forward_position_controller"] + + controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [ + controller_spawner(name, active=False) for name in controller_spawner_inactive_names + ] + + # 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", + arguments=[ + initial_joint_controller, + "-c", + "/controller_manager", + "--controller-manager-timeout", + controller_spawner_timeout, + ], + condition=IfCondition(activate_joint_controller), + ) + initial_joint_controller_spawner_stopped = Node( + package="controller_manager", + executable="spawner", + arguments=[ + initial_joint_controller, + "-c", + "/controller_manager", + "--controller-manager-timeout", + controller_spawner_timeout, + "--inactive", + ], + condition=UnlessCondition(activate_joint_controller), + ) + + nodes_to_start = [ + control_node, + ur_control_node, + robotiq_gripper_controller_spawner, + robotiq_activation_controller_spawner, + dashboard_client_node, + tool_communication_node, + controller_stopper_node, + urscript_interface, + robot_state_publisher_node, + rviz_node, + initial_joint_controller_spawner_stopped, + initial_joint_controller_spawner_started, + ] + controller_spawners + + return nodes_to_start + + +def generate_launch_description(): +# Declare Arguments + declared_arguments = [] + # UR specific arguments + declared_arguments.append( + DeclareLaunchArgument( + "ur_type", + description="Type/series of used UR robot.", + default_value="ur3e", + choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", description="IP address by which the robot can be reached." + ) + ) + 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_robot_driver", + 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_robotiq_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="ur_robotiq_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( + "ur_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_robotiq.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tf_prefix", + default_value="", + description="tf_prefix of the joint names, useful for \ + multi-robot setup. If changed, 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="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( + "headless_mode", + default_value="false", + description="Enable headless mode for robot control", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "controller_spawner_timeout", + default_value="10", + description="Timeout used when spawning controllers.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") + ) + declared_arguments.append( + DeclareLaunchArgument( + "launch_dashboard_client", default_value="true", description="Launch Dashboard Client?" + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_tool_communication", + default_value="false", + description="Only available for e series!", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_parity", + default_value="0", + description="Parity configuration for serial communication. Only effective, if \ + use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_baud_rate", + default_value="115200", + description="Baud rate configuration for serial communication. Only effective, if \ + use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_stop_bits", + default_value="1", + description="Stop bits configuration for serial communication. Only effective, if \ + use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_rx_idle_chars", + default_value="1.5", + description="RX idle chars configuration for serial communication. Only effective, \ + if use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_tx_idle_chars", + default_value="3.5", + description="TX idle chars configuration for serial communication. Only effective, \ + if use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_device_name", + default_value="/tmp/ttyUR", + description="File descriptor that will be generated for the tool communication device. \ + The user has be be allowed to write to this location. \ + Only effective, if use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_tcp_port", + default_value="54321", + description="Remote port that will be used for bridging the tool's serial device. \ + Only effective, if use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_voltage", + default_value="0", # 0 being a conservative value that won't destroy anything + description="Tool voltage that will be setup.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "reverse_ip", + default_value="0.0.0.0", + description="IP that will be used for the robot controller to communicate back to the driver.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "script_command_port", + default_value="50004", + description="Port that will be opened to forward URScript commands to the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "reverse_port", + default_value="50001", + description="Port that will be opened to send cyclic instructions from the driver to the robot controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "script_sender_port", + default_value="50002", + description="The driver will offer an interface to query the external_control URScript on this port.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "trajectory_port", + default_value="50003", + description="Port that will be opened for trajectory control.", + ) + ) + + + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) \ No newline at end of file diff --git a/src/ur_robotiq_description/package.xml b/src/ur_robotiq_description/package.xml index 00c2660..f3faf4b 100644 --- a/src/ur_robotiq_description/package.xml +++ b/src/ur_robotiq_description/package.xml @@ -9,6 +9,26 @@ ament_cmake + joint_state_publisher_gui + launch + launch_ros + robot_state_publisher + rviz2 + urdf + xacro + + ros2_control + ros2_controllers + + ur_description + ur_robot_driver + ur_controllers + robotiq_description + robotiq_driver + robotiq_controllers + gripper_controllers + joint_state_broadcaster + ament_lint_auto ament_lint_common diff --git a/src/ur_robotiq_moveit_config/rviz/view_robot.rviz b/src/ur_robotiq_description/rviz/view_robot.rviz similarity index 63% rename from src/ur_robotiq_moveit_config/rviz/view_robot.rviz rename to src/ur_robotiq_description/rviz/view_robot.rviz index 7a4b6ff..a7bcd29 100644 --- a/src/ur_robotiq_moveit_config/rviz/view_robot.rviz +++ b/src/ur_robotiq_description/rviz/view_robot.rviz @@ -1,16 +1,12 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 87 Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /RobotModel1/Status1 - - /RobotModel1/Links1 + - /RobotModel1/Description Topic1 Splitter Ratio: 0.5 - Tree Height: 746 + Tree Height: 1096 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -24,11 +20,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" Visualization Manager: Class: "" Displays: @@ -90,69 +81,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - ft_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - left_inner_finger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_inner_finger_pad: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_inner_knuckle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_outer_finger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_outer_knuckle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_inner_finger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_inner_finger_pad: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_inner_knuckle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_outer_finger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_outer_knuckle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robotiq_140_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robotiq_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false shoulder_link: Alpha: 1 Show Axes: false @@ -167,15 +95,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - ur_to_robotiq_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false wrist_1_link: Alpha: 1 Show Axes: false @@ -191,9 +110,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - Mass Properties: - Inertia: false - Mass: false Name: RobotModel TF Prefix: "" Update Interval: 0 @@ -202,7 +118,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world + Fixed Frame: base_link Frame Rate: 30 Name: root Tools: @@ -214,9 +130,6 @@ Visualization Manager: - 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 @@ -245,41 +158,39 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 1.6701574325561523 + Distance: 3.493516445159912 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 + X: -0.0457618348300457 + Y: -0.07058511674404144 + Z: 0.49734944105148315 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.785398006439209 + Pitch: 0.15039828419685364 Target Frame: Value: Orbit (rviz) - Yaw: 0.785398006439209 + Yaw: 0.5353983640670776 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1043 + Height: 1379 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000002a000000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003c50000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004f0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000048000004f00000010101000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004f0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000048000004f0000000db01000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004f000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - Time: - collapsed: false Tool Properties: collapsed: false Views: collapsed: false - Width: 1920 + Width: 2560 X: 0 - Y: 0 + Y: 30 diff --git a/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro b/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro index 0f19577..4b8adac 100644 --- a/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro +++ b/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro @@ -5,7 +5,7 @@ - + diff --git a/src/ur_robotiq_description/urdf/ur_to_robotiq_adapter.urdf.xacro b/src/ur_robotiq_description/urdf/ur_to_robotiq_adapter.urdf.xacro new file mode 100644 index 0000000..d5509bf --- /dev/null +++ b/src/ur_robotiq_description/urdf/ur_to_robotiq_adapter.urdf.xacro @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/ur_robotiq_moveit_config/CMakeLists.txt b/src/ur_robotiq_moveit_config/CMakeLists.txt index d156ca7..07d23b8 100644 --- a/src/ur_robotiq_moveit_config/CMakeLists.txt +++ b/src/ur_robotiq_moveit_config/CMakeLists.txt @@ -5,8 +5,7 @@ find_package(ament_cmake REQUIRED) ament_package() -install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} - PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(DIRECTORY srdf DESTINATION share/${PROJECT_NAME}) install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) diff --git a/src/ur_robotiq_moveit_config/config/controllers.yaml b/src/ur_robotiq_moveit_config/config/controllers.yaml index 784ebd3..95f43c9 100644 --- a/src/ur_robotiq_moveit_config/config/controllers.yaml +++ b/src/ur_robotiq_moveit_config/config/controllers.yaml @@ -1,6 +1,8 @@ controller_names: - scaled_joint_trajectory_controller - joint_trajectory_controller + - robotiq_gripper_controller + - robotiq_activation_controller scaled_joint_trajectory_controller: @@ -27,3 +29,16 @@ joint_trajectory_controller: - wrist_1_joint - wrist_2_joint - wrist_3_joint + +robotiq_gripper_controller: + action_ns: antipodal_gripper_action_controller + type: GripperActionController + default: true + joint: robotiq_85_left_knuckle_joint + use_effort_interface: true + use_speed_interface: true + +robotiq_activation_controller: + action_ns: robotiq_controllers + type: RobotiqActivationController + default: true diff --git a/src/ur_robotiq_moveit_config/config/initial_positions.yaml b/src/ur_robotiq_moveit_config/config/initial_positions.yaml index 3a72ceb..5671c30 100644 --- a/src/ur_robotiq_moveit_config/config/initial_positions.yaml +++ b/src/ur_robotiq_moveit_config/config/initial_positions.yaml @@ -6,4 +6,5 @@ initial_positions: shoulder_pan_joint: 0 wrist_1_joint: 0 wrist_2_joint: 0 - wrist_3_joint: 0 \ No newline at end of file + wrist_3_joint: 0 + finger_joint: 0.695 \ No newline at end of file diff --git a/src/ur_robotiq_moveit_config/config/robotiq_update_rate.yaml b/src/ur_robotiq_moveit_config/config/robotiq_update_rate.yaml new file mode 100644 index 0000000..66ef3d7 --- /dev/null +++ b/src/ur_robotiq_moveit_config/config/robotiq_update_rate.yaml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz