add modular robot type

rbs_arm/CMakeLists.txt

@@ -11,7 +11,7 @@ find_package(ament_cmake REQUIRED)
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
-install(DIRECTORY urdf meshes 
+install(DIRECTORY urdf meshes launch config materials
 DESTINATION share/${PROJECT_NAME})
 
 if(BUILD_TESTING)

rbs_arm/config/rbs_arm_controllers.yaml

@@ -0,0 +1,31 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+    forward_position_controller:
+      type: position_controllers/JointGroupPositionController
+    cartesian_compliance_controller:
+      type: cartesian_compliance_controller/CartesianComplianceController
+
+joint_trajetctory_controller:
+  ros__parameters:
+    joints:
+      - $(var prefix)fork0_link_joint
+      - $(var prefix)main0_link_joint
+      - $(var prefix)fork1_link_joint
+      - $(var prefix)main1_link_joint
+      - $(var prefix)fork2_link_joint
+      - $(var prefix)ee_link_joint
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+
+#TODO: add cartesian_compliance_controller

rbs_arm/config/rbs_arm_controllers_gazebosim.yaml

@@ -0,0 +1,28 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 1000
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - fork0_link_joint
+      - main0_link_joint
+      - fork1_link_joint
+      - main1_link_joint
+      - fork2_link_joint
+      - ee_link_joint
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+
+
+#TODO: add cartesian_compliance_controller

rbs_arm/config/view_robot.rviz

@@ -0,0 +1,241 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 1362
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ee_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fork.002_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fork.003_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fork_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        main.001_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        main_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tool0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        ee_link:
+          Value: true
+        fork.002_link:
+          Value: true
+        fork.003_link:
+          Value: true
+        fork_link:
+          Value: true
+        main.001_link:
+          Value: true
+        main_link:
+          Value: true
+        tool0:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 0.5
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        world:
+          base_link:
+            fork_link:
+              main_link:
+                fork.002_link:
+                  main.001_link:
+                    fork.003_link:
+                      ee_link:
+                        tool0:
+                          {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - 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
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 2.805307626724243
+      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
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6503983736038208
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.7503929734230042
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1862
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000020000000648fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000af00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006d000006480000018000fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000016600000648fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006d000006480000012f00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000c0800000058fc0100000002fb0000000800540069006d0065010000000000000c08000004d300fffffffb0000000800540069006d00650100000000000004500000000000000000000009fc0000064800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 3080
+  X: 120
+  Y: 64

rbs_arm/launch/display.launch.py

@@ -0,0 +1,76 @@
+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 = []
+    # General arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_package",
+            default_value="rbs_arm",
+            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="rbs_arm_modular.xacro",
+            description="URDF/XACRO description file with the robot.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "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.",
+        )
+    )
+    
+    description_package = LaunchConfiguration("description_package")
+    description_file = LaunchConfiguration("description_file")
+
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare(description_package), "config", "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)

rbs_arm/launch/display_gazebosim.launch.py

@@ -0,0 +1,130 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+    declared_arguments = []
+    # General arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_package",
+            default_value="rbs_arm",
+            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="rbs_arm_modular.xacro",
+            description="URDF/XACRO description file with the robot.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "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.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "hardware",
+            default_value='',
+            # choices=["mock", "gazebo"],
+            description="The name of hardware", 
+        )
+    )
+    prefix = LaunchConfiguration("prefix")
+    description_package = LaunchConfiguration("description_package")
+    description_file = LaunchConfiguration("description_file")
+    hardware = LaunchConfiguration("hardware")
+
+    
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare(description_package), "config", "view_robot.rviz"]
+    )
+    initial_joint_controllers = PathJoinSubstitution(
+        [FindPackageShare("rbs_arm"), "config", "rbs_arm_controllers_gazebosim.yaml"]
+    )
+
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
+            " ",
+            "gripper_name:=", "", " ",
+            "prefix:=", prefix, " ",
+            "hardware:=", hardware, " ",
+            "simulation_controllers:=", initial_joint_controllers, " ",
+
+        ]
+    )
+
+    robot_description = {"robot_description": robot_description_content}
+
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    load_joint_trajectory_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_trajectory_controller'],
+        output='screen'
+    ) 
+
+    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],
+    )
+
+    gazebo = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [os.path.join(get_package_share_directory('ros_gz_sim'),
+                            'launch', 'gz_sim.launch.py')]),
+            launch_arguments=[('gz_args', [' -r ', "empty.sdf"])],
+    )
+    # Spawn robot
+    gazebo_spawn_robot = Node(
+        package='ros_gz_sim', 
+        executable='create',
+        arguments=[
+            '-name', "rbs_arm",
+            '-x', '0.0',
+            '-z', '0.0',
+            '-y', '0.0',
+            '-string', robot_description_content
+        ],
+        output='screen'
+    )
+
+    nodes_to_start = [
+        load_joint_state_broadcaster,
+        load_joint_trajectory_controller,
+        robot_state_publisher_node,
+        rviz_node,
+        gazebo,
+        gazebo_spawn_robot
+    ]
+
+    return LaunchDescription(declared_arguments + nodes_to_start)

rbs_arm/launch/get_urdf.launch.py

@@ -0,0 +1,26 @@
+import os
+import xacro
+import launch
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+
+    initial_joint_controllers_file_path = os.path.join(
+        get_package_share_directory('rbs_arm'), 'config', 'rbs_arm_controllers_gazebosim.yaml'
+    )
+
+    doc = xacro.process_file(os.path.join(get_package_share_directory("rbs_arm"), 'urdf', 'rbs_arm_modular.xacro'), mappings={
+        "gripper_name": "rbs-gripper",
+        "prefix": "",
+        "hardware": "gazebo",
+        "simulation_controllers": initial_joint_controllers_file_path
+    })
+
+    robot_desc = doc.toprettyxml(indent='  ')
+    part1, part2 = robot_desc.split('?>')
+    m_encoding = 'UTF-8'
+    with open("current.urdf", 'w') as xfile:
+        xfile.write(part1 + 'encoding=\"{}\"?>\n'.format(m_encoding) + part2)
+        xfile.close()
+    return launch.LaunchDescription()

rbs_arm/package.xml

@@ -8,6 +8,11 @@
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  
+  <depend>ros-gz</depend>
+  <depend>ros2-control</depend>
+  <depend>ros2-controllers</depend>
+  <depend>joint-trajectory-controller</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rbs_arm/urdf/current.urdf

