Реализован спавн робота с контроллерами в виртуальной среде webots

README.md

@@ -0,0 +1,3 @@
+Подмена файла по пути обязательна: `/opt/ros/rolling/lib/webots_ros2_driver/ros2_supervisor.py`
+
+> необходимо `warn` заменить на `warning` в логере

src/iiwa_bringup/config/iiwa_controller.yaml

@@ -0,0 +1,30 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 31
+
+    joint_state_broadcaster:
+      type: "joint_state_broadcaster/JointStateBroadcaster"
+
+    joint_trajectory_controller:
+      type: "joint_trajectory_controller/JointTrajectoryController"
+
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+      - joint7  
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+
+    allow_partial_joints_goal: false
+    interpolate_from_desired_state: true

src/iiwa_bringup/launch/protospawner.launch.py

@@ -0,0 +1,170 @@
+# TODO: код на будущее
+
+import os
+import re
+from pathlib import Path
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    RegisterEventHandler,
+    OpaqueFunction,
+)
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+from webots_ros2_driver.webots_launcher import WebotsLauncher
+from webots_ros2_driver.urdf_spawner import URDFSpawner
+
+
+def _patch_proto_controller(proto_path: str):
+    pattern = r'(field\s+SFString\s+controller\s+)"void"'
+    replacement = r'\1"exec"'
+    proto = Path(proto_path)
+    text = proto.read_text(encoding='utf-8')
+    new_text, count = re.subn(pattern, replacement, text)
+
+    if count != 0:
+        proto.write_text(new_text, encoding='utf-8')
+
+
+def _after_xacro(context, *args, **kwargs):
+    """Выполняется после завершения xacro2proto: патчим proto и возвращаем actions (webots, rsp)."""
+    proto_path = kwargs['proto_path']
+    # проверим существование
+    if not Path(proto_path).exists():
+        raise RuntimeError(f"Expected proto file not found: {proto_path}")
+
+    # Патчим
+    _patch_proto_controller(proto_path)
+
+    # создаём действия, которые будут запущены ПОСЛЕ xacro2proto
+    world_path = LaunchConfiguration('world').perform(context)
+    webots = WebotsLauncher(world=world_path, 
+                            ros2_supervisor=True)
+
+    # robot_state_publisher можно запускать после webots (или вместе)
+    robot_description_sub: Command = kwargs['robot_description_sub']
+    robot_description_str = robot_description_sub.perform(context)
+    rsp_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[{
+            "robot_description": robot_description_str,
+            "use_sim_time": True
+        }]
+    )
+
+    # вернуть список действий, которые launch затем подключит
+    return [ webots, rsp_node ]
+
+def _runtime_setup(context, *args, **kwargs):
+    # Получаем значения
+    model_path = LaunchConfiguration('model').perform(context)
+    protos_path = LaunchConfiguration('protos_path').perform(context)
+    robot_name = LaunchConfiguration('robot_name').perform(context)
+    tool_slot = LaunchConfiguration('tool_slot').perform(context)
+    transform_proto = LaunchConfiguration('transform_proto').perform(context)
+    rotation_proto = LaunchConfiguration('rotation_proto').perform(context)
+
+
+
+    # убедимся, что директория существует
+    os.makedirs(protos_path, exist_ok=True)
+
+    proto_output = f"{protos_path}/{robot_name}.proto"   # <- явное .proto
+
+    xacro2proto_node = Node(
+        package="webots_ros2_importer",
+        executable="xacro2proto",
+        name="xacro2proto",
+        output="screen",
+        arguments=[
+            "--input", str(model_path),
+            "--output", proto_output,
+            "--tool-slot", str(tool_slot),
+            "--translation", str(transform_proto).replace(",", " "),
+            "--rotation", str(rotation_proto).replace(",", " "),
+        ]
+    )
+
+    # обработчик: после завершения xacro2proto запускаем OpaqueFunction, который патчит и запускает webots+rsp
+    after_xacro = RegisterEventHandler(
+        OnProcessExit(
+            target_action=xacro2proto_node,
+            on_exit=[
+                OpaqueFunction(
+                    function=_after_xacro,
+                    kwargs={
+                        'proto_path': proto_output,
+                        'robot_description_sub': kwargs['robot_description_sub']
+                    }
+                )
+            ]
+        )
+    )
+
+    return [ xacro2proto_node, after_xacro ]
+
+def generate_launch_description():
+    description_pkg = "iiwa_description"
+
+    declare_model_arg = DeclareLaunchArgument(
+        name="model",
+        default_value=PathJoinSubstitution([
+            FindPackageShare(description_pkg),
+            "urdf",
+            "iiwa7.urdf.xacro"
+        ]),
+    )
+
+    declare_robot_name_arg = DeclareLaunchArgument(name="robot_name", default_value="iiwa7")
+    declare_world_arg = DeclareLaunchArgument(
+        name="world",
+        default_value=PathJoinSubstitution([
+            FindPackageShare(description_pkg),
+            "worlds",
+            "simple_world.wbt"
+        ]),
+    )
+    declare_proto_arg = DeclareLaunchArgument(
+        name="protos_path",
+        default_value=PathJoinSubstitution([
+            FindPackageShare(description_pkg),
+            "protos"
+        ]),
+    )
+    declare_transform_arg = DeclareLaunchArgument(
+        name="transform_proto",
+        default_value="0 0 0"
+    )
+    declare_rotation_arg = DeclareLaunchArgument(
+        name="rotation_proto",
+        default_value="0 0 1 0"
+    )
+    declare_tool_arg = DeclareLaunchArgument(name="tool_slot", default_value="link7_ee")
+
+
+    robot_description_cmd = Command([
+        "xacro ", LaunchConfiguration("model"),
+        " robot_name:=", LaunchConfiguration("robot_name")
+    ])
+
+    runtime_setup = OpaqueFunction(
+        function=_runtime_setup,
+        kwargs={'robot_description_sub': robot_description_cmd}
+    )
+
+    ld = LaunchDescription([
+        declare_model_arg,
+        declare_robot_name_arg,
+        declare_world_arg,
+        declare_proto_arg,
+        declare_transform_arg,
+        declare_rotation_arg,
+        declare_tool_arg,
+        runtime_setup,
+    ])
+    return ld

src/iiwa_bringup/launch/webots.launch.py

