Split ros2_control and kinematics description

urdf/inc/ur_common.xacro → ur_description/urdf/inc/ur_common.xacro

@@ -41,6 +41,12 @@
     <mesh filename="${mesh}"/>
   </xacro:macro>
 
+  <xacro:macro name="get_kinematics_hash" params="kinematics_parameters_file">
+    <xacro:property name="config_kinematics_parameters" value="${xacro.load_yaml(kinematics_parameters_file)}"/>
+    <xacro:property name="sec_kinematics" value="${config_kinematics_parameters['kinematics']}" />
+    <xacro:property name="kinematics_hash" value="${sec_kinematics['hash']}" scope="parent"/>
+  </xacro:macro>
+
   <xacro:macro name="read_model_data" params="joint_limits_parameters_file kinematics_parameters_file physical_parameters_file visual_parameters_file force_abs_paths">
 
     <xacro:property name="force_abs_paths" value="${force_abs_paths}" scope="parent"/>
@@ -187,7 +193,5 @@
     <xacro:property name="forearm_radius" value="${sec_inertia_parameters['forearm_radius']}" scope="parent"/>
     <xacro:property name="wrist_radius" value="${sec_inertia_parameters['wrist_radius']}" scope="parent"/>
 
-    <!--KINEMATICS HASH-->
-    <xacro:property name="kinematics_hash" value="${sec_kinematics['hash']}" scope="parent"/>
   </xacro:macro>
 </robot>

ur_description/urdf/ur.urdf.xacro

@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
+   <!-- robot name parameter -->
+   <xacro:arg name="name" default="ur"/>
+   <!-- import main macro -->
+   <xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
+
+   <!-- possible 'ur_type' values: ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e -->
+   <!-- the default value should raise an error in case this was called without defining the type -->
+   <xacro:arg name="ur_type" default="ur5x"/>
+
+   <!-- parameters -->
+   <xacro:arg name="tf_prefix" default="" />
+   <xacro:arg name="joint_limit_params" default="$(find ur_description)/config/$(arg ur_type)/joint_limits.yaml"/>
+   <xacro:arg name="kinematics_params" default="$(find ur_description)/config/$(arg ur_type)/default_kinematics.yaml"/>
+   <xacro:arg name="physical_params" default="$(find ur_description)/config/$(arg ur_type)/physical_parameters.yaml"/>
+   <xacro:arg name="visual_params" default="$(find ur_description)/config/$(arg ur_type)/visual_parameters.yaml"/>
+   <xacro:arg name="safety_limits" default="false"/>
+   <xacro:arg name="safety_pos_margin" default="0.15"/>
+   <xacro:arg name="safety_k_position" default="20"/>
+   <!-- initial position for simulations (Fake Hardware, Gazebo, Ignition) -->
+   <xacro:arg name="initial_positions_file" default="$(find ur_description)/config/initial_positions.yaml"/>
+
+   <!-- convert to property to use substitution in function -->
+   <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>
+
+   <!-- create link fixed to the "world" -->
+   <link name="world" />
+
+   <!-- arm -->
+   <xacro:ur_robot
+     name="$(arg name)"
+     tf_prefix="$(arg tf_prefix)"
+     parent="world"
+     joint_limits_parameters_file="$(arg joint_limit_params)"
+     kinematics_parameters_file="$(arg kinematics_params)"
+     physical_parameters_file="$(arg physical_params)"
+     visual_parameters_file="$(arg visual_params)"
+     safety_limits="$(arg safety_limits)"
+     safety_pos_margin="$(arg safety_pos_margin)"
+     safety_k_position="$(arg safety_k_position)"
+     >
+     <origin xyz="0 0 0" rpy="0 0 0" />          <!-- position robot in the world -->
+   </xacro:ur_robot>
+</robot>

urdf/ur_macro.xacro → ur_description/urdf/ur_macro.xacro

@@ -50,7 +50,6 @@
      - Muhammad Asif Rana
   -->
 
-  <xacro:include filename="$(find ur_description)/urdf/inc/ur_transmissions.xacro" />
   <xacro:include filename="$(find ur_description)/urdf/inc/ur_common.xacro" />
 
   <xacro:macro name="ur_robot" params="
@@ -62,86 +61,19 @@
     kinematics_parameters_file
     physical_parameters_file
     visual_parameters_file
-    transmission_hw_interface:=hardware_interface/PositionJointInterface
     safety_limits:=false
     safety_pos_margin:=0.15
     safety_k_position:=20
-    use_fake_hardware:=false
-    fake_sensor_commands:=false
-    sim_gazebo:=false
-    sim_ignition:=false
-    headless_mode:=false
-    initial_positions:=${dict(shoulder_pan_joint=0.0,shoulder_lift_joint=-1.57,elbow_joint=0.0,wrist_1_joint=-1.57,wrist_2_joint=0.0,wrist_3_joint=0.0)}
-    use_tool_communication:=false
-    tool_voltage:=0
-    tool_parity:=0
-    tool_baud_rate:=115200
-    tool_stop_bits:=1
-    tool_rx_idle_chars:=1.5
-    tool_tx_idle_chars:=3.5
-    tool_device_name:=/tmp/ttyUR
-    tool_tcp_port:=54321
-    robot_ip:=0.0.0.0
-    script_filename:=to_be_filled_by_ur_robot_driver
-    output_recipe_filename:=to_be_filled_by_ur_robot_driver
-    input_recipe_filename:=to_be_filled_by_ur_robot_driver
-    reverse_port:=50001
-    script_sender_port:=50002
-    reverse_ip:=0.0.0.0
-    script_command_port:=50004
-    trajectory_port:=50003
-    non_blocking_read:=false
-    keep_alive_count:=2"
-
-    >
+    force_abs_paths:=false
+    ">
 
     <!-- Load configuration data from the provided .yaml files -->
     <xacro:read_model_data
       joint_limits_parameters_file="${joint_limits_parameters_file}"
       kinematics_parameters_file="${kinematics_parameters_file}"
       physical_parameters_file="${physical_parameters_file}"
       visual_parameters_file="${visual_parameters_file}"