@@ -1,464 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from /home/bill-finger/ros_humble_ws/install/rbs_arm/share/rbs_arm/urdf/rbs_arm.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="rbs_arm">
-  <ros2_control name="rbs_arm" type="system">
-    <hardware>
-    </hardware>
-    <joint name="fork_link_joint">
-      <command_interface name="position">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <param name="p">1000</param>
-      <param name="d">0.5</param>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="main_link_joint">
-      <command_interface name="position">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <param name="p">1000</param>
-      <param name="d">0.5</param>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="fork.002_link_joint">
-      <command_interface name="position">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <param name="p">500</param>
-      <param name="d">0.5</param>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="main.001_link_joint">
-      <command_interface name="position">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <param name="p">100</param>
-      <param name="d">0.5</param>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="fork.003_link_joint">
-      <command_interface name="position">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <param name="p">10</param>
-      <param name="d">0.5</param>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="ee_link_joint">
-      <command_interface name="position">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <param name="p">10</param>
-      <param name="d">0.5</param>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-  </ros2_control>
-  <joint name="base_link_joint" type="fixed">
-    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
-    <parent link="world"/>
-    <child link="base_link"/>
-  </joint>
-  <joint name="ee_link_joint" type="revolute">
-    <limit effort="0.00000" lower="-3.14000" upper="3.14000" velocity="0.00000"/>
-    <origin rpy="-0.00000 0.00000 0.00000" xyz="0.00025 -0.10000 0.09473"/>
-    <parent link="fork.003_link"/>
-    <child link="ee_link"/>
-    <axis xyz="1.00000 0.00000 0.00000"/>
-  </joint>
-  <joint name="fork.002_link_joint" type="revolute">
-    <limit effort="0.00000" lower="-3.14000" upper="3.14000" velocity="0.00000"/>
-    <origin rpy="3.14159 0.00000 -3.14159" xyz="0.00000 0.00000 -0.13300"/>
-    <parent link="main_link"/>
-    <child link="fork.002_link"/>
-    <axis xyz="0.00000 0.00000 1.00000"/>
-  </joint>
-  <joint name="fork.003_link_joint" type="revolute">
-    <limit effort="0.00000" lower="-3.14000" upper="3.14000" velocity="0.00000"/>
-    <origin rpy="3.14159 0.00000 -3.14159" xyz="0.00000 0.00000 -0.13300"/>
-    <parent link="main.001_link"/>
-    <child link="fork.003_link"/>
-    <axis xyz="0.00000 0.00000 1.00000"/>
-  </joint>
-  <joint name="fork_link_joint" type="revolute">
-    <limit effort="0.00000" lower="-3.14000" upper="3.14000" velocity="0.00000"/>
-    <origin rpy="0.00000 0.00000 -0.00000" xyz="0.00000 -0.00000 0.17833"/>
-    <parent link="base_link"/>
-    <child link="fork_link"/>
-    <axis xyz="0.00000 0.00000 1.00000"/>
-  </joint>
-  <joint name="main.001_link_joint" type="revolute">
-    <limit effort="0.00000" lower="-3.14000" upper="3.14000" velocity="0.00000"/>
-    <origin rpy="-0.00000 0.00000 0.00000" xyz="0.00025 -0.10000 0.09400"/>
-    <parent link="fork.002_link"/>
-    <child link="main.001_link"/>
-    <axis xyz="1.00000 0.00000 0.00000"/>
-  </joint>
-  <joint name="main_link_joint" type="revolute">
-    <limit effort="0.00000" lower="-3.14000" upper="3.14000" velocity="0.00000"/>
-    <origin rpy="0.00000 0.00000 0.00000" xyz="0.00025 -0.10000 0.09400"/>
-    <parent link="fork_link"/>
-    <child link="main_link"/>
-    <axis xyz="1.00000 0.00000 0.00000"/>
-  </joint>
-  <joint name="tool0_joint" type="fixed">
-    <origin rpy="-0.00000 0.00000 -0.00000" xyz="0.00001 -0.00010 -0.11000"/>
-    <parent link="ee_link"/>
-    <child link="tool0"/>
-  </joint>
-  <link name="base_link">
-    <collision name="base_collision">
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/base.dae" scale="0.00100 0.00100 0.00100"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <inertia ixx="0.00476" ixy="-0.00000" ixz="0.00001" iyy="0.00471" iyz="0.00012" izz="0.00129"/>
-      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.00004 -0.00477 0.09651"/>
-      <mass value="1.00000"/>
-    </inertial>
-    <visual name="base_visual">
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
-      <material name="M_start_link"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/start_link.dae" scale="1.00000 1.00000 1.00000"/>
-      </geometry>
-    </visual>
-  </link>
-  <link name="ee_link">
-    <collision name="ee_collision">
-      <origin rpy="-0.00000 0.00000 -0.00000" xyz="0.00000 -0.00000 0.00000"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/ee.dae" scale="0.00100 0.00100 0.00100"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <inertia ixx="0.00329" ixy="-0.00000" ixz="0.00001" iyy="0.00319" iyz="-0.00002" izz="0.00111"/>
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00003 0.00443 -0.03359"/>
-      <mass value="1.00000"/>
-    </inertial>
-    <visual name="ee_visual">
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 -0.00000 0.00000"/>
-      <material name="M_tail_link"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/tail_link.dae" scale="1.00000 1.00000 1.00000"/>
-      </geometry>
-    </visual>
-  </link>
-  <link name="fork.002_link">
-    <collision name="fork.002_collision">
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 -0.00000 0.00000"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/fork.001.dae" scale="0.00100 0.00100 0.00100"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <inertia ixx="0.00320" ixy="-0.00014" ixz="-0.00007" iyy="0.00243" iyz="-0.00122" izz="0.00329"/>
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00582 -0.04787 0.05532"/>
-      <mass value="1.00000"/>
-    </inertial>
-    <visual name="fork.002_visual">
-      <origin rpy="0.00000 -0.00000 -0.00000" xyz="0.00000 -0.00000 0.00000"/>
-      <material name="M_fork"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/fork.dae" scale="1.00000 1.00000 1.00000"/>
-      </geometry>
-    </visual>
-  </link>
-  <link name="fork.003_link">
-    <collision name="fork.003_collision">
-      <origin rpy="-0.00000 0.00000 0.00000" xyz="0.00000 -0.00000 0.00000"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/fork.003.dae" scale="0.00100 0.00100 0.00100"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <inertia ixx="0.00320" ixy="0.00014" ixz="-0.00007" iyy="0.00243" iyz="0.00122" izz="0.00329"/>
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00582 -0.04787 0.05532"/>
-      <mass value="1.00000"/>
-    </inertial>
-    <visual name="fork.003_visual">
-      <origin rpy="-0.00000 0.00000 -0.00000" xyz="0.00000 -0.00000 0.00000"/>
-      <material name="M_fork"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/fork.dae" scale="1.00000 1.00000 1.00000"/>
-      </geometry>
-    </visual>
-  </link>
-  <link name="fork_link">
-    <collision name="fork_collision">
-      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.00000 0.00000 0.00000"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/fork.002.dae" scale="0.00100 0.00100 0.00100"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <inertia ixx="0.00320" ixy="0.00014" ixz="-0.00007" iyy="0.00243" iyz="0.00122" izz="0.00329"/>
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00582 -0.04787 0.05532"/>
-      <mass value="1.00000"/>
-    </inertial>
-    <visual name="fork_visual">
-      <origin rpy="0.00000 0.00000 -0.00000" xyz="-0.00000 0.00000 0.00000"/>
-      <material name="M_fork"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/fork.dae" scale="1.00000 1.00000 1.00000"/>
-      </geometry>
-    </visual>
-  </link>
-  <link name="main.001_link">
-    <collision name="main.001_collision">
-      <origin rpy="0.00000 0.00000 3.14159" xyz="0.00000 0.00000 0.00000"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/main.dae" scale="0.00100 0.00100 0.00100"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <inertia ixx="0.00307" ixy="-0.00001" ixz="0.00001" iyy="0.00299" iyz="0.00006" izz="0.00118"/>
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00001 -0.00520 -0.04713"/>
-      <mass value="1.00000"/>
-    </inertial>
-    <visual name="main.001_visual">
-      <origin rpy="0.00000 -0.00000 -0.00000" xyz="0.00000 0.00000 0.00000"/>
-      <material name="M_main_link"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/main_link.dae" scale="1.00000 1.00000 1.00000"/>
-      </geometry>
-    </visual>
-  </link>
-  <link name="main_link">
-    <collision name="main_collision">
-      <origin rpy="-0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/main.001.dae" scale="0.00100 0.00100 0.00100"/>
-      </geometry>
-    </collision>
-    <visual name="main_visual">
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
-      <material name="M_main_link"/>
-      <geometry>
-        <mesh filename="package://rbs_arm/meshes/dae/main_link.dae" scale="1.00000 1.00000 1.00000"/>
-      </geometry>
-    </visual>
-  </link>
-  <link name="tool0"/>
-  <link name="world"/>
-  <material name="M_fork">
-    <color rgba="0.80000 0.80000 0.80000 1.00000"/>
-  </material>
-  <material name="M_main_link">
-    <color rgba="0.80000 0.80000 0.80000 1.00000"/>
-  </material>
-  <material name="M_start_link">
-    <color rgba="0.80000 0.80000 0.80000 1.00000"/>
-  </material>
-  <material name="M_tail_link">
-    <color rgba="0.80000 0.80000 0.80000 1.00000"/>
-  </material>
-  <ros2_control name="rbs_gripper" type="system">
-    <hardware>
-    </hardware>
-    <joint name="grip_rotator_link_joint">
-      <command_interface name="position">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <param name="p">10</param>
-      <param name="d">0.5</param>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="grip2_link_joint">
-      <command_interface name="position">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <param name="p">10</param>
-      <param name="d">0.5</param>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="grip_link_joint">
-      <param name="mimic">grip2_link_joint</param>
-      <param name="multiplier">1</param>
-      <command_interface name="position">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <param name="p">10</param>
-      <param name="d">0.5</param>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-  </ros2_control>
-  <joint name="grip2_link_joint" type="prismatic">
-    <limit effort="0.00000" lower="0.00000" upper="0.06400" velocity="0.00000"/>
-    <origin rpy="0.00000 0.00000 -3.14159" xyz="-0.06800 -0.02375 0.00500"/>
-    <parent link="grip_rotator_link"/>
-    <child link="grip2_link"/>
-    <axis xyz="-1.00000 0.00000 0.00000"/>
-  </joint>
-  <joint name="grip_link_joint" type="prismatic">
-    <limit effort="0.00000" lower="0.00000" upper="0.06400" velocity="0.00000"/>
-    <mimic joint="grip2_link_joint" multiplier="1.00000" offset="0.00000"/>
-    <origin rpy="0.00000 0.00000 0.00000" xyz="0.06800 0.02375 0.00500"/>
-    <parent link="grip_rotator_link"/>
-    <child link="grip_link"/>
-    <axis xyz="-1.00000 0.00000 0.00000"/>
-  </joint>
-  <joint name="grip_rotator_link_joint" type="continuous">
-    <limit effort="0.00000" lower="0" upper="0" velocity="0.00000"/>
-    <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.10950"/>
-    <parent link="mount_link"/>
-    <child link="grip_rotator_link"/>
-    <axis xyz="0.00000 0.00000 1.00000"/>
-  </joint>
-  <joint name="mount_joint" type="fixed">
-    <origin rpy="0.0 3.14159 0.0" xyz="0.0 0.0 0.0"/>
-    <parent link="tool0"/>
-    <child link="mount_link"/>
-  </joint>
-  <link name="grip2_link">
-    <collision name="grip2_collision">
-      <origin rpy="0.00000 0.00000 -0.00000" xyz="0.00000 0.00000 0.00000"/>
-      <geometry>
-        <mesh filename="package://rbs_gripper/meshes/stl/grip.001.stl" scale="0.00100 0.00100 0.00100"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <inertia ixx="0.00028" ixy="-0.00003" ixz="-0.00010" iyy="0.00051" iyz="-0.00007" izz="0.00032"/>
-      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.06460 -0.00553 0.02536"/>
-      <mass value="0.45100"/>
-    </inertial>
-    <visual name="grip2_visual">
-      <origin rpy="0.00000 0.00000 -0.00000" xyz="0.00000 0.00000 0.00000"/>
-      <material name="M_grip_tool"/>
-      <geometry>
-        <mesh filename="package://rbs_gripper/meshes/stl/grip.006.stl" scale="1.00000 1.00000 1.00000"/>
-      </geometry>
-    </visual>
-  </link>
-  <link name="grip_link">
-    <collision name="grip_collision">
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
-      <geometry>
-        <mesh filename="package://rbs_gripper/meshes/stl/grip.stl" scale="0.00100 0.00100 0.00100"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <inertia ixx="0.00028" ixy="-0.00003" ixz="0.00010" iyy="0.00051" iyz="0.00007" izz="0.00032"/>
-      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.06460 -0.00553 0.02536"/>
-      <mass value="0.45100"/>
-    </inertial>
-    <visual name="grip_visual">
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
-      <material name="M_grip_tool"/>
-      <geometry>
-        <mesh filename="package://rbs_gripper/meshes/stl/grip.005.stl" scale="1.00000 1.00000 1.00000"/>
-      </geometry>
-    </visual>
-  </link>
-  <link name="grip_rotator_link">
-    <collision name="grip_rotator_collision">
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 -0.00000"/>
-      <geometry>
-        <mesh filename="package://rbs_gripper/meshes/stl/grip_rotator.stl" scale="0.00100 0.00100 0.00100"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <inertia ixx="0.00100" ixy="0.00000" ixz="0.00000" iyy="0.00100" iyz="0.00000" izz="0.00100"/>
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 -0.00000"/>
-      <mass value="0.45100"/>
-    </inertial>
-    <visual name="grip_rotator_visual">
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 -0.00000"/>
-      <material name="M_grip_tool"/>
-      <geometry>
-        <mesh filename="package://rbs_gripper/meshes/stl/Part__Feature007.003.stl" scale="1.00000 1.00000 1.00000"/>
-      </geometry>
-    </visual>
-  </link>
-  <link name="mount_link">
-    <collision name="mount_collision">
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
-      <geometry>
-        <mesh filename="package://rbs_gripper/meshes/stl/mount.stl" scale="0.00100 0.00100 0.00100"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <inertia ixx="0.00064" ixy="-0.00002" ixz="-0.00005" iyy="0.00163" iyz="-0.00000" izz="0.00158"/>
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.03168 0.00121 0.05940"/>
-      <mass value="0.45100"/>
-    </inertial>
-    <visual name="mount_visual">
-      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
-      <material name="M_grip_tool"/>
-      <geometry>
-        <mesh filename="package://rbs_gripper/meshes/stl/Part__Feature.003.stl" scale="1.00000 1.00000 1.00000"/>
-      </geometry>
-    </visual>
-  </link>
-  <material name="M_grip_tool">
-    <color rgba="0.80000 0.80000 0.80000 1.00000"/>
-  </material>
-</robot>