@@ -0,0 +1,245 @@
+import xacro
+from pathlib import Path
+from launch.events import Shutdown
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    RegisterEventHandler,
+    OpaqueFunction,
+    EmitEvent,
+    TimerAction
+)
+from launch.event_handlers import OnProcessExit, OnProcessStart
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+from webots_ros2_driver.webots_launcher import WebotsLauncher
+from webots_ros2_driver.urdf_spawner import URDFSpawner
+from webots_ros2_driver.webots_controller import WebotsController
+
+
+def _runtime_controller(robot_name: str,
+                        robot_description_str: str,
+                        transform_proto: str = "0 0 0",
+                        rotation_proto: str = "0 0 1 0",
+                        controller_manager_timer: int = 50):
+    tmo = ['--controller-manager-timeout', str(controller_manager_timer)]
+
+    jsb = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        arguments=['joint_state_broadcaster'] + tmo,
+        parameters=[{'use_sim_time': False}],
+    )
+
+    jtc = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        arguments=['joint_trajectory_controller'] + tmo, # название с yaml файла
+        parameters=[{'use_sim_time': False}],
+    )
+
+    spawner_urdf = URDFSpawner(
+        name=robot_name,
+        robot_description=robot_description_str,
+        translation=transform_proto,
+        rotation=rotation_proto
+    )
+
+    return [jsb, jtc, spawner_urdf]
+
+
+def _runtime_setup(context, *args, **kwargs):
+    # Получение параметров от пользователя при запуске launch
+    model = LaunchConfiguration('model').perform(context)
+    robot_name = LaunchConfiguration('robot_name').perform(context)
+    transform_proto = LaunchConfiguration('transform_proto').perform(context)
+    rotation_proto = LaunchConfiguration('rotation_proto').perform(context)
+    world_path = LaunchConfiguration('world').perform(context)
+    rviz_status = LaunchConfiguration('rviz').perform(context).lower() in ['true', '1', 'yes']
+    controller_manager = LaunchConfiguration('controller_manager').perform(context)
+
+    # Загрузка описания робота
+    model_path = Path(model)
+    suffix = model_path.suffix.lower()
+    if suffix == ".xacro":
+        robot_description_str = xacro.process_file(model, mappings={'name': str(robot_name)}).toxml()
+    elif suffix==".urdf":
+        robot_description_str = Path(model).read_text(encoding="utf-8")
+    else:
+        raise FileNotFoundError(f"Поддерживаются форматы файла: xacro/urdf")
+
+    # Запуск узлов
+    webots = WebotsLauncher(world=world_path, 
+                            ros2_supervisor=True)
+
+    rsp_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[{
+            "robot_description": robot_description_str,
+            "use_sim_time": False
+        }]
+    )
+
+    # работа с драйверами
+    driver = WebotsController(
+        robot_name=robot_name,
+        parameters=[
+            {
+                "robot_description": model,
+                "use_sim_time": False,
+                "set_robot_state_publisher": False
+            },
+            LaunchConfiguration('controller').perform(context)
+        ],
+        respawn=True
+    )
+
+    # Во время запуска драйвера спавниться робот и ros2_controllers
+    spawn_on_driver_start = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=driver,
+            on_start=lambda evt, ctx: [
+                TimerAction(period=2.0,
+                            actions=_runtime_controller(robot_name=robot_name,
+                                                        robot_description_str=robot_description_str,
+                                                        transform_proto=transform_proto,
+                                                        rotation_proto=rotation_proto,
+                                                        controller_manager_timer=controller_manager))
+            ]
+        )
+    )
+
+    # При закрытии webots закрываем все
+    shutdown_on_webots_exit = RegisterEventHandler(
+        OnProcessExit(
+            target_action=webots,
+            on_exit=[EmitEvent(event=Shutdown())]
+        )
+    )
+
+    setup = [webots, 
+             webots._supervisor, 
+             rsp_node, 
+             driver,
+             spawn_on_driver_start,
+             shutdown_on_webots_exit
+            ]
+
+
+    # Настройка rviz
+    if rviz_status:
+
+        rviz_config = PathJoinSubstitution([
+            FindPackageShare(kwargs['package_name']),
+            'config',
+            'rviz_iiwa.rviz'
+        ])
+
+        rviz = Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_config],
+            output='log'
+        )
+
+        # При закрытии rviz закрываем все
+        shutdown_on_rviz_exit = RegisterEventHandler(
+            OnProcessExit(
+                target_action=rviz,
+                on_exit=[EmitEvent(event=Shutdown())]
+            )
+        )
+
+        setup += [rviz, shutdown_on_rviz_exit]
+
+    return setup
+
+
+def generate_launch_description():
+    package_name="iiwa_bringup"
+    description_pkg = "iiwa_description"
+
+    # Объявление аргументов командной строки
+    declare_model_arg = DeclareLaunchArgument(
+        name="model",
+        default_value=PathJoinSubstitution([
+            FindPackageShare(description_pkg),
+            "urdf",
+            "iiwa7.urdf.xacro"
+        ]),
+        description="Path to robot xacro or urdf file (used to build robot_description)."
+    )
+
+    declare_robot_name_arg = DeclareLaunchArgument(
+        name="robot_name",
+        default_value="iiwa7",
+        description="Robot name (used for TF and naming spawned robot)."
+    )
+
+    declare_world_arg = DeclareLaunchArgument(
+        name="world",
+        default_value=PathJoinSubstitution([
+            FindPackageShare(description_pkg),
+            "worlds",
+            "iiwa.wbt"
+        ]),
+        description="Path to the Webots world (.wbt) to launch."
+    )
+
+    declare_controller_arg = DeclareLaunchArgument(
+        name="controller",
+        default_value=PathJoinSubstitution([
+            FindPackageShare(package_name),
+            "config",
+            "iiwa_controller.yaml"
+        ]),
+        description="Path to controllers YAML file (used by spawner and controller_manager)."
+    )
+
+    declare_transform_arg = DeclareLaunchArgument(
+        name="transform_proto",
+        default_value="-0.25 0 0.79",
+        description="Translation applied when spawning the robot in the Webots world (x y z)."
+    )
+
+    declare_rotation_arg = DeclareLaunchArgument(
+        name="rotation_proto",
+        default_value="0 0 1 0",
+        description="Rotation (axis-angle) applied when spawning the robot in the Webots world."
+    )
+
+    declare_rviz_arg = DeclareLaunchArgument(
+        name="rviz",
+        default_value="0",
+        description="If true|1|yes then launch RViz and joint_state_publisher_gui instead of controllers."
+    )
+
+    declare_controller_manager_arg = DeclareLaunchArgument(
+        name="controller_manager",
+        default_value="50",
+        description="Timeout (seconds) for controller_manager spawners (--controller-manager-timeout)."
+    )
+
+    runtime_setup = OpaqueFunction(
+        function=_runtime_setup,
+        kwargs={'package_name': package_name}
+    )
+
+    return LaunchDescription([
+        declare_model_arg,
+        declare_robot_name_arg,
+        declare_world_arg,
+        declare_controller_arg,
+        declare_transform_arg,
+        declare_rotation_arg,
+        declare_rviz_arg,
+        declare_controller_manager_arg,
+        runtime_setup,
+    ])