-      force_abs_paths="${sim_gazebo or sim_ignition}"/>
-
-
-    <!-- ros2 control include -->
-    <xacro:include filename="$(find ur_description)/urdf/ur.ros2_control.xacro" />
-    <!-- ros2 control instance -->
-    <xacro:ur_ros2_control
-      name="${name}"
-      use_fake_hardware="${use_fake_hardware}"
-      initial_positions="${initial_positions}"
-      fake_sensor_commands="${fake_sensor_commands}"
-      headless_mode="${headless_mode}"
-      sim_gazebo="${sim_gazebo}"
-      sim_ignition="${sim_ignition}"
-      script_filename="${script_filename}"
-      output_recipe_filename="${output_recipe_filename}"
-      input_recipe_filename="${input_recipe_filename}"
-      tf_prefix="${tf_prefix}"
-      hash_kinematics="${kinematics_hash}"
-      robot_ip="${robot_ip}"
-      use_tool_communication="${use_tool_communication}"
-      tool_voltage="${tool_voltage}"
-      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}"
-      reverse_port="${reverse_port}"
-      script_sender_port="${script_sender_port}"
-      reverse_ip="${reverse_ip}"
-      script_command_port="${script_command_port}"
-      trajectory_port="${trajectory_port}"
-      non_blocking_read="${non_blocking_read}"
-      keep_alive_count="${keep_alive_count}"
-      />
-
-    <!-- Add URDF transmission elements (for ros_control) -->
-    <!--<xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" />-->
-    <!-- Placeholder for ros2_control transmission which don't yet exist -->
+      force_abs_paths="${force_abs_paths}"/>
 
     <!-- links -  main serial chain -->
     <link name="${tf_prefix}base_link"/>

ur_ros2_control_support/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.8)
+project(ur_ros2_control_support)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY launch urdf
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

ur_ros2_control_support/launch/view_ur.launch.py

@@ -0,0 +1,162 @@
+# Copyright (c) 2021 PickNik, Inc.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * 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.
+#
+#    * 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.
+#
+# Author: Denis Stogl
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+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"],
+        )
+    )
+    # TODO(anyone): enable this when added into ROS2-foxy
+    # choices=['ur3', 'ur3e', 'ur5', 'ur5e', 'ur10', 'ur10e', 'ur16e']))
+    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(
+            "description_package",
+            default_value="ur_ros2_control_support",
+            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(
+            "tf_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.",
+        )
+    )
+
+    # Initialize Arguments
+    ur_type = LaunchConfiguration("ur_type")
+    safety_limits = LaunchConfiguration("safety_limits")
+    safety_pos_margin = LaunchConfiguration("safety_pos_margin")
+    safety_k_position = LaunchConfiguration("safety_k_position")
+    # General arguments
+    description_package = LaunchConfiguration("description_package")
+    description_file = LaunchConfiguration("description_file")
+    tf_prefix = LaunchConfiguration("tf_prefix")
+
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution([FindPackageShare(description_package), "urdfs", description_file]),
+            " ",
+            "safety_limits:=",
+            safety_limits,
+            " ",
+            "safety_pos_margin:=",
+            safety_pos_margin,
+            " ",
+            "safety_k_position:=",
+            safety_k_position,
+            " ",
+            "name:=",
+            "ur",
+            " ",
+            "ur_type:=",
+            ur_type,
+            " ",
+            "tf_prefix:=",
+            tf_prefix,
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("ur_description"), "rviz", "view_robot.rviz"]
+    )
+
+    joint_state_publisher_node = Node(
+        package="joint_state_publisher_gui",
+        executable="joint_state_publisher_gui",
+    )
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+    )
+
+    nodes_to_start = [
+        joint_state_publisher_node,
+        robot_state_publisher_node,
+        rviz_node,
+    ]
+
+    return LaunchDescription(declared_arguments + nodes_to_start)

ur_ros2_control_support/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ur_ros2_control_support</name>
+  <version>2.0.1</version>
+  <description>ros2_control hardware description for Universal Robots</description>
+  <maintainer email="[email protected]">Felix Exner</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

urdf/ur.urdf.xacro → ur_ros2_control_support/urdf/ur.urdf.xacro

@@ -3,7 +3,7 @@
    <!-- robot name parameter -->
    <xacro:arg name="name" default="ur"/>
    <!-- import main macro -->
-   <xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
+   <xacro:include filename="$(find ur_ros2_control_support)/urdf/ur_macro.xacro"/>
 
    <!-- possible 'ur_type' values: ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e -->
    <!-- the default value should raise an error in case this was called without defining the type -->
@@ -55,7 +55,7 @@
    <link name="world" />
 
    <!-- arm -->
-   <xacro:ur_robot
+   <xacro:ur_robot_ros2_controlled
      name="$(arg name)"
      tf_prefix="$(arg tf_prefix)"
      parent="world"
@@ -90,7 +90,7 @@
      script_command_port="$(arg script_command_port)"
      >
      <origin xyz="0 0 0" rpy="0 0 0" />          <!-- position robot in the world -->
-   </xacro:ur_robot>
+   </xacro:ur_robot_ros2_controlled>
 
    <xacro:if value="$(arg sim_gazebo)">
     <!-- Gazebo plugins -->