rbs_arm/urdf/inc/joint_hardware.xacro

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/joint_interface.xacro" />
+    <xacro:macro name="joint_hardware" params="joint_name hardware p d">
+        <ros2_control name="${joint_name}_hardware_interface" type="actuator">
+            <hardware>
+                <xacro:if value="${hardware=='gazebo'}">
+                    <plugin>ign_ros2_control/IgnitionSystem</plugin>
+                </xacro:if>
+                <xacro:if value="${hardware=='mock'}">
+                    <plugin>mock_components/GenericSystem</plugin>
+                </xacro:if>
+            </hardware>
+            <xacro:joint_interface name="${joint_name}" p="${p}" d="${d}" />
+        </ros2_control>
+    </xacro:macro>
+</robot>

rbs_arm/urdf/inc/materials.xacro

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="rbs_materials" params="link_name link_type">
+    <gazebo reference="${link_name}">
+      <visual>
+        <material>
+          <diffuse>0.8 0.8 0.8 1</diffuse>
+          <specular>0.6 0.6 0.6 1</specular>
+          <ambient>1.0 1.0 1.0</ambient>
+          <lighting>true</lighting>
+          <emissive>0 0 0 1</emissive>
+          <pbr>
+            <metal>
+              <albedo_map>$(find rbs_arm)/materials/textures/${link_type}_d.png</albedo_map>
+              <normal_map>$(find rbs_arm)/materials/textures/${link_type}_n.png</normal_map>
+              <ambient_occlusion_map>$(find rbs_arm)/materials/textures/${link_type}_ao.png</ambient_occlusion_map>
+              <roughness_map>$(find rbs_arm)/materials/textures/${link_type}_r.png</roughness_map>
+            </metal>
+          </pbr>
+        </material>
+      </visual>
+    </gazebo>
+  </xacro:macro>
+</robot>