src/iiwa_bringup/setup.py

@@ -27,6 +27,7 @@ def data_files_from_tree(src_dir: str, dst_root: str) -> list:
 
 package_name = 'iiwa_bringup'
 
+resource_intsall_root = f"share/{package_name}/resource"
 launch_intsall_root = f"share/{package_name}/launch"
 config_intsall_root = f"share/{package_name}/config"
 
@@ -35,6 +36,7 @@ data_files = [
     ('share/' + package_name, ['package.xml']),
 ]
 
+data_files += data_files_from_tree('resource', resource_intsall_root)
 data_files += data_files_from_tree('launch', launch_intsall_root)
 data_files += data_files_from_tree('config', config_intsall_root)
 

src/iiwa_description/resource/protos/CollaborativeRobotTable.proto

@@ -0,0 +1,152 @@
+#VRML_SIM R2025a utf8
+# license: Apache License 2.0
+# license url: https://www.apache.org/licenses/LICENSE-2.0
+# keywords: industrial/other
+# A table for placing collaborative work and working with it
+# It is possible to add a network cabinet on the back of the table
+# template language: javascript
+
+
+PROTO CollaborativeRobotTable [
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+  field SFString name "CollaborativeRobotTable"
+  field SFString contactMaterial "TableMaterial"
+  field SFBool locked FALSE
+  field SFBool showCabinet FALSE # Defines the display of the network cabinet
+]
+{
+
+  %<
+    let showCabinet = fields.showCabinet.value;
+  >%
+
+  Solid {
+    translation IS translation
+    rotation IS rotation
+    name IS name
+    locked IS locked
+    contactMaterial IS contactMaterial
+
+    children [
+      %< if (showCabinet) { >%
+      Group {
+        children [
+          Solid {
+            translation -0.66 4.26e-06 0.099998
+            rotation 0.5773509358560258 -0.577349935856137 -0.577349935856137 2.09439
+            children [
+              Pose {
+                translation -0.3135 0.19 0
+                children [
+                  Shape {
+                    appearance PBRAppearance {
+                      baseColor 0.57 0.57 0.57
+                      roughness 1
+                    }
+                    geometry Mesh {
+                      url [
+                        "./meshes/iiwaTable/ElectricCabinet.obj"
+                      ]
+                    }
+                  }
+                ]
+                translationStep 0.001
+              }
+              Shape {
+                appearance PBRAppearance {
+                  baseColor 0.57 0.57 0.57
+                  roughness 1
+                }
+                geometry Mesh {
+                  url [
+                    "./meshes/iiwaTable/CabinetFrame.obj"
+                  ]
+                }
+              }
+            ]
+            name "Cabinet"
+          }
+        ]
+      }
+      %< } >%
+      Group {
+        children [
+          Solid {
+            children [
+              Pose {
+                translation 0 0 0.79
+                children [
+                  Shape {
+                    appearance PBRAppearance {
+                      roughness 1
+                      metalness 0
+                    }
+                    geometry Box {
+                      size 1.28 1.08 0.02
+                    }
+                  }
+                ]
+              }
+              Shape {
+                appearance PBRAppearance {
+                  baseColor 0.57 0.57 0.57
+                  roughness 1
+                }
+                geometry Mesh {
+                  url [
+                    "./meshes/iiwaTable/TableFrame.obj"
+                  ]
+                }
+              }
+              Pose {
+                translation -0.263 0 0.8
+                children [
+                  Shape {
+                    appearance PBRAppearance {
+                      baseColor 0.517647 0.513726 0.568627
+                      roughness 1
+                    }
+                    geometry Mesh {
+                      url [
+                        "./meshes/iiwaTable/PlateUnderRobot.obj"
+                      ]
+                    }
+                  }
+                ]
+                translationStep 0.001
+              }
+            ]
+            name "Table"
+          }
+        ]
+      }
+    ]
+    boundingObject Group {
+      children [
+        %< if (showCabinet) { >%
+        Pose {
+          translation -0.78 4.97601e-06 0.609998
+          rotation 0.5773509358560258 -0.577349935856137 -0.577349935856137 2.09439
+          children [
+            Box {
+              size 0.66 0.96 0.28
+            }
+          ]
+        }
+        %< } >%
+        Pose {
+          translation 0 0 0.4035
+          children [
+            Box {
+              size 1.28 1.08 0.7926
+            }
+          ]
+        }
+      ]
+    }
+  }
+
+}
+
+

src/iiwa_description/resource/protos/IntelRealsenseD455.proto

@@ -0,0 +1,111 @@
+#VRML_SIM R2025a utf8
+# license: Apache License 2.0
+# license url: http://www.apache.org/licenses/LICENSE-2.0
+# Model of the Intel Realsense D455 RGBD camera.
+# keywords: sensor/range-finder, sensor/camera
+# template language: javascript
+
+PROTO IntelRealsenseD455 [
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+  field SFString name "IntelRealsenseD455"
+  field SFBool locked FALSE
+  field SFString controller "<none>"
+  field MFString controllerArgs [ ]
+  field SFBool supervisor FALSE
+  field SFString{"HD", "VGA", "FULL"} resolution "VGA"
+  field SFFloat fieldOfView 1.4746  # Diagonal ~84.43° from inr parametrs
+  field SFFloat minRange 0.01
+  field SFFloat maxRange 6.0
+  field SFNode focus NULL
+  field SFNode recognition Recognition{}
+  field SFNode physics Physics {
+    density -1
+    mass 0.35
+    centerOfMass [0 0 0]
+  }
+]
+{
+  %<
+    const cameraName = fields.name.value;
+    const resMode = fields.resolution.value.toUpperCase();
+    let width = 1280;
+    let height = 800;
+
+    if (resMode === "VGA") {
+      width = 640;
+      height = 480;
+    } else if (resMode === "HD") {
+      width = 1280;
+      height = 720;
+    } else if (resMode === "FULL") {
+      width = 1280;
+      height = 800;
+    }
+  >%
+
+  Robot {
+    translation IS translation
+    rotation IS rotation
+    name IS name
+    locked IS locked
+    controller IS controller
+    controllerArgs IS controllerArgs
+    supervisor IS supervisor
+
+    children [
+      RangeFinder {
+        translation 0.005 0 0
+        name %<='"' + cameraName + '_depth"'>%
+        locked TRUE
+        radarCrossSection 0.01
+        fieldOfView IS fieldOfView
+        width %<= width >%
+        height %<= height >%
+        near IS minRange
+        minRange IS minRange
+        maxRange IS maxRange
+        noise 0.01
+        resolution 0.001
+      }
+
+      Camera {
+        translation 0.005 0 0
+        name %<='"' + cameraName + '_rgb"'>%
+        locked TRUE
+        fieldOfView IS fieldOfView
+        width %<= width >%
+        height %<= height >%
+        near IS minRange
+        far 10
+        exposure 0.5
+        antiAliasing TRUE
+        noise 0.01
+        focus IS focus
+        recognition IS recognition
+      }
+
+      Solid {
+        translation 0.013 0 0
+        rotation 0.5773489358556708 0.5773509358554485 0.5773509358554485 2.0944
+        children [
+          CadShape {
+            url [
+              "./meshes/cameras/visual/IntelRealsenseD455.dae"
+            ]
+          }
+        ]
+        locked TRUE
+      }
+    ]
+
+    boundingObject Pose {
+      children [
+        Box {
+          size 0.027 0.123 0.025
+        }
+      ]
+    }
+    physics IS physics
+  }
+}

