Add UR5e, 2F_robotiq gripper, Husky mobile robot for ROS2 Iron

full_husky/full_husky_control/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.8)
+project(full_husky_control)
+
+find_package(ament_cmake REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+
+add_executable(static_state_publisher src/static_state_publisher.cpp)
+ament_target_dependencies(static_state_publisher rclcpp sensor_msgs)
+install(TARGETS
+ static_state_publisher
+DESTINATION lib/${PROJECT_NAME})
+
+install(
+  DIRECTORY launch config urdf
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

full_husky/full_husky_control/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2022, Universal Robots A/S
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

full_husky/full_husky_control/config/my_robot_calibration.yaml

@@ -0,0 +1,44 @@
+kinematics:
+  shoulder:
+    x: 0
+    y: 0
+    z: 0.16272426310862534
+    roll: -0
+    pitch: 0
+    yaw: -4.5709925010350982e-07
+  upper_arm:
+    x: 0.00019379813467043831
+    y: 0
+    z: 0
+    roll: 1.5706520581553929
+    pitch: 0
+    yaw: -3.6410252466367869e-06
+  forearm:
+    x: -0.42504980106801638
+    y: 0
+    z: 0
+    roll: 0.0025160263202623446
+    pitch: -0.00033895810441263049
+    yaw: 2.3673358755801655e-05
+  wrist_1:
+    x: -0.39271124046388295
+    y: -0.00095727880026714313
+    z: 0.1326644817298441
+    roll: 0.0072156629559113113
+    pitch: -0.0028759082553286397
+    yaw: 3.1087788595795942e-07
+  wrist_2:
+    x: 1.8891076800006509e-05
+    y: -0.099707767611820833
+    z: 1.7018396386939214e-05
+    roll: 1.570625644042396
+    pitch: 0
+    yaw: 1.5317567644210217e-06
+  wrist_3:
+    x: 3.6145501165240635e-05
+    y: 0.099585286028588696
+    z: 7.1494831296936063e-05
+    roll: 1.571514252322501
+    pitch: 3.1415926535897931
+    yaw: -3.1415926117084068
+  hash: calib_2089462667388424822

full_husky/full_husky_control/config/ros2_controllers.yaml

@@ -0,0 +1,94 @@
+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
+
+    scaled_joint_trajectory_controller:
+      type: ur_controllers/ScaledJointTrajectoryController
+
+    forward_velocity_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+    forward_position_controller:
+      type: position_controllers/JointGroupPositionController
+
+
+speed_scaling_state_broadcaster:
+  ros__parameters:
+    state_publish_rate: 100.0
+    tf_prefix: "ur5e_"
+
+io_and_status_controller:
+  ros__parameters:
+    tf_prefix: "ur5e_"
+
+force_torque_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: ur5e_tcp_fts_sensor
+    state_interface_names:
+      - force.x
+      - force.y
+      - force.z
+      - torque.x
+      - torque.y
+      - torque.z
+    frame_id: ur5e_tool0
+    topic_name: ft_data
+
+scaled_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - ur5e_shoulder_pan_joint
+      - ur5e_shoulder_lift_joint
+      - ur5e_elbow_joint
+      - ur5e_wrist_1_joint
+      - ur5e_wrist_2_joint
+      - ur5e_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
+      ur5e_shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
+      ur5e_shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
+      ur5e_elbow_joint: { trajectory: 0.2, goal: 0.1 }
+      ur5e_wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
+      ur5e_wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
+      ur5e_wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+    speed_scaling_interface_name: ur5e_speed_scaling/speed_scaling_factor
+
+forward_velocity_controller:
+  ros__parameters:
+    joints:
+      - ur5e_shoulder_pan_joint
+      - ur5e_shoulder_lift_joint
+      - ur5e_elbow_joint
+      - ur5e_wrist_1_joint
+      - ur5e_wrist_2_joint
+      - ur5e_wrist_3_joint
+    interface_name: velocity
+
+forward_position_controller:
+  ros__parameters:
+    joints:
+      - ur5e_shoulder_pan_joint
+      - ur5e_shoulder_lift_joint
+      - ur5e_elbow_joint
+      - ur5e_wrist_1_joint
+      - ur5e_wrist_2_joint
+      - ur5e_wrist_3_joint

full_husky/full_husky_control/config/ur5e_update_rate.yaml

@@ -0,0 +1,3 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz

full_husky/full_husky_control/launch/rsp.launch.py

@@ -0,0 +1,132 @@
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
+
+
+def generate_launch_description():
+    ur_type = LaunchConfiguration("ur_type")
+    robot_ip = LaunchConfiguration("robot_ip")
+
+    use_mock_hardware = LaunchConfiguration("use_mock_hardware")
+    mock_sensor_commands = LaunchConfiguration("mock_sensor_commands")
+
+    headless_mode = LaunchConfiguration("headless_mode")
+
+    kinematics_parameters_file = LaunchConfiguration("kinematics_parameters_file")
+
+    # Load description with necessary parameters
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("full_husky_control"),
+                    "urdf",
+                    "full_husky_controlled.urdf.xacro",
+                ]
+            ),
+            " ",
+            "robot_ip:=",
+            robot_ip,
+            " ",
+            "ur_type:=",
+            ur_type,
+            " ",
+            "kinematics_parameters_file:=",
+            kinematics_parameters_file,
+            " ",
+            "use_mock_hardware:=",
+            use_mock_hardware,
+            " ",
+            "mock_sensor_commands:=",
+            mock_sensor_commands,
+            " ",
+            "headless_mode:=",
+            headless_mode,
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    declared_arguments = []
+    # UR specific arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_type",
+            description="Typo/series of used UR robot.",
+            choices=[
+                "ur3",
+                "ur3e",
+                "ur5",
+                "ur5e",
+                "ur10",
+                "ur10e",
+                "ur16e",
+                "ur20",
+                "ur30",
+            ],
+            default_value="ur5e",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip",
+            default_value="10.130.1.100",
+            description="IP address by which the robot can be reached."
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "kinematics_parameters_file",
+            default_value=PathJoinSubstitution(
+                [
+                    FindPackageShare("full_husky_control"),
+                    "config",
+                    "my_robot_calibration.yaml",
+                ]
+            ),
+            description="The calibration configuration of the actual robot used.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_mock_hardware",
+            default_value="false",
+            description="Start robot with mock hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "mock_sensor_commands",
+            default_value="false",
+            description="Enable mock command interfaces for sensors used for simple simulations. "
+            "Used only if 'use_mock_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "headless_mode",
+            default_value="false",
+            description="Enable headless mode for robot control",
+        )
+    )
+
+    return LaunchDescription(
+        declared_arguments
+        + [
+            Node(
+                package="robot_state_publisher",
+                executable="robot_state_publisher",
+                output="both",
+                parameters=[robot_description],
+            ),
+        ]
+    )