rbs_arm/urdf/inc/rbs_base_link.xacro

@@ -0,0 +1,42 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="base_link" params="parent prefix *origin hardware">
+
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/joint_hardware.xacro" />
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/materials.xacro"/>
+
+    <joint name="${prefix}base_link_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent}" />
+      <child link="${prefix}base_link" />
+    </joint>
+
+    <link name="${prefix}base_link">
+      <collision name="${prefix}base_link_collision">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/collision/start_link.stl"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </collision>
+      <inertial>
+        <inertia ixx="0.00503302470272442" ixy="0.000343817346410954" ixz="-4.74990755448368E-06"
+          iyy="0.00337962410057753" iyz="-2.3099255620051E-05" izz="0.00405858207282473" />
+        <origin rpy="0.00000 0.00000 0.00000"
+          xyz="-0.000297002857922682 0.0964721185617698 -0.000361033370053684" />
+        <mass value="1.88031044620482" />
+      </inertial>
+      <visual name="${prefix}base_link_visual">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/visual/start_link.dae"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </visual>
+    </link>
+
+    <!-- <xacro:joint_hardware joint_name="${prefix}base_link" hardware="${hardware}" p="100"/> -->
+    <xacro:rbs_materials link_name="${prefix}base_link" link_type="base_link" />
+
+  </xacro:macro>
+</robot>

rbs_arm/urdf/inc/rbs_ee_link.xacro

@@ -0,0 +1,52 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="ee_link" params="prefix parent hardware">
+
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/joint_hardware.xacro" />
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/materials.xacro" />
+
+    <joint name="${prefix}ee_link_joint" type="revolute">
+      <limit lower="-1.5708" upper="3.14159" effort="78" velocity="0.52" />
+      <origin rpy="0.00000 0.00000 0.00000" xyz="0.10000 0.00000 0.09473" />
+      <parent link="${prefix}${parent}" />
+      <child link="${prefix}ee_link" />
+      <axis xyz="0 1 0" />
+    </joint>
+
+    <joint name="${prefix}tool0_joint" type="fixed">
+      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.11000" />
+      <parent link="${prefix}ee_link" />
+      <child link="${prefix}tool0" />
+    </joint>
+
+    <link name="${prefix}tool0" />
+
+    <link name="${prefix}ee_link">
+      <collision name="${prefix}ee_link_collision">
+        <origin rpy="-0.00000 0.00000 -0.00000" xyz="0.00000 -0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/collision/tail_link.stl"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </collision>
+      <inertial>
+        <inertia ixx="0.00147695259043549" ixy="-2.66894744420299E-05" ixz="-4.40871314563273E-05"
+          iyy="0.00135500487881796" iyz="-3.19001462979333E-05" izz="0.00087582892706912" />
+        <origin rpy="0.00000 0.00000 0.00000"
+          xyz="-9.7531539777207E-06 -0.000888494418875867 0.0342332199538358" />
+        <mass value="1.88031044620482" />
+      </inertial>
+      <visual name="${prefix}ee_link_visual">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 -0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/visual/tail_link.dae"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </visual>
+    </link>
+
+    <xacro:joint_hardware joint_name="${prefix}ee_link_joint" hardware="${hardware}" p="10" d="0.5" />
+    <xacro:rbs_materials link_name="${prefix}ee_link" link_type="ee_link" />
+
+  </xacro:macro>
+</robot>

rbs_arm/urdf/inc/rbs_fork_link.xacro

@@ -0,0 +1,49 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="fork_link" params="prefix parent name hardware">
+
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/joint_hardware.xacro" />
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/materials.xacro" />
+
+    <joint name="${prefix}${name}_joint" type="revolute">
+      <limit lower="-3.14159" upper="3.14159" effort="78" velocity="0.52" />
+      <xacro:if value="${parent=='base_link'}">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.17833" />
+      </xacro:if>
+      <xacro:unless value="${parent=='base_link'}">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.13300" />
+      </xacro:unless>
+      <parent link="${prefix}${parent}" />
+      <child link="${prefix}${name}" />
+      <axis xyz="0 0 1" />
+    </joint>
+
+    <link name="${prefix}${name}">
+      <collision name="${prefix}${name}_collision">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 -0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/collision/fork.stl"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </collision>
+      <inertial>
+        <inertia ixx="0.00367804533572758" ixy="1.04277925521833E-05" ixz="-0.00149971410403071"
+          iyy="0.00415208849477534" iyz="-0.00122" izz="0.00329" />
+        <origin rpy="0.00000 0.00000 0.00000"
+          xyz="0.0472051139085306 0.00208890925682996 0.0557265410642575" />
+        <mass value="1.12472202892859" />
+      </inertial>
+      <visual name="${prefix}${name}_visual">
+        <origin rpy="0.00000 -0.00000 -0.00000" xyz="0.00000 -0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/visual/fork.dae"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </visual>
+    </link>
+
+    <xacro:joint_hardware joint_name="${prefix}${name}_joint" hardware="${hardware}" p="500" d="0.5" />
+    <xacro:rbs_materials link_name="${prefix}${name}" link_type="fork" />
+
+  </xacro:macro>
+</robot>

rbs_arm/urdf/inc/rbs_main_link.xacro

@@ -0,0 +1,44 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="main_link" params="prefix parent name hardware">
+
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/joint_hardware.xacro" />
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/materials.xacro"/>
+
+    <joint name="${prefix}${name}_joint" type="revolute">
+      <limit lower="-1.5708" upper="3.14159" effort="78" velocity="0.52" />
+      <origin rpy="-0.00000 0.00000 0.00000" xyz="0.10000 0.00000 0.09400" />
+      <parent link="${prefix}${parent}" />
+      <child link="${prefix}${name}" />
+      <axis xyz="0 1 0" />
+    </joint>
+
+    <link name="${prefix}${name}">
+      <collision name="${prefix}${name}_collision">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/collision/main_link.stl"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </collision>
+      <inertial>
+        <inertia ixx="0.00315699373090845" ixy="2.84713820858537E-05" ixz="-7.01601261191721E-05"
+          iyy="0.00343729241263707" iyz="-0.000101485203138902" izz="0.00125534890134052" />
+        <origin rpy="0.00000 0.00000 0.00000"
+          xyz="0.00186712264682627 -0.000412152188777604 0.0516389446895805" />
+        <mass value="1.58688811563124" />
+      </inertial>
+      <visual name="${prefix}${name}_visual">
+        <origin rpy="0.00000 -0.00000 -0.00000" xyz="0.00000 0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/visual/main_link.dae"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </visual>
+    </link>
+
+    <xacro:joint_hardware joint_name="${prefix}${name}_joint" hardware="${hardware}" p="100" d="0.5"/>
+    <xacro:rbs_materials link_name="${prefix}${name}" link_type="main_link" />
+
+  </xacro:macro>
+</robot>