src/iiwa_description/resource/protos/WorkspaceLimiter.proto

@@ -0,0 +1,83 @@
+#VRML_SIM R2025a utf8
+# license: Apache License 2.0
+# license url: https://www.apache.org/licenses/LICENSE-2.0
+# keywords: industrial/other
+# Workspace limiter in the form of square blocks that can be increased in size
+# template language: javascript
+
+
+PROTO WorkspaceLimiter [
+  field SFVec3f translation 0 0 0
+  field SFRotation rotation 0 0 1 0
+  field SFString name "WorkspaceLimiter"
+  field SFBool locked FALSE
+  field SFVec2f countLimiter 1 1 # Defines the number of blocks by X and Y
+  field SFVec3f scale 1 1 1 # Defines the size of the blocks
+  field SFFloat spacing 0.02 # Defines the distance between blocks
+]
+{
+
+  %<
+    let countLimiter = fields.countLimiter.value;
+    let scale = fields.scale.value;
+    let spacing = fields.spacing.value;
+
+    let widthCount = countLimiter.x;
+    let heightCount = countLimiter.y;
+
+    let boxWidth = 1.25;
+    let boxHeight = 1;
+    let boxDepth = 0.01;
+
+    if (spacing <= 0) {
+      console.error("The indentation between the blocks cannot be negative")
+      spacing = 0.02
+    }
+
+  >%
+
+  Solid {
+    translation IS translation
+    rotation IS rotation
+    name IS name
+    locked IS locked
+    children [
+      %<
+        for (let i = 0; i < widthCount; i++) {
+          for (let j = 0; j < heightCount; j++) {
+            let x = (i * (boxWidth + spacing) - (widthCount - 1) * (boxWidth + spacing) / 2) * scale.x;
+            let y = (j * (boxHeight + spacing) - (heightCount - 1) * (boxHeight + spacing) / 2) * scale.y;
+      >%
+        Transform {
+          translation %<= x >% 0 %<= y >%
+          scale %<= scale.x >% %<= scale.y >% %<= scale.z >%
+          children [
+            Shape {
+              appearance PBRAppearance {
+                roughness 1
+                metalness 0
+                IBLStrength 5
+              }
+              geometry Box {
+                size %<= boxWidth >% %<= boxDepth >% %<= boxHeight >%
+              }
+            }
+            Shape {
+              appearance PBRAppearance {
+                baseColor 0.57 0.57 0.57
+                roughness 1
+                metalness 0
+              }
+              geometry Mesh {
+                url ["./meshes/WorkspaceLimiter/fence.obj"]
+              }
+            }
+          ]
+        }
+      %<
+          }
+        }
+      >%
+    ]
+  }
+}

src/iiwa_description/resource/protos/meshes/WorkspaceLimiter/fence.obj

@@ -0,0 +1,130 @@
+#OBJ
+#T-FLEX CAD ObjGenerator generate date:21.11.2024 15:39:40
+v 0.625 0.05 0.5
+v 0.625 0.05 -0.5
+v 0.574999 0.05 -0.449999
+v -0.625 0.05 -0.5
+v -0.574999 0.05 -0.449999
+v -0.625 0.05 0.5
+v -0.574999 0.05 0.449999
+v 0.574999 0.05 0.449999
+v 0.625 0 -0.5
+v 0.625 0 0.5
+v 0.574999 0 0.449999
+v -0.625 0 0.5
+v -0.574999 0 0.449999
+v -0.625 0 -0.5
+v -0.574999 0 -0.449999
+v 0.574999 0 -0.449999
+v 0.574999 0.05 0.449999
+v 0.574999 0 0.449999
+v -0.574999 0 0.449999
+v -0.574999 0.05 0.449999
+v 0.574999 0.05 -0.449999
+v 0.574999 0 -0.449999
+v 0.574999 0 0.449999
+v 0.574999 0.05 0.449999
+v -0.574999 0.05 -0.449999
+v -0.574999 0 -0.449999
+v 0.574999 0 -0.449999
+v 0.574999 0.05 -0.449999
+v -0.574999 0.05 0.449999
+v -0.574999 0 0.449999
+v -0.574999 0 -0.449999
+v -0.574999 0.05 -0.449999
+v 0.625 0.05 -0.5
+v 0.625 0 -0.5
+v -0.625 0 -0.5
+v -0.625 0.05 -0.5
+v 0.625 0.05 0.5
+v 0.625 0 0.5
+v 0.625 0 -0.5
+v 0.625 0.05 -0.5
+v -0.625 0.05 0.5
+v -0.625 0 0.5
+v 0.625 0 0.5
+v 0.625 0.05 0.5
+v -0.625 0.05 -0.5
+v -0.625 0 -0.5
+v -0.625 0 0.5
+v -0.625 0.05 0.5
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+f 1//1 2//2 3//3
+f 2//2 4//4 5//5
+f 4//4 6//6 7//7
+f 6//6 1//1 8//8
+f 5//5 3//3 2//2
+f 8//8 7//7 6//6
+f 7//7 5//5 4//4
+f 3//3 8//8 1//1
+f 9//9 10//10 11//11
+f 10//10 12//12 13//13
+f 12//12 14//14 15//15
+f 14//14 9//9 16//16
+f 13//13 11//11 10//10
+f 16//16 15//15 14//14
+f 15//15 13//13 12//12
+f 11//11 16//16 9//9
+f 17//17 18//18 19//19
+f 19//19 20//20 17//17
+f 21//21 22//22 23//23
+f 23//23 24//24 21//21
+f 25//25 26//26 27//27
+f 27//27 28//28 25//25
+f 29//29 30//30 31//31
+f 31//31 32//32 29//29
+f 33//33 34//34 35//35
+f 35//35 36//36 33//33
+f 37//37 38//38 39//39
+f 39//39 40//40 37//37
+f 41//41 42//42 43//43
+f 43//43 44//44 41//41
+f 45//45 46//46 47//47
+f 47//47 48//48 45//45

