Added ur

Signed-off-by: Jakub Delicat <[email protected]>

config/ur_controllers.yaml

@@ -0,0 +1,95 @@
+/**:
+  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
+
+  forward_position_controller:
+    ros__parameters:
+      joints:
+        - shoulder_pan_joint
+        - shoulder_lift_joint
+        - elbow_joint
+        - wrist_1_joint
+        - wrist_2_joint
+        - wrist_3_joint

launch/gz_components.launch.py

@@ -67,6 +67,11 @@ def get_launch_descriptions_from_yaml_node(
         if component["type"] == "CAM01":
             actions.append(get_launch_description("orbbec_astra", package, namespace, component))
 
+        if component["type"] == "MAN01":
+            actions.append(get_launch_description("ur", package, namespace, component))
+
+        if component["type"] == "MAN02":
+            actions.append(get_launch_description("ur", package, namespace, component))
 
     return actions
 

launch/gz_ur.launch.py

@@ -0,0 +1,133 @@
+# Copyright 2024 Husarion sp. z o.o.
+#
+# 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.
+
+from launch import LaunchDescription
+from launch.conditions import IfCondition, UnlessCondition, LaunchConfigurationNotEquals
+from launch.actions import DeclareLaunchArgument, LogInfo
+from launch_ros.actions import Node
+from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from nav2_common.launch import ReplaceString
+
+def generate_launch_description():
+    robot_namespace = LaunchConfiguration("robot_namespace")
+    device_namespace = LaunchConfiguration("device_namespace")
+    tf_prefix = LaunchConfiguration("tf_prefix")
+    start_joint_controller = LaunchConfiguration("start_joint_controller")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    controllers_file = LaunchConfiguration("controllers_file")
+
+    initial_joint_controllers = PathJoinSubstitution(
+        [FindPackageShare("ros_components_description"), "config", controllers_file]
+    )
+
+    namespace_warn = LogInfo(
+        msg="Namespace is not implemented with manipulators. Look here: https://github.com/ros-controls/ros2_control/issues/1506",
+        condition= LaunchConfigurationNotEquals(robot_namespace, "None")
+    )
+
+    # Using tf as namespace is caused by
+    # https://github.com/ros-controls/ros2_control/issues/1506
+    # After this fix the device_namespace should be used.
+    namespaced_initial_joint_controllers_path = ReplaceString(
+        source_file=initial_joint_controllers,
+        replacements={
+            "shoulder_pan_joint": [tf_prefix, "shoulder_pan_joint"],
+            "shoulder_lift_joint": [tf_prefix, "shoulder_lift_joint"],
+            "elbow_joint": [tf_prefix, "elbow_joint"],
+            "wrist_1_joint": [tf_prefix, "wrist_1_joint"],
+            "wrist_2_joint": [tf_prefix, "wrist_2_joint"],
+            "wrist_3_joint": [tf_prefix, "wrist_3_joint"],
+            "tool0": [tf_prefix, "tool0"],
+            "  joint_trajectory_controller:": ["  ", tf_prefix, "joint_trajectory_controller:"],
+        },
+    )
+
+    declare_device_namespace = DeclareLaunchArgument(
+        "device_namespace",
+        default_value="",
+        description="Sensor namespace that will appear before all non absolute topics and TF frames, used for distinguishing multiple cameras on the same robot.",
+    )
+
+    declare_robot_namespace = DeclareLaunchArgument(
+        "robot_namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Namespace which will appear in front of all topics (including /tf and /tf_static).",
+    )
+
+    declare_tf_prefix = DeclareLaunchArgument(
+        "tf_prefix",
+        default_value="",
+        description="Prefix added for all links of device. Here used as fix for static transform publisher.",
+    )
+
+    declare_start_joint_controller = DeclareLaunchArgument(
+        "start_joint_controller",
+        default_value="true",
+        description="Enable headless mode for robot control",
+    )
+
+    declare_initial_joint_controller = DeclareLaunchArgument(
+        "initial_joint_controller",
+        default_value=[tf_prefix, "joint_trajectory_controller"],
+        description="Robot controller to start.",
+    )
+
+    declare_controllers_file = DeclareLaunchArgument(
+        "controllers_file",
+        default_value="ur_controllers.yaml",
+        description="YAML file with the controllers configuration.",
+    )
+
+    # 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=[
+            [tf_prefix, "joint_trajectory_controller"],
+            "-t",
+            "joint_trajectory_controller/JointTrajectoryController",
+            "-c",
+            "controller_manager",
+            "--controller-manager-timeout",
+            "10",
+            "--namespace",
+            device_namespace,
+            "--param-file",
+            namespaced_initial_joint_controllers_path,
+        ],
+        namespace=robot_namespace,
+        condition=IfCondition(start_joint_controller),
+    )
+
+    initial_joint_controller_spawner_stopped = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[initial_joint_controller, "-c", "/controller_manager", "--stopped"],
+        condition=UnlessCondition(start_joint_controller),
+    )
+
+    return LaunchDescription(
+        [
+            declare_device_namespace,
+            declare_robot_namespace,
+            declare_tf_prefix,
+            declare_start_joint_controller,
+            declare_initial_joint_controller,
+            declare_controllers_file,
+            namespace_warn,
+            initial_joint_controller_spawner_started,
+            initial_joint_controller_spawner_stopped,
+        ]
+    )

package.xml

@@ -20,6 +20,7 @@
   <depend>robot_state_publisher</depend>
   <depend>urdf</depend>
   <depend>xacro</depend>
+  <depend>ur_description</depend>
 
   <depend condition="($HUSARION_ROS_BUILD_TYPE == simulation) and ($SIMULATION_ENGINE == gazebo-classic)">
     gazebo_plugins

urdf/components.urdf.xacro

@@ -34,7 +34,8 @@
         <xacro:property name="fixed_namespace" value="None" scope="parent" />
       </xacro:if>
       <xacro:unless value="${device_namespace == 'None' and robot_namespace == 'None'}">
-        <xacro:property name="fixed_namespace" value="/${robot_namespace + device_namespace}" scope="parent" />
+        <xacro:property name="fixed_namespace" value="/${robot_namespace + device_namespace}"
+          scope="parent" />
       </xacro:unless>
 
       <xacro:if value="${device_namespace == 'None' and robot_namespace != 'None'}">
@@ -130,6 +131,37 @@
         />
       </xacro:if>
 
+      <xacro:if value="${type == 'MAN01'}">
+        <xacro:include
+          filename="$(find ros_components_description)/urdf/ur.urdf.xacro"
+          ns="manipulator"
+        />
+
+        <xacro:manipulator.ur
+          parent_link="${parent_link}"
+          xyz="${xyz}"
+          rpy="${rpy}"
+          tf_prefix="${tf_prefix}"
+          ur_type="ur3e"
+        />
+      </xacro:if>
+
+      <xacro:if value="${type == 'MAN02'}">
+        <xacro:include
+          filename="$(find ros_components_description)/urdf/ur.urdf.xacro"
+          ns="manipulator"
+        />
+
+        <xacro:manipulator.ur
+          parent_link="${parent_link}"
+          xyz="${xyz}"
+          rpy="${rpy}"
+          tf_prefix="${tf_prefix}"
+          ur_type="ur5e"
+        />
+      </xacro:if>
+
+
       <xacro:if value="${index}">
         <xacro:load_componenet
           counter="${index}"