rbs_arm/urdf/rbs_arm.xacro

@@ -1,28 +1,30 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rbs_arm">
-    <xacro:arg name="gripper_name" default="rbs_gripper"/>
-    <xacro:arg name="robot_prefix" default=""/>
-    <xacro:arg name="sim" default="fake"/>
+    <xacro:arg name="gripper_name" default="rbs_gripper" />
+    <xacro:arg name="hardware" default="fake" />
+    <xacro:arg name="simulation_controllers" default="" />
+    <xacro:arg name="prefix" default="" />
 
-    <!-- <xacro:property name="gripper_name_prop" value="$(arg gripper_name)"/> -->
+    <xacro:property name="hardware" default="$(arg hardware)" />
+    <xacro:property name="prefix" default="$(arg prefix)" />
+
+    <xacro:include filename="$(find rbs_arm)/urdf/rbs_arm_macro.xacro" />
     
-    <!-- <xacro:property name="gripper_filepath" value="$(find ${gripper_name_prop})/urdf/${gripper_name_prop}_macro.xacro"/> -->
-    
-    
-    <xacro:include filename="$(find rbs_arm)/urdf/rbs_arm_macro.xacro"/>
-    
-    <!-- <xacro:include filename="${gripper_filepath}"/> -->
-    
-    
-    <xacro:rbs_arm parent="world" sim="$(arg sim)" prefix="$(arg robot_prefix)">
-        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+    <link name="world" />
+    <!-- ROBOT-->
+    <xacro:rbs_arm parent="world" hardware="$(arg hardware)" prefix="$(arg prefix)">
+        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
     </xacro:rbs_arm>
+    <!-- GRIPPER-->
 
-
-    <!-- <xacro:gripper prefix="" parent="$(arg robot_prefix)tool0" sim="$(arg sim)">
-        <origin xyz="0.0 0.0 0.0" rpy="0.0 3.14159 0.0"/>
-    </xacro:gripper> -->
-
-
-
-</robot>
+    <!-- ADDITION-->
+    <xacro:if value="${hardware=='gazebo'}">
+        <gazebo>
+            <plugin filename="libign_ros2_control-system.so"
+                name="ign_ros2_control::IgnitionROS2ControlPlugin">
+                <parameters>$(arg simulation_controllers)</parameters>
+                <controller_manager_node_name>${prefix}/controller_manager</controller_manager_node_name>
+            </plugin>
+        </gazebo>
+    </xacro:if>
+</robot>

rbs_arm/urdf/rbs_arm_macro.xacro

@@ -1,217 +1,229 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <xacro:include filename="$(find rbs_arm)/urdf/rbs_arm_ros2_control.xacro"/>
-    
-    <xacro:macro name="rbs_arm" params="prefix *origin parent sim">
+  <xacro:include filename="$(find rbs_arm)/urdf/rbs_arm_ros2_control.xacro" />
+  <xacro:include filename="$(find rbs_arm)/urdf/inc/materials.xacro" />
 