src/iiwa_description/resource/protos/meshes/iiwaTable/CabinetFrame.obj

@@ -0,0 +1,194 @@
+#OBJ
+#T-FLEX CAD ObjGenerator generate date:20.11.2024 23:59:52
+v 0.27 0.23 -0.019999
+v 0.27 0.949999 -0.019999
+v 0.31 0.99 -0.019999
+v -0.31 0.99 -0.019999
+v -0.27 0.949999 -0.019999
+v -0.27 0.23 -0.019999
+v 0.27 0.189999 -0.019999
+v -0.31 0 -0.019999
+v -0.27 0.189999 -0.019999
+v 0.31 0 -0.019999
+v 0.27 0 -0.019999
+v -0.27 -0 -0.019999
+v -0.27 0.23 0.019999
+v -0.27 0.189999 0.019999
+v 0.27 0.189999 0.019999
+v -0.31 0.99 0.019999
+v -0.31 0 0.019999
+v -0.27 -0 0.019999
+v 0.27 0.949999 0.019999
+v 0.31 0.99 0.019999
+v 0.27 0.23 0.019999
+v 0.31 0 0.019999
+v -0.27 0.949999 0.019999
+v 0.27 0 0.019999
+v 0.27 0.23 0.019999
+v 0.27 0.23 -0.019999
+v -0.27 0.23 -0.019999
+v -0.27 0.23 0.019999
+v -0.31 0.99 0.019999
+v -0.31 0.99 -0.019999
+v -0.31 0 -0.019999
+v -0.31 0 0.019999
+v -0.31 0.99 0.019999
+v 0.31 0.99 0.019999
+v 0.31 0.99 -0.019999
+v -0.31 0.99 -0.019999
+v 0.31 0 0.019999
+v 0.31 0 -0.019999
+v 0.31 0.99 -0.019999
+v 0.31 0.99 0.019999
+v 0.27 0 0.019999
+v 0.27 0 -0.019999
+v 0.31 0 -0.019999
+v 0.31 0 0.019999
+v 0.27 0 -0.019999
+v 0.27 0 0.019999
+v 0.27 0.189999 0.019999
+v 0.27 0.189999 -0.019999
+v -0.27 0.189999 0.019999
+v -0.27 0.189999 -0.019999
+v 0.27 0.189999 -0.019999
+v 0.27 0.189999 0.019999
+v -0.27 -0 0.019999
+v -0.27 -0 -0.019999
+v -0.27 0.189999 -0.019999
+v -0.27 0.189999 0.019999
+v -0.31 0 0.019999
+v -0.31 0 -0.019999
+v -0.27 -0 -0.019999
+v -0.27 -0 0.019999
+v 0.27 0.23 -0.019999
+v 0.27 0.23 0.019999
+v 0.27 0.949999 0.019999
+v 0.27 0.949999 -0.019999
+v 0.27 0.949999 -0.019999
+v 0.27 0.949999 0.019999
+v -0.27 0.949999 0.019999
+v -0.27 0.949999 -0.019999
+v -0.27 0.949999 -0.019999
+v -0.27 0.949999 0.019999
+v -0.27 0.23 0.019999
+v -0.27 0.23 -0.019999
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 1 -0 0
+vn 1 -0 0
+vn 1 -0 0
+vn 1 -0 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn -1 -0 0
+vn -1 -0 0
+vn -1 -0 0
+vn -1 -0 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn -0 -1 0
+vn -0 -1 0
+vn -0 -1 0
+vn -0 -1 0
+vn -1 -0 0
+vn -1 -0 0
+vn -1 -0 0
+vn -1 -0 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+f 1//1 2//2 3//3
+f 4//4 5//5 6//6
+f 4//4 3//3 2//2
+f 2//2 5//5 4//4
+f 3//3 7//7 1//1
+f 6//6 1//1 7//7
+f 8//8 6//6 9//9
+f 7//7 9//9 6//6
+f 6//6 8//8 4//4
+f 10//10 11//11 7//7
+f 7//7 3//3 10//10
+f 9//9 12//12 8//8
+f 13//13 14//14 15//15
+f 14//14 16//16 17//17
+f 17//17 18//18 14//14
+f 19//19 20//20 16//16
+f 21//21 15//15 20//20
+f 15//15 21//21 13//13
+f 22//22 20//20 15//15
+f 20//20 19//19 21//21
+f 16//16 23//23 19//19
+f 16//16 14//14 13//13
+f 13//13 23//23 16//16
+f 15//15 24//24 22//22
+f 25//25 26//26 27//27
+f 27//27 28//28 25//25
+f 29//29 30//30 31//31
+f 31//31 32//32 29//29
+f 33//33 34//34 35//35
+f 35//35 36//36 33//33
+f 37//37 38//38 39//39
+f 39//39 40//40 37//37
+f 41//41 42//42 43//43
+f 43//43 44//44 41//41
+f 45//45 46//46 47//47
+f 47//47 48//48 45//45
+f 49//49 50//50 51//51
+f 51//51 52//52 49//49
+f 53//53 54//54 55//55
+f 55//55 56//56 53//53
+f 57//57 58//58 59//59
+f 59//59 60//60 57//57
+f 61//61 62//62 63//63
+f 63//63 64//64 61//61
+f 65//65 66//66 67//67
+f 67//67 68//68 65//65
+f 69//69 70//70 71//71
+f 71//71 72//72 69//69

src/iiwa_description/resource/protos/meshes/iiwaTable/ElectricCabinet.obj

