diff --git a/config/ur_controllers.yaml b/config/ur_controllers.yaml
new file mode 100644
index 0000000..8a03e26
--- /dev/null
+++ b/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
diff --git a/launch/gz_components.launch.py b/launch/gz_components.launch.py
index cd922b7..d5a2e85 100644
--- a/launch/gz_components.launch.py
+++ b/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
diff --git a/launch/gz_ur.launch.py b/launch/gz_ur.launch.py
new file mode 100644
index 0000000..108cb30
--- /dev/null
+++ b/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,
+ ]
+ )
diff --git a/package.xml b/package.xml
index aefb6fb..a2e1c78 100644
--- a/package.xml
+++ b/package.xml
@@ -20,6 +20,7 @@
robot_state_publisher
urdf
xacro
+ ur_description
gazebo_plugins
diff --git a/urdf/components.urdf.xacro b/urdf/components.urdf.xacro
index 2a2211d..1ab0a63 100644
--- a/urdf/components.urdf.xacro
+++ b/urdf/components.urdf.xacro
@@ -34,7 +34,8 @@
-
+
@@ -130,6 +131,37 @@
/>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ ign_ros2_control/IgnitionSystem
+
+
+
+ {-2*pi}
+ {2*pi}
+
+
+ -3.15
+ 3.15
+
+
+
+ ${initial_positions['shoulder_pan_joint']}
+
+
+
+
+
+
+ {-2*pi}
+ {2*pi}
+
+
+ -3.15
+ 3.15
+
+
+
+ ${initial_positions['shoulder_lift_joint']}
+
+
+
+
+
+
+ {-pi}
+ {pi}
+
+
+ -3.15
+ 3.15
+
+
+
+ ${initial_positions['elbow_joint']}
+
+
+
+
+
+
+ {-2*pi}
+ {2*pi}
+
+
+ -3.2
+ 3.2
+
+
+
+ ${initial_positions['wrist_1_joint']}
+
+
+
+
+
+
+ {-2*pi}
+ {2*pi}
+
+
+ -3.2
+ 3.2
+
+
+
+ ${initial_positions['wrist_2_joint']}
+
+
+
+
+
+
+ {-2*pi}
+ {2*pi}
+
+
+ -3.2
+ 3.2
+
+
+
+ ${initial_positions['wrist_3_joint']}
+
+
+
+
+
+
+
+