-        <xacro:rbs_arm_ros2_control name="rbs_arm" prefix="${prefix}" sim="${sim}"/>
-
-        <joint name="${prefix}${prefix}base_link_joint" type="fixed">
-            <xacro:insert_block name="origin"/>
-            <parent link="${parent}"/>
-            <child link="${prefix}base_link"/>
-          </joint>
-          <joint name="${prefix}ee_link_joint" type="revolute">
-            <limit lower="-1.5708" upper="3.14000" effort="78" velocity="0.52"/>
-            <origin rpy="-0.00000 0.00000 0.00000" xyz="0.00025 -0.10000 0.09473"/>
-            <parent link="${prefix}fork.003_link"/>
-            <child link="${prefix}ee_link"/>
-            <axis xyz="1.00000 0.00000 0.00000"/>
-          </joint>
-          <joint name="${prefix}fork.002_link_joint" type="revolute">
-            <limit lower="-3.14000" upper="3.14000" effort="78" velocity="0.52"/>
-            <origin rpy="3.14159 0.00000 -3.14159" xyz="0.00000 0.00000 -0.13300"/>
-            <parent link="${prefix}main_link"/>
-            <child link="${prefix}fork.002_link"/>
-            <axis xyz="0.00000 0.00000 1.00000"/>
-          </joint>
-          <joint name="${prefix}fork.003_link_joint" type="revolute">
-            <limit lower="-3.14000" upper="3.14000" effort="78" velocity="0.52"/>
-            <origin rpy="3.14159 0.00000 -3.14159" xyz="0.00000 0.00000 -0.13300"/>
-            <parent link="${prefix}main.001_link"/>
-            <child link="${prefix}fork.003_link"/>
-            <axis xyz="0.00000 0.00000 1.00000"/>
-          </joint>
-          <joint name="${prefix}fork_link_joint" type="revolute">
-            <limit lower="-3.14000" upper="3.14000" effort="78" velocity="0.52"/>
-            <origin rpy="0.00000 0.00000 -0.00000" xyz="0.00000 -0.00000 0.17833"/>
-            <parent link="${prefix}base_link"/>
-            <child link="${prefix}fork_link"/>
-            <axis xyz="0.00000 0.00000 1.00000"/>
-          </joint>
-          <joint name="${prefix}main.001_link_joint" type="revolute">
-            <limit lower="-1.5708" upper="3.14000" effort="78" velocity="0.52"/>
-            <origin rpy="-0.00000 0.00000 0.00000" xyz="0.00025 -0.10000 0.09400"/>
-            <parent link="${prefix}fork.002_link"/>
-            <child link="${prefix}main.001_link"/>
-            <axis xyz="1.00000 0.00000 0.00000"/>
-          </joint>
-          <joint name="${prefix}main_link_joint" type="revolute">
-            <limit lower="-1.5708" upper="3.14000" effort="78" velocity="0.52"/>
-            <origin rpy="0.00000 0.00000 0.00000" xyz="0.00025 -0.10000 0.09400"/>
-            <parent link="${prefix}fork_link"/>
-            <child link="${prefix}main_link"/>
-            <axis xyz="1.00000 0.00000 0.00000"/>
-          </joint>
-          <joint name="${prefix}tool0_joint" type="fixed">
-            <origin rpy="-0.00000 0.00000 -0.00000" xyz="0.00001 -0.00010 -0.11000"/>
-            <parent link="${prefix}ee_link"/>
-            <child link="${prefix}tool0"/>
-          </joint>
-          <link name="${prefix}base_link">
-            <collision name="${prefix}base_collision">
-              <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/collision/base.stl" scale="0.00100 0.00100 0.00100"/>
-              </geometry>
-            </collision>
-            <inertial>
-              <inertia ixx="0.00503302470272442" ixy="0.000343817346410954" ixz="-4.74990755448368E-06" iyy="0.00337962410057753" iyz="-2.3099255620051E-05" izz="0.00405858207282473"/>
-              <origin rpy="0.00000 0.00000 0.00000" xyz="-0.000297002857922682 0.0964721185617698 -0.000361033370053684"/>
-              <mass value="1.88031044620482"/>
-            </inertial>
-            <visual name="${prefix}base_visual">
-              <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000"/>
-              <material name="M_start_link"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/visual/start_link.dae" scale="1.00000 1.00000 1.00000"/>
-              </geometry>
-            </visual>
-          </link>
-          <link name="${prefix}ee_link">
-            <collision name="${prefix}ee_collision">
-              <origin rpy="-0.00000 0.00000 -0.00000" xyz="0.00000 -0.00000 0.00000"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/collision/ee.stl" scale="0.00100 0.00100 0.00100"/>
-              </geometry>
-            </collision>
-            <inertial>
-              <inertia ixx="0.00147695259043549" ixy="-2.66894744420299E-05" ixz="-4.40871314563273E-05" iyy="0.00135500487881796" iyz="--3.19001462979333E-05" izz="0.00087582892706912"/>
-              <origin rpy="0.00000 0.00000 0.00000" xyz="-9.7531539777207E-06 -0.000888494418875867 0.0342332199538358"/>
-              <mass value="1.88031044620482"/>
-            </inertial>
-            <visual name="${prefix}ee_visual">
-              <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 -0.00000 0.00000"/>
-              <material name="M_tail_link"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/visual/tail_link.dae" scale="1.00000 1.00000 1.00000"/>
-              </geometry>
-            </visual>
-          </link>
-          <link name="${prefix}fork.002_link">
-            <collision name="${prefix}fork.002_collision">
-              <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 -0.00000 0.00000"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/collision/fork.001.stl" scale="0.00100 0.00100 0.00100"/>
-              </geometry>
-            </collision>
-            <inertial>
-              <inertia ixx="0.00367804533572758" ixy="1.04277925521833E-05" ixz="-0.00149971410403071" iyy="0.00415208849477534" iyz="-0.00122" izz="0.00329"/>
-              <origin rpy="0.00000 0.00000 0.00000" xyz="0.0472051139085306 0.00208890925682996 0.0557265410642575"/>
-              <mass value="1.12472202892859"/>
-            </inertial>
-            <visual name="${prefix}fork.002_visual">
-              <origin rpy="0.00000 -0.00000 -0.00000" xyz="0.00000 -0.00000 0.00000"/>
-              <material name="M_fork"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/visual/fork.dae" scale="1.00000 1.00000 1.00000"/>
-              </geometry>
-            </visual>
-          </link>
-          <link name="${prefix}fork.003_link">
-            <collision name="${prefix}fork.003_collision">
-              <origin rpy="-0.00000 0.00000 0.00000" xyz="0.00000 -0.00000 0.00000"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/collision/fork.003.stl" scale="0.00100 0.00100 0.00100"/>
-              </geometry>
-            </collision>
-            <inertial>
-              <inertia ixx="0.00367804533572758" ixy="1.04277925521833E-05" ixz="-0.00149971410403071" iyy="0.00415208849477534" iyz="-0.00122" izz="0.00329"/>
-              <origin rpy="0.00000 0.00000 0.00000" xyz="0.0472051139085306 0.00208890925682996 0.0557265410642575"/>
-              <mass value="1.12472202892859"/>
-            </inertial>
-            <visual name="${prefix}fork.003_visual">
-              <origin rpy="-0.00000 0.00000 -0.00000" xyz="0.00000 -0.00000 0.00000"/>
-              <material name="M_fork"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/visual/fork.dae" scale="1.00000 1.00000 1.00000"/>
-              </geometry>
-            </visual>
-          </link>
-          <link name="${prefix}fork_link">
-            <collision name="${prefix}fork_collision">
-              <origin rpy="0.00000 0.00000 0.00000" xyz="-0.00000 0.00000 0.00000"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/visual/fork.dae" scale="0.00100 0.00100 0.00100"/>
-              </geometry>
-            </collision>
-            <inertial>
-              <inertia ixx="0.00367804533572758" ixy="1.04277925521833E-05" ixz="-0.00149971410403071" iyy="0.00415208849477534" iyz="-0.00122" izz="0.00329"/>
-              <origin rpy="0.00000 0.00000 0.00000" xyz="0.0472051139085306 0.00208890925682996 0.0557265410642575"/>
-              <mass value="1.12472202892859"/>
-            </inertial>
-            <visual name="${prefix}fork_visual">
-              <origin rpy="0.00000 0.00000 -0.00000" xyz="-0.00000 0.00000 0.00000"/>
-              <material name="M_fork"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/visual/fork.dae" scale="1.00000 1.00000 1.00000"/>
-              </geometry>
-            </visual>
-          </link>
-          <link name="${prefix}main.001_link">
-            <collision name="${prefix}main.001_collision">
-              <origin rpy="0.00000 0.00000 3.14159" xyz="0.00000 0.00000 0.00000"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/collision/main.stl" scale="0.00100 0.00100 0.00100"/>
-              </geometry>
-            </collision>
-            <inertial>
-              <inertia ixx="0.00315699373090845" ixy="2.84713820858537E-05" ixz="-7.01601261191721E-05" iyy="0.00343729241263707" iyz="-0.000101485203138902" izz="0.00125534890134052"/>
-              <origin rpy="0.00000 0.00000 0.00000" xyz="0.00186712264682627 -0.000412152188777604 0.0516389446895805"/>
-              <mass value="1.58688811563124"/>
-            </inertial>
-            <visual name="${prefix}main.001_visual">
-              <origin rpy="0.00000 -0.00000 -0.00000" xyz="0.00000 0.00000 0.00000"/>
-              <material name="M_main_link"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/visual/main_link.dae" scale="1.00000 1.00000 1.00000"/>
-              </geometry>
-            </visual>
-          </link>
-          <link name="${prefix}main_link">
-            <collision name="${prefix}main_collision">
-              <origin rpy="0.00000 0.00000 3.14159" xyz="0.00000 0.00000 0.00000"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/collision/main.stl" scale="0.00100 0.00100 0.00100"/>
-              </geometry>
-            </collision>
-            <inertial>
-              <inertia ixx="0.00315699373090845" ixy="2.84713820858537E-05" ixz="-7.01601261191721E-05" iyy="0.00343729241263707" iyz="-0.000101485203138902" izz="0.00125534890134052"/>
-              <origin rpy="0.00000 0.00000 0.00000" xyz="0.00186712264682627 -0.000412152188777604 0.0516389446895805"/>
-              <mass value="1.58688811563124"/>
-            </inertial>
-            <visual name="${prefix}main_visual">
-              <origin rpy="0.00000 -0.00000 -0.00000" xyz="0.00000 0.00000 0.00000"/>
-              <material name="M_main_link"/>
-              <geometry>
-                <mesh filename="package://rbs_arm/meshes/visual/main_link.dae" scale="1.00000 1.00000 1.00000"/>
-              </geometry>
-            </visual>
-          </link>
-          <link name="${prefix}tool0"/>
-          <link name="${prefix}world"/>
-          <material name="M_fork">
-            <color rgba="0.80000 0.80000 0.80000 1.00000"/>
-          </material>
-          <material name="M_main_link">
-            <color rgba="0.80000 0.80000 0.80000 1.00000"/>
-          </material>
-          <material name="M_start_link">
-            <color rgba="0.80000 0.80000 0.80000 1.00000"/>
-          </material>
-          <material name="M_tail_link">
-            <color rgba="0.80000 0.80000 0.80000 1.00000"/>
-          </material>
-    </xacro:macro>
-    
-</robot>
+  <xacro:macro name="rbs_arm" params="prefix *origin parent hardware">
+    <!-- ros2_control description macros-->
+    <xacro:rbs_arm_ros2_control name="rbs_arm_hardware_interface" prefix="${prefix}"
+      hardware="${hardware}" />
+    <!-- JOINTS-->
+    <joint name="${prefix}base_link_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent}" />
+      <child link="${prefix}base_link" />
+    </joint>
+    <joint name="${prefix}ee_link_joint" type="revolute">
+      <limit lower="-1.5708" upper="3.14159" effort="78" velocity="0.52" />
+      <origin rpy="0.00000 0.00000 0.00000" xyz="0.10000 0.00000 0.09473" />
+      <parent link="${prefix}fork.003_link" />
+      <child link="${prefix}ee_link" />
+      <axis xyz="0 1 0" />
+    </joint>
+    <joint name="${prefix}fork002_link_joint" type="revolute">
+      <limit lower="-3.14159" upper="3.14159" effort="78" velocity="0.52" />
+      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.13300" />
+      <parent link="${prefix}main_link" />
+      <child link="${prefix}fork.002_link" />
+      <axis xyz="0 0 1" />
+    </joint>
+    <joint name="${prefix}fork003_link_joint" type="revolute">
+      <limit lower="-3.14159" upper="3.14159" effort="78" velocity="0.52" />
+      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.13300" />
+      <parent link="${prefix}main.001_link" />
+      <child link="${prefix}fork.003_link" />
+      <axis xyz="0 0 1" />
+    </joint>
+    <joint name="${prefix}fork001_link_joint" type="revolute">
+      <limit lower="-3.14159" upper="3.14159" effort="78" velocity="0.52" />
+      <origin rpy="0.00000 0.00000 -0.00000" xyz="0.00000 -0.00000 0.17833" />
+      <parent link="${prefix}base_link" />
+      <child link="${prefix}fork_link" />
+      <axis xyz="0 0 1" />
+    </joint>
+    <joint name="${prefix}main002_link_joint" type="revolute">
+      <limit lower="-1.5708" upper="3.14159" effort="78" velocity="0.52" />
+      <origin rpy="-0.00000 0.00000 0.00000" xyz="0.10000 0.00000 0.09400" />
+      <parent link="${prefix}fork.002_link" />
+      <child link="${prefix}main.001_link" />
+      <axis xyz="0 1 0" />
+    </joint>
+    <joint name="${prefix}main001_link_joint" type="revolute">
+      <limit lower="-1.5708" upper="3.14159" effort="78" velocity="0.52" />
+      <origin rpy="0.00000 0.00000 0.00000" xyz="0.10000 0.00000 0.09400" />
+      <parent link="${prefix}fork_link" />
+      <child link="${prefix}main_link" />
+      <axis xyz="0 1 0" />
+    </joint>
+    <joint name="${prefix}tool0_joint" type="fixed">
+      <origin rpy="-0.00000 0.00000 -0.00000" xyz="0.00001 -0.00010 0.11000" />
+      <parent link="${prefix}ee_link" />
+      <child link="${prefix}tool0" />
+    </joint>
+    <!-- LINKS-->
+    <link name="${prefix}base_link">
+      <collision name="${prefix}base_collision">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/collision/start_link.stl"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </collision>
+      <inertial>
+        <inertia ixx="0.00503302470272442" ixy="0.000343817346410954" ixz="-4.74990755448368E-06"
+          iyy="0.00337962410057753" iyz="-2.3099255620051E-05" izz="0.00405858207282473" />
+        <origin rpy="0.00000 0.00000 0.00000"
+          xyz="-0.000297002857922682 0.0964721185617698 -0.000361033370053684" />
+        <mass value="1.88031044620482" />
+      </inertial>
+      <visual name="${prefix}base_visual">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/visual/start_link.dae"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </visual>
+    </link>
+    <link name="${prefix}ee_link">
+      <collision name="${prefix}ee_collision">
+        <origin rpy="-0.00000 0.00000 -0.00000" xyz="0.00000 -0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/collision/tail_link.stl"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </collision>
+      <inertial>
+        <inertia ixx="0.00147695259043549" ixy="-2.66894744420299E-05" ixz="-4.40871314563273E-05"
+          iyy="0.00135500487881796" iyz="-3.19001462979333E-05" izz="0.00087582892706912" />
+        <origin rpy="0.00000 0.00000 0.00000"
+          xyz="-9.7531539777207E-06 -0.000888494418875867 0.0342332199538358" />
+        <mass value="1.88031044620482" />
+      </inertial>
+      <visual name="${prefix}ee_visual">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 -0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/visual/tail_link.dae"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </visual>
+    </link>
+    <link name="${prefix}fork.002_link">
+      <collision name="${prefix}fork.002_collision">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 -0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/collision/fork.stl"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </collision>
+      <inertial>
+        <inertia ixx="0.00367804533572758" ixy="1.04277925521833E-05" ixz="-0.00149971410403071"
+          iyy="0.00415208849477534" iyz="-0.00122" izz="0.00329" />
+        <origin rpy="0.00000 0.00000 0.00000"
+          xyz="0.0472051139085306 0.00208890925682996 0.0557265410642575" />
+        <mass value="1.12472202892859" />
+      </inertial>
+      <visual name="${prefix}fork.002_visual">
+        <origin rpy="0.00000 -0.00000 -0.00000" xyz="0.00000 -0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/visual/fork.dae"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </visual>
+    </link>
+    <link name="${prefix}fork.003_link">
+      <collision name="${prefix}fork.003_collision">
+        <origin rpy="-0.00000 0.00000 0.00000" xyz="0.00000 -0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/collision/fork.stl"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </collision>
+      <inertial>
+        <inertia ixx="0.00367804533572758" ixy="1.04277925521833E-05" ixz="-0.00149971410403071"
+          iyy="0.00415208849477534" iyz="-0.00122" izz="0.00329" />
+        <origin rpy="0.00000 0.00000 0.00000"
+          xyz="0.0472051139085306 0.00208890925682996 0.0557265410642575" />
+        <mass value="1.12472202892859" />
+      </inertial>
+      <visual name="${prefix}fork.003_visual">
+        <origin rpy="-0.00000 0.00000 -0.00000" xyz="0.00000 -0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/visual/fork.dae"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </visual>
+    </link>
+    <link name="${prefix}fork_link">
+      <collision name="${prefix}fork_collision">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="-0.00000 0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/collision/fork.stl"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </collision>
+      <inertial>
+        <inertia ixx="0.00367804533572758" ixy="1.04277925521833E-05" ixz="-0.00149971410403071"
+          iyy="0.00415208849477534" iyz="-0.00122" izz="0.00329" />
+        <origin rpy="0.00000 0.00000 0.00000"
+          xyz="0.0472051139085306 0.00208890925682996 0.0557265410642575" />
+        <mass value="1.12472202892859" />
+      </inertial>
+      <visual name="${prefix}fork_visual">
+        <origin rpy="0.00000 0.00000 -0.00000" xyz="-0.00000 0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/visual/fork.dae"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </visual>
+    </link>
+    <link name="${prefix}main.001_link">
+      <collision name="${prefix}main.001_collision">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/collision/main_link.stl"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </collision>
+      <inertial>
+        <inertia ixx="0.00315699373090845" ixy="2.84713820858537E-05" ixz="-7.01601261191721E-05"
+          iyy="0.00343729241263707" iyz="-0.000101485203138902" izz="0.00125534890134052" />
+        <origin rpy="0.00000 0.00000 0.00000"
+          xyz="0.00186712264682627 -0.000412152188777604 0.0516389446895805" />
+        <mass value="1.58688811563124" />
+      </inertial>
+      <visual name="${prefix}main.001_visual">
+        <origin rpy="0.00000 -0.00000 -0.00000" xyz="0.00000 0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/visual/main_link.dae"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </visual>
+    </link>
+    <link name="${prefix}main_link">
+      <collision name="${prefix}main_collision">
+        <origin rpy="0.00000 0.00000 0.00000" xyz="0.00000 0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/collision/main_link.stl"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </collision>
+      <inertial>
+        <inertia ixx="0.00315699373090845" ixy="2.84713820858537E-05" ixz="-7.01601261191721E-05"
+          iyy="0.00343729241263707" iyz="-0.000101485203138902" izz="0.00125534890134052" />
+        <origin rpy="0.00000 0.00000 0.00000"
+          xyz="0.00186712264682627 -0.000412152188777604 0.0516389446895805" />
+        <mass value="1.58688811563124" />
+      </inertial>
+      <visual name="${prefix}main_visual">
+        <origin rpy="0.00000 -0.00000 -0.00000" xyz="0.00000 0.00000 0.00000" />
+        <geometry>
+          <mesh filename="file://$(find rbs_arm)/meshes/visual/main_link.dae"
+            scale="1.00000 1.00000 1.00000" />
+        </geometry>
+      </visual>
+    </link>
+    <link name="${prefix}tool0" />
+    <!-- MATERIALS-->
+    <xacro:rbs_materials link_name="${prefix}main_link" link_type="main_link" />
+  </xacro:macro>
+</robot>