@@ -0,0 +1,222 @@
+#OBJ
+#T-FLEX CAD ObjGenerator generate date:20.11.2024 23:59:6
+v 0.625 0.541 0.019999
+v 0.625 0.764999 0.019999
+v 0.675 0.721 0.019999
+v 0.634999 0.764999 0.019999
+v 0.675 0.725 0.019999
+v 0.634999 0.541 0.019999
+v 0.675 0.721 0.208
+v 0.625 0.764999 0.208
+v 0.625 0.541 0.208
+v 0.675 0.725 0.208
+v 0.634999 0.764999 0.208
+v 0.634999 0.541 0.208
+v 0.634999 0.764999 0.208
+v 0.675 0.725 0.208
+v 0.675 0.725 0.019999
+v 0.634999 0.764999 0.019999
+v 0.675 0.721 0.208
+v 0.634999 0.541 0.208
+v 0.634999 0.541 0.019999
+v 0.675 0.721 0.019999
+v 0.625 0.541 0.208
+v 0.625 0.541 0.019999
+v 0.634999 0.541 0.019999
+v 0.634999 0.541 0.208
+v 0.675 0.725 0.208
+v 0.675 0.721 0.208
+v 0.675 0.721 0.019999
+v 0.675 0.725 0.019999
+v 0.634999 0.764999 0.208
+v 0.634999 0.764999 0.019999
+v 0.625 0.764999 0.019999
+v 0.625 0.764999 0.208
+v 0.6125 0.787999 0.261999
+v 0.0125 0.787999 0.261999
+v 0.0125 0.012 0.261999
+v 0.6125 0.012 0.261999
+v 0.0125 0.787999 0.25
+v 0.0125 0.787999 0.261999
+v 0.6125 0.787999 0.261999
+v 0.6125 0.787999 0.25
+v 0.6125 0.787999 0.25
+v 0.6125 0.787999 0.261999
+v 0.6125 0.012 0.261999
+v 0.6125 0.012 0.25
+v 0.6125 0.012 0.25
+v 0.6125 0.012 0.261999
+v 0.0125 0.012 0.261999
+v 0.0125 0.012 0.25
+v 0.0125 0.012 0.25
+v 0.0125 0.012 0.261999
+v 0.0125 0.787999 0.261999
+v 0.0125 0.787999 0.25
+v 0.625 0 0
+v 0 0 0
+v 0 0.8 0
+v 0.625 0.8 0
+v 0 0 0.25
+v 0.0125 0.012 0.25
+v 0.0125 0.787999 0.25
+v 0.625 0 0.25
+v 0.6125 0.012 0.25
+v 0.625 0.8 0.25
+v 0.6125 0.787999 0.25
+v 0 0.8 0.25
+v 0.625 0.8 0.25
+v 0.625 0.8 0
+v 0 0.8 0
+v 0 0.8 0.25
+v 0 0 0.25
+v 0 0 0
+v 0.625 0 0
+v 0.625 0 0.25
+v 0 0 0
+v 0 0 0.25
+v 0 0.8 0.25
+v 0 0.8 0
+v 0.625 0.8 0
+v 0.625 0.764999 0.019999
+v 0.625 0.541 0.019999
+v 0.625 0.541 0.208
+v 0.625 0.764999 0.208
+v 0.625 0.8 0.25
+v 0.625 0 0
+v 0.625 0 0.25
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0.707106 0.707106 0
+vn 0.707106 0.707106 0
+vn 0.707106 0.707106 0
+vn 0.707106 0.707106 0
+vn 0.976187 -0.21693 0
+vn 0.976187 -0.21693 0
+vn 0.976187 -0.21693 0
+vn 0.976187 -0.21693 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn -0 1 0
+vn -0 1 0
+vn -0 1 0
+vn -0 1 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn -1 -0 0
+vn -1 -0 0
+vn -1 -0 0
+vn -1 -0 0
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+f 1//1 2//2 3//3
+f 2//2 4//4 5//5
+f 5//5 3//3 2//2
+f 3//3 6//6 1//1
+f 7//7 8//8 9//9
+f 7//7 10//10 11//11
+f 9//9 12//12 7//7
+f 11//11 8//8 7//7
+f 13//13 14//14 15//15
+f 15//15 16//16 13//13
+f 17//17 18//18 19//19
+f 19//19 20//20 17//17
+f 21//21 22//22 23//23
+f 23//23 24//24 21//21
+f 25//25 26//26 27//27
+f 27//27 28//28 25//25
+f 29//29 30//30 31//31
+f 31//31 32//32 29//29
+f 33//33 34//34 35//35
+f 35//35 36//36 33//33
+f 37//37 38//38 39//39
+f 39//39 40//40 37//37
+f 41//41 42//42 43//43
+f 43//43 44//44 41//41
+f 45//45 46//46 47//47
+f 47//47 48//48 45//45
+f 49//49 50//50 51//51
+f 51//51 52//52 49//49
+f 53//53 54//54 55//55
+f 55//55 56//56 53//53
+f 57//57 58//58 59//59
+f 60//60 61//61 58//58
+f 58//58 57//57 60//60
+f 60//60 62//62 63//63
+f 64//64 59//59 63//63
+f 59//59 64//64 57//57
+f 63//63 62//62 64//64
+f 63//63 61//61 60//60
+f 65//65 66//66 67//67
+f 67//67 68//68 65//65
+f 69//69 70//70 71//71
+f 71//71 72//72 69//69
+f 73//73 74//74 75//75
+f 75//75 76//76 73//73
+f 77//77 78//78 79//79
+f 80//80 81//81 82//82
+f 79//79 83//83 77//77
+f 81//81 78//78 77//77
+f 80//80 84//84 83//83
+f 77//77 82//82 81//81
+f 83//83 79//79 80//80
+f 82//82 84//84 80//80

src/iiwa_description/setup.py

@@ -27,15 +27,23 @@ def data_files_from_tree(src_dir: str, dst_root: str) -> list:
 
 package_name = 'iiwa_description'
 
+resource_intsall_root = f"share/{package_name}/resource"
 meshes_intsall_root = f"share/{package_name}/meshes"
 urdf_intsall_root = f"share/{package_name}/urdf"
+worlds_intsall_root = f"share/{package_name}/worlds"
+protos_intsall_root = f"share/{package_name}/protos"
+
+
 
 data_files = [
     ('share/' + package_name, ['package.xml']),
 ]
 
+data_files += data_files_from_tree('resource', resource_intsall_root)
 data_files += data_files_from_tree('meshes', meshes_intsall_root)
 data_files += data_files_from_tree('urdf', urdf_intsall_root)