rbs_arm/urdf/rbs_arm_modular.xacro

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rbs_arm">
+  <xacro:arg name="gripper_name" default="" />
+  <xacro:arg name="hardware" default="gazebo" />
+  <xacro:arg name="simulation_controllers" default="" />
+  <xacro:arg name="prefix" default="" />
+
+  <xacro:include filename="$(find rbs_arm)/urdf/rbs_arm_modular_macro.xacro" />
+  <link name="world" />
+  <xacro:rbs_arm parent="world" prefix="$(arg prefix)" hardware="$(arg hardware)"
+    gripper_name="$(arg gripper_name)" controllers="$(arg simulation_controllers)" />
+</robot>

rbs_arm/urdf/rbs_arm_modular_macro.xacro

@@ -0,0 +1,40 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <xacro:macro name="rbs_arm" params="parent prefix hardware gripper_name controllers">
+   
+
+  <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_base_link.xacro" />
+  <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_ee_link.xacro" />
+  <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_fork_link.xacro" />
+  <xacro:include filename="$(find rbs_arm)/urdf/inc/rbs_main_link.xacro" />
+  <!-- origin to define robot position in the world-->
+  <xacro:base_link parent="${parent}" prefix="${prefix}" hardware="${hardware}">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </xacro:base_link>
+  <xacro:fork_link parent="base_link"  name="fork0_link" prefix="${prefix}" hardware="${hardware}" />
+  <xacro:main_link parent="fork0_link" name="main0_link" prefix="${prefix}" hardware="${hardware}" />
+  <xacro:fork_link parent="main0_link" name="fork1_link" prefix="${prefix}" hardware="${hardware}" />
+  <xacro:main_link parent="fork1_link" name="main1_link" prefix="${prefix}" hardware="${hardware}" />
+  <xacro:fork_link parent="main1_link" name="fork2_link" prefix="${prefix}" hardware="${hardware}" />
+  <!-- ee link also contain tool0-->
+  <xacro:ee_link parent="fork2_link" prefix="${prefix}" hardware="${hardware}" />
+
+  <xacro:if value="${gripper_name=='rbs_gripper'}">
+    <xacro:include filename="$(find rbs_gripper)/urdf/rbs_gripper_macro.xacro" />
+    <xacro:rbs_gripper prefix="${prefix}" gripper_name="rbs_gripper" parent="tool0"
+      hardware="${hardware}">
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+    </xacro:rbs_gripper>
+  </xacro:if>
+
+  <xacro:if value="${hardware=='gazebo'}">
+    <gazebo>
+      <plugin filename="libign_ros2_control-system.so"
+        name="ign_ros2_control::IgnitionROS2ControlPlugin">
+        <parameters>$(arg simulation_controllers)</parameters>
+        <controller_manager_node_name>${prefix}/controller_manager</controller_manager_node_name>
+      </plugin>
+    </gazebo>
+  </xacro:if>
+ </xacro:macro>
+</robot>