+data_files += data_files_from_tree('worlds', worlds_intsall_root)
+data_files += data_files_from_tree('protos', protos_intsall_root)
 
 
 setup(

src/iiwa_description/urdf/iiwa7.urdf

@@ -0,0 +1,280 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from ./src/iiwa_description/urdf/iiwa7.urdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="iiwa7">
+  <link name="world"/>
+  <link name="base_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.012856764088336 7.05371084939836E-07 0.0679655592591469"/>
+      <mass value="30.2763517543289"/>
+      <inertia ixx="0.111239565538359" ixy="-3.81097437253422e-07" ixz="0.00486659750522703" iyy="0.139017104291389" iyz="7.87871531503135e-07" izz="0.133028621926226"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/visual/base_link.DAE"/>
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/collision/base_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link1">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-4.78790441849064E-07 -0.0348192058752328 -0.0692021315991948"/>
+      <mass value="21.1627406257303"/>
+      <inertia ixx="0.133321610552378" ixy="-5.3830316300325e-07" ixz="9.16989471201082e-07" iyy="0.127213841326705" iyz="0.0236704924620788" izz="0.047155320135412"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/visual/link1.DAE"/>
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/collision/link1.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link2">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-5.2906167189587E-07 -0.0869460684741495 0.0287906957294968"/>
+      <mass value="25.1403147192251"/>
+      <inertia ixx="0.197631241693207" ixy="9.50708464039702e-08" ixz="-3.51963818841194e-07" iyy="0.0602720613821397" iyz="-0.0367196702125505" izz="0.187557269753206"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/visual/link2.DAE"/>
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/collision/link2.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link3">
+    <inertial>
+      <origin rpy="0 0 0" xyz="8.94264666317923E-07 0.0343188244798595 -0.0692020521907963"/>
+      <mass value="21.1627874292289"/>
+      <inertia ixx="0.133322417523041" ixy="-2.72387851582384e-07" ixz="-2.31579646685682e-07" iyy="0.127213838333813" iyz="-0.0236710393468367" izz="0.0471557500559207"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/visual/link3.DAE"/>
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/collision/link3.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link4">
+    <inertial>
+      <origin rpy="0 0 0" xyz="9.9343785475714E-08 -0.0869462342636022 -0.0292907044657978"/>
+      <mass value="25.140328983289"/>
+      <inertia ixx="0.197630839812683" ixy="-3.9082887807781e-07" ixz="-3.24564289064112e-07" iyy="0.0602723071447973" iyz="0.0367195588674331" izz="0.187556898167347"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/visual/link4.DAE"/>
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/collision/link4.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link5">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-1.5059645121912E-07 -0.0298223596156005 -0.116233672352317"/>
+      <mass value="9.53466992761605"/>
+      <inertia ixx="0.0529038960536523" ixy="6.86274869084724e-08" ixz="3.57472026918582e-07" iyy="0.0444942713885967" iyz="0.0174914068345989" izz="0.0239916848880951"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/visual/link5.DAE"/>
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/collision/link5.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link6">
+    <inertial>
+      <origin rpy="0 0 0" xyz="3.09820422451923E-06 -0.000588422963829549 0.000348569863276505"/>
+      <mass value="14.2007599322815"/>
+      <inertia ixx="0.0384515762635731" ixy="-6.70279497724119e-08" ixz="2.19173188415509e-06" iyy="0.0280010940276735" iyz="-0.00192249677468735" izz="0.0368124806983828"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/visual/link6.DAE"/>
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/collision/link6.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link7">
+    <inertial>
+      <origin rpy="0 0 0" xyz="5.28878934317328E-07 3.37199922245852E-07 -0.0274928177649973"/>
+      <mass value="2.47156969383589"/>
+      <inertia ixx="0.00177365937026673" ixy="-1.2410521430667e-07" ixz="-2.95409038849194e-08" iyy="0.00177328722902027" iyz="-1.57567641084958e-08" izz="0.00272748710623445"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/visual/link7.DAE"/>
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iiwa_description/resource/meshes/collision/link7.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_ee"/>
+  <joint name="world_base_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="joint1" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.3375"/>
+    <parent link="base_link"/>
+    <child link="link1"/>
+    <axis xyz="0 0 -1"/>
+    <limit effort="176" lower="-2.97" upper="2.97" velocity="1.71"/>
+  </joint>
+  <joint name="joint2" type="revolute">
+    <origin rpy="-1.5708 0 0" xyz="0 0 0"/>
+    <parent link="link1"/>
+    <child link="link2"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="176" lower="-2.09" upper="2.09" velocity="1.71"/>
+  </joint>
+  <joint name="joint3" type="revolute">
+    <origin rpy="1.5708 0 0" xyz="0 -0.3993 0"/>
+    <parent link="link2"/>
+    <child link="link3"/>
+    <axis xyz="0 0 -1"/>
+    <limit effort="110" lower="-2.97" upper="2.97" velocity="1.75"/>
+  </joint>
+  <joint name="joint4" type="revolute">
+    <origin rpy="-1.5708 0 0" xyz="0 0 0"/>
+    <parent link="link3"/>
+    <child link="link4"/>
+    <axis xyz="0 0 -1"/>
+    <limit effort="110" lower="-2.09" upper="2.09" velocity="2.27"/>
+  </joint>
+  <joint name="joint5" type="revolute">
+    <origin rpy="1.5708 0 0" xyz="0 -0.3993 0"/>
+    <parent link="link4"/>
+    <child link="link5"/>
+    <axis xyz="0 0 -1"/>
+    <limit effort="110" lower="-2.97" upper="2.97" velocity="2.44"/>
+  </joint>
+  <joint name="joint6" type="revolute">
+    <origin rpy="-1.5708 0 0" xyz="0 0 0"/>
+    <parent link="link5"/>
+    <child link="link6"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="40" lower="-2.09" upper="2.09" velocity="3.14"/>
+  </joint>
+  <joint name="joint7" type="revolute">
+    <origin rpy="1.5708 0 0" xyz="0 -0.126 0"/>
+    <parent link="link6"/>
+    <child link="link7"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="40" lower="-3.05" upper="3.05" velocity="3.14"/>
+  </joint>
+  <joint name="tools_joint" type="fixed">
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+    <parent link="link7"/>
+    <child link="link_ee"/>
+  </joint>
+  <webots>
+    <plugin type="webots_ros2_control::Ros2Control"/>
+  </webots>
+  <ros2_control name="iiwaWebotsControl" type="system">
+    <hardware>
+      <plugin>webots_ros2_control::Ros2ControlSystem</plugin>
+    </hardware>
+    <joint name="joint1">
+      <state_interface name="position"/>
+      <command_interface name="position"/>
+    </joint>
+    <joint name="joint2">
+      <state_interface name="position"/>
+      <command_interface name="position"/>
+    </joint>
+    <joint name="joint3">
+      <state_interface name="position"/>
+      <command_interface name="position"/>
+    </joint>
+    <joint name="joint4">
+      <state_interface name="position"/>
+      <command_interface name="position"/>
+    </joint>
+    <joint name="joint5">
+      <state_interface name="position"/>
+      <command_interface name="position"/>
+    </joint>
+    <joint name="joint6">
+      <state_interface name="position"/>
+      <command_interface name="position"/>
+    </joint>
+    <joint name="joint7">
+      <state_interface name="position"/>
+      <command_interface name="position"/>
+    </joint>
+  </ros2_control>
+</robot>

src/iiwa_description/urdf/iiwa7.urdf.xacro

@@ -6,5 +6,51 @@
     <xacro:include filename="$(find iiwa_description)/urdf/links.xacro"/>
     <xacro:include filename="$(find iiwa_description)/urdf/joints.xacro"/>
 
+    <webots>
+        <plugin type="webots_ros2_control::Ros2Control"/>
+    </webots>
+
+    <ros2_control name="iiwaWebotsControl" type="system">
+        <hardware>
+            <plugin>webots_ros2_control::Ros2ControlSystem</plugin>
+        </hardware>
+
+        <joint name="joint1">
+            <state_interface name="position"/>
+            <command_interface name="position"/>
+        </joint>
+
+        <joint name="joint2">
+            <state_interface name="position"/>
+            <command_interface name="position"/>
+        </joint>
+
+        <joint name="joint3">
+            <state_interface name="position"/>
+            <command_interface name="position"/>
+        </joint>
+
+        <joint name="joint4">
+            <state_interface name="position"/>
+            <command_interface name="position"/>
+        </joint>
+
+        <joint name="joint5">
+            <state_interface name="position"/>
+            <command_interface name="position"/>
+        </joint>
+
+        <joint name="joint6">
+            <state_interface name="position"/>
+            <command_interface name="position"/>
+        </joint>
+
+        <joint name="joint7">
+            <state_interface name="position"/>
+            <command_interface name="position"/>
+        </joint>
+
+    </ros2_control>
+
 
 </robot>

src/iiwa_description/urdf/joints.xacro

@@ -12,48 +12,48 @@
     <!-- joint1 -->
     <xacro:revolute_joint jname="joint1" parent="base_link" child="link1"
                         xyz="0 0 0.3375" rpy="0 0 0" axis="0 0 -1"
-                        lower="-2.97" upper="2.97" effort="176" velocity="1.71"/>
+                        lower="-2.97" upper="2.97" effort="500" velocity="1.71"/>
 
 
     <!-- joint2 -->
     <xacro:revolute_joint jname="joint2" parent="link1" child="link2"
                         xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
-                        lower="-2.09" upper="2.09" effort="176" velocity="1.71"/>
+                        lower="-2.09" upper="2.09" effort="500" velocity="1.71"/>
 
 
     <!-- joint3 -->
     <xacro:revolute_joint jname="joint3" parent="link2" child="link3"
                         xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 -1"
-                        lower="-2.97" upper="2.97" effort="110" velocity="1.75"/>
+                        lower="-2.97" upper="2.97" effort="500" velocity="1.75"/>
 
 
     <!-- joint4 -->
     <xacro:revolute_joint jname="joint4" parent="link3" child="link4"
                         xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 -1"
-                        lower="-2.09" upper="2.09" effort="110" velocity="2.27"/>
+                        lower="-2.09" upper="2.09" effort="500" velocity="2.27"/>
 
 
     <!-- joint5 -->
     <xacro:revolute_joint jname="joint5" parent="link4" child="link5"
                         xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 -1"
-                        lower="-2.97" upper="2.97" effort="110" velocity="2.44"/>
+                        lower="-2.97" upper="2.97" effort="500" velocity="2.44"/>
 
 
     <!-- joint6 -->
     <xacro:revolute_joint jname="joint6" parent="link5" child="link6"
                         xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
-                        lower="-2.09" upper="2.09" effort="40" velocity="3.14"/>
+                        lower="-2.09" upper="2.09" effort="500" velocity="3.14"/>
 
 
     <!-- joint7 -->
     <xacro:revolute_joint jname="joint7" parent="link6" child="link7"
                         xyz="0 -0.126 0" rpy="1.5708 0 0" axis="0 0 1"
-                        lower="-3.05" upper="3.05" effort="40" velocity="3.14"/>
+                        lower="-3.05" upper="3.05" effort="500" velocity="3.14"/>
 
     <joint name="tools_joint" type="fixed">
         <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
         <parent link="link7"/>
-        <child link="link7_ee"/>
+        <child link="link_ee"/>
     </joint>
 
 </robot>

src/iiwa_description/urdf/links.xacro

@@ -101,6 +101,6 @@
                      mesh_vision="${mesh_link7_v}" 
                      mesh_collision="${mesh_link7_c}"/>
 
-    <link name="link7_ee" />
+    <link name="link_ee" />
 
 </robot>

src/iiwa_description/urdf/meshes.xacro

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" xacro:version="1.0">
 
-    <xacro:property name="pkg" value="package://iiwa_description/meshes"/>
+    <xacro:property name="pkg" value="package://iiwa_description/resource/meshes"/>
 
     <!-- Меши отображения -->
     <xacro:property name="mesh_base_v" value="${pkg}/visual/base_link.DAE"/>

src/iiwa_description/worlds/iiwa.wbt

@@ -0,0 +1,67 @@
+#VRML_SIM R2025a utf8
+
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackground.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/floors/protos/RectangleArena.proto"
+EXTERNPROTO "../resource/protos/CollaborativeRobotTable.proto"
+EXTERNPROTO "../resource/protos/WorkspaceLimiter.proto"
+
+WorldInfo {
+  title "iiwa robotics"
+  contactProperties [
+    ContactProperties {
+      material1 "TableMaterial"
+      coulombFriction [
+        0
+      ]
+      bounce 0
+      bounceVelocity 0
+      softERP 1
+      softCFM 0.0001
+    }
+  ]
+}
+Viewpoint {
+  orientation 0.19675345789351045 -0.08356742903703787 -0.976885132249993 3.928186370440618
+  position 5.324715157515194 -4.7927780008585446 4.166958679133935
+  followType "Mounted Shot"
+  ambientOcclusionRadius 3
+}
+TexturedBackground {
+}
+TexturedBackgroundLight {
+  texture "noon_sunny_empty"
+  luminosity 2
+}
+RectangleArena {
+  name "Room"
+  floorSize 12 12
+  wallHeight 2
+}
+CollaborativeRobotTable {
+  translation 0 0 -0.007
+  locked TRUE
+  showCabinet TRUE
+}
+
+Group {
+  children [
+    WorkspaceLimiter {
+      translation -1.56 -0.219999 1.01
+      rotation 0 0 1 1.5708
+      name "WorkspaceLimiter_back"
+      locked TRUE
+      scale 2.32 1 2
+    }
+    WorkspaceLimiter {
+      translation -0.04 1.18 1.01
+      name "WorkspaceLimiter_right"
+      locked TRUE
+      scale 2.43 1 2
+    }
+  ]
+}
+DirectionalLight {
+  direction -0.1 0.1 0
+  intensity 2
+}

src/iiwa_description/worlds/simple_world.wbt

@@ -0,0 +1,19 @@
+#VRML_SIM R2025a utf8
+
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackground.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/floors/protos/RectangleArena.proto"
+
+WorldInfo {
+}
+Viewpoint {
+  orientation -0.20135401485081894 0.14705590432610455 0.9684168119700772 1.9104172436184537
+  position 2.126114183750861 -6.172386590547907 2.2530769187603243
+}
+TexturedBackground {
+}
+TexturedBackgroundLight {
+}
+RectangleArena {
+  floorSize 5 5
+}