rbs_arm/urdf/rbs_arm_ros2_control.xacro

@@ -1,23 +1,22 @@
+<?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <xacro:include filename="$(find rbs_arm)/urdf/inc/joint_interface.xacro"/>
-    <xacro:macro name="rbs_arm_ros2_control" params="name sim prefix">
+    <xacro:include filename="$(find rbs_arm)/urdf/inc/joint_interface.xacro" />
+    <xacro:macro name="rbs_arm_ros2_control" params="name hardware prefix">
         <ros2_control name="${name}" type="system">
             <hardware>
-                <xacro:if value="${sim=='gazebo'}">
+                <xacro:if value="${hardware=='gazebo'}">
                     <plugin>ign_ros2_control/IgnitionSystem</plugin>
                 </xacro:if>
-                <xacro:unless value="${sim=='fake'}">
+                <xacro:if value="${hardware=='mock'}">
                     <plugin>mock_components/GenericSystem</plugin>
-                    <!-- <param name="mock_sensor_commands">${fake_sensor_commands}</param>
-                    <param name="state_following_offset">0.0</param> -->
-                </xacro:unless>
+                </xacro:if>
             </hardware>
-            <xacro:joint_interface name="${prefix}fork_link_joint"         p="1000"    d="0.5" />
-            <xacro:joint_interface name="${prefix}main_link_joint"         p="1000"    d="0.5" />
-            <xacro:joint_interface name="${prefix}fork.002_link_joint"     p="500"     d="0.5" />
-            <xacro:joint_interface name="${prefix}main.001_link_joint"     p="100"     d="0.5" />
-            <xacro:joint_interface name="${prefix}fork.003_link_joint"     p="10"      d="0.5" />
-            <xacro:joint_interface name="${prefix}ee_link_joint"           p="10"      d="0.5" />
+            <xacro:joint_interface name="${prefix}fork001_link_joint" p="1000" d="0.5" />
+            <xacro:joint_interface name="${prefix}main001_link_joint" p="1000" d="0.5" />
+            <xacro:joint_interface name="${prefix}fork002_link_joint" p="500" d="0.5" />
+            <xacro:joint_interface name="${prefix}main002_link_joint" p="100" d="0.5" />
+            <xacro:joint_interface name="${prefix}fork003_link_joint" p="10" d="0.5" />
+            <xacro:joint_interface name="${prefix}ee_link_joint" p="10" d="0.5" />
         </ros2_control>
     </xacro:macro>
-</robot>
+</robot>