Запрос на слияние 'dev' (#1) из dev в master

README.md

@@ -0,0 +1,31 @@
+Подмена файла по пути обязательна: `/opt/ros/rolling/lib/webots_ros2_driver/ros2_supervisor.py`
+
+> необходимо `warn` заменить на `warning` в логере
+
+```bash
+
+sudo apt install -y ros-${ROS_DISTRO}-webots-ros2 \
+                    ros-${ROS_DISTRO}-ros2-control \ 
+                    ros-${ROS_DISTRO}-ros2-controllers \
+                    ros-${ROS_DISTRO}-moveit-* \
+```
+
+Установка moveit2 (внимательно проверяй)
+```bash
+sudo apt install -y build-essential \
+                    cmake \
+                    git \
+                    python3-colcon-common-extensions \
+                    python3-flake8 \
+                    python3-rosdep \
+                    python3-setuptools \
+                    python3-vcstool \
+                    wget
+
+git clone https://github.com/moveit/moveit2.git
+vcs import --recursive < moveit2/moveit2.repos
+sudo apt remove ros-$ROS_DISTRO-moveit*
+rosdep install -r --from-paths ./src/ --ignore-src --rosdistro $ROS_DISTRO --os=ubuntu:noble -y
+
+colcon build --mixin release
+```

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"
+
+    iiwa_arm_controller:
+      type: "joint_trajectory_controller/JointTrajectoryController"
+
+
+iiwa_arm_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/digital_twin.launch.py

@@ -0,0 +1,156 @@
+from iiwa_bringup.utils import converter
+
+
+from launch_ros.actions import Node
+from launch import LaunchDescription
+from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import (IncludeLaunchDescription, 
+                            DeclareLaunchArgument, 
+                            OpaqueFunction)
+
+
+PACKAGE = "iiwa_bringup"
+DESCRIPTION_PKG = "iiwa_description"
+
+
+def _runtime_setup(context, *args, **kwatgs):
+    setup = []
+
+    model_path = LaunchConfiguration('model').perform(context)
+    robot_name = LaunchConfiguration('robot_name').perform(context)
+    world_path = LaunchConfiguration('world').perform(context)
+    rviz_status = LaunchConfiguration('rviz').perform(context).lower() in ['true', '1', 'yes']
+    
+    transform = LaunchConfiguration('transform').perform(context)
+    rotation = LaunchConfiguration('rotation').perform(context)
+    timer = LaunchConfiguration('controller_timer').perform(context)
+
+    robot_description = converter.load_robot_description(model_path=model_path,
+                                                         robot_name=robot_name)
+
+    rsp_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[{
+            "robot_description": robot_description,
+            "use_sim_time": False
+        }]
+    )
+    
+    webots_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare(PACKAGE),
+                'launch',
+                'supported', 'webots_spawn.launch.py'
+            ])
+        ),
+        launch_arguments={
+            'robot_name': robot_name,
+            'model': model_path,
+            'world': world_path,
+            'transform': transform,
+            'rotation': rotation,
+            'controller_timer': timer
+        }.items()
+    )
+
+
+    setup += [rsp_node, webots_launch]
+
+
+    if rviz_status:
+        rviz_launch = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution([FindPackageShare(PACKAGE),
+                "launch", "supported", 'rviz.launch.py'
+            ])),
+            launch_arguments={'package_name': PACKAGE}.items()
+        )
+
+        setup.append(rviz_launch)
+
+    return setup
+
+def generate_launch_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),
+            "config",
+            "iiwa_controller.yaml"
+        ]),
+        description="Path to controllers YAML file (used by spawner and controller_manager)."
+    )
+
+    declare_transform_arg = DeclareLaunchArgument(
+        name="transform",
+        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",
+        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_timer",
+        default_value="50",
+        description="Timeout (seconds) for controller_manager spawners (--controller-manager-timeout)."
+    )
+
+    runtime_setup = OpaqueFunction(
+        function=_runtime_setup
+    )
+
+    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/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/rviz_iiwa.launch.py

@@ -1,8 +1,10 @@
 from launch_ros.actions import Node
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler
 from launch_ros.substitutions import FindPackageShare
 from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
+from launch.events import Shutdown
+from launch.event_handlers import OnProcessExit
 
 
 def generate_launch_description():
@@ -62,10 +64,18 @@ def generate_launch_description():
         output='log'
     )
 
+    shutdown_on_rviz_exit = RegisterEventHandler(
+        OnProcessExit(
+            target_action=rviz,
+            on_exit=[EmitEvent(event=Shutdown())]
+        )
+    )
+
     return LaunchDescription([
         declare_model_arg,
         declare_robot_name_arg,
         robot_state_publisher,
         joint_state_publisher_gui,
-        rviz
+        rviz,
+        shutdown_on_rviz_exit
     ])

src/iiwa_bringup/launch/supported/rviz.launch.py

@@ -0,0 +1,46 @@
+from launch_ros.actions import Node
+from launch.events import Shutdown
+from launch import LaunchDescription
+from launch.event_handlers import OnProcessExit
+from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.actions import RegisterEventHandler, EmitEvent, DeclareLaunchArgument
+ 
+
+def generate_launch_description():
+
+    declare_package_arg = DeclareLaunchArgument(
+        'package_name',
+        default_value='iiwa_bringup',
+        description='Package name where RViz config is stored'
+    )
+
+    package_name = LaunchConfiguration('package_name')
+
+    rviz_config = PathJoinSubstitution([
+        FindPackageShare(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())]
+        )
+    )
+
+    return LaunchDescription([
+        declare_package_arg,
+        rviz,
+        shutdown_on_rviz_exit
+    ])

src/iiwa_bringup/launch/supported/webots_controllers.launch.py

@@ -0,0 +1,51 @@
+from launch_ros.actions import Node
+from webots_ros2_driver.urdf_spawner import URDFSpawner
+
+from launch import LaunchDescription
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from iiwa_bringup.utils import converter
+
+
+def _setup_controllers(context, *args, **kwargs):
+    model_path = LaunchConfiguration('model').perform(context)
+    robot_name = LaunchConfiguration('robot_name').perform(context)
+    transform = LaunchConfiguration('transform').perform(context)
+    rotation = LaunchConfiguration('rotation').perform(context)
+    timer = LaunchConfiguration('controller_timer').perform(context)
+
+    robot_description = converter.load_robot_description(model_path=model_path,
+                                                         robot_name=robot_name)
+    
+    tmo = ['--controller-manager-timeout', str(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=['iiwa_arm_controller'] + tmo,
+        parameters=[{'use_sim_time': False}],
+    )
+
+    spawner_urdf = URDFSpawner(
+        name=robot_name,
+        robot_description=robot_description,
+        translation=transform,
+        rotation=rotation
+    )
+
+    return [jsb, jtc, spawner_urdf]
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=_setup_controllers)
+    ])

src/iiwa_bringup/launch/supported/webots_spawn.launch.py

@@ -0,0 +1,90 @@
+from launch.events import Shutdown
+from launch import LaunchDescription
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import LaunchConfiguration
+from launch_ros.substitutions import FindPackageShare
+from launch.event_handlers import OnProcessExit, OnProcessStart
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import (IncludeLaunchDescription, 
+                            OpaqueFunction,
+                            TimerAction,
+                            RegisterEventHandler,
+                            EmitEvent)
+
+
+from webots_ros2_driver.webots_launcher import WebotsLauncher
+from webots_ros2_driver.webots_controller import WebotsController
+
+
+def _spawn_setup(context, *args, **kwargs):
+    model_path = LaunchConfiguration('model').perform(context)
+    robot_name = LaunchConfiguration('robot_name').perform(context)
+    world_path = LaunchConfiguration('world').perform(context)
+    transform = LaunchConfiguration('transform').perform(context)
+    rotation = LaunchConfiguration('rotation').perform(context)
+    timer = LaunchConfiguration('controller_timer').perform(context)
+
+    webots = WebotsLauncher(world=world_path, 
+                            ros2_supervisor=True)
+    
+    driver = WebotsController(
+        robot_name=robot_name,
+        parameters=[
+            {
+                "robot_description": model_path,
+                "use_sim_time": False,
+                "set_robot_state_publisher": False
+            },
+            LaunchConfiguration('controller').perform(context)
+        ],
+        respawn=True
+    )
+
+    controllers_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare('iiwa_bringup'),
+                'launch',
+                'supported',
+                'webots_controllers.launch.py'
+            ])
+        ),
+        launch_arguments={
+            'robot_name': robot_name,
+            'model': model_path,
+            'transform': transform,
+            'rotation': rotation,
+            'controller_timer': timer
+        }.items()
+    )
+
+    spawn_on_driver_start = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=driver,
+            on_start=lambda evt, ctx: [
+                TimerAction(period=5.0,
+                            actions=[controllers_launch]
+                            )
+            ]
+        )
+    )
+
+    shutdown_on_webots_exit = RegisterEventHandler(
+        OnProcessExit(
+            target_action=webots,
+            on_exit=[EmitEvent(event=Shutdown())]
+        )
+    )
+
+    return [webots,
+            webots._supervisor, 
+            driver,
+            spawn_on_driver_start,
+            shutdown_on_webots_exit]
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=_spawn_setup)
+    ])

src/iiwa_bringup/setup.py

@@ -1,63 +1,55 @@
 import os
-from setuptools import find_packages, setup
+from setuptools import setup
-
-EXCLUDES = {'.DS_Store', 'Thumbs.db'}
-EXCLUDE_DIRS = {'__pycache__', '.pytest_cache', '.git', '.idea'}
-
-def data_files_from_tree(src_dir: str, dst_root: str) -> list:
-    """
-    Собирает data_files в формате, подходящем для setuptools.
-    Возвращает список пар (dst_path, [file1, file2, ...]).
-    """
-    entries = []
-
-    for root, dirs, files in os.walk(src_dir):
-        # исключаем служебные директории
-        dirs[:] = [d for d in dirs if d not in EXCLUDE_DIRS]
-
-        file_list = [os.path.join(root, f) for f in files if f not in EXCLUDES]
-        if not file_list:
-            continue
-
-        rel = os.path.relpath(root, src_dir)
-        dst_dir = os.path.join(dst_root, rel) if rel != '.' else dst_root
-        entries.append((dst_dir, file_list))
-
-    return entries
 
 package_name = 'iiwa_bringup'
 
-launch_intsall_root = f"share/{package_name}/launch"
+# Собираем список пакетов: основной + вложенные (если они есть).
-config_intsall_root = f"share/{package_name}/config"
+packages = [package_name]
+# Если есть подпапка utils с __init__.py — зарегистрируем её как iiwa_bringup.utils
+if os.path.isdir('utils') and os.path.isfile(os.path.join('utils', '__init__.py')):
+    packages.append(f'{package_name}.utils')
 
+# Соответствие имени пакета -> директория на диске.
+# iiwa_bringup -> текущая папка '.'
+# iiwa_bringup.utils -> ./utils
+package_dir = {
+    package_name: '.',
+}
+if f'{package_name}.utils' in packages:
+    package_dir[f'{package_name}.utils'] = os.path.join('.', 'utils')
 
-data_files = [
+def data_files_from_tree(src_dir: str, dst_root: str) -> list:
-    ('share/' + package_name, ['package.xml']),
+    entries = []
-]
+    if not os.path.isdir(src_dir):
-
+        return entries
-data_files += data_files_from_tree('launch', launch_intsall_root)
+    EXCLUDE_DIRS = {'.git', '__pycache__', '.pytest_cache', '.idea'}
-data_files += data_files_from_tree('config', config_intsall_root)
+    EXCLUDES = {'.DS_Store', 'Thumbs.db'}
-
+    for root, dirs, files in os.walk(src_dir):
+        dirs[:] = [d for d in dirs if d not in EXCLUDE_DIRS]
+        file_list = [os.path.join(root, f) for f in files if f not in EXCLUDES]
+        if not file_list:
+            continue
+        rel = os.path.relpath(root, src_dir)
+        dst_dir = os.path.join(dst_root, rel) if rel != '.' else dst_root
+        entries.append((dst_dir, file_list))
+    return entries
 
+data_files = [(f'share/{package_name}', ['package.xml'])]
+data_files += data_files_from_tree('launch', f'share/{package_name}/launch')
+data_files += data_files_from_tree('config', f'share/{package_name}/config')
+data_files += data_files_from_tree('resource', f'share/{package_name}/resource')
 
 setup(
     name=package_name,
     version='0.0.1',
-    packages=find_packages(exclude=['test']),
+    packages=packages,
+    package_dir=package_dir,
+    include_package_data=True,
     data_files=data_files,
     install_requires=['setuptools'],
-    zip_safe=True,
+    zip_safe=False,
     maintainer='Grabar Daniil',
     maintainer_email='grabardm@ml-dev.ru',
-    description='TODO: Package description',
+    description='iiwa bringup package',
     license='Apache-2.0',
-    extras_require={
-        'test': [
-            'pytest',
-        ],
-    },
-    entry_points={
-        'console_scripts': [
-        ],
-    },
 )

src/iiwa_bringup/utils/converter.py

@@ -0,0 +1,12 @@
+import xacro
+from pathlib import Path
+
+
+def load_robot_description(model_path: Path, robot_name: str) -> str:
+    suffix = Path(model_path).suffix.lower()
+    if suffix == ".xacro":
+        return xacro.process_file(model_path, mappings={'name': str(robot_name)}).toxml()
+    elif suffix == ".urdf":
+        return Path(model_path).read_text(encoding="utf-8")
+    else:
+        raise FileNotFoundError(f"Supported file formats: xacro/urdf")

src/iiwa_controller/CMakeLists.txt

@@ -0,0 +1,104 @@
+cmake_minimum_required(VERSION 3.8)
+project(iiwa_controller)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+# FRI headers / sources
+set(FRI_HEADER
+    external/libFRI/include
+    external/libFRI/src/protobuf_gen
+    external/libFRI/src/nanopb-0.2.8
+    external/libFRI/src/protobuf
+    external/libFRI/src/connection
+    external/libFRI/src/client_lbr
+    external/libFRI/src/base
+  )
+
+set(FRI_SRC
+    external/libFRI/src/base/friClientApplication.cpp
+    external/libFRI/src/client_lbr/friLBRClient.cpp
+    external/libFRI/src/client_lbr/friLBRCommand.cpp
+    external/libFRI/src/client_lbr/friLBRState.cpp
+    external/libFRI/src/connection/friUdpConnection.cpp
+    external/libFRI/src/protobuf/friCommandMessageEncoder.cpp
+    external/libFRI/src/protobuf/friMonitoringMessageDecoder.cpp
+    external/libFRI/src/protobuf/pb_frimessages_callbacks.c
+    external/libFRI/src/protobuf_gen/FRIMessages.pb.c
+    external/libFRI/src/nanopb-0.2.8/pb_decode.c
+    external/libFRI/src/nanopb-0.2.8/pb_encode.c
+    external/libFRI/src/client_trafo/friTransformationClient.cpp
+  )
+
+add_library(${PROJECT_NAME}
+  SHARED
+  src/IIWAHardwareInterface.cpp
+  ${FRI_SRC}
+  src/FRIClient.cpp
+)
+
+target_include_directories(${PROJECT_NAME}
+  PRIVATE
+    include
+    ${FRI_HEADER}
+)
+
+target_compile_definitions(${PROJECT_NAME}
+  PRIVATE
+    PB_FIELD_16BIT
+    HAVE_SOCKLEN_T
+    PB_FIELD_16BIT 
+    PB_NO_ERRMSG
+)
+
+target_link_libraries(${PROJECT_NAME}
+  PUBLIC
+    hardware_interface::hardware_interface
+    pluginlib::pluginlib
+    rclcpp::rclcpp
+    rclcpp_lifecycle::rclcpp_lifecycle
+    Eigen3::Eigen
+)
+
+# Export plugin description for pluginlib
+pluginlib_export_plugin_description_file(hardware_interface iiwa_controller_plugin.xml)
+
+# Installation
+install(TARGETS ${PROJECT_NAME}
+  DESTINATION lib
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+
+ament_export_dependencies(
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  Eigen3
+)
+
+ament_package()

src/iiwa_controller/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

src/iiwa_controller/external/libFRI/include/friClientApplication.h

@@ -0,0 +1,155 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+
+#ifndef _KUKA_FRI_CLIENT_APPLICATION_H
+#define _KUKA_FRI_CLIENT_APPLICATION_H
+
+/** Kuka namespace */
+namespace KUKA
+{
+/** Fast Robot Interface (FRI) namespace */
+namespace FRI
+{
+
+   // forward declarations
+   class IClient;
+   class TransformationClient;
+   class IConnection;
+   struct ClientData;
+
+   /**
+    * \brief FRI client application class.
+    * 
+    * A client application takes an instance of the IConnection interface and
+    * an instance of an IClient interface to provide the functionality
+    * needed to set up an FRI client application. It can be used to easily 
+    * integrate the FRI client code within other applications.
+    * The algorithmic functionality of an FRI client application is implemented
+    * using the IClient interface.
+    */
+   class ClientApplication
+   {
+   
+   public:
+   
+      /**
+       * \brief Constructor without transformation client.
+       * 
+       * This constructor takes an instance of the IConnection interface and
+       * an instance of the IClient interface as parameters.
+       * @param connection FRI connection class
+       * @param client FRI client class
+       */
+      ClientApplication(IConnection& connection, IClient& client);
+     
+      /**
+       * \brief Constructor with transformation client.
+       * 
+       * This constructor takes an instance of the IConnection interface and
+       * an instance of the IClient interface and an instance of a
+       * TransformationClient as parameters.
+       * @param connection FRI connection class
+       * @param client FRI client class
+       * @param trafoClient FRI transformation client class
+       */
+      ClientApplication(IConnection& connection, IClient& client, TransformationClient& trafoClient);
+
+      /** \brief Destructor. */
+      ~ClientApplication();
+   
+      /**
+       * \brief Connect the FRI client application with a KUKA Sunrise controller.
+       * 
+       * @param port The port ID
+       * @param remoteHost The address of the remote host
+       * @return True if connection was established
+       */
+      bool connect(int port, const char *remoteHost = NULL);
+      
+      /**
+       * \brief Disconnect the FRI client application from a KUKA Sunrise controller.
+       */
+      void disconnect();
+   
+      /**
+       * \brief Run a single processing step.
+       * 
+       * The processing step consists of receiving a new FRI monitoring message,
+       * calling the corresponding client callback and sending the resulting 
+       * FRI command message back to the KUKA Sunrise controller.
+       * @return True if all of the substeps succeeded.
+       */
+      bool step();
+
+   protected:
+   
+      IConnection&   _connection;          //!< connection interface
+      IClient*       _robotClient;         //!< robot client interface
+      TransformationClient* _trafoClient;  //!< transformation client interface
+      ClientData*    _data;                //!< client data structure (for internal use)
+
+   };
+
+}
+}
+
+
+#endif // _KUKA_FRI_CLIENT_APPLICATION_H
+

src/iiwa_controller/external/libFRI/include/friClientIf.h

@@ -0,0 +1,202 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#ifndef _KUKA_FRI_CLIENT_H
+#define _KUKA_FRI_CLIENT_H
+
+
+
+/** Kuka namespace */
+namespace KUKA
+{
+/** Fast Robot Interface (FRI) namespace */
+namespace FRI
+{
+
+   // forward declarations
+   struct ClientData;
+   
+
+   /** \brief Enumeration of the FRI session state. */
+   enum ESessionState
+   {
+      IDLE = 0,               //!< no session available
+      MONITORING_WAIT = 1,    //!< monitoring mode with insufficient connection quality
+      MONITORING_READY = 2,   //!< monitoring mode with connection quality sufficient for command mode
+      COMMANDING_WAIT = 3,    //!< command mode about to start (overlay motion queued)
+      COMMANDING_ACTIVE = 4   //!< command mode active
+   };
+
+   /** \brief Enumeration of the FRI connection quality. */
+   enum EConnectionQuality
+   {
+      POOR = 0,               //!< poor connection quality
+      FAIR = 1,               //!< connection quality insufficient for command mode
+      GOOD = 2,               //!< connection quality sufficient for command mode
+      EXCELLENT = 3           //!< excellent connection quality
+   };
+
+   /** \brief Enumeration of the controller's safety state. */
+   enum ESafetyState
+   {
+      NORMAL_OPERATION = 0,   //!< No safety stop request present.
+      SAFETY_STOP_LEVEL_0 = 1,//!< Safety stop request STOP0 or STOP1 present.
+      SAFETY_STOP_LEVEL_1 = 2,//!< Safety stop request STOP1 (on-path) present.
+      SAFETY_STOP_LEVEL_2 = 3 //!< Safety stop request STOP2 present.
+   };
+
+   /** \brief Enumeration of the controller's current mode of operation. */
+   enum EOperationMode
+   {
+      TEST_MODE_1 = 0,        //!< test mode 1 with reduced speed (T1)
+      TEST_MODE_2 = 1,        //!< test mode 2 (T2)
+      AUTOMATIC_MODE = 2      //!< automatic operation mode (AUT)
+   };
+
+   /** \brief Enumeration of a drive's state. */
+   enum EDriveState
+   {
+      OFF = 0,                //!< drive is not being used
+      TRANSITIONING = 1,      //!< drive is in a transitioning state (before or after motion)
+      ACTIVE = 2              //!< drive is being actively commanded
+   };
+   
+   /** \brief Enumeration of control mode. */
+   enum EControlMode
+   {
+      POSITION_CONTROL_MODE = 0,  //!< position control mode
+      CART_IMP_CONTROL_MODE = 1,  //!< cartesian impedance control mode
+      JOINT_IMP_CONTROL_MODE = 2, //!< joint impedance control mode
+      NO_CONTROL = 3              //!< drives are not used
+   };
+   
+   
+   /** \brief Enumeration of the client command mode. */
+   enum EClientCommandMode
+   {
+      NO_COMMAND_MODE = 0,  //!< no client command mode available
+      POSITION = 1,   //!< commanding joint positions by the client
+      WRENCH = 2,     //!< commanding wrenches and joint positions by the client
+      TORQUE = 3      //!< commanding joint torques and joint positions by the client 
+   };
+   
+   /** \brief Enumeration of the overlay type. */
+   enum EOverlayType
+   {
+      NO_OVERLAY = 0,   //!< no overlay type available
+      JOINT = 1,        //!< joint overlay  
+      CARTESIAN = 2     //!< cartesian overlay
+   };
+   
+   
+   /**
+    * \brief FRI client interface. 
+    * 
+    * This is the callback interface that should be implemented by FRI clients. 
+    * Callbacks are automatically called by the client application
+    * (ClientApplication) whenever new FRI messages arrive.
+    */
+   class IClient
+   {
+      friend class ClientApplication;
+      
+   public:
+      
+      /** \brief Virtual destructor. */
+      virtual ~IClient() {}
+   
+      /** 
+       * \brief Callback that is called whenever the FRI session state changes.
+       * 
+       * @param oldState previous FRI session state
+       * @param newState current FRI session state
+       */
+      virtual void onStateChange(ESessionState oldState, ESessionState newState) = 0;
+   
+      /**
+       * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'.
+       */
+      virtual void monitor() = 0;
+   
+      /**
+       * \brief Callback for the FRI session state 'Commanding Wait'.
+       */
+      virtual void waitForCommand() = 0;
+   
+      /**
+       * \brief Callback for the FRI session state 'Commanding'.
+       */
+      virtual void command() = 0;
+      
+   protected:
+      
+      /**
+       * \brief Method to create and initialize the client data structure (used internally).
+       * 
+       * @return newly allocated client data structure
+       */
+      virtual ClientData* createData() = 0;
+      
+   };
+   
+}
+}
+
+
+#endif // _KUKA_FRI_CLIENT_H

src/iiwa_controller/external/libFRI/include/friConnectionIf.h

@@ -0,0 +1,130 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#ifndef _KUKA_FRI_CONNECTION_H
+#define _KUKA_FRI_CONNECTION_H
+
+
+/** Kuka namespace */
+namespace KUKA
+{
+namespace FRI
+{
+   
+   /**
+    * \brief FRI connection interface. 
+    * 
+    * Connections to the KUKA Sunrise controller have to be implemented using
+    * this interface.
+    */
+   class IConnection
+   {
+   
+   public:
+      
+      /** \brief Virtual destructor. */
+      virtual ~IConnection() {}
+   
+      /**
+       * \brief Open a connection to the KUKA Sunrise controller.
+       * 
+       * @param port The port ID
+       * @param remoteHost The address of the remote host
+       * @return True if connection was established
+       */
+      virtual bool open(int port, const char *remoteHost) = 0;
+   
+      /**
+       * \brief Close a connection to the KUKA Sunrise controller.
+       */
+      virtual void close() = 0;
+      
+      /**
+       * \brief Checks whether a connection to the KUKA Sunrise controller is established.
+       * 
+       * @return True if connection is established
+       */
+      virtual bool isOpen() const = 0;
+   
+      /**
+       * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. 
+       * 
+       * This method blocks until a new message arrives.
+       * @param buffer Pointer to the allocated buffer that will hold the FRI message
+       * @param maxSize Size in bytes of the allocated buffer
+       * @return Number of bytes received (0 when connection was terminated, negative in case of errors)
+       */
+      virtual int receive(char *buffer, int maxSize) = 0;
+   
+      /**
+       * \brief Send a new FRI command message to the KUKA Sunrise controller.
+       * 
+       * @param buffer Pointer to the buffer holding the FRI message
+       * @param size Size in bytes of the message to be send
+       * @return True if successful
+       */
+      virtual bool send(const char* buffer, int size) = 0;   
+   
+   };
+   
+}
+}
+
+
+#endif // _KUKA_FRI_CONNECTION_H

src/iiwa_controller/external/libFRI/include/friException.h

@@ -0,0 +1,153 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#ifndef _KUKA_FRI_EXCEPTION_H
+#define _KUKA_FRI_EXCEPTION_H
+
+#include "stdio.h"
+
+/** Kuka namespace */
+namespace KUKA
+{
+namespace FRI
+{
+   
+   /**
+    * \brief Standard exception for the FRI Client
+    * 
+    * \note For realtime considerations the internal message buffer is static. 
+    * So don't use this exception in more than one thread per process. 
+    */
+   class FRIException
+   {
+         
+   public:
+      
+      /**
+       * \brief FRIException Constructor
+       * 
+       * @param message Error message
+       */
+      FRIException(const char* message) 
+      { 
+         strncpy(_buffer, message, sizeof(_buffer) - 1);
+         _buffer[sizeof(_buffer) - 1] = 0; // ensure string termination
+         printf("FRIException: ");
+         printf(_buffer);
+         printf("\n");
+      }
+      
+      /**
+       * \brief FRIException Constructor
+       * 
+       * @param message Error message which may contain one "%s" parameter 
+       * @param param1 First format parameter for parameter message.
+       */
+      FRIException(const char* message, const char* param1) 
+      {
+#ifdef _MSC_VER
+         _snprintf( // visual studio compilers (up to VS 2013) only know this method
+#else
+         snprintf(
+#endif
+            _buffer, sizeof(_buffer), message, param1);
+         printf("FRIException: ");
+         printf(_buffer);
+         printf("\n");
+      }
+      
+      /**
+       * \brief FRIException Constructor
+       * 
+       * @param message Error message which may contain two "%s" parameter 
+       * @param param1 First format parameter for parameter message.
+       * @param param2 Second format parameter for parameter message.
+       */
+      FRIException(const char* message, const char* param1, const char* param2) 
+      {
+#ifdef _MSC_VER
+         _snprintf( // visual studio compilers (up to VS 2013) only know this method
+#else
+         snprintf(
+#endif
+            _buffer, sizeof(_buffer), message, param1, param2);
+         printf("FRIException: ");
+         printf(_buffer);
+         printf("\n");
+      }
+      
+      /**
+       * \brief Get error string.
+       * @return Error message stored in the exception.
+       */
+      const char* getErrorMessage() const { return _buffer; }
+      
+      /** \brief Virtual destructor. */
+      virtual ~FRIException() {}
+   
+   protected:
+      static char _buffer[1024];
+   
+   };
+   
+}
+}
+
+
+#endif // _KUKA_FRI_EXCEPTION_H

src/iiwa_controller/external/libFRI/include/friLBRClient.h

@@ -0,0 +1,145 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#ifndef _KUKA_FRI_LBR_CLIENT_H
+#define _KUKA_FRI_LBR_CLIENT_H
+
+#include "friClientIf.h"
+#include "friLBRState.h"
+#include "friLBRCommand.h"
+
+/** Kuka namespace */
+namespace KUKA
+{
+namespace FRI
+{
+
+   /**
+    * \brief Implementation of the IClient interface for the KUKA LBR (lightweight) robots.
+    * 
+    * Provides access to the current LBR state and the possibility to send new 
+    * commands to the LBR.  
+    */
+   class LBRClient : public IClient
+   {
+   
+   public:
+      
+      /** \brief Constructor. */
+      LBRClient();
+      
+      /** \brief Destructor. */
+      ~LBRClient();
+         
+      /** 
+       * \brief Callback that is called whenever the FRI session state changes.
+       * 
+       * @param oldState previous FRI session state
+       * @param newState current FRI session state
+       */
+      virtual void onStateChange(ESessionState oldState, ESessionState newState);
+   
+      /**
+       * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'.
+       */
+      virtual void monitor();
+   
+      /**
+       * \brief Callback for the FRI session state 'Commanding Wait'.
+       */
+      virtual void waitForCommand();
+   
+      /**
+       * \brief Callback for the FRI session state 'Commanding'.
+       */
+      virtual void command();
+   
+      /**
+       * \brief Provide read access to the current robot state.
+       * 
+       * @return Reference to the LBRState instance
+       */
+      const LBRState& robotState() const { return _robotState; }
+      
+      /**
+       * \brief Provide write access to the robot commands.
+       * 
+       * @return Reference to the LBRCommand instance
+       */
+      LBRCommand& robotCommand() { return _robotCommand; }
+      
+   private:
+      
+      LBRState _robotState;      //!< wrapper class for the FRI monitoring message
+      LBRCommand _robotCommand;  //!< wrapper class for the FRI command message
+      
+      /**
+       * \brief Method to create and initialize the client data structure (used internally).
+       * 
+       * @return newly allocated client data structure
+       */
+      virtual ClientData* createData();
+      
+   };
+   
+}
+}
+
+
+#endif // _KUKA_FRI_LBR_CLIENT_H

src/iiwa_controller/external/libFRI/include/friLBRCommand.h

@@ -0,0 +1,162 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#ifndef _KUKA_FRI_LBR_COMMAND_H
+#define _KUKA_FRI_LBR_COMMAND_H
+
+
+// forward declarations
+typedef struct _FRICommandMessage FRICommandMessage;
+
+/** Kuka namespace */
+namespace KUKA
+{
+namespace FRI
+{
+   
+   /**
+    * \brief Wrapper class for the FRI command message for a KUKA LBR (leightweight) robot.
+    */
+   class LBRCommand
+   {
+      friend class LBRClient;
+   
+   public:
+            
+      /**
+       * \brief Set the joint positions for the current interpolation step.
+       * 
+       * This method is only effective when the client is in a commanding state.
+       * @param values Array with the new joint positions (in rad)
+       */
+      void setJointPosition(const double* values);
+      
+      /**
+       * \brief Set the applied wrench vector of the current interpolation step.
+       * 
+       * The wrench vector consists of:
+       * [F_x, F_y, F_z, tau_A, tau_B, tau_C]
+       * 
+       * F ... forces (in N) applied along the Cartesian axes of the 
+       * currently used motion center.
+       * tau ... torques (in Nm) applied along the orientation angles 
+       * (Euler angles A, B, C) of the currently used motion center.
+       *  
+       * This method is only effective when the client is in a commanding state.
+       * The ControlMode of the robot has to be Cartesian impedance control mode. The
+       * Client Command Mode has to be wrench.
+       * 
+       * @param wrench Applied Cartesian wrench vector.
+       */
+      void setWrench(const double* wrench);
+
+      /**
+       * \brief Set the applied joint torques for the current interpolation step.
+       * 
+       * This method is only effective when the client is in a commanding state.
+       * The ControlMode of the robot has to be joint impedance control mode. The
+       * Client Command Mode has to be torque.
+       * 
+       * @param torques Array with the applied torque values (in Nm)
+       */
+      void setTorque(const double* torques);
+      
+      /**
+       * \brief Set boolean output value.
+       * 
+       * @throw FRIException Throws a FRIException if more outputs are set than can be registered. 
+       * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output.
+       * @param name Full name of the IO (Syntax "IOGroupName.IOName").
+       * @param value Boolean value to set. 
+       */
+      void setBooleanIOValue(const char* name, const bool value);
+      
+      /**
+       * \brief Set digital output value.
+       * 
+       * @throw FRIException Throws a FRIException if more outputs are set than can be registered. 
+       * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output.
+       * @param name Full name of the IO (Syntax "IOGroupName.IOName").
+       * @param value Digital value to set. 
+       */
+      void setDigitalIOValue(const char* name, const unsigned long long value);
+      
+      /**
+       * \brief Set analog output value.
+       * 
+       * @throw FRIException Throws a FRIException if more outputs are set than can be registered. 
+       * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output.
+       * @param name Full name of the IO (Syntax "IOGroupName.IOName").
+       * @param value Analog value to set. 
+       */
+      void setAnalogIOValue(const char* name, const double value);      
+      
+   protected:
+            
+      static const int LBRCOMMANDMESSAGEID = 0x34001; //!< type identifier for the FRI command message corresponding to a KUKA LBR robot 
+      FRICommandMessage* _cmdMessage;                 //!< FRI command message (protobuf struct)
+      FRIMonitoringMessage* _monMessage;                 //!< FRI monitoring message (protobuf struct)
+
+   };
+
+}
+}
+
+
+#endif // _KUKA_FRI_LBR_COMMAND_H

src/iiwa_controller/external/libFRI/include/friLBRState.h

@@ -0,0 +1,278 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#ifndef _KUKA_FRI_LBR_STATE_H
+#define _KUKA_FRI_LBR_STATE_H
+
+#include "friClientIf.h"
+
+// forward declarations
+typedef struct _FRIMonitoringMessage FRIMonitoringMessage;
+
+/** Kuka namespace */
+namespace KUKA
+{
+namespace FRI
+{   
+   
+   /**
+    * \brief Wrapper class for the FRI monitoring message for a KUKA LBR (leightweight) robot.
+    */
+   class LBRState
+   {
+      friend class LBRClient;
+      
+   public:
+
+      enum
+      {
+          NUMBER_OF_JOINTS = 7      //!< number of axes of the KUKA LBR robot   
+      };
+
+      LBRState();
+      
+      /**
+       * \brief Get the sample time in seconds. 
+       * 
+       * This is the period in which the KUKA Sunrise controller is sending 
+       * FRI packets.
+       * @return sample time in seconds
+       */
+      double getSampleTime() const; // sec
+      
+      /**
+       * \brief Get the current FRI session state.
+       * 
+       * @return current FRI session state
+       */
+      ESessionState getSessionState() const;
+      
+      /**
+       * \brief Get the current FRI connection quality.
+       * 
+       * @return current FRI connection quality
+       */
+      EConnectionQuality getConnectionQuality() const;
+      
+      /**
+       * \brief Get the current safety state of the KUKA Sunrise controller.
+       * 
+       * @return current safety state
+       */
+      ESafetyState getSafetyState() const;
+      
+      /**
+       * \brief Get the current operation mode of the KUKA Sunrise controller.
+       * 
+       * @return current operation mode
+       */
+      EOperationMode getOperationMode() const; 
+      
+      /**
+       * \brief Get the accumulated drive state over all drives of the KUKA LBR controller.
+       * 
+       * If the drive states differ between drives, the following rule applies:
+       * 1) The drive state is OFF if all drives are OFF.
+       * 2) The drive state is ACTIVE if all drives are ACTIVE.
+       * 3) otherwise the drive state is TRANSITIONING.
+       * @return accumulated drive state
+       */
+      EDriveState getDriveState() const;
+            
+      /**
+      * \brief Get the client command mode specified by the client.
+      * 
+      * @return the client command mode specified by the client.
+      */
+      EClientCommandMode getClientCommandMode() const;
+      
+      /**
+       * \brief Get the overlay type specified by the client.
+       * 
+       * @return the overlay type specified by the client.
+       */
+      EOverlayType getOverlayType() const;
+            
+      /**
+       * \brief Get the control mode of the KUKA LBR robot.
+       * 
+       * @return current control mode of the KUKA LBR robot.
+       */
+      EControlMode getControlMode() const;
+      
+      /** 
+       * \brief Get the timestamp of the current robot state in Unix time. 
+       * 
+       * This method returns the seconds since 0:00, January 1st, 1970 (UTC).
+       * Use getTimestampNanoSec() to increase your timestamp resolution when
+       * seconds are insufficient. 
+       * @return timestamp encoded as Unix time (seconds)
+       */
+      unsigned int getTimestampSec() const;
+      
+      /** 
+       * \brief Get the nanoseconds elapsed since the last second (in Unix time).
+       * 
+       * This method complements getTimestampSec() to get a high resolution 
+       * timestamp. 
+       * @return timestamp encoded as Unix time (nanoseconds part)
+       */
+      unsigned int getTimestampNanoSec() const;
+      
+      /**
+       * \brief Get the currently measured joint positions of the robot.
+       * 
+       * @return array of the measured joint positions in radians
+       */
+      const double* getMeasuredJointPosition() const;
+      
+      /**
+       * \brief Get the last commanded joint positions of the robot.
+       * 
+       * @return array of the commanded joint positions in radians
+       */
+      const double* getCommandedJointPosition() const;
+      
+      /**
+       * \brief Get the currently measured joint torques of the robot.
+       * 
+       * @return array of the measured torques in Nm
+       */
+      const double* getMeasuredTorque() const;
+      
+      /**
+       * \brief Get the last commanded joint torques of the robot.
+       * 
+       * @return array of the commanded torques in Nm
+       */
+      const double* getCommandedTorque() const;
+      
+      /**
+       * \brief Get the currently measured external joint torques of the robot.
+       * 
+       * The external torques corresponds to the measured torques when removing
+       * the torques induced by the robot itself. 
+       * @return array of the external torques in Nm
+       */
+      const double* getExternalTorque() const;
+      
+      /**
+       * \brief Get the joint positions commanded by the interpolator.
+       * 
+       * When commanding a motion overlay in your robot application, this method
+       * will give access to the joint positions currently commanded by the
+       * motion interpolator.
+       * @throw FRIException This method will throw an FRIException during monitoring mode.
+       * @return array of the ipo joint positions in radians 
+       */
+      const double* getIpoJointPosition() const;
+      
+      /**
+       * \brief Get an indicator for the current tracking performance of the commanded robot.
+       * 
+       * The tracking performance is an indicator on how well the commanded robot
+       * is able to follow the commands of the FRI client. The best possible value
+       * 1.0 is reached when the robot executes the given commands instantaneously.
+       * The tracking performance drops towards 0 when latencies are induced, 
+       * e.g. when the commanded velocity, acceleration or jerk exceeds the 
+       * capabilities of the robot.
+       * The tracking performance will always be 0 when the session state does
+       * not equal COMMANDING_ACTIVE.
+       * @return current tracking performance
+       */
+      double getTrackingPerformance() const;
+      
+      /**
+       * \brief Get boolean IO value.
+       *
+       * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown.
+       * @param name Full name of the IO (Syntax "IOGroupName.IOName"). 
+       * @return Returns IO's boolean value.
+       */
+      bool getBooleanIOValue(const char* name) const;
+      
+      /**
+       * \brief Get digital IO value.
+       *
+       * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown.
+       * @param name Full name of the IO (Syntax "IOGroupName.IOName"). 
+       * @return Returns IO's digital value.
+       */
+      unsigned long long getDigitalIOValue(const char* name) const;
+      
+      /**
+       * \brief Get analog IO value.
+       *
+       * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown.
+       * @param name Full name of the IO (Syntax "IOGroupName.IOName"). 
+       * @return Returns IO's analog value.
+       */
+      double getAnalogIOValue(const char* name) const;
+      
+   protected:
+      
+      static const int LBRMONITORMESSAGEID  = 0x245142;  //!< type identifier for the FRI monitoring message corresponding to a KUKA LBR robot 
+      FRIMonitoringMessage* _message;                    //!< FRI monitoring message (protobuf struct)
+   };
+
+}
+}
+
+
+#endif // _KUKA_FRI_LBR_STATE_H

src/iiwa_controller/external/libFRI/include/friTransformationClient.h

@@ -0,0 +1,267 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#ifndef _KUKA_FRI_TRANSFORMATION_CLIENT_H
+#define _KUKA_FRI_TRANSFORMATION_CLIENT_H
+
+#include <vector>
+#include "friClientIf.h"
+
+/** Kuka namespace */
+namespace KUKA
+{
+namespace FRI
+{
+
+	// forward declaration
+	struct ClientData;
+
+   /**
+    * \brief Abstract FRI transformation client. 
+    * 
+    * A transformation client enables the user to send transformation matrices cyclically to the 
+    * KUKA Sunrise controller for manipulating the transformations of dynamic frames in the
+    * Sunrise scenegraph.
+    * Usually, these matrices will be provided by external sensors. 
+    * <br>
+    * Custom transformation clients have to be derived from this class and need to
+    * implement the provide() callback. This callback is called once by the
+    * client application whenever a new FRI message arrives.
+    * 
+    * <b>This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.</b> 
+    */
+   class TransformationClient
+   {
+      
+      friend class ClientApplication;
+     
+   public:
+      
+      /**
+       * <br>  <b>This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.</b>  <br> 
+       * \brief Constructor.
+       **/
+      TransformationClient();
+      
+      /** <br> <b>This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.</b>   <br> 
+       * \brief Virtual destructor. 
+       **/
+      virtual ~TransformationClient();
+      
+      /** 
+       * <br>  <b>This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.</b>  <br>
+       * \brief Callback which is called whenever a new FRI message arrives. 
+       *
+       *  In this callback all requested transformations have to be set.
+       *  
+       *  \see getRequestedTransformationIDs(), setTransformation()
+       */
+      virtual void provide() = 0;
+      
+      /**
+       * \brief Get the sample time in seconds. 
+       * 
+       * This is the period in which the KUKA Sunrise controller is sending 
+       * FRI packets.
+       * @return sample time in seconds
+       */
+      double getSampleTime() const; // sec
+      
+      /**
+       * \brief Get the current FRI connection quality.
+       * 
+       * @return current FRI connection quality
+       */
+      EConnectionQuality getConnectionQuality() const;      
+
+      /** 
+       * <br>  <b>This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.</b>  <br>
+       * \brief  Returns a vector of identifiers of all requested transformation matrices.
+       * 
+       * The custom TransformationClient has to provide data for transformation matrices with these
+       * identifiers. 
+       * 
+       * @return reference to vector of IDs of requested transformations
+       */
+      const std::vector<const char*>& getRequestedTransformationIDs() const;
+      
+      /** 
+       * <br>  <b>This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.</b>  <br>
+       * 
+       * \brief Get the timestamp of the current received FRI monitor message in Unix time. 
+       * 
+       * This method returns the seconds since 0:00, January 1st, 1970 (UTC).
+       * Use getTimestampNanoSec() to increase your timestamp resolution when
+       * seconds are insufficient. 
+       * 
+       * @return timestamp encoded as Unix time (seconds)
+       */
+      const unsigned int getTimestampSec() const;
+      
+      /** 
+       * <br>  <b>This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.</b>  <br>
+       * \brief Get the nanoseconds elapsed since the last second (in Unix time).
+       * 
+       * This method complements getTimestampSec() to get a high resolution 
+       * timestamp. 
+       * 
+       * @return timestamp encoded as Unix time (nanoseconds part)
+       */
+      const unsigned int getTimestampNanoSec() const;
+      
+      /**
+       * <br>  <b>This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.</b>  <br>
+       * \brief Provides a requested transformation matrix.
+       * 
+       * A transformation matrix has 3x4 elements. It consists of a rotational matrix (3x3 elements)
+       * and a translational vector (3x1 elements). The complete transformation matrix has the
+       * following structure: <br>
+       * [Transformation(3x4)] = [Rotation(3x3) | Translation(3x1) ]
+       * <p>
+       * All provided transformation matrices need a timestamp that corresponds to their
+       * time of acquisiton. This timestamp must be synchronized to the timestamp
+       * provided by the KUKA Sunrise controller (see getTimestampSec(), getTimestampNanoSec()).
+       * <p>
+       * If an update to the last transformation is not yet available when the provide()
+       * callback is executed, the last transformation (including its timestamp) should be
+       * repeated until a new transformation is available. 
+       *
+       * @throw FRIException Throws a FRIException if the maximum number of transformations is exceeded.  
+       * @param transformationID Identifier string of the transformation matrix
+       * @param transformationMatrix Provided transformation matrix
+       * @param timeSec Timestamp encoded as Unix time (seconds)
+       * @param timeNanoSec Timestamp encoded as Unix time (nanoseconds part)
+       */
+      void setTransformation(const char* transformationID, const double transformationMatrix[3][4],
+            unsigned int timeSec, unsigned int timeNanoSec);
+      
+      /**
+       * \brief Set boolean output value.
+       * 
+       * @throw FRIException Throws a FRIException if more outputs are set than can be registered. 
+       * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output.
+       * @param name Full name of the IO (Syntax "IOGroupName.IOName").
+       * @param value Boolean value to set. 
+       */
+      void setBooleanIOValue(const char* name, const bool value);
+      
+      /**
+       * \brief Set digital output value.
+       * 
+       * @throw FRIException Throws a FRIException if more outputs are set than can be registered. 
+       * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output.
+       * @param name Full name of the IO (Syntax "IOGroupName.IOName").
+       * @param value Digital value to set. 
+       */
+      void setDigitalIOValue(const char* name, const unsigned long long value);
+      
+      /**
+       * \brief Set analog output value.
+       * 
+       * @throw FRIException Throws a FRIException if more outputs are set than can be registered. 
+       * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output.
+       * @param name Full name of the IO (Syntax "IOGroupName.IOName").
+       * @param value Analog value to set. 
+       */
+      void setAnalogIOValue(const char* name, const double value);  
+      
+      /**
+       * \brief Get boolean IO value.
+       *
+       * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown.
+       * @param name Full name of the IO (Syntax "IOGroupName.IOName"). 
+       * @return Returns IO's boolean value.
+       */
+      bool getBooleanIOValue(const char* name) const;
+      
+      /**
+       * \brief Get digital IO value.
+       *
+       * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown.
+       * @param name Full name of the IO (Syntax "IOGroupName.IOName"). 
+       * @return Returns IO's digital value.
+       */
+      unsigned long long getDigitalIOValue(const char* name) const;
+      
+      /**
+       * \brief Get analog IO value.
+       *
+       * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown.
+       * @param name Full name of the IO (Syntax "IOGroupName.IOName"). 
+       * @return Returns IO's analog value.
+       */
+      double getAnalogIOValue(const char* name) const;
+      
+   private: 
+      
+      ClientData* _data;   //!< the client data structure
+            
+      /**
+       * \brief Method to link the client data structure (used internally).
+       * 
+       * @param clientData the client data structure
+       */
+      void linkData(ClientData* clientData);
+      
+   };
+   
+}
+}
+
+#endif // _KUKA_FRI_TRANSFORMATION_CLIENT_H

src/iiwa_controller/external/libFRI/include/friUdpConnection.h

@@ -0,0 +1,163 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#ifndef _KUKA_FRI_UDP_CONNECTION_H
+#define _KUKA_FRI_UDP_CONNECTION_H
+
+#include <cstdlib>
+
+#ifdef _WIN32
+   #include <winsock2.h>
+#else
+   // if linux or a other unix system is used, select uses the following include
+   #ifdef __unix__
+      #include <sys/select.h>
+   #endif
+   // for VxWorks
+   #ifdef VXWORKS
+      #include <selectLib.h>
+      #include <sockLib.h>
+   #endif
+   #include <netinet/in.h>
+   #include <arpa/inet.h>
+#endif
+
+#include "friConnectionIf.h"
+
+/** Kuka namespace */
+namespace KUKA
+{
+namespace FRI
+{
+
+   /**
+    * \brief This class implements the IConnection interface using UDP sockets.
+    */
+   class UdpConnection : public IConnection
+   {
+   
+   public:
+   
+      /**
+       * \brief Constructor with an optional parameter for setting a receive timeout.
+       * 
+       * @param receiveTimeout Timeout (in ms) for receiving a UDP message (0 = wait forever) 
+       * */
+      UdpConnection(unsigned int receiveTimeout = 0);
+   
+      /** \brief Destructor. */
+      ~UdpConnection();
+   
+      /**
+       * \brief Open a connection to the KUKA Sunrise controller.
+       * 
+       * @param port The port ID for the connection  
+       * @param controllerAddress The IPv4 address of the KUKA Sunrise controller. 
+       *                          If NULL, the FRI Client accepts connections from any
+       *                          address. 
+       * @return True if connection was established, false otherwise
+       */
+      virtual bool open(int port, const char *controllerAddress = NULL);
+   
+      /**
+       * \brief Close a connection to the KUKA Sunrise controller.
+       */
+      virtual void close();
+      
+      /**
+       * \brief Checks whether a connection to the KUKA Sunrise controller is established.
+       * 
+       * @return True if connection is established
+       */
+      virtual bool isOpen() const;
+   
+      /**
+       * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. 
+       * 
+       * This method blocks until a new message arrives.
+       * @param buffer Pointer to the allocated buffer that will hold the FRI message
+       * @param maxSize Size in bytes of the allocated buffer
+       * @return Number of bytes received (0 when connection was terminated, 
+       *    negative in case of errors or receive timeout)
+       */
+      virtual int receive(char *buffer, int maxSize);
+   
+      /**
+       * \brief Send a new FRI command message to the KUKA Sunrise controller.
+       * 
+       * @param buffer Pointer to the buffer holding the FRI message
+       * @param size Size in bytes of the message to be send
+       * @return True if successful
+       */
+      virtual bool send(const char* buffer, int size);
+   
+   private:
+   
+      int _udpSock;                          //!< UDP socket handle
+      struct sockaddr_in _controllerAddr;    //!< the controller's socket address
+      unsigned int _receiveTimeout;
+      fd_set _filedescriptor;
+   
+   };
+   
+}
+}
+
+
+#endif // _KUKA_FRI_UDP_CONNECTION_H

src/iiwa_controller/external/libFRI/src/base/Makefile

@@ -0,0 +1,11 @@
+BASE_DIR	= ../..
+include $(BASE_DIR)/build/GNUMake/paths.mak
+include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK)
+
+CXX_SRC 		 = friClientApplication.cpp
+
+INC_DIR		+= $(NANOPB_DIR) $(PROTOBUF_DIR) $(PROTOBUF_GEN_DIR)
+CXXFLAGS 	+= 
+LDFLAGS 		+= 
+
+include $(BASE_DIR)/build/GNUMake/rules.mak

src/iiwa_controller/external/libFRI/src/base/friClientApplication.cpp

@@ -0,0 +1,211 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#include <cstdio>
+#include "friClientApplication.h"
+#include "friClientIf.h"
+#include "friConnectionIf.h"
+#include "friClientData.h"
+#include "FRIMessages.pb.h"
+#include "friTransformationClient.h"
+
+using namespace KUKA::FRI;
+
+//******************************************************************************
+ClientApplication::ClientApplication(IConnection& connection, IClient& client)
+   : _connection(connection), _robotClient(&client),_trafoClient(NULL), _data(NULL)
+{
+   _data = _robotClient->createData();
+}
+
+//******************************************************************************
+ClientApplication::ClientApplication(IConnection& connection, IClient& client, TransformationClient& trafoClient)
+   : _connection(connection), _robotClient(&client),_trafoClient(&trafoClient), _data(NULL)
+{
+   _data = _robotClient->createData();
+   _trafoClient->linkData(_data);
+}
+
+//******************************************************************************
+ClientApplication::~ClientApplication()
+{
+   disconnect();
+   delete _data;
+}
+
+//******************************************************************************
+bool ClientApplication::connect(int port, const char *remoteHost)
+{
+   if (_connection.isOpen()) 
+   {
+      printf("Warning: client application already connected!\n");
+      return true;
+   }
+   
+   return _connection.open(port, remoteHost);
+}
+   
+//******************************************************************************
+void ClientApplication::disconnect()
+{
+   if (_connection.isOpen()) _connection.close();
+}
+
+//******************************************************************************
+bool ClientApplication::step()
+{
+   if (!_connection.isOpen())
+   {
+      printf("Error: client application is not connected!\n");
+      return false;
+   }
+
+   // **************************************************************************
+   // Receive and decode new monitoring message
+   // **************************************************************************
+   int size = _connection.receive(_data->receiveBuffer, FRI_MONITOR_MSG_MAX_SIZE);
+   
+   if (size <= 0)
+   {  // TODO: size == 0 -> connection closed (maybe go to IDLE instead of stopping?)
+      printf("Error: failed while trying to receive monitoring message!\n");
+      return false;
+   }
+   
+   if (!_data->decoder.decode(_data->receiveBuffer, size))
+   {
+      return false;
+   }
+   
+   // check message type (so that our wrappers match)
+   if (_data->expectedMonitorMsgID != _data->monitoringMsg.header.messageIdentifier)
+   {
+      printf("Error: incompatible IDs for received message (got: %d expected %d)!\n",
+            (int)_data->monitoringMsg.header.messageIdentifier,
+            (int)_data->expectedMonitorMsgID);
+      return false;
+   }   
+   
+   // **************************************************************************
+   // callbacks
+   // **************************************************************************
+   // reset commmand message before callbacks
+   _data->resetCommandMessage();
+   
+   // callbacks for robot client
+   ESessionState currentState = (ESessionState)_data->monitoringMsg.connectionInfo.sessionState;
+   
+   if (_data->lastState != currentState)
+   {
+      _robotClient->onStateChange(_data->lastState, currentState);
+      _data->lastState = currentState;
+   }
+   
+   switch (currentState)
+   {
+      case MONITORING_WAIT:
+      case MONITORING_READY:
+         _robotClient->monitor();
+         break;
+      case COMMANDING_WAIT:
+         _robotClient->waitForCommand();
+         break;
+      case COMMANDING_ACTIVE:
+         _robotClient->command();
+         break;
+      case IDLE:
+      default:
+         return true; // nothing to send back
+   }
+
+   // callback for transformation client
+   if(_trafoClient != NULL)
+   {
+      _trafoClient->provide();
+   }
+   
+   // **************************************************************************
+   // Encode and send command message
+   // **************************************************************************
+   
+   _data->lastSendCounter++;
+   // check if its time to send an answer
+   if (_data->lastSendCounter >= _data->monitoringMsg.connectionInfo.receiveMultiplier)
+   {
+      _data->lastSendCounter = 0;
+      
+      // set sequence counters
+      _data->commandMsg.header.sequenceCounter = _data->sequenceCounter++;
+      _data->commandMsg.header.reflectedSequenceCounter = 
+            _data->monitoringMsg.header.sequenceCounter;
+      
+      if (!_data->encoder.encode(_data->sendBuffer, size))
+      {
+         return false;
+      }
+      
+      if (!_connection.send(_data->sendBuffer, size))
+      {
+         printf("Error: failed while trying to send command message!\n");
+         return false;
+      }
+   }
+   
+   return true;
+}
+

src/iiwa_controller/external/libFRI/src/base/friClientData.h

@@ -0,0 +1,240 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#ifndef _KUKA_FRI_CLIENT_DATA_H
+#define _KUKA_FRI_CLIENT_DATA_H
+
+#include <vector>
+
+#include "FRIMessages.pb.h"
+#include "friMonitoringMessageDecoder.h"
+#include "friCommandMessageEncoder.h"
+#include "friClientIf.h"
+#include "friException.h"
+
+namespace KUKA
+{
+namespace FRI
+{
+
+   struct ClientData
+   {
+      char receiveBuffer[FRI_MONITOR_MSG_MAX_SIZE];//!< monitoring message receive buffer
+      char sendBuffer[FRI_COMMAND_MSG_MAX_SIZE];   //!< command message send buffer
+
+      FRIMonitoringMessage monitoringMsg;          //!< monitoring message struct
+      FRICommandMessage commandMsg;                //!< command message struct
+      
+      MonitoringMessageDecoder decoder;            //!< monitoring message decoder
+      CommandMessageEncoder encoder;               //!< command message encoder
+      
+      ESessionState lastState;                     //!< last FRI state
+      uint32_t sequenceCounter;                    //!< sequence counter for command messages
+      uint32_t lastSendCounter;                    //!< steps since last send command
+      uint32_t expectedMonitorMsgID;               //!< expected ID for received monitoring messages     
+
+      const size_t MAX_REQUESTED_TRANSFORMATIONS;  //!< maximum count of requested transformations
+      const size_t MAX_SIZE_TRANSFORMATION_ID;     //!< maximum size in bytes of a transformation ID
+      std::vector<const char*> requestedTrafoIDs;  //!< list of requested transformation ids
+      
+      ClientData(int numDofs) 
+      : decoder(&monitoringMsg, numDofs), 
+        encoder(&commandMsg, numDofs),
+        lastState(IDLE),
+        sequenceCounter(0),
+        lastSendCounter(0),
+        expectedMonitorMsgID(0),
+        MAX_REQUESTED_TRANSFORMATIONS(sizeof(monitoringMsg.requestedTransformations) /
+                                      sizeof(monitoringMsg.requestedTransformations[0])),
+        MAX_SIZE_TRANSFORMATION_ID(sizeof(monitoringMsg.requestedTransformations[0].name))
+      {
+         requestedTrafoIDs.reserve(MAX_REQUESTED_TRANSFORMATIONS);
+      }
+      
+      void resetCommandMessage()
+      {
+         commandMsg.commandData.has_jointPosition = false;
+         commandMsg.commandData.has_cartesianWrenchFeedForward = false;
+         commandMsg.commandData.has_jointTorque = false;
+         commandMsg.commandData.commandedTransformations_count = 0;
+         commandMsg.has_commandData = false;     
+         commandMsg.commandData.writeIORequest_count = 0;
+      }
+      
+      //******************************************************************************
+      static const FriIOValue& getBooleanIOValue(const FRIMonitoringMessage* message, const char* name)
+      {
+         return getIOValue(message, name, FriIOType_BOOLEAN);
+      }
+
+      //******************************************************************************
+      static const FriIOValue& getDigitalIOValue(const FRIMonitoringMessage* message, const char* name)
+      {
+         return getIOValue(message, name, FriIOType_DIGITAL);
+      }
+
+      //******************************************************************************
+      static const FriIOValue& getAnalogIOValue(const FRIMonitoringMessage* message, const char* name)
+      {
+         return getIOValue(message, name, FriIOType_ANALOG);
+      }
+
+      //******************************************************************************
+      static void setBooleanIOValue(FRICommandMessage* message, const char* name, const bool value, 
+            const FRIMonitoringMessage* monMessage)
+      {
+         setIOValue(message, name, monMessage, FriIOType_BOOLEAN).digitalValue = value;
+      }
+
+      //******************************************************************************
+      static void setDigitalIOValue(FRICommandMessage* message, const char* name, const unsigned long long value, 
+            const FRIMonitoringMessage* monMessage)
+      {
+         setIOValue(message, name, monMessage, FriIOType_DIGITAL).digitalValue = value;
+      }
+
+      //******************************************************************************
+      static void setAnalogIOValue(FRICommandMessage* message, const char* name, const double value, 
+            const FRIMonitoringMessage* monMessage)
+      {
+         setIOValue(message, name, monMessage, FriIOType_ANALOG).analogValue = value;
+      }
+   
+   protected:
+      
+      //******************************************************************************
+      static const FriIOValue& getIOValue(const FRIMonitoringMessage* message, const char* name, 
+            const FriIOType ioType)
+      {
+         if(message != NULL && message->has_monitorData == true)
+         {
+            const MessageMonitorData& monData = message->monitorData;
+            const bool analogValue = (ioType == FriIOType_ANALOG); 
+            const bool digitalValue = (ioType == FriIOType_DIGITAL | ioType == FriIOType_BOOLEAN); 
+            for(size_t i = 0; i < monData.readIORequest_count; i++)
+            {
+               const FriIOValue& ioValue = monData.readIORequest[i];
+               if(strcmp(name, ioValue.name) == 0)
+               {
+                  if(ioValue.type == ioType &&
+                     ioValue.has_digitalValue == digitalValue && 
+                     ioValue.has_analogValue == analogValue)
+                  {
+                     return ioValue;
+                  }
+                  
+                  const char* ioTypeName;
+                  switch(ioType)
+                  {
+                     case FriIOType_ANALOG: ioTypeName = "analog value"; break;
+                     case FriIOType_DIGITAL: ioTypeName = "digital value"; break;
+                     case FriIOType_BOOLEAN: ioTypeName = "boolean"; break;
+                     default: ioTypeName = "?"; break;
+                  }
+                  
+                  throw FRIException("IO %s is not of type %s.", name, ioTypeName);
+               }
+            }
+         }
+
+         throw FRIException("Could not locate IO %s in monitor message.", name);
+      }
+      
+      //******************************************************************************
+      static FriIOValue& setIOValue(FRICommandMessage* message, const char* name,  
+            const FRIMonitoringMessage* monMessage, const FriIOType ioType)
+      {
+         MessageCommandData& cmdData = message->commandData;
+         const size_t maxIOs = sizeof(cmdData.writeIORequest) / sizeof(cmdData.writeIORequest[0]);
+         if(cmdData.writeIORequest_count < maxIOs)
+         {
+            // call getter which will raise an exception if the output doesn't exist
+            // or is of wrong type.
+            if(getIOValue(monMessage, name, ioType).direction != FriIODirection_OUTPUT)
+            {
+               throw FRIException("IO %s is not an output value.", name);
+            }
+            
+            // add IO value to command message
+            FriIOValue& ioValue = cmdData.writeIORequest[cmdData.writeIORequest_count];
+            
+            strncpy(ioValue.name, name, sizeof(ioValue.name) - 1);
+            ioValue.name[sizeof(ioValue.name) - 1] = 0; // ensure termination
+            ioValue.type = ioType;
+            ioValue.has_digitalValue = (ioType == FriIOType_DIGITAL | ioType == FriIOType_BOOLEAN);
+            ioValue.has_analogValue = (ioType == FriIOType_ANALOG);
+            ioValue.direction = FriIODirection_OUTPUT;
+            
+            cmdData.writeIORequest_count ++;
+            message->has_commandData = true;
+            
+            return ioValue;
+         }
+         else
+         {
+            throw FRIException("Exceeded maximum number of IOs that can be set.");
+         }
+      }      
+   };
+   
+}
+}
+
+
+#endif // _KUKA_FRI_CLIENT_DATA_H

src/iiwa_controller/external/libFRI/src/client_lbr/Makefile

@@ -0,0 +1,13 @@
+BASE_DIR	= ../..
+include $(BASE_DIR)/build/GNUMake/paths.mak
+include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK)
+
+CXX_SRC 		 = friLBRClient.cpp \
+			 		friLBRCommand.cpp \
+			 		friLBRState.cpp
+
+INC_DIR		+= $(CLIENTBASE_DIR) $(NANOPB_DIR) $(PROTOBUF_DIR) $(PROTOBUF_GEN_DIR)
+CXXFLAGS 	+= 
+LDFLAGS 		+= 
+
+include $(BASE_DIR)/build/GNUMake/rules.mak

src/iiwa_controller/external/libFRI/src/client_lbr/friLBRClient.cpp

@@ -0,0 +1,120 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#include <cstdio>
+#include "friLBRClient.h"
+#include "friClientData.h"
+
+using namespace KUKA::FRI;
+char FRIException::_buffer[1024] = { 0 };
+
+//******************************************************************************
+LBRClient::LBRClient()
+{
+   
+}
+
+//******************************************************************************
+LBRClient::~LBRClient()
+{
+   
+}
+
+//******************************************************************************
+void LBRClient::onStateChange(ESessionState oldState, ESessionState newState)
+{
+   // TODO: String converter function for states
+   printf("LBRiiwaClient state changed from %d to %d\n", oldState, newState);
+}
+
+//******************************************************************************
+void LBRClient::monitor()
+{
+   robotCommand().setJointPosition(robotState().getCommandedJointPosition());
+}
+
+//******************************************************************************
+void LBRClient::waitForCommand()
+{
+   robotCommand().setJointPosition(robotState().getIpoJointPosition());
+}
+
+//******************************************************************************
+void LBRClient::command()
+{
+   robotCommand().setJointPosition(robotState().getIpoJointPosition());
+}
+
+//******************************************************************************
+ClientData* LBRClient::createData()
+{
+   ClientData* data = new ClientData(_robotState.NUMBER_OF_JOINTS);
+   
+   // link monitoring and command message to wrappers
+   _robotState._message = &data->monitoringMsg;
+   _robotCommand._cmdMessage = &data->commandMsg;
+   _robotCommand._monMessage = &data->monitoringMsg;
+
+   // set specific message IDs
+   data->expectedMonitorMsgID = _robotState.LBRMONITORMESSAGEID;
+   data->commandMsg.header.messageIdentifier = _robotCommand.LBRCOMMANDMESSAGEID;
+   
+   return data;
+}
+

src/iiwa_controller/external/libFRI/src/client_lbr/friLBRCommand.cpp

@@ -0,0 +1,113 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#include "friLBRState.h"
+#include "friLBRCommand.h"
+#include "friClientData.h"
+#include "pb_frimessages_callbacks.h"
+
+using namespace KUKA::FRI;
+
+//******************************************************************************
+void LBRCommand::setJointPosition(const double* values)
+{
+   _cmdMessage->has_commandData = true;
+   _cmdMessage->commandData.has_jointPosition = true;
+   tRepeatedDoubleArguments *dest =
+            (tRepeatedDoubleArguments*)_cmdMessage->commandData.jointPosition.value.arg;
+   memcpy(dest->value, values, LBRState::NUMBER_OF_JOINTS * sizeof(double));
+}
+
+//******************************************************************************
+void LBRCommand::setWrench(const double* wrench)
+{
+   _cmdMessage->has_commandData = true;
+   _cmdMessage->commandData.has_cartesianWrenchFeedForward = true;
+
+   double *dest = _cmdMessage->commandData.cartesianWrenchFeedForward.element;
+   memcpy(dest, wrench, 6 * sizeof(double));
+}
+//******************************************************************************
+void LBRCommand::setTorque(const double* torques)
+{
+   _cmdMessage->has_commandData = true;
+   _cmdMessage->commandData.has_jointTorque= true;
+
+   tRepeatedDoubleArguments *dest =
+              (tRepeatedDoubleArguments*)_cmdMessage->commandData.jointTorque.value.arg;
+   memcpy(dest->value, torques, LBRState::NUMBER_OF_JOINTS * sizeof(double));
+}
+
+//******************************************************************************
+void LBRCommand::setBooleanIOValue(const char* name, const bool value)
+{
+   ClientData::setBooleanIOValue(_cmdMessage, name, value, _monMessage);
+}
+
+//******************************************************************************
+void LBRCommand::setAnalogIOValue(const char* name, const double value)
+{
+   ClientData::setAnalogIOValue(_cmdMessage, name, value, _monMessage);
+}
+
+//******************************************************************************
+void LBRCommand::setDigitalIOValue(const char* name, const unsigned long long value)
+{
+   ClientData::setDigitalIOValue(_cmdMessage, name, value, _monMessage);
+}

src/iiwa_controller/external/libFRI/src/client_lbr/friLBRState.cpp

@@ -0,0 +1,234 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#include "friLBRState.h"
+#include "friClientData.h"
+#include "pb_frimessages_callbacks.h"
+
+using namespace KUKA::FRI;
+
+
+LBRState::LBRState():_message(0)
+{
+    
+}
+//******************************************************************************
+double LBRState::getSampleTime() const
+{
+   return _message->connectionInfo.sendPeriod * 0.001;
+}
+
+//******************************************************************************
+ESessionState LBRState::getSessionState() const
+{
+   return (ESessionState)_message->connectionInfo.sessionState;
+}
+
+//******************************************************************************
+EConnectionQuality LBRState::getConnectionQuality() const
+{
+   return (EConnectionQuality)_message->connectionInfo.quality;
+}
+
+//******************************************************************************
+ESafetyState LBRState::getSafetyState() const
+{
+   return (ESafetyState)_message->robotInfo.safetyState;
+}
+
+//******************************************************************************
+EOperationMode LBRState::getOperationMode() const
+{
+   return (EOperationMode)_message->robotInfo.operationMode;
+}
+
+//******************************************************************************
+EDriveState LBRState::getDriveState() const
+{
+   tRepeatedIntArguments *values =
+         (tRepeatedIntArguments *)_message->robotInfo.driveState.arg;
+   int firstState = (int)values->value[0];
+   for (int i=1; i<NUMBER_OF_JOINTS; i++)
+   {
+      int state = (int)values->value[i];
+      if (state != firstState)
+      {
+         return TRANSITIONING;
+      }
+   }
+   return (EDriveState)firstState;
+}
+
+
+//********************************************************************************
+EOverlayType LBRState::getOverlayType() const
+{
+   return (EOverlayType)_message->ipoData.overlayType;
+}
+
+//********************************************************************************
+EClientCommandMode LBRState::getClientCommandMode() const
+{
+   return (EClientCommandMode)_message->ipoData.clientCommandMode;
+}
+
+
+//******************************************************************************
+EControlMode LBRState::getControlMode() const
+{
+   return (EControlMode)_message->robotInfo.controlMode;
+}
+
+//******************************************************************************
+unsigned int LBRState::getTimestampSec() const
+{
+   return _message->monitorData.timestamp.sec;
+}
+
+//******************************************************************************
+unsigned int LBRState::getTimestampNanoSec() const
+{
+   return _message->monitorData.timestamp.nanosec;
+}
+
+//******************************************************************************
+const double* LBRState::getMeasuredJointPosition() const
+{
+   tRepeatedDoubleArguments *values =
+         (tRepeatedDoubleArguments*)_message->monitorData.measuredJointPosition.value.arg;
+   return (double*)values->value;
+}
+
+//******************************************************************************
+const double* LBRState::getCommandedJointPosition() const
+{
+   tRepeatedDoubleArguments *values =
+            (tRepeatedDoubleArguments*)_message->monitorData.commandedJointPosition.value.arg;
+   return (double*)values->value;
+}
+
+//******************************************************************************
+const double* LBRState::getMeasuredTorque() const
+{
+   tRepeatedDoubleArguments *values =
+            (tRepeatedDoubleArguments*)_message->monitorData.measuredTorque.value.arg;
+   return (double*)values->value;
+}
+
+//******************************************************************************
+const double* LBRState::getCommandedTorque() const
+{
+   tRepeatedDoubleArguments *values =
+            (tRepeatedDoubleArguments*)_message->monitorData.commandedTorque.value.arg;
+   return (double*)values->value;
+}
+
+//******************************************************************************
+const double* LBRState::getExternalTorque() const
+{
+   tRepeatedDoubleArguments *values =
+            (tRepeatedDoubleArguments*)_message->monitorData.externalTorque.value.arg;
+   return (double*)values->value;
+}
+
+//******************************************************************************
+const double* LBRState::getIpoJointPosition() const
+{
+   if (!_message->ipoData.has_jointPosition)
+   {
+      throw FRIException("IPO joint position not available in monitoring mode.");
+      return NULL;
+   }
+   
+   tRepeatedDoubleArguments *values =
+            (tRepeatedDoubleArguments*)_message->ipoData.jointPosition.value.arg;
+   return (double*)values->value;
+}
+
+//******************************************************************************
+double LBRState::getTrackingPerformance() const
+{
+   if (!_message->ipoData.has_trackingPerformance) return 0.0;
+   
+   return _message->ipoData.trackingPerformance;
+}
+
+//******************************************************************************
+bool LBRState::getBooleanIOValue(const char* name) const
+{
+   return ClientData::getBooleanIOValue(_message, name).digitalValue != 0;
+}
+
+//******************************************************************************
+unsigned long long LBRState::getDigitalIOValue(const char* name) const
+{
+   return ClientData::getDigitalIOValue(_message, name).digitalValue;
+}
+
+//******************************************************************************
+double LBRState::getAnalogIOValue(const char* name) const
+{
+   return ClientData::getAnalogIOValue(_message, name).analogValue;
+}
+
+//******************************************************************************
+/*const std::vector<const char*>& LBRState::getRequestedIO_IDs() const
+{
+   return _clientData->getRequestedIO_IDs();
+}*/

src/iiwa_controller/external/libFRI/src/client_trafo/Makefile

@@ -0,0 +1,11 @@
+BASE_DIR	= ../..
+include $(BASE_DIR)/build/GNUMake/paths.mak
+include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK)
+
+CXX_SRC 		 = friTransformationClient.cpp
+
+INC_DIR		+= $(CLIENTBASE_DIR) $(NANOPB_DIR) $(PROTOBUF_DIR) $(PROTOBUF_GEN_DIR)
+CXXFLAGS 	+= 
+LDFLAGS 		+= 
+
+include $(BASE_DIR)/build/GNUMake/rules.mak

src/iiwa_controller/external/libFRI/src/client_trafo/friTransformationClient.cpp

@@ -0,0 +1,191 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+
+#include <cstring>
+#include <cstdio>
+
+#include "friTransformationClient.h"
+#include "friClientData.h"
+
+#include "FRIMessages.pb.h"
+#include "pb_frimessages_callbacks.h"
+
+using namespace KUKA::FRI;
+
+//******************************************************************************
+TransformationClient::TransformationClient()
+{
+}
+      
+//******************************************************************************
+TransformationClient::~TransformationClient()
+{
+}
+
+//******************************************************************************
+const std::vector<const char*>& TransformationClient::getRequestedTransformationIDs() const
+{
+   unsigned int trafoCount = _data->monitoringMsg.requestedTransformations_count;
+   _data->requestedTrafoIDs.resize(trafoCount);
+   for (unsigned int i=0; i<trafoCount; i++)
+   {
+      _data->requestedTrafoIDs[i] = _data->monitoringMsg.requestedTransformations[i].name;
+   }   
+	return _data->requestedTrafoIDs;
+}
+
+//******************************************************************************
+const unsigned int TransformationClient::getTimestampSec() const 
+{
+	return _data->monitoringMsg.monitorData.timestamp.sec;
+}
+
+//******************************************************************************
+const unsigned int TransformationClient::getTimestampNanoSec() const
+{
+	return _data->monitoringMsg.monitorData.timestamp.nanosec;
+}
+
+//******************************************************************************
+void TransformationClient::setTransformation(const char* transformationID, 
+      const double transformationMatrix[3][4], unsigned int timeSec, unsigned int timeNanoSec)
+{
+	_data->commandMsg.has_commandData = true;
+	
+	unsigned int currentSize = _data->commandMsg.commandData.commandedTransformations_count;
+	
+   if (currentSize < _data->MAX_REQUESTED_TRANSFORMATIONS)
+	{
+	   _data->commandMsg.commandData.commandedTransformations_count++;
+	   Transformation& dest = _data->commandMsg.commandData.commandedTransformations[currentSize];
+      strncpy(dest.name, transformationID, _data->MAX_SIZE_TRANSFORMATION_ID);
+      dest.name[_data->MAX_SIZE_TRANSFORMATION_ID - 1] = '\0';
+      dest.matrix_count = 12;
+	   memcpy(dest.matrix, transformationMatrix, 12 * sizeof(double));
+	   dest.has_timestamp = true;
+	   dest.timestamp.sec = timeSec;
+	   dest.timestamp.nanosec = timeNanoSec;
+	}
+   else
+   {
+      throw FRIException("Exceeded maximum number of transformations.");
+   }
+}
+
+//******************************************************************************
+void TransformationClient::linkData(ClientData* clientData)
+{
+	_data = clientData;
+}
+
+//******************************************************************************
+double TransformationClient::getSampleTime() const
+{
+   return _data->monitoringMsg.connectionInfo.sendPeriod * 0.001;
+}
+
+//******************************************************************************
+EConnectionQuality TransformationClient::getConnectionQuality() const
+{
+   return (EConnectionQuality)_data->monitoringMsg.connectionInfo.quality;
+}
+
+
+//******************************************************************************
+void TransformationClient::setBooleanIOValue(const char* name, const bool value)
+{
+   ClientData::setBooleanIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg);
+}
+
+//******************************************************************************
+void TransformationClient::setAnalogIOValue(const char* name, const double value)
+{
+   ClientData::setAnalogIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg);
+}
+
+//******************************************************************************
+void TransformationClient::setDigitalIOValue(const char* name, const unsigned long long value)
+{
+   ClientData::setDigitalIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg);
+}
+
+//******************************************************************************
+bool TransformationClient::getBooleanIOValue(const char* name) const
+{
+   return ClientData::getBooleanIOValue(&_data->monitoringMsg, name).digitalValue != 0;
+}
+
+//******************************************************************************
+unsigned long long TransformationClient::getDigitalIOValue(const char* name) const
+{
+   return ClientData::getDigitalIOValue(&_data->monitoringMsg, name).digitalValue;
+}
+
+//******************************************************************************
+double TransformationClient::getAnalogIOValue(const char* name) const
+{
+   return ClientData::getAnalogIOValue(&_data->monitoringMsg, name).analogValue;
+}
+
+//******************************************************************************
+/*const std::vector<const char*>& TransformationClient::getRequestedIO_IDs() const
+{
+   return _data->getRequestedIO_IDs();
+}*/

src/iiwa_controller/external/libFRI/src/connection/Makefile

@@ -0,0 +1,14 @@
+BASE_DIR	= ../..
+include $(BASE_DIR)/build/GNUMake/paths.mak
+include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK)
+
+CXX_SRC 		 = friUdpConnection.cpp
+
+CXXFLAGS 	+= 
+LDFLAGS 		+= 
+
+################################################################################
+### Include general makefile (at the end)
+################################################################################
+
+include $(BASE_DIR)/build/GNUMake/rules.mak

src/iiwa_controller/external/libFRI/src/connection/friUdpConnection.cpp

@@ -0,0 +1,243 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software �KUKA Sunrise.Connectivity FRI Client SDK� is targeted to work in
+conjunction with the �KUKA Sunrise.Connectivity FastRobotInterface� toolkit.
+In the following, the term �software� refers to all material directly
+belonging to the provided SDK �Software development kit�, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#include <cstring>
+#include <cstdio>
+#ifndef _MSC_VER
+#include <unistd.h>
+#endif
+
+#include "friUdpConnection.h"
+
+
+#ifdef WIN32
+#include <winsock2.h>
+#include <Ws2tcpip.h>
+#ifdef _MSC_VER
+#pragma comment(lib, "ws2_32.lib")
+#endif
+#endif
+
+using namespace KUKA::FRI;
+
+//******************************************************************************
+UdpConnection::UdpConnection(unsigned int receiveTimeout) : 
+      _udpSock(-1),
+      _receiveTimeout(receiveTimeout)
+{
+#ifdef WIN32
+   WSADATA WSAData;
+   WSAStartup(MAKEWORD(2,0), &WSAData);
+#endif
+}
+   
+//******************************************************************************
+UdpConnection::~UdpConnection()
+{
+   close();
+#ifdef WIN32
+   WSACleanup();
+#endif
+}
+   
+//******************************************************************************
+bool UdpConnection::open(int port, const char *controllerAddress)
+{
+   struct sockaddr_in servAddr;
+   memset(&servAddr, 0, sizeof(servAddr));
+   memset(&_controllerAddr, 0, sizeof(_controllerAddr));
+
+   // socket creation
+   _udpSock = socket(PF_INET, SOCK_DGRAM, 0);
+   if (_udpSock < 0)
+   {
+      printf("opening socket failed!\n");
+      return false;
+   }
+
+   // use local server port
+   servAddr.sin_family = AF_INET;
+   servAddr.sin_port = htons(port);
+   servAddr.sin_addr.s_addr = htonl(INADDR_ANY);
+   
+   if (bind(_udpSock, (struct sockaddr *)&servAddr, sizeof(servAddr)) < 0)
+   {
+      printf("binding port number %d failed!\n", port);
+      close();
+      return false;
+   }
+   // initialize the socket properly
+   _controllerAddr.sin_family = AF_INET;
+   _controllerAddr.sin_port = htons(port);
+   if (controllerAddress)
+   {
+#ifndef __MINGW32__
+      inet_pton(AF_INET, controllerAddress, &_controllerAddr.sin_addr);
+#else
+      _controllerAddr.sin_addr.s_addr = inet_addr(controllerAddress);
+#endif
+      if ( connect(_udpSock, (struct sockaddr *)&_controllerAddr, sizeof(_controllerAddr)) < 0)
+      {
+         printf("connecting to controller with address %s failed !\n", controllerAddress);
+         close();
+         return false;
+      }
+   }
+   else
+   {
+       _controllerAddr.sin_addr.s_addr = htonl(INADDR_ANY);
+   }
+   return true;
+}
+   
+//******************************************************************************
+void UdpConnection::close()
+{
+   if (isOpen())
+   {
+#ifdef WIN32
+      closesocket(_udpSock);
+#else
+      ::close(_udpSock);
+#endif
+   }
+   _udpSock = -1;
+}
+
+//******************************************************************************
+bool UdpConnection::isOpen() const
+{
+   return (_udpSock >= 0);
+}
+   
+//******************************************************************************
+int UdpConnection::receive(char *buffer, int maxSize)
+{
+   if (isOpen())
+   {
+      /** HAVE_SOCKLEN_T
+       Yes - unbelievable: There are differences in standard calling parameters (types) to recvfrom 
+       Windows winsock, VxWorks and QNX use int
+       newer Posix (most Linuxes) use socklen_t
+       */
+#ifdef HAVE_SOCKLEN_T
+      socklen_t sockAddrSize;
+#else
+      int sockAddrSize;
+#endif
+      sockAddrSize = static_cast<socklen_t>(sizeof(struct sockaddr_in));
+      /** check for timeout
+       Because SO_RCVTIMEO is in Windows not correctly implemented, select is used for the receive time out.
+       If a timeout greater than 0 is given, wait until the timeout is reached or a message was received.
+       If t, abort the function with an error. 
+       */
+      if(_receiveTimeout > 0)
+      {
+          
+          // Set up struct timeval
+          struct timeval tv;
+          tv.tv_sec = _receiveTimeout / 1000;
+          tv.tv_usec = (_receiveTimeout % 1000) * 1000;
+
+          // initialize file descriptor
+          /**
+          * Replace FD_ZERO with memset, because bzero is not available for VxWorks 
+          * User Space Aplications(RTPs). Therefore the macro FD_ZERO does not compile.
+          */
+#ifndef VXWORKS
+          FD_ZERO(&_filedescriptor);
+#else
+          memset((char *)(&_filedescriptor), 0, sizeof(*(&_filedescriptor)));
+#endif
+          FD_SET(_udpSock, &_filedescriptor);
+          
+          // wait until something was received
+          int numberActiveFileDescr = select(_udpSock+1, &_filedescriptor,NULL,NULL,&tv);
+          // 0 indicates a timeout
+          if(numberActiveFileDescr == 0)
+          {
+              printf("The connection has timed out. Timeout is %d\n", _receiveTimeout);
+              return -1;
+          }
+          // a negative value indicates an error
+          else if(numberActiveFileDescr == -1)
+          {
+              printf("An error has occured \n");
+              return -1;
+          }
+      }
+
+      return recvfrom(_udpSock, buffer, maxSize, 0, (struct sockaddr *)&_controllerAddr, &sockAddrSize);
+   }
+   return -1;
+}
+   
+//******************************************************************************
+bool UdpConnection::send(const char* buffer, int size)
+{
+   if ((isOpen()) && (ntohs(_controllerAddr.sin_port) != 0))
+   {
+      int sent = sendto(_udpSock, const_cast<char*>(buffer), size, 0, (struct sockaddr *)&_controllerAddr, sizeof(_controllerAddr));
+      if (sent == size)
+      {
+         return true;
+      }
+   }
+   return false;
+}

src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/CHANGELOG.txt

@@ -0,0 +1,150 @@
+nanopb-0.2.8 (2014-05-20)
+ Fix security issue with PB_ENABLE_MALLOC. (issue 117)
+ Add option to not add timestamps to .pb.h and .pb.c preambles. (issue 115)
+ Documentation updates
+ Improved tests
+
+nanopb-0.2.7 (2014-04-07)
+ Fix bug with default values for extension fields (issue 111)
+ Fix some MISRA-C warnings (issue 91)
+ Implemented optional malloc() support (issue 80)
+ Changed pointer-type bytes field datatype
+ Add a "found" field to pb_extension_t (issue 112)
+ Add convenience function pb_get_encoded_size() (issue 16)
+
+nanopb-0.2.6 (2014-02-15)
+ Fix generator error with bytes callback fields (issue 99)
+ Fix warnings about large integer constants (issue 102)
+ Add comments to where STATIC_ASSERT is used (issue 96)
+ Add warning about unknown field names on .options (issue 105)
+ Move descriptor.proto to google/protobuf subdirectory (issue 104)
+ Improved tests
+
+nanopb-0.2.5 (2014-01-01)
+ Fix a bug with encoding negative values in int32 fields (issue 97)
+ Create binary packages of the generator + dependencies (issue 47)
+ Add support for pointer-type fields to the encoder (part of issue 80)
+ Fixed path in FindNanopb.cmake (issue 94)
+ Improved tests
+
+nanopb-0.2.4 (2013-11-07)
+ Remove the deprecated NANOPB_INTERNALS functions from public API.
+ Document the security model.
+ Check array and bytes max sizes when encoding (issue 90)
+ Add #defines for maximum encoded message size (issue 89)
+ Add #define tags for extension fields (issue 93)
+ Fix MISRA C violations (issue 91)
+ Clean up pb_field_t definition with typedefs.
+
+nanopb-0.2.3 (2013-09-18)
+ Improve compatibility by removing ternary operator from initializations (issue 88)
+ Fix build error on Visual C++ (issue 84, patch by Markus Schwarzenberg)
+ Don't stop on unsupported extension fields (issue 83)
+ Add an example pb_syshdr.h file for non-C99 compilers
+ Reorganize tests and examples into subfolders (issue 63)
+ Switch from Makefiles to scons for building the tests
+ Make the tests buildable on Windows
+
+nanopb-0.2.2 (2013-08-18)
+ Add support for extension fields (issue 17)
+ Fix unknown fields in empty message (issue 78)
+ Include the field tags in the generated .pb.h file.
+ Add pb_decode_delimited and pb_encode_delimited wrapper functions (issue 74)
+ Add a section in top of pb.h for changing compilation settings (issue 76)
+ Documentation improvements (issues 12, 77 and others)
+ Improved tests
+
+nanopb-0.2.1 (2013-04-14)
+ NOTE: The default callback function signature has changed.
+       If you don't want to update your code, define PB_OLD_CALLBACK_STYLE.
+ 
+ Change the callback function to use void** (issue 69)
+ Add support for defining the nanopb options in a separate file (issue 12)
+ Add support for packed structs in IAR and MSVC (in addition to GCC) (issue 66)
+ Implement error message support for the encoder side (issue 7)
+ Handle unterminated strings when encoding (issue 68)
+ Fix bug with empty strings in repeated string callbacks (issue 73)
+ Fix regression in 0.2.0 with optional callback fields (issue 70)
+ Fix bugs with empty message types (issues 64, 65)
+ Fix some compiler warnings on clang (issue 67)
+ Some portability improvements (issues 60, 62)
+ Various new generator options
+ Improved tests
+
+nanopb-0.2.0 (2013-03-02)
+ NOTE: This release requires you to regenerate all .pb.c
+       files. Files generated by older versions will not
+       compile anymore.
+
+ Reformat generated .pb.c files using macros (issue 58)
+ Rename PB_HTYPE_ARRAY -> PB_HTYPE_REPEATED
+ Separate PB_HTYPE to PB_ATYPE and PB_HTYPE
+ Move STATIC_ASSERTs to .pb.c file
+ Added CMake file (by Pavel Ilin)
+ Add option to give file extension to generator (by Michael Haberler)
+ Documentation updates
+
+nanopb-0.1.9 (2013-02-13)
+ Fixed error message bugs (issues 52, 56)
+ Sanitize #ifndef filename (issue 50)
+ Performance improvements
+ Add compile-time option PB_BUFFER_ONLY
+ Add Java package name to nanopb.proto
+ Check for sizeof(double) == 8 (issue 54)
+ Added generator option to ignore some fields. (issue 51)
+ Added generator option to make message structs packed. (issue 49)
+ Add more test cases.
+
+nanopb-0.1.8 (2012-12-13)
+ Fix bugs in the enum short names introduced in 0.1.7 (issues 42, 43)
+ Fix STATIC_ASSERT macro when using multiple .proto files. (issue 41)
+ Fix missing initialization of istream.errmsg
+ Make tests/Makefile work for non-gcc compilers (issue 40)
+
+nanopb-0.1.7 (2012-11-11)
+ Remove "skip" mode from pb_istream_t callbacks. Example implementation had a bug. (issue 37)
+ Add option to use shorter names for enum values (issue 38)
+ Improve options support in generator (issues 12, 30)
+ Add nanopb version number to generated files (issue 36)
+ Add extern "C" to generated headers (issue 35)
+ Add names for structs to allow forward declaration (issue 39)
+ Add buffer size check in example (issue 34)
+ Fix build warnings on MS compilers (issue 33)
+
+nanopb-0.1.6 (2012-09-02)
+ Reorganize the field decoder interface (issue 2)
+ Improve performance in submessage decoding (issue 28)
+ Implement error messages in the decoder side (issue 7)
+ Extended testcases (alltypes test is now complete).
+ Fix some compiler warnings (issues 25, 26, 27, 32).
+
+nanopb-0.1.5 (2012-08-04)
+ Fix bug in decoder with packed arrays (issue 23).
+ Extended testcases.
+ Fix some compiler warnings.
+
+nanopb-0.1.4 (2012-07-05)
+ Add compile-time options for easy-to-use >255 field support.
+ Improve the detection of missing required fields.
+ Added example on how to handle union messages.
+ Fix generator error with .proto without messages.
+ Fix problems that stopped the code from compiling with some compilers.
+ Fix some compiler warnings.
+
+nanopb-0.1.3 (2012-06-12)
+ Refactor the field encoder interface.
+ Improve generator error messages (issue 5)
+ Add descriptor.proto into the #include exclusion list
+ Fix some compiler warnings.
+
+nanopb-0.1.2 (2012-02-15)
+ Make the generator to generate include for other .proto files (issue 4).
+ Fixed generator not working on Windows (issue 3)
+
+nanopb-0.1.1 (2012-01-14)
+ Fixed bug in encoder with 'bytes' fields (issue 1).
+ Fixed a bug in the generator that caused a compiler error on sfixed32 and sfixed64 fields.
+ Extended testcases.
+
+nanopb-0.1.0 (2012-01-06)
+ First stable release.

src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/LICENSE.txt

@@ -0,0 +1,20 @@
+Copyright (c) 2011 Petteri Aimonen <jpa at nanopb.mail.kapsi.fi>
+
+This software is provided 'as-is', without any express or 
+implied warranty. In no event will the authors be held liable 
+for any damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any 
+purpose, including commercial applications, and to alter it and 
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you 
+   must not claim that you wrote the original software. If you use 
+   this software in a product, an acknowledgment in the product 
+   documentation would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and 
+   must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source 
+   distribution.

src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/Makefile

@@ -0,0 +1,15 @@
+BASE_DIR	= ../..
+include $(BASE_DIR)/build/GNUMake/paths.mak
+include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK)
+
+CC_SRC 		 = pb_encode.c \
+				   pb_decode.c
+
+CFLAGS 		+=
+LDFLAGS 		+= 
+
+################################################################################
+### Include general makefile (at the end)
+################################################################################
+
+include $(BASE_DIR)/build/GNUMake/rules.mak

src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/README.txt

@@ -0,0 +1,61 @@
+Nanopb is a small code-size Protocol Buffers implementation in ansi C. It is
+especially suitable for use in microcontrollers, but fits any memory
+restricted system.
+
+Homepage: http://kapsi.fi/~jpa/nanopb/
+
+
+
+
+Using the nanopb library
+========================
+To use the nanopb library, you need to do two things:
+
+1) Compile your .proto files for nanopb, using protoc.
+2) Include pb_encode.c and pb_decode.c in your project.
+
+The easiest way to get started is to study the project in "examples/simple".
+It contains a Makefile, which should work directly under most Linux systems.
+However, for any other kind of build system, see the manual steps in
+README.txt in that folder.
+
+
+
+Using the Protocol Buffers compiler (protoc)
+============================================
+The nanopb generator is implemented as a plugin for the Google's own protoc
+compiler. This has the advantage that there is no need to reimplement the
+basic parsing of .proto files. However, it does mean that you need the
+Google's protobuf library in order to run the generator.
+
+If you have downloaded a binary package for nanopb (either Windows, Linux or
+Mac OS X version), the 'protoc' binary is included in the 'generator-bin'
+folder. In this case, you are ready to go. Simply run this command:
+
+    generator-bin/protoc --nanopb_out=. myprotocol.proto
+
+However, if you are using a git checkout or a plain source distribution, you
+need to provide your own version of protoc and the Google's protobuf library.
+On Linux, the necessary packages are protobuf-compiler and python-protobuf.
+On Windows, you can either build Google's protobuf library from source or use
+one of the binary distributions of it. In either case, if you use a separate
+protoc, you need to manually give the path to nanopb generator:
+
+    protoc --plugin=protoc-gen-nanopb=nanopb/generator/protoc-gen-nanopb ...
+
+
+
+Running the tests
+=================
+If you want to perform further development of the nanopb core, or to verify
+its functionality using your compiler and platform, you'll want to run the
+test suite. The build rules for the test suite are implemented using Scons,
+so you need to have that installed. To run the tests:
+
+    cd tests
+    scons
+
+This will show the progress of various test cases. If the output does not
+end in an error, the test cases were successful.
+
+

src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb.h

@@ -0,0 +1,519 @@
+/* Common parts of the nanopb library. Most of these are quite low-level
+ * stuff. For the high-level interface, see pb_encode.h and pb_decode.h.
+ */
+
+#ifndef _PB_H_
+#define _PB_H_
+
+/*****************************************************************
+ * Nanopb compilation time options. You can change these here by *
+ * uncommenting the lines, or on the compiler command line.      *
+ *****************************************************************/
+
+/* Enable support for dynamically allocated fields */
+/* #define PB_ENABLE_MALLOC 1 */
+
+/* Define this if your CPU architecture is big endian, i.e. it
+ * stores the most-significant byte first. */
+/* #define __BIG_ENDIAN__ 1 */
+
+/* Increase the number of required fields that are tracked.
+ * A compiler warning will tell if you need this. */
+/* #define PB_MAX_REQUIRED_FIELDS 256 */
+
+/* Add support for tag numbers > 255 and fields larger than 255 bytes. */
+/* #define PB_FIELD_16BIT 1 */
+
+/* Add support for tag numbers > 65536 and fields larger than 65536 bytes. */
+/* #define PB_FIELD_32BIT 1 */
+
+/* Disable support for error messages in order to save some code space. */
+/* #define PB_NO_ERRMSG 1 */
+
+/* Disable support for custom streams (support only memory buffers). */
+/* #define PB_BUFFER_ONLY 1 */
+
+/* Switch back to the old-style callback function signature.
+ * This was the default until nanopb-0.2.1. */
+/* #define PB_OLD_CALLBACK_STYLE */
+
+
+/******************************************************************
+ * You usually don't need to change anything below this line.     *
+ * Feel free to look around and use the defined macros, though.   *
+ ******************************************************************/
+
+
+/* Version of the nanopb library. Just in case you want to check it in
+ * your own program. */
+#define NANOPB_VERSION nanopb-0.2.8
+
+/* Include all the system headers needed by nanopb. You will need the
+ * definitions of the following:
+ * - strlen, memcpy, memset functions
+ * - [u]int8_t, [u]int16_t, [u]int32_t, [u]int64_t
+ * - size_t
+ * - bool
+ *
+ * If you don't have the standard header files, you can instead provide
+ * a custom header that defines or includes all this. In that case,
+ * define PB_SYSTEM_HEADER to the path of this file.
+ */
+#ifdef PB_SYSTEM_HEADER
+#include PB_SYSTEM_HEADER
+#else
+#include <stdint.h>
+#include <stddef.h>
+#include <stdbool.h>
+#include <string.h>
+
+#ifdef PB_ENABLE_MALLOC
+#include <stdlib.h>
+#endif
+#endif
+
+/* Macro for defining packed structures (compiler dependent).
+ * This just reduces memory requirements, but is not required.
+ */
+#if defined(__GNUC__) || defined(__clang__)
+    /* For GCC and clang */
+#   define PB_PACKED_STRUCT_START
+#   define PB_PACKED_STRUCT_END
+#   define pb_packed __attribute__((packed))
+#elif defined(__ICCARM__)
+    /* For IAR ARM compiler */
+#   define PB_PACKED_STRUCT_START _Pragma("pack(push, 1)")
+#   define PB_PACKED_STRUCT_END _Pragma("pack(pop)")
+#   define pb_packed
+#elif defined(_MSC_VER) && (_MSC_VER >= 1500)
+    /* For Microsoft Visual C++ */
+#   define PB_PACKED_STRUCT_START __pragma(pack(push, 1))
+#   define PB_PACKED_STRUCT_END __pragma(pack(pop))
+#   define pb_packed
+#else
+    /* Unknown compiler */
+#   define PB_PACKED_STRUCT_START
+#   define PB_PACKED_STRUCT_END
+#   define pb_packed
+#endif
+
+/* Handly macro for suppressing unreferenced-parameter compiler warnings. */
+#ifndef UNUSED
+#define UNUSED(x) (void)(x)
+#endif
+
+/* Compile-time assertion, used for checking compatible compilation options.
+ * If this does not work properly on your compiler, use #define STATIC_ASSERT
+ * to disable it.
+ *
+ * But before doing that, check carefully the error message / place where it
+ * comes from to see if the error has a real cause. Unfortunately the error
+ * message is not always very clear to read, but you can see the reason better
+ * in the place where the STATIC_ASSERT macro was called.
+ */
+#ifndef STATIC_ASSERT
+#define STATIC_ASSERT(COND,MSG) typedef char STATIC_ASSERT_MSG(MSG, __LINE__, __COUNTER__)[(COND)?1:-1];
+#define STATIC_ASSERT_MSG(MSG, LINE, COUNTER) STATIC_ASSERT_MSG_(MSG, LINE, COUNTER)
+#define STATIC_ASSERT_MSG_(MSG, LINE, COUNTER) static_assertion_##MSG##LINE##COUNTER
+#endif
+
+/* Number of required fields to keep track of. */
+#ifndef PB_MAX_REQUIRED_FIELDS
+#define PB_MAX_REQUIRED_FIELDS 64
+#endif
+
+#if PB_MAX_REQUIRED_FIELDS < 64
+#error You should not lower PB_MAX_REQUIRED_FIELDS from the default value (64).
+#endif
+
+/* List of possible field types. These are used in the autogenerated code.
+ * Least-significant 4 bits tell the scalar type
+ * Most-significant 4 bits specify repeated/required/packed etc.
+ */
+
+typedef uint8_t pb_type_t;
+
+/**** Field data types ****/
+
+/* Numeric types */
+#define PB_LTYPE_VARINT  0x00 /* int32, int64, enum, bool */
+#define PB_LTYPE_UVARINT 0x01 /* uint32, uint64 */
+#define PB_LTYPE_SVARINT 0x02 /* sint32, sint64 */
+#define PB_LTYPE_FIXED32 0x03 /* fixed32, sfixed32, float */
+#define PB_LTYPE_FIXED64 0x04 /* fixed64, sfixed64, double */
+
+/* Marker for last packable field type. */
+#define PB_LTYPE_LAST_PACKABLE 0x04
+
+/* Byte array with pre-allocated buffer.
+ * data_size is the length of the allocated PB_BYTES_ARRAY structure. */
+#define PB_LTYPE_BYTES 0x05
+
+/* String with pre-allocated buffer.
+ * data_size is the maximum length. */
+#define PB_LTYPE_STRING 0x06
+
+/* Submessage
+ * submsg_fields is pointer to field descriptions */
+#define PB_LTYPE_SUBMESSAGE 0x07
+
+/* Extension pseudo-field
+ * The field contains a pointer to pb_extension_t */
+#define PB_LTYPE_EXTENSION 0x08
+
+/* Number of declared LTYPES */
+#define PB_LTYPES_COUNT 9
+#define PB_LTYPE_MASK 0x0F
+
+/**** Field repetition rules ****/
+
+#define PB_HTYPE_REQUIRED 0x00
+#define PB_HTYPE_OPTIONAL 0x10
+#define PB_HTYPE_REPEATED 0x20
+#define PB_HTYPE_MASK     0x30
+
+/**** Field allocation types ****/
+ 
+#define PB_ATYPE_STATIC   0x00
+#define PB_ATYPE_POINTER  0x80
+#define PB_ATYPE_CALLBACK 0x40
+#define PB_ATYPE_MASK     0xC0
+
+#define PB_ATYPE(x) ((x) & PB_ATYPE_MASK)
+#define PB_HTYPE(x) ((x) & PB_HTYPE_MASK)
+#define PB_LTYPE(x) ((x) & PB_LTYPE_MASK)
+
+/* Data type used for storing sizes of struct fields
+ * and array counts.
+ */
+#if defined(PB_FIELD_32BIT)
+    typedef uint32_t pb_size_t;
+    typedef int32_t pb_ssize_t;
+#elif defined(PB_FIELD_16BIT)
+    typedef uint16_t pb_size_t;
+    typedef int16_t pb_ssize_t;
+#else
+    typedef uint8_t pb_size_t;
+    typedef int8_t pb_ssize_t;
+#endif
+
+/* This structure is used in auto-generated constants
+ * to specify struct fields.
+ * You can change field sizes if you need structures
+ * larger than 256 bytes or field tags larger than 256.
+ * The compiler should complain if your .proto has such
+ * structures. Fix that by defining PB_FIELD_16BIT or
+ * PB_FIELD_32BIT.
+ */
+PB_PACKED_STRUCT_START
+typedef struct _pb_field_t pb_field_t;
+struct _pb_field_t {
+    pb_size_t tag;
+    pb_type_t type;
+    pb_size_t data_offset; /* Offset of field data, relative to previous field. */
+    pb_ssize_t size_offset; /* Offset of array size or has-boolean, relative to data */
+    pb_size_t data_size; /* Data size in bytes for a single item */
+    pb_size_t array_size; /* Maximum number of entries in array */
+    
+    /* Field definitions for submessage
+     * OR default value for all other non-array, non-callback types
+     * If null, then field will zeroed. */
+    const void *ptr;
+} pb_packed;
+PB_PACKED_STRUCT_END
+
+/* Make sure that the standard integer types are of the expected sizes.
+ * All kinds of things may break otherwise.. atleast all fixed* types.
+ *
+ * If you get errors here, it probably means that your stdint.h is not
+ * correct for your platform.
+ */
+STATIC_ASSERT(sizeof(int8_t) == 1, INT8_T_WRONG_SIZE)
+STATIC_ASSERT(sizeof(uint8_t) == 1, UINT8_T_WRONG_SIZE)
+STATIC_ASSERT(sizeof(int16_t) == 2, INT16_T_WRONG_SIZE)
+STATIC_ASSERT(sizeof(uint16_t) == 2, UINT16_T_WRONG_SIZE)
+STATIC_ASSERT(sizeof(int32_t) == 4, INT32_T_WRONG_SIZE)
+STATIC_ASSERT(sizeof(uint32_t) == 4, UINT32_T_WRONG_SIZE)
+STATIC_ASSERT(sizeof(int64_t) == 8, INT64_T_WRONG_SIZE)
+STATIC_ASSERT(sizeof(uint64_t) == 8, UINT64_T_WRONG_SIZE)
+
+/* This structure is used for 'bytes' arrays.
+ * It has the number of bytes in the beginning, and after that an array.
+ * Note that actual structs used will have a different length of bytes array.
+ */
+#define PB_BYTES_ARRAY_T(n) struct { size_t size; uint8_t bytes[n]; }
+#define PB_BYTES_ARRAY_T_ALLOCSIZE(n) ((size_t)n + offsetof(pb_bytes_array_t, bytes))
+
+struct _pb_bytes_array_t {
+    size_t size;
+    uint8_t bytes[1];
+};
+typedef struct _pb_bytes_array_t pb_bytes_array_t;
+
+/* This structure is used for giving the callback function.
+ * It is stored in the message structure and filled in by the method that
+ * calls pb_decode.
+ *
+ * The decoding callback will be given a limited-length stream
+ * If the wire type was string, the length is the length of the string.
+ * If the wire type was a varint/fixed32/fixed64, the length is the length
+ * of the actual value.
+ * The function may be called multiple times (especially for repeated types,
+ * but also otherwise if the message happens to contain the field multiple
+ * times.)
+ *
+ * The encoding callback will receive the actual output stream.
+ * It should write all the data in one call, including the field tag and
+ * wire type. It can write multiple fields.
+ *
+ * The callback can be null if you want to skip a field.
+ */
+typedef struct _pb_istream_t pb_istream_t;
+typedef struct _pb_ostream_t pb_ostream_t;
+typedef struct _pb_callback_t pb_callback_t;
+struct _pb_callback_t {
+#ifdef PB_OLD_CALLBACK_STYLE
+    /* Deprecated since nanopb-0.2.1 */
+    union {
+        bool (*decode)(pb_istream_t *stream, const pb_field_t *field, void *arg);
+        bool (*encode)(pb_ostream_t *stream, const pb_field_t *field, const void *arg);
+    } funcs;
+#else
+    /* New function signature, which allows modifying arg contents in callback. */
+    union {
+        bool (*decode)(pb_istream_t *stream, const pb_field_t *field, void **arg);
+        bool (*encode)(pb_ostream_t *stream, const pb_field_t *field, void * const *arg);
+    } funcs;
+#endif    
+    
+    /* Free arg for use by callback */
+    void *arg;
+};
+
+/* Wire types. Library user needs these only in encoder callbacks. */
+typedef enum {
+    PB_WT_VARINT = 0,
+    PB_WT_64BIT  = 1,
+    PB_WT_STRING = 2,
+    PB_WT_32BIT  = 5
+} pb_wire_type_t;
+
+/* Structure for defining the handling of unknown/extension fields.
+ * Usually the pb_extension_type_t structure is automatically generated,
+ * while the pb_extension_t structure is created by the user. However,
+ * if you want to catch all unknown fields, you can also create a custom
+ * pb_extension_type_t with your own callback.
+ */
+typedef struct _pb_extension_type_t pb_extension_type_t;
+typedef struct _pb_extension_t pb_extension_t;
+struct _pb_extension_type_t {
+    /* Called for each unknown field in the message.
+     * If you handle the field, read off all of its data and return true.
+     * If you do not handle the field, do not read anything and return true.
+     * If you run into an error, return false.
+     * Set to NULL for default handler.
+     */
+    bool (*decode)(pb_istream_t *stream, pb_extension_t *extension,
+                   uint32_t tag, pb_wire_type_t wire_type);
+    
+    /* Called once after all regular fields have been encoded.
+     * If you have something to write, do so and return true.
+     * If you do not have anything to write, just return true.
+     * If you run into an error, return false.
+     * Set to NULL for default handler.
+     */
+    bool (*encode)(pb_ostream_t *stream, const pb_extension_t *extension);
+    
+    /* Free field for use by the callback. */
+    const void *arg;
+};
+
+struct _pb_extension_t {
+    /* Type describing the extension field. Usually you'll initialize
+     * this to a pointer to the automatically generated structure. */
+    const pb_extension_type_t *type;
+    
+    /* Destination for the decoded data. This must match the datatype
+     * of the extension field. */
+    void *dest;
+    
+    /* Pointer to the next extension handler, or NULL.
+     * If this extension does not match a field, the next handler is
+     * automatically called. */
+    pb_extension_t *next;
+
+    /* The decoder sets this to true if the extension was found.
+     * Ignored for encoding. */
+    bool found;
+};
+
+/* Memory allocation functions to use. You can define pb_realloc and
+ * pb_free to custom functions if you want. */
+#ifdef PB_ENABLE_MALLOC
+#   ifndef pb_realloc
+#       define pb_realloc(ptr, size) realloc(ptr, size)
+#   endif
+#   ifndef pb_free
+#       define pb_free(ptr) free(ptr)
+#   endif
+#endif
+
+/* These macros are used to declare pb_field_t's in the constant array. */
+/* Size of a structure member, in bytes. */
+#define pb_membersize(st, m) (sizeof ((st*)0)->m)
+/* Number of entries in an array. */
+#define pb_arraysize(st, m) (pb_membersize(st, m) / pb_membersize(st, m[0]))
+/* Delta from start of one member to the start of another member. */
+#define pb_delta(st, m1, m2) ((int)offsetof(st, m1) - (int)offsetof(st, m2))
+/* Marks the end of the field list */
+#define PB_LAST_FIELD {0,(pb_type_t) 0,0,0,0,0,0}
+
+/* Macros for filling in the data_offset field */
+/* data_offset for first field in a message */
+#define PB_DATAOFFSET_FIRST(st, m1, m2) (offsetof(st, m1))
+/* data_offset for subsequent fields */
+#define PB_DATAOFFSET_OTHER(st, m1, m2) (offsetof(st, m1) - offsetof(st, m2) - pb_membersize(st, m2))
+/* Choose first/other based on m1 == m2 (deprecated, remains for backwards compatibility) */
+#define PB_DATAOFFSET_CHOOSE(st, m1, m2) (int)(offsetof(st, m1) == offsetof(st, m2) \
+                                  ? PB_DATAOFFSET_FIRST(st, m1, m2) \
+                                  : PB_DATAOFFSET_OTHER(st, m1, m2))
+
+/* Required fields are the simplest. They just have delta (padding) from
+ * previous field end, and the size of the field. Pointer is used for
+ * submessages and default values.
+ */
+#define PB_REQUIRED_STATIC(tag, st, m, fd, ltype, ptr) \
+    {tag, PB_ATYPE_STATIC | PB_HTYPE_REQUIRED | ltype, \
+    fd, 0, pb_membersize(st, m), 0, ptr}
+
+/* Optional fields add the delta to the has_ variable. */
+#define PB_OPTIONAL_STATIC(tag, st, m, fd, ltype, ptr) \
+    {tag, PB_ATYPE_STATIC | PB_HTYPE_OPTIONAL | ltype, \
+    fd, \
+    pb_delta(st, has_ ## m, m), \
+    pb_membersize(st, m), 0, ptr}
+
+/* Repeated fields have a _count field and also the maximum number of entries. */
+#define PB_REPEATED_STATIC(tag, st, m, fd, ltype, ptr) \
+    {tag, PB_ATYPE_STATIC | PB_HTYPE_REPEATED | ltype, \
+    fd, \
+    pb_delta(st, m ## _count, m), \
+    pb_membersize(st, m[0]), \
+    pb_arraysize(st, m), ptr}
+
+/* Allocated fields carry the size of the actual data, not the pointer */
+#define PB_REQUIRED_POINTER(tag, st, m, fd, ltype, ptr) \
+    {tag, PB_ATYPE_POINTER | PB_HTYPE_REQUIRED | ltype, \
+    fd, 0, pb_membersize(st, m[0]), 0, ptr}
+
+/* Optional fields don't need a has_ variable, as information would be redundant */
+#define PB_OPTIONAL_POINTER(tag, st, m, fd, ltype, ptr) \
+    {tag, PB_ATYPE_POINTER | PB_HTYPE_OPTIONAL | ltype, \
+    fd, 0, pb_membersize(st, m[0]), 0, ptr}
+
+/* Repeated fields have a _count field and a pointer to array of pointers */
+#define PB_REPEATED_POINTER(tag, st, m, fd, ltype, ptr) \
+    {tag, PB_ATYPE_POINTER | PB_HTYPE_REPEATED | ltype, \
+    fd, pb_delta(st, m ## _count, m), \
+    pb_membersize(st, m[0]), 0, ptr}
+
+/* Callbacks are much like required fields except with special datatype. */
+#define PB_REQUIRED_CALLBACK(tag, st, m, fd, ltype, ptr) \
+    {tag, PB_ATYPE_CALLBACK | PB_HTYPE_REQUIRED | ltype, \
+    fd, 0, pb_membersize(st, m), 0, ptr}
+
+#define PB_OPTIONAL_CALLBACK(tag, st, m, fd, ltype, ptr) \
+    {tag, PB_ATYPE_CALLBACK | PB_HTYPE_OPTIONAL | ltype, \
+    fd, 0, pb_membersize(st, m), 0, ptr}
+    
+#define PB_REPEATED_CALLBACK(tag, st, m, fd, ltype, ptr) \
+    {tag, PB_ATYPE_CALLBACK | PB_HTYPE_REPEATED | ltype, \
+    fd, 0, pb_membersize(st, m), 0, ptr}
+
+/* Optional extensions don't have the has_ field, as that would be redundant. */
+#define PB_OPTEXT_STATIC(tag, st, m, fd, ltype, ptr) \
+    {tag, PB_ATYPE_STATIC | PB_HTYPE_OPTIONAL | ltype, \
+    0, \
+    0, \
+    pb_membersize(st, m), 0, ptr}
+
+#define PB_OPTEXT_CALLBACK(tag, st, m, fd, ltype, ptr) \
+    {tag, PB_ATYPE_CALLBACK | PB_HTYPE_OPTIONAL | ltype, \
+    0, 0, pb_membersize(st, m), 0, ptr}
+
+/* The mapping from protobuf types to LTYPEs is done using these macros. */
+#define PB_LTYPE_MAP_BOOL       PB_LTYPE_VARINT
+#define PB_LTYPE_MAP_BYTES      PB_LTYPE_BYTES
+#define PB_LTYPE_MAP_DOUBLE     PB_LTYPE_FIXED64
+#define PB_LTYPE_MAP_ENUM       PB_LTYPE_VARINT
+#define PB_LTYPE_MAP_FIXED32    PB_LTYPE_FIXED32
+#define PB_LTYPE_MAP_FIXED64    PB_LTYPE_FIXED64
+#define PB_LTYPE_MAP_FLOAT      PB_LTYPE_FIXED32
+#define PB_LTYPE_MAP_INT32      PB_LTYPE_VARINT
+#define PB_LTYPE_MAP_INT64      PB_LTYPE_VARINT
+#define PB_LTYPE_MAP_MESSAGE    PB_LTYPE_SUBMESSAGE
+#define PB_LTYPE_MAP_SFIXED32   PB_LTYPE_FIXED32
+#define PB_LTYPE_MAP_SFIXED64   PB_LTYPE_FIXED64
+#define PB_LTYPE_MAP_SINT32     PB_LTYPE_SVARINT
+#define PB_LTYPE_MAP_SINT64     PB_LTYPE_SVARINT
+#define PB_LTYPE_MAP_STRING     PB_LTYPE_STRING
+#define PB_LTYPE_MAP_UINT32     PB_LTYPE_UVARINT
+#define PB_LTYPE_MAP_UINT64     PB_LTYPE_UVARINT
+#define PB_LTYPE_MAP_EXTENSION  PB_LTYPE_EXTENSION
+
+/* This is the actual macro used in field descriptions.
+ * It takes these arguments:
+ * - Field tag number
+ * - Field type:   BOOL, BYTES, DOUBLE, ENUM, FIXED32, FIXED64,
+ *                 FLOAT, INT32, INT64, MESSAGE, SFIXED32, SFIXED64
+ *                 SINT32, SINT64, STRING, UINT32, UINT64 or EXTENSION
+ * - Field rules:  REQUIRED, OPTIONAL or REPEATED
+ * - Allocation:   STATIC or CALLBACK
+ * - Message name
+ * - Field name
+ * - Previous field name (or field name again for first field)
+ * - Pointer to default value or submsg fields.
+ */
+
+#define PB_FIELD(tag, type, rules, allocation, message, field, prevfield, ptr) \
+    PB_ ## rules ## _ ## allocation(tag, message, field, \
+        PB_DATAOFFSET_CHOOSE(message, field, prevfield), \
+        PB_LTYPE_MAP_ ## type, ptr)
+
+/* This is a new version of the macro used by nanopb generator from
+ * version 0.2.3 onwards. It avoids the use of a ternary expression in
+ * the initialization, which confused some compilers.
+ *
+ * - Placement: FIRST or OTHER, depending on if this is the first field in structure.
+ *
+ */
+#define PB_FIELD2(tag, type, rules, allocation, placement, message, field, prevfield, ptr) \
+    PB_ ## rules ## _ ## allocation(tag, message, field, \
+        PB_DATAOFFSET_ ## placement(message, field, prevfield), \
+        PB_LTYPE_MAP_ ## type, ptr)
+
+
+/* These macros are used for giving out error messages.
+ * They are mostly a debugging aid; the main error information
+ * is the true/false return value from functions.
+ * Some code space can be saved by disabling the error
+ * messages if not used.
+ */
+#ifdef PB_NO_ERRMSG
+#define PB_RETURN_ERROR(stream,msg) \
+    do {\
+        UNUSED(stream); \
+        return false; \
+    } while(0)
+#define PB_GET_ERROR(stream) "(errmsg disabled)"
+#else
+#define PB_RETURN_ERROR(stream,msg) \
+    do {\
+        if ((stream)->errmsg == NULL) \
+            (stream)->errmsg = (msg); \
+        return false; \
+    } while(0)
+#define PB_GET_ERROR(stream) ((stream)->errmsg ? (stream)->errmsg : "(none)")
+#endif
+
+#endif

src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_decode.h

@@ -0,0 +1,149 @@
+/* pb_decode.h: Functions to decode protocol buffers. Depends on pb_decode.c.
+ * The main function is pb_decode. You also need an input stream, and the
+ * field descriptions created by nanopb_generator.py.
+ */
+
+#ifndef _PB_DECODE_H_
+#define _PB_DECODE_H_
+
+#include "pb.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Structure for defining custom input streams. You will need to provide
+ * a callback function to read the bytes from your storage, which can be
+ * for example a file or a network socket.
+ * 
+ * The callback must conform to these rules:
+ *
+ * 1) Return false on IO errors. This will cause decoding to abort.
+ * 2) You can use state to store your own data (e.g. buffer pointer),
+ *    and rely on pb_read to verify that no-body reads past bytes_left.
+ * 3) Your callback may be used with substreams, in which case bytes_left
+ *    is different than from the main stream. Don't use bytes_left to compute
+ *    any pointers.
+ */
+struct _pb_istream_t
+{
+#ifdef PB_BUFFER_ONLY
+    /* Callback pointer is not used in buffer-only configuration.
+     * Having an int pointer here allows binary compatibility but
+     * gives an error if someone tries to assign callback function.
+     */
+    int *callback;
+#else
+    bool (*callback)(pb_istream_t *stream, uint8_t *buf, size_t count);
+#endif
+
+    void *state; /* Free field for use by callback implementation */
+    size_t bytes_left;
+    
+#ifndef PB_NO_ERRMSG
+    const char *errmsg;
+#endif
+};
+
+/***************************
+ * Main decoding functions *
+ ***************************/
+ 
+/* Decode a single protocol buffers message from input stream into a C structure.
+ * Returns true on success, false on any failure.
+ * The actual struct pointed to by dest must match the description in fields.
+ * Callback fields of the destination structure must be initialized by caller.
+ * All other fields will be initialized by this function.
+ *
+ * Example usage:
+ *    MyMessage msg = {};
+ *    uint8_t buffer[64];
+ *    pb_istream_t stream;
+ *    
+ *    // ... read some data into buffer ...
+ *
+ *    stream = pb_istream_from_buffer(buffer, count);
+ *    pb_decode(&stream, MyMessage_fields, &msg);
+ */
+bool pb_decode(pb_istream_t *stream, const pb_field_t fields[], void *dest_struct);
+
+/* Same as pb_decode, except does not initialize the destination structure
+ * to default values. This is slightly faster if you need no default values
+ * and just do memset(struct, 0, sizeof(struct)) yourself.
+ *
+ * This can also be used for 'merging' two messages, i.e. update only the
+ * fields that exist in the new message.
+ *
+ * Note: If this function returns with an error, it will not release any
+ * dynamically allocated fields. You will need to call pb_release() yourself.
+ */
+bool pb_decode_noinit(pb_istream_t *stream, const pb_field_t fields[], void *dest_struct);
+
+/* Same as pb_decode, except expects the stream to start with the message size
+ * encoded as varint. Corresponds to parseDelimitedFrom() in Google's
+ * protobuf API.
+ */
+bool pb_decode_delimited(pb_istream_t *stream, const pb_field_t fields[], void *dest_struct);
+
+#ifdef PB_ENABLE_MALLOC
+/* Release any allocated pointer fields. If you use dynamic allocation, you should
+ * call this for any successfully decoded message when you are done with it. If
+ * pb_decode() returns with an error, the message is already released.
+ */
+void pb_release(const pb_field_t fields[], void *dest_struct);
+#endif
+
+
+/**************************************
+ * Functions for manipulating streams *
+ **************************************/
+
+/* Create an input stream for reading from a memory buffer.
+ *
+ * Alternatively, you can use a custom stream that reads directly from e.g.
+ * a file or a network socket.
+ */
+pb_istream_t pb_istream_from_buffer(uint8_t *buf, size_t bufsize);
+
+/* Function to read from a pb_istream_t. You can use this if you need to
+ * read some custom header data, or to read data in field callbacks.
+ */
+bool pb_read(pb_istream_t *stream, uint8_t *buf, size_t count);
+
+
+/************************************************
+ * Helper functions for writing field callbacks *
+ ************************************************/
+
+/* Decode the tag for the next field in the stream. Gives the wire type and
+ * field tag. At end of the message, returns false and sets eof to true. */
+bool pb_decode_tag(pb_istream_t *stream, pb_wire_type_t *wire_type, uint32_t *tag, bool *eof);
+
+/* Skip the field payload data, given the wire type. */
+bool pb_skip_field(pb_istream_t *stream, pb_wire_type_t wire_type);
+
+/* Decode an integer in the varint format. This works for bool, enum, int32,
+ * int64, uint32 and uint64 field types. */
+bool pb_decode_varint(pb_istream_t *stream, uint64_t *dest);
+
+/* Decode an integer in the zig-zagged svarint format. This works for sint32
+ * and sint64. */
+bool pb_decode_svarint(pb_istream_t *stream, int64_t *dest);
+
+/* Decode a fixed32, sfixed32 or float value. You need to pass a pointer to
+ * a 4-byte wide C variable. */
+bool pb_decode_fixed32(pb_istream_t *stream, void *dest);
+
+/* Decode a fixed64, sfixed64 or double value. You need to pass a pointer to
+ * a 8-byte wide C variable. */
+bool pb_decode_fixed64(pb_istream_t *stream, void *dest);
+
+/* Make a limited-length substream for reading a PB_WT_STRING field. */
+bool pb_make_string_substream(pb_istream_t *stream, pb_istream_t *substream);
+void pb_close_string_substream(pb_istream_t *stream, pb_istream_t *substream);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif

src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_encode.c

@@ -0,0 +1,671 @@
+/* pb_encode.c -- encode a protobuf using minimal resources
+ *
+ * 2011 Petteri Aimonen <jpa@kapsi.fi>
+ */
+
+#include "pb.h"
+#include "pb_encode.h"
+
+/* Use the GCC warn_unused_result attribute to check that all return values
+ * are propagated correctly. On other compilers and gcc before 3.4.0 just
+ * ignore the annotation.
+ */
+#if !defined(__GNUC__) || ( __GNUC__ < 3) || (__GNUC__ == 3 && __GNUC_MINOR__ < 4)
+    #define checkreturn
+#else
+    #define checkreturn __attribute__((warn_unused_result))
+#endif
+
+/**************************************
+ * Declarations internal to this file *
+ **************************************/
+typedef bool (*pb_encoder_t)(pb_ostream_t *stream, const pb_field_t *field, const void *src) checkreturn;
+
+static bool checkreturn buf_write(pb_ostream_t *stream, const uint8_t *buf, size_t count);
+static bool checkreturn encode_array(pb_ostream_t *stream, const pb_field_t *field, const void *pData, size_t count, pb_encoder_t func);
+static bool checkreturn encode_field(pb_ostream_t *stream, const pb_field_t *field, const void *pData);
+static bool checkreturn default_extension_encoder(pb_ostream_t *stream, const pb_extension_t *extension);
+static bool checkreturn encode_extension_field(pb_ostream_t *stream, const pb_field_t *field, const void *pData);
+static bool checkreturn pb_enc_varint(pb_ostream_t *stream, const pb_field_t *field, const void *src);
+static bool checkreturn pb_enc_uvarint(pb_ostream_t *stream, const pb_field_t *field, const void *src);
+static bool checkreturn pb_enc_svarint(pb_ostream_t *stream, const pb_field_t *field, const void *src);
+static bool checkreturn pb_enc_fixed32(pb_ostream_t *stream, const pb_field_t *field, const void *src);
+static bool checkreturn pb_enc_fixed64(pb_ostream_t *stream, const pb_field_t *field, const void *src);
+static bool checkreturn pb_enc_bytes(pb_ostream_t *stream, const pb_field_t *field, const void *src);
+static bool checkreturn pb_enc_string(pb_ostream_t *stream, const pb_field_t *field, const void *src);
+static bool checkreturn pb_enc_submessage(pb_ostream_t *stream, const pb_field_t *field, const void *src);
+
+/* --- Function pointers to field encoders ---
+ * Order in the array must match pb_action_t LTYPE numbering.
+ */
+static const pb_encoder_t PB_ENCODERS[PB_LTYPES_COUNT] = {
+    &pb_enc_varint,
+    &pb_enc_uvarint,
+    &pb_enc_svarint,
+    &pb_enc_fixed32,
+    &pb_enc_fixed64,
+    
+    &pb_enc_bytes,
+    &pb_enc_string,
+    &pb_enc_submessage,
+    NULL /* extensions */
+};
+
+/*******************************
+ * pb_ostream_t implementation *
+ *******************************/
+
+static bool checkreturn buf_write(pb_ostream_t *stream, const uint8_t *buf, size_t count)
+{
+    uint8_t *dest = (uint8_t*)stream->state;
+    stream->state = dest + count;
+    
+    while (count--)
+        *dest++ = *buf++;
+    
+    return true;
+}
+
+pb_ostream_t pb_ostream_from_buffer(uint8_t *buf, size_t bufsize)
+{
+    pb_ostream_t stream;
+#ifdef PB_BUFFER_ONLY
+    stream.callback = (void*)1; /* Just a marker value */
+#else
+    stream.callback = &buf_write;
+#endif
+    stream.state = buf;
+    stream.max_size = bufsize;
+    stream.bytes_written = 0;
+#ifndef PB_NO_ERRMSG
+    stream.errmsg = NULL;
+#endif
+    return stream;
+}
+
+bool checkreturn pb_write(pb_ostream_t *stream, const uint8_t *buf, size_t count)
+{
+    if (stream->callback != NULL)
+    {
+        if (stream->bytes_written + count > stream->max_size)
+            PB_RETURN_ERROR(stream, "stream full");
+
+#ifdef PB_BUFFER_ONLY
+        if (!buf_write(stream, buf, count))
+            PB_RETURN_ERROR(stream, "io error");
+#else        
+        if (!stream->callback(stream, buf, count))
+            PB_RETURN_ERROR(stream, "io error");
+#endif
+    }
+    
+    stream->bytes_written += count;
+    return true;
+}
+
+/*************************
+ * Encode a single field *
+ *************************/
+
+/* Encode a static array. Handles the size calculations and possible packing. */
+static bool checkreturn encode_array(pb_ostream_t *stream, const pb_field_t *field,
+                         const void *pData, size_t count, pb_encoder_t func)
+{
+    size_t i;
+    const void *p;
+    size_t size;
+    
+    if (count == 0)
+        return true;
+
+    if (PB_ATYPE(field->type) != PB_ATYPE_POINTER && count > field->array_size)
+        PB_RETURN_ERROR(stream, "array max size exceeded");
+    
+    /* We always pack arrays if the datatype allows it. */
+    if (PB_LTYPE(field->type) <= PB_LTYPE_LAST_PACKABLE)
+    {
+        if (!pb_encode_tag(stream, PB_WT_STRING, field->tag))
+            return false;
+        
+        /* Determine the total size of packed array. */
+        if (PB_LTYPE(field->type) == PB_LTYPE_FIXED32)
+        {
+            size = 4 * count;
+        }
+        else if (PB_LTYPE(field->type) == PB_LTYPE_FIXED64)
+        {
+            size = 8 * count;
+        }
+        else
+        { 
+            pb_ostream_t sizestream = PB_OSTREAM_SIZING;
+            p = pData;
+            for (i = 0; i < count; i++)
+            {
+                if (!func(&sizestream, field, p))
+                    return false;
+                p = (const char*)p + field->data_size;
+            }
+            size = sizestream.bytes_written;
+        }
+        
+        if (!pb_encode_varint(stream, (uint64_t)size))
+            return false;
+        
+        if (stream->callback == NULL)
+            return pb_write(stream, NULL, size); /* Just sizing.. */
+        
+        /* Write the data */
+        p = pData;
+        for (i = 0; i < count; i++)
+        {
+            if (!func(stream, field, p))
+                return false;
+            p = (const char*)p + field->data_size;
+        }
+    }
+    else
+    {
+        p = pData;
+        for (i = 0; i < count; i++)
+        {
+            if (!pb_encode_tag_for_field(stream, field))
+                return false;
+
+            /* Normally the data is stored directly in the array entries, but
+             * for pointer-type string and bytes fields, the array entries are
+             * actually pointers themselves also. So we have to dereference once
+             * more to get to the actual data. */
+            if (PB_ATYPE(field->type) == PB_ATYPE_POINTER &&
+                (PB_LTYPE(field->type) == PB_LTYPE_STRING ||
+                 PB_LTYPE(field->type) == PB_LTYPE_BYTES))
+            {
+                if (!func(stream, field, *(const void* const*)p))
+                    return false;      
+            }
+            else
+            {
+                if (!func(stream, field, p))
+                    return false;
+            }
+            p = (const char*)p + field->data_size;
+        }
+    }
+    
+    return true;
+}
+
+/* Encode a field with static or pointer allocation, i.e. one whose data
+ * is available to the encoder directly. */
+static bool checkreturn encode_basic_field(pb_ostream_t *stream,
+    const pb_field_t *field, const void *pData)
+{
+    pb_encoder_t func;
+    const void *pSize;
+    bool implicit_has = true;
+    
+    func = PB_ENCODERS[PB_LTYPE(field->type)];
+    
+    if (field->size_offset)
+        pSize = (const char*)pData + field->size_offset;
+    else
+        pSize = &implicit_has;
+
+    if (PB_ATYPE(field->type) == PB_ATYPE_POINTER)
+    {
+        /* pData is a pointer to the field, which contains pointer to
+         * the data. If the 2nd pointer is NULL, it is interpreted as if
+         * the has_field was false.
+         */
+        
+        pData = *(const void* const*)pData;
+        implicit_has = (pData != NULL);
+    }
+
+    switch (PB_HTYPE(field->type))
+    {
+        case PB_HTYPE_REQUIRED:
+            if (!pData)
+                PB_RETURN_ERROR(stream, "missing required field");
+            if (!pb_encode_tag_for_field(stream, field))
+                return false;
+            if (!func(stream, field, pData))
+                return false;
+            break;
+        
+        case PB_HTYPE_OPTIONAL:
+            /*
+             * KUKA adjustment for VxWorks 
+             */
+
+            if (*(const char*)pSize)
+            {
+                if (!pb_encode_tag_for_field(stream, field))
+                    return false;
+            
+                if (!func(stream, field, pData))
+                    return false;
+            }
+            break;
+        
+        case PB_HTYPE_REPEATED:
+            if (!encode_array(stream, field, pData, *(const size_t*)pSize, func))
+                return false;
+            break;
+        
+        default:
+            PB_RETURN_ERROR(stream, "invalid field type");
+    }
+    
+    return true;
+}
+
+/* Encode a field with callback semantics. This means that a user function is
+ * called to provide and encode the actual data. */
+static bool checkreturn encode_callback_field(pb_ostream_t *stream,
+    const pb_field_t *field, const void *pData)
+{
+    const pb_callback_t *callback = (const pb_callback_t*)pData;
+    
+#ifdef PB_OLD_CALLBACK_STYLE
+    const void *arg = callback->arg;
+#else
+    void * const *arg = &(callback->arg);
+#endif    
+    
+    if (callback->funcs.encode != NULL)
+    {
+        if (!callback->funcs.encode(stream, field, arg))
+            PB_RETURN_ERROR(stream, "callback error");
+    }
+    return true;
+}
+
+/* Encode a single field of any callback or static type. */
+static bool checkreturn encode_field(pb_ostream_t *stream,
+    const pb_field_t *field, const void *pData)
+{
+    switch (PB_ATYPE(field->type))
+    {
+        case PB_ATYPE_STATIC:
+        case PB_ATYPE_POINTER:
+            return encode_basic_field(stream, field, pData);
+        
+        case PB_ATYPE_CALLBACK:
+            return encode_callback_field(stream, field, pData);
+        
+        default:
+            PB_RETURN_ERROR(stream, "invalid field type");
+    }
+}
+
+/* Default handler for extension fields. Expects to have a pb_field_t
+ * pointer in the extension->type->arg field. */
+static bool checkreturn default_extension_encoder(pb_ostream_t *stream,
+    const pb_extension_t *extension)
+{
+    const pb_field_t *field = (const pb_field_t*)extension->type->arg;
+    return encode_field(stream, field, extension->dest);
+}
+
+/* Walk through all the registered extensions and give them a chance
+ * to encode themselves. */
+static bool checkreturn encode_extension_field(pb_ostream_t *stream,
+    const pb_field_t *field, const void *pData)
+{
+    const pb_extension_t *extension = *(const pb_extension_t* const *)pData;
+    UNUSED(field);
+    
+    while (extension)
+    {
+        bool status;
+        if (extension->type->encode)
+            status = extension->type->encode(stream, extension);
+        else
+            status = default_extension_encoder(stream, extension);
+
+        if (!status)
+            return false;
+        
+        extension = extension->next;
+    }
+    
+    return true;
+}
+
+/*********************
+ * Encode all fields *
+ *********************/
+
+bool checkreturn pb_encode(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct)
+{
+    const pb_field_t *field = fields;
+    const void *pData = src_struct;
+    size_t prev_size = 0;
+    
+    while (field->tag != 0)
+    {
+        pData = (const char*)pData + prev_size + field->data_offset;
+        if (PB_ATYPE(field->type) == PB_ATYPE_POINTER)
+            prev_size = sizeof(const void*);
+        else
+            prev_size = field->data_size;
+        
+        /* Special case for static arrays */
+        if (PB_ATYPE(field->type) == PB_ATYPE_STATIC &&
+            PB_HTYPE(field->type) == PB_HTYPE_REPEATED)
+        {
+            prev_size *= field->array_size;
+        }
+        
+        if (PB_LTYPE(field->type) == PB_LTYPE_EXTENSION)
+        {
+            /* Special case for the extension field placeholder */
+            if (!encode_extension_field(stream, field, pData))
+                return false;
+        }
+        else
+        {
+            /* Regular field */
+            if (!encode_field(stream, field, pData))
+                return false;
+        }
+    
+        field++;
+    }
+    
+    return true;
+}
+
+bool pb_encode_delimited(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct)
+{
+    return pb_encode_submessage(stream, fields, src_struct);
+}
+
+bool pb_get_encoded_size(size_t *size, const pb_field_t fields[], const void *src_struct)
+{
+    pb_ostream_t stream = PB_OSTREAM_SIZING;
+    
+    if (!pb_encode(&stream, fields, src_struct))
+        return false;
+    
+    *size = stream.bytes_written;
+    return true;
+}
+
+/********************
+ * Helper functions *
+ ********************/
+bool checkreturn pb_encode_varint(pb_ostream_t *stream, uint64_t value)
+{
+    uint8_t buffer[10];
+    size_t i = 0;
+    
+    if (value == 0)
+        return pb_write(stream, (uint8_t*)&value, 1);
+    
+    while (value)
+    {
+        buffer[i] = (uint8_t)((value & 0x7F) | 0x80);
+        value >>= 7;
+        i++;
+    }
+    buffer[i-1] &= 0x7F; /* Unset top bit on last byte */
+    
+    return pb_write(stream, buffer, i);
+}
+
+bool checkreturn pb_encode_svarint(pb_ostream_t *stream, int64_t value)
+{
+    uint64_t zigzagged;
+    if (value < 0)
+        zigzagged = ~((uint64_t)value << 1);
+    else
+        zigzagged = (uint64_t)value << 1;
+    
+    return pb_encode_varint(stream, zigzagged);
+}
+
+bool checkreturn pb_encode_fixed32(pb_ostream_t *stream, const void *value)
+{
+    #ifdef __BIG_ENDIAN__
+    const uint8_t *bytes = value;
+    uint8_t lebytes[4];
+    lebytes[0] = bytes[3];
+    lebytes[1] = bytes[2];
+    lebytes[2] = bytes[1];
+    lebytes[3] = bytes[0];
+    return pb_write(stream, lebytes, 4);
+    #else
+    return pb_write(stream, (const uint8_t*)value, 4);
+    #endif
+}
+
+bool checkreturn pb_encode_fixed64(pb_ostream_t *stream, const void *value)
+{
+    #ifdef __BIG_ENDIAN__
+    const uint8_t *bytes = value;
+    uint8_t lebytes[8];
+    lebytes[0] = bytes[7];
+    lebytes[1] = bytes[6];
+    lebytes[2] = bytes[5];
+    lebytes[3] = bytes[4];
+    lebytes[4] = bytes[3];
+    lebytes[5] = bytes[2];
+    lebytes[6] = bytes[1];
+    lebytes[7] = bytes[0];
+    return pb_write(stream, lebytes, 8);
+    #else
+    return pb_write(stream, (const uint8_t*)value, 8);
+    #endif
+}
+
+bool checkreturn pb_encode_tag(pb_ostream_t *stream, pb_wire_type_t wiretype, uint32_t field_number)
+{
+    uint64_t tag = ((uint64_t)field_number << 3) | wiretype;
+    return pb_encode_varint(stream, tag);
+}
+
+bool checkreturn pb_encode_tag_for_field(pb_ostream_t *stream, const pb_field_t *field)
+{
+    pb_wire_type_t wiretype;
+    switch (PB_LTYPE(field->type))
+    {
+        case PB_LTYPE_VARINT:
+        case PB_LTYPE_UVARINT:
+        case PB_LTYPE_SVARINT:
+            wiretype = PB_WT_VARINT;
+            break;
+        
+        case PB_LTYPE_FIXED32:
+            wiretype = PB_WT_32BIT;
+            break;
+        
+        case PB_LTYPE_FIXED64:
+            wiretype = PB_WT_64BIT;
+            break;
+        
+        case PB_LTYPE_BYTES:
+        case PB_LTYPE_STRING:
+        case PB_LTYPE_SUBMESSAGE:
+            wiretype = PB_WT_STRING;
+            break;
+        
+        default:
+            PB_RETURN_ERROR(stream, "invalid field type");
+    }
+    
+    return pb_encode_tag(stream, wiretype, field->tag);
+}
+
+bool checkreturn pb_encode_string(pb_ostream_t *stream, const uint8_t *buffer, size_t size)
+{
+    if (!pb_encode_varint(stream, (uint64_t)size))
+        return false;
+    
+    return pb_write(stream, buffer, size);
+}
+
+bool checkreturn pb_encode_submessage(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct)
+{
+    /* First calculate the message size using a non-writing substream. */
+    pb_ostream_t substream = PB_OSTREAM_SIZING;
+    size_t size;
+    bool status;
+    
+    if (!pb_encode(&substream, fields, src_struct))
+    {
+#ifndef PB_NO_ERRMSG
+        stream->errmsg = substream.errmsg;
+#endif
+        return false;
+    }
+    
+    size = substream.bytes_written;
+    
+    if (!pb_encode_varint(stream, (uint64_t)size))
+        return false;
+    
+    if (stream->callback == NULL)
+        return pb_write(stream, NULL, size); /* Just sizing */
+    
+    if (stream->bytes_written + size > stream->max_size)
+        PB_RETURN_ERROR(stream, "stream full");
+        
+    /* Use a substream to verify that a callback doesn't write more than
+     * what it did the first time. */
+    substream.callback = stream->callback;
+    substream.state = stream->state;
+    substream.max_size = size;
+    substream.bytes_written = 0;
+#ifndef PB_NO_ERRMSG
+    substream.errmsg = NULL;
+#endif
+    
+    status = pb_encode(&substream, fields, src_struct);
+    
+    stream->bytes_written += substream.bytes_written;
+    stream->state = substream.state;
+#ifndef PB_NO_ERRMSG
+    stream->errmsg = substream.errmsg;
+#endif
+    
+    if (substream.bytes_written != size)
+        PB_RETURN_ERROR(stream, "submsg size changed");
+    
+    return status;
+}
+
+/* Field encoders */
+
+static bool checkreturn pb_enc_varint(pb_ostream_t *stream, const pb_field_t *field, const void *src)
+{
+    int64_t value = 0;
+    
+    /* Cases 1 and 2 are for compilers that have smaller types for bool
+     * or enums. */
+    switch (field->data_size)
+    {
+        case 1: value = *(const int8_t*)src; break;
+        case 2: value = *(const int16_t*)src; break;
+        case 4: value = *(const int32_t*)src; break;
+        case 8: value = *(const int64_t*)src; break;
+        default: PB_RETURN_ERROR(stream, "invalid data_size");
+    }
+    
+    return pb_encode_varint(stream, (uint64_t)value);
+}
+
+static bool checkreturn pb_enc_uvarint(pb_ostream_t *stream, const pb_field_t *field, const void *src)
+{
+    uint64_t value = 0;
+    
+    switch (field->data_size)
+    {
+        case 4: value = *(const uint32_t*)src; break;
+        case 8: value = *(const uint64_t*)src; break;
+        default: PB_RETURN_ERROR(stream, "invalid data_size");
+    }
+    
+    return pb_encode_varint(stream, value);
+}
+
+static bool checkreturn pb_enc_svarint(pb_ostream_t *stream, const pb_field_t *field, const void *src)
+{
+    int64_t value = 0;
+    
+    switch (field->data_size)
+    {
+        case 4: value = *(const int32_t*)src; break;
+        case 8: value = *(const int64_t*)src; break;
+        default: PB_RETURN_ERROR(stream, "invalid data_size");
+    }
+    
+    return pb_encode_svarint(stream, value);
+}
+
+static bool checkreturn pb_enc_fixed64(pb_ostream_t *stream, const pb_field_t *field, const void *src)
+{
+    UNUSED(field);
+    return pb_encode_fixed64(stream, src);
+}
+
+static bool checkreturn pb_enc_fixed32(pb_ostream_t *stream, const pb_field_t *field, const void *src)
+{
+    UNUSED(field);
+    return pb_encode_fixed32(stream, src);
+}
+
+static bool checkreturn pb_enc_bytes(pb_ostream_t *stream, const pb_field_t *field, const void *src)
+{
+    const pb_bytes_array_t *bytes = (const pb_bytes_array_t*)src;
+    
+    if (src == NULL)
+    {
+        /* Threat null pointer as an empty bytes field */
+        return pb_encode_string(stream, NULL, 0);
+    }
+    
+    if (PB_ATYPE(field->type) == PB_ATYPE_STATIC &&
+        PB_BYTES_ARRAY_T_ALLOCSIZE(bytes->size) > field->data_size)
+    {
+        PB_RETURN_ERROR(stream, "bytes size exceeded");
+    }
+    
+    return pb_encode_string(stream, bytes->bytes, bytes->size);
+}
+
+static bool checkreturn pb_enc_string(pb_ostream_t *stream, const pb_field_t *field, const void *src)
+{
+    /* strnlen() is not always available, so just use a loop */
+    size_t size = 0;
+    size_t max_size = field->data_size;
+    const char *p = (const char*)src;
+    
+    if (PB_ATYPE(field->type) == PB_ATYPE_POINTER)
+        max_size = (size_t)-1;
+
+    if (src == NULL)
+    {
+        size = 0; /* Threat null pointer as an empty string */
+    }
+    else
+    {
+        while (size < max_size && *p != '\0')
+        {
+            size++;
+            p++;
+        }
+    }
+
+    return pb_encode_string(stream, (const uint8_t*)src, size);
+}
+
+static bool checkreturn pb_enc_submessage(pb_ostream_t *stream, const pb_field_t *field, const void *src)
+{
+    if (field->ptr == NULL)
+        PB_RETURN_ERROR(stream, "invalid field descriptor");
+    
+    return pb_encode_submessage(stream, (const pb_field_t*)field->ptr, src);
+}
+

src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_encode.h

@@ -0,0 +1,154 @@
+/* pb_encode.h: Functions to encode protocol buffers. Depends on pb_encode.c.
+ * The main function is pb_encode. You also need an output stream, and the
+ * field descriptions created by nanopb_generator.py.
+ */
+
+#ifndef _PB_ENCODE_H_
+#define _PB_ENCODE_H_
+
+#include "pb.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Structure for defining custom output streams. You will need to provide
+ * a callback function to write the bytes to your storage, which can be
+ * for example a file or a network socket.
+ *
+ * The callback must conform to these rules:
+ *
+ * 1) Return false on IO errors. This will cause encoding to abort.
+ * 2) You can use state to store your own data (e.g. buffer pointer).
+ * 3) pb_write will update bytes_written after your callback runs.
+ * 4) Substreams will modify max_size and bytes_written. Don't use them
+ *    to calculate any pointers.
+ */
+struct _pb_ostream_t
+{
+#ifdef PB_BUFFER_ONLY
+    /* Callback pointer is not used in buffer-only configuration.
+     * Having an int pointer here allows binary compatibility but
+     * gives an error if someone tries to assign callback function.
+     * Also, NULL pointer marks a 'sizing stream' that does not
+     * write anything.
+     */
+    int *callback;
+#else
+    bool (*callback)(pb_ostream_t *stream, const uint8_t *buf, size_t count);
+#endif
+    void *state;          /* Free field for use by callback implementation. */
+    size_t max_size;      /* Limit number of output bytes written (or use SIZE_MAX). */
+    size_t bytes_written; /* Number of bytes written so far. */
+    
+#ifndef PB_NO_ERRMSG
+    const char *errmsg;
+#endif
+};
+
+/***************************
+ * Main encoding functions *
+ ***************************/
+
+/* Encode a single protocol buffers message from C structure into a stream.
+ * Returns true on success, false on any failure.
+ * The actual struct pointed to by src_struct must match the description in fields.
+ * All required fields in the struct are assumed to have been filled in.
+ *
+ * Example usage:
+ *    MyMessage msg = {};
+ *    uint8_t buffer[64];
+ *    pb_ostream_t stream;
+ *
+ *    msg.field1 = 42;
+ *    stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
+ *    pb_encode(&stream, MyMessage_fields, &msg);
+ */
+bool pb_encode(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct);
+
+/* Same as pb_encode, but prepends the length of the message as a varint.
+ * Corresponds to writeDelimitedTo() in Google's protobuf API.
+ */
+bool pb_encode_delimited(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct);
+
+/* Encode the message to get the size of the encoded data, but do not store
+ * the data. */
+bool pb_get_encoded_size(size_t *size, const pb_field_t fields[], const void *src_struct);
+
+/**************************************
+ * Functions for manipulating streams *
+ **************************************/
+
+/* Create an output stream for writing into a memory buffer.
+ * The number of bytes written can be found in stream.bytes_written after
+ * encoding the message.
+ *
+ * Alternatively, you can use a custom stream that writes directly to e.g.
+ * a file or a network socket.
+ */
+pb_ostream_t pb_ostream_from_buffer(uint8_t *buf, size_t bufsize);
+
+/* Pseudo-stream for measuring the size of a message without actually storing
+ * the encoded data.
+ * 
+ * Example usage:
+ *    MyMessage msg = {};
+ *    pb_ostream_t stream = PB_OSTREAM_SIZING;
+ *    pb_encode(&stream, MyMessage_fields, &msg);
+ *    printf("Message size is %d\n", stream.bytes_written);
+ */
+#ifndef PB_NO_ERRMSG
+#define PB_OSTREAM_SIZING {0,0,0,0,0}
+#else
+#define PB_OSTREAM_SIZING {0,0,0,0}
+#endif
+
+/* Function to write into a pb_ostream_t stream. You can use this if you need
+ * to append or prepend some custom headers to the message.
+ */
+bool pb_write(pb_ostream_t *stream, const uint8_t *buf, size_t count);
+
+
+/************************************************
+ * Helper functions for writing field callbacks *
+ ************************************************/
+
+/* Encode field header based on type and field number defined in the field
+ * structure. Call this from the callback before writing out field contents. */
+bool pb_encode_tag_for_field(pb_ostream_t *stream, const pb_field_t *field);
+
+/* Encode field header by manually specifing wire type. You need to use this
+ * if you want to write out packed arrays from a callback field. */
+bool pb_encode_tag(pb_ostream_t *stream, pb_wire_type_t wiretype, uint32_t field_number);
+
+/* Encode an integer in the varint format.
+ * This works for bool, enum, int32, int64, uint32 and uint64 field types. */
+bool pb_encode_varint(pb_ostream_t *stream, uint64_t value);
+
+/* Encode an integer in the zig-zagged svarint format.
+ * This works for sint32 and sint64. */
+bool pb_encode_svarint(pb_ostream_t *stream, int64_t value);
+
+/* Encode a string or bytes type field. For strings, pass strlen(s) as size. */
+bool pb_encode_string(pb_ostream_t *stream, const uint8_t *buffer, size_t size);
+
+/* Encode a fixed32, sfixed32 or float value.
+ * You need to pass a pointer to a 4-byte wide C variable. */
+bool pb_encode_fixed32(pb_ostream_t *stream, const void *value);
+
+/* Encode a fixed64, sfixed64 or double value.
+ * You need to pass a pointer to a 8-byte wide C variable. */
+bool pb_encode_fixed64(pb_ostream_t *stream, const void *value);
+
+/* Encode a submessage field.
+ * You need to pass the pb_field_t array and pointer to struct, just like
+ * with pb_encode(). This internally encodes the submessage twice, first to
+ * calculate message size and then to actually write it out.
+ */
+bool pb_encode_submessage(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif

src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_syshdr.h

@@ -0,0 +1,127 @@
+/* This is an example of a header file for platforms/compilers that do
+ * not come with stdint.h/stddef.h/stdbool.h/string.h. To use it, define
+ * PB_SYSTEM_HEADER as "pb_syshdr.h", including the quotes, and add the
+ * extra folder to your include path.
+ *
+ * It is very likely that you will need to customize this file to suit
+ * your platform. For any compiler that supports C99, this file should
+ * not be necessary.
+ * 
+ * KUKA: Added VXWORKS support
+ */
+
+#ifndef _PB_SYSHDR_H_
+#define _PB_SYSHDR_H_
+
+/* KUKA VxWorks 6.8 support */
+#ifdef VXWORKS
+   #define HAVE_STRING_H
+   #define HAVE_STDLIB_H
+#ifdef _WRS_KERNEL
+   #include <types/vxTypes.h> // int32_t, int64_t, ...
+   #define HAVE_STDINT_H_ALTERNATIVE
+   #define HAVE_STDDEF_H_ALTERNATIVE
+#else
+   #define HAVE_STDINT_H
+   #define HAVE_STDDEF_H
+#endif // _WRS_KERNEL
+#endif // VXWORKS
+
+/* KUKA: size_t is defined in stddef.h, stdlib.h or string.h */
+#if !defined(HAVE_STDDEF_H) && !defined(HAVE_STDLIB_H) && !defined(HAVE_STRING_H)
+typedef uint32_t size_t;
+#endif
+
+
+
+/* stdint.h subset */
+#ifdef HAVE_STDINT_H
+#include <stdint.h>
+#else
+#ifndef HAVE_STDINT_H_ALTERNATIVE
+/* You will need to modify these to match the word size of your platform. */
+typedef signed char int8_t;
+typedef unsigned char uint8_t;
+typedef signed short int16_t;
+typedef unsigned short uint16_t;
+typedef signed int int32_t;
+typedef unsigned int uint32_t;
+typedef signed long long int64_t;
+typedef unsigned long long uint64_t;
+#endif // HAVE_STDINT_H_ALTERNATIVE
+#endif // HAVE_STDINT_H
+
+/* stddef.h subset */
+#ifdef HAVE_STDDEF_H
+#include <stddef.h>
+#else
+#ifndef HAVE_STDDEF_H_ALTERNATIVE
+#define offsetof(st, m) ((size_t)(&((st *)0)->m))
+#ifndef NULL
+#define NULL 0
+#endif // NULL
+#endif // HAVE_STDDEF_H_ALTERNATIVE
+#endif // HAVE_STDDEF_H
+
+/* stdbool.h subset */
+#ifdef HAVE_STDBOOL_H
+#include <stdbool.h>
+#else
+
+#ifndef __cplusplus
+typedef int bool;
+#define false 0
+#define true 1
+#endif
+   
+#endif // HAVE_STDBOOL_H
+
+/* stdlib.h subset */
+#ifdef PB_ENABLE_MALLOC
+#ifdef HAVE_STDLIB_H
+#include <stdlib.h>
+#else
+void *realloc(void *ptr, size_t size);
+void free(void *ptr);
+#endif // HAVE_STDLIB_H
+#endif // PB_ENABLE_MALLOC
+
+/* string.h subset */
+#ifdef HAVE_STRING_H
+#include <string.h>
+#else
+
+/* Implementations are from the Public Domain C Library (PDCLib). */
+static size_t strlen( const char * s )
+{
+    size_t rc = 0;
+    while ( s[rc] )
+    {
+        ++rc;
+    }
+    return rc;
+}
+
+static void * memcpy( void *s1, const void *s2, size_t n )
+{
+    char * dest = (char *) s1;
+    const char * src = (const char *) s2;
+    while ( n-- )
+    {
+        *dest++ = *src++;
+    }
+    return s1;
+}
+
+static void * memset( void * s, int c, size_t n )
+{
+    unsigned char * p = (unsigned char *) s;
+    while ( n-- )
+    {
+        *p++ = (unsigned char) c;
+    }
+    return s;
+}
+#endif // HAVE_STRING_H
+
+#endif // _PB_SYSHDR_H_

src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_syshdr_win.h

@@ -0,0 +1,119 @@
+/* This is a headerfile customized for Microsoft Visual Studio 2010 
+ * - based on example of a header file for platforms/compilers that do
+ * not come with stdint.h/stddef.h/stdbool.h/string.h. To use it, define
+ * PB_SYSTEM_HEADER as "pb_syshdr_win.h", including the quotes, and add the
+ * extra folder to your include path.
+ *
+ *  Authorship: This file was altered/created by KUKA Deutschland GmbH, Augsburg, Germany in 2014 
+ *  
+ */
+
+#ifndef _PB_SYSHDR_WIN_H_
+#define _PB_SYSHDR_WIN_H_
+
+/* stdint.h subset */
+#ifdef HAVE_STDINT_H
+#include <stdint.h>
+#else
+/* You will need to modify these to match the word size of your platform. */
+typedef signed char int8_t;
+typedef unsigned char uint8_t;
+typedef signed short int16_t;
+typedef unsigned short uint16_t;
+typedef signed int int32_t;
+typedef unsigned int uint32_t;
+typedef signed long long int64_t;
+typedef unsigned long long uint64_t;
+
+// from stdint.h
+#define INT8_MAX	0x7f
+#define INT16_MAX	0x7fff
+#define INT32_MAX	0x7fffffff
+#define UINT8_MAX	0xffU
+#define UINT16_MAX	0xffffU
+#define UINT32_MAX	0xffffffffU
+#endif
+
+/* stddef.h subset */
+#ifdef HAVE_STDDEF_H
+#include <stddef.h>
+#else
+
+//typedef uint32_t size_t;  // wird dieser typedef wirklich benötigt?!?
+#ifndef offsetof
+#define offsetof(st, m) ((size_t)(&((st *)0)->m))
+#endif
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+#endif
+
+/* stdbool.h subset */
+#ifdef HAVE_STDBOOL_H
+#include <stdbool.h>
+#else
+
+#ifndef __cplusplus
+typedef int bool;
+#define false 0
+#define true 1
+#endif
+
+#endif
+
+/* stdlib.h subset */
+#ifdef PB_ENABLE_MALLOC
+#ifdef HAVE_STDLIB_H
+#include <stdlib.h>
+#else
+void *realloc(void *ptr, size_t size);
+void free(void *ptr);
+#endif
+#endif
+
+/* string.h subset */
+#ifdef HAVE_STRING_H
+#include <string.h>
+#else
+
+#ifndef WIN32
+/* Implementations are from the Public Domain C Library (PDCLib). */
+static size_t strlen( const char * s )
+{
+    size_t rc = 0;
+    while ( s[rc] )
+    {
+        ++rc;
+    }
+    return rc;
+}
+
+static void * memcpy( void *s1, const void *s2, size_t n )
+{
+    char * dest = (char *) s1;
+    const char * src = (const char *) s2;
+    while ( n-- )
+    {
+        *dest++ = *src++;
+    }
+    return s1;
+}
+
+static void * memset( void * s, int c, size_t n )
+{
+    unsigned char * p = (unsigned char *) s;
+    while ( n-- )
+    {
+        *p++ = (unsigned char) c;
+    }
+    return s;
+}
+#else
+#include <string.h>
+#endif
+
+#endif
+
+#endif

src/iiwa_controller/external/libFRI/src/protobuf/Makefile

@@ -0,0 +1,19 @@
+BASE_DIR	= ../..
+include $(BASE_DIR)/build/GNUMake/paths.mak
+include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK)
+
+CC_SRC 		 = pb_frimessages_callbacks.c
+
+CXX_SRC		 = friMonitoringMessageDecoder.cpp \
+					friCommandMessageEncoder.cpp
+
+INC_DIR		+= $(NANOPB_DIR) $(PROTOBUF_GEN_DIR)
+CFLAGS 		+= 
+CXXFLAGS		+= 
+LDFLAGS 		+= 
+
+################################################################################
+### Include general makefile (at the end)
+################################################################################
+
+include $(BASE_DIR)/build/GNUMake/rules.mak

src/iiwa_controller/external/libFRI/src/protobuf/friCommandMessageEncoder.cpp

@@ -0,0 +1,121 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#include <cstdio>
+#include "friCommandMessageEncoder.h"
+#include "pb_encode.h"
+
+using namespace KUKA::FRI;
+
+//******************************************************************************
+CommandMessageEncoder::CommandMessageEncoder(FRICommandMessage* pMessage, int num)
+   : m_nNum(num), m_pMessage(pMessage)
+{
+   initMessage();
+}
+
+//******************************************************************************
+CommandMessageEncoder::~CommandMessageEncoder()
+{
+
+}
+
+//******************************************************************************
+void CommandMessageEncoder::initMessage()
+{
+   m_pMessage->has_commandData = false;
+   m_pMessage->has_endOfMessageData = false;
+   m_pMessage->commandData.has_jointPosition = false;
+   m_pMessage->commandData.has_cartesianWrenchFeedForward = false;
+   m_pMessage->commandData.has_jointTorque = false;
+   m_pMessage->commandData.commandedTransformations_count = 0;
+   m_pMessage->header.messageIdentifier = 0;
+   // init with 0. Necessary for creating the correct reflected sequence count in the monitoring msg
+   m_pMessage->header.sequenceCounter = 0;
+   m_pMessage->header.reflectedSequenceCounter = 0;
+
+   m_pMessage->commandData.writeIORequest_count = 0;
+
+   // allocate and map memory for protobuf repeated structures
+   map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, 
+         &m_pMessage->commandData.jointPosition.value,
+         &m_tRecvContainer.jointPosition);
+   map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, 
+         &m_pMessage->commandData.jointTorque.value,
+         &m_tRecvContainer.jointTorque);
+   
+   // nanopb encoding needs to know how many elements the static array contains
+   // a Cartesian wrench feed forward vector always contains 6 elements
+   m_pMessage->commandData.cartesianWrenchFeedForward.element_count = 6;
+}
+
+//******************************************************************************
+bool CommandMessageEncoder::encode(char* buffer, int& size)
+{
+    // generate stream for encoding
+    pb_ostream_t stream = pb_ostream_from_buffer((uint8_t*)buffer, FRI_COMMAND_MSG_MAX_SIZE);
+    // encode monitoring Message to stream
+    bool status = pb_encode(&stream, FRICommandMessage_fields, m_pMessage);
+    size = stream.bytes_written;
+    if (!status)
+    {
+        printf("!!encoding error: %s!!\n", PB_GET_ERROR(&stream));
+    }
+    return status;
+}

src/iiwa_controller/external/libFRI/src/protobuf/friCommandMessageEncoder.h

@@ -0,0 +1,118 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#ifndef _KUKA_FRI_COMMANDMESSAGEENCODER_H
+#define _KUKA_FRI_COMMANDMESSAGEENCODER_H
+
+
+#include "FRIMessages.pb.h"
+#include "pb_frimessages_callbacks.h"
+
+
+
+namespace KUKA
+{
+namespace FRI
+{
+
+   static const int FRI_COMMAND_MSG_MAX_SIZE = 1500;   //!< max size of a FRI command message
+
+   class CommandMessageEncoder
+   {
+      
+   public:
+      
+      CommandMessageEncoder(FRICommandMessage* pMessage, int num);
+      
+      ~CommandMessageEncoder();
+      
+      bool encode(char* buffer, int& size);
+      
+   private:
+      
+      struct LocalCommandDataContainer
+      {
+         tRepeatedDoubleArguments jointPosition;
+         tRepeatedDoubleArguments jointTorque;
+         
+         LocalCommandDataContainer()
+         {
+            init_repeatedDouble(&jointPosition);
+            init_repeatedDouble(&jointTorque);
+         }
+         
+         ~LocalCommandDataContainer()
+         {
+            free_repeatedDouble(&jointPosition);
+            free_repeatedDouble(&jointTorque);
+         }
+      };
+      
+      int m_nNum;
+      
+      LocalCommandDataContainer m_tRecvContainer;
+      FRICommandMessage* m_pMessage;
+      
+      void initMessage();
+   };
+   
+}
+}
+
+#endif // _KUKA_FRI_COMMANDMESSAGEENCODER_H

src/iiwa_controller/external/libFRI/src/protobuf/friMonitoringMessageDecoder.cpp

@@ -0,0 +1,145 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#include <cstdio>
+#include "friMonitoringMessageDecoder.h"
+#include "pb_decode.h"
+
+
+using namespace KUKA::FRI;
+
+//******************************************************************************
+MonitoringMessageDecoder::MonitoringMessageDecoder(FRIMonitoringMessage* pMessage, int num)
+   : m_nNum(num), m_pMessage(pMessage)
+{
+   initMessage();
+}
+
+//******************************************************************************
+MonitoringMessageDecoder::~MonitoringMessageDecoder()
+{
+
+}
+
+//******************************************************************************
+void MonitoringMessageDecoder::initMessage()
+{
+   // set initial data
+   // it is assumed that no robot information and monitoring data is available and therefore the 
+   // optional fields are initialized with false
+   m_pMessage->has_robotInfo = false;
+   m_pMessage->has_monitorData = false;
+   m_pMessage->has_connectionInfo = true;
+   m_pMessage->has_ipoData = false;
+   m_pMessage->requestedTransformations_count = 0;
+   m_pMessage->has_endOfMessageData = false;
+   
+   
+   m_pMessage->header.messageIdentifier = 0;
+   m_pMessage->header.reflectedSequenceCounter = 0;
+   m_pMessage->header.sequenceCounter = 0;
+
+   m_pMessage->connectionInfo.sessionState = FRISessionState_IDLE;
+   m_pMessage->connectionInfo.quality = FRIConnectionQuality_POOR;
+
+   m_pMessage->monitorData.readIORequest_count = 0;
+
+   // allocate and map memory for protobuf repeated structures
+   map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, 
+         &m_pMessage->monitorData.measuredJointPosition.value,
+         &m_tSendContainer.m_AxQMsrLocal);
+   
+   map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, 
+         &m_pMessage->monitorData.measuredTorque.value,
+         &m_tSendContainer.m_AxTauMsrLocal);
+   
+   map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, 
+         &m_pMessage->monitorData.commandedJointPosition.value,
+         &m_tSendContainer.m_AxQCmdT1mLocal);
+   
+   map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, 
+         &m_pMessage->monitorData.commandedTorque.value,
+         &m_tSendContainer.m_AxTauCmdLocal);
+   
+   map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, 
+         &m_pMessage->monitorData.externalTorque.value,
+         &m_tSendContainer.m_AxTauExtMsrLocal);
+   
+   map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE,m_nNum,
+         &m_pMessage->ipoData.jointPosition.value,
+         &m_tSendContainer.m_AxQCmdIPO);
+
+   map_repeatedInt(FRI_MANAGER_NANOPB_DECODE, m_nNum, 
+         &m_pMessage->robotInfo.driveState,
+         &m_tSendContainer.m_AxDriveStateLocal);
+}
+
+//******************************************************************************
+bool MonitoringMessageDecoder::decode(char* buffer, int size)
+{
+    pb_istream_t stream = pb_istream_from_buffer((uint8_t*)buffer, size);
+
+    bool status = pb_decode(&stream, FRIMonitoringMessage_fields, m_pMessage);
+    if (!status)
+    {
+        printf("!!decoding error: %s!!\n", PB_GET_ERROR(&stream));
+    }
+
+    return status;
+}

src/iiwa_controller/external/libFRI/src/protobuf/friMonitoringMessageDecoder.h

@@ -0,0 +1,133 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#ifndef _KUKA_FRI_MONITORINGMESSAGEDECODER_H
+#define _KUKA_FRI_MONITORINGMESSAGEDECODER_H
+
+#include "FRIMessages.pb.h"
+#include "pb_frimessages_callbacks.h"
+
+
+namespace KUKA
+{
+namespace FRI
+{
+
+   static const int FRI_MONITOR_MSG_MAX_SIZE = 1500;   //!< max size of a FRI monitoring message
+   
+
+   class MonitoringMessageDecoder
+   {
+
+   public:
+
+      MonitoringMessageDecoder(FRIMonitoringMessage* pMessage, int num);
+    
+      ~MonitoringMessageDecoder();
+    
+      bool decode(char* buffer, int size);
+    
+    
+   private:
+    
+      struct LocalMonitoringDataContainer
+      {
+         tRepeatedDoubleArguments m_AxQMsrLocal;
+         tRepeatedDoubleArguments m_AxTauMsrLocal;
+         tRepeatedDoubleArguments m_AxQCmdT1mLocal;
+         tRepeatedDoubleArguments m_AxTauCmdLocal;
+         tRepeatedDoubleArguments m_AxTauExtMsrLocal;
+         tRepeatedIntArguments m_AxDriveStateLocal;        
+         tRepeatedDoubleArguments m_AxQCmdIPO;
+        
+         LocalMonitoringDataContainer()
+         {
+            init_repeatedDouble(&m_AxQMsrLocal);
+            init_repeatedDouble(&m_AxTauMsrLocal);
+            init_repeatedDouble(&m_AxQCmdT1mLocal);
+            init_repeatedDouble(&m_AxTauCmdLocal);
+            init_repeatedDouble(&m_AxTauExtMsrLocal);
+            init_repeatedDouble(&m_AxQCmdIPO);
+            init_repeatedInt(&m_AxDriveStateLocal);
+         }
+         
+         ~LocalMonitoringDataContainer()
+         {
+            free_repeatedDouble(&m_AxQMsrLocal);
+            free_repeatedDouble(&m_AxTauMsrLocal);
+            free_repeatedDouble(&m_AxQCmdT1mLocal);
+            free_repeatedDouble(&m_AxTauCmdLocal);
+            free_repeatedDouble(&m_AxTauExtMsrLocal);
+            free_repeatedDouble(&m_AxQCmdIPO);
+            free_repeatedInt(&m_AxDriveStateLocal);
+         }
+      };
+
+      int m_nNum;
+
+      LocalMonitoringDataContainer m_tSendContainer;
+      FRIMonitoringMessage* m_pMessage;
+       
+      void initMessage();
+   };
+
+}
+}
+   
+#endif // _KUKA_FRI_MONITORINGMESSAGEDECODER_H

src/iiwa_controller/external/libFRI/src/protobuf/pb_frimessages_callbacks.c

@@ -0,0 +1,267 @@
+/**
+
+ The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+
+ \file
+ \version {1.16}
+ */
+#include <stdio.h>
+#include <stdlib.h> 
+
+#include "pb_frimessages_callbacks.h"
+#include "pb_encode.h"
+#include "pb_decode.h"
+
+bool encode_repeatedDouble(pb_ostream_t *stream, const pb_field_t *field, void * const *arg)
+{
+   size_t i = 0;
+
+   tRepeatedDoubleArguments* arguments = 0;
+   size_t count = 0;
+   double* values = 0;
+
+   if (arg == NULL || *arg == NULL)
+   {
+      return false;
+   }
+   arguments = ((tRepeatedDoubleArguments*) (*arg));
+
+   count = arguments->max_size;
+   values = arguments->value;
+
+   for (i = 0; i < count; i++)
+   {
+
+      if (!pb_encode_tag_for_field(stream, field))
+      {
+         return false;
+      }
+
+      if (!pb_encode_fixed64(stream, &values[i]))
+      {
+         return false;
+      }
+   }
+   return true;
+}
+
+bool decode_repeatedDouble(pb_istream_t *stream, const pb_field_t *field, void **arg)
+{
+   tRepeatedDoubleArguments* arguments = 0;
+   size_t i = 0;
+   double* values = 0;
+
+   if (arg == NULL || *arg == NULL)
+   {
+      return false;
+   }
+
+   arguments = (tRepeatedDoubleArguments*) *arg;
+   i = arguments->size;
+   values = arguments->value;
+
+   if (values == NULL)
+   {
+      return true;
+   }
+
+   if (!pb_decode_fixed64(stream, &values[i]))
+   {
+      return false;
+   }
+
+   arguments->size++;
+   if (arguments->size >= arguments->max_size)
+   {
+      arguments->size = 0;
+   }
+   return true;
+}
+
+bool encode_repeatedInt(pb_ostream_t *stream, const pb_field_t *field, void * const *arg)
+{
+   int i = 0;
+   tRepeatedIntArguments* arguments = 0;
+   int count = 0;
+   int64_t* values = 0;
+
+   if (arg == NULL || *arg == NULL)
+   {
+      return false;
+   }
+   arguments = (tRepeatedIntArguments*) *arg;
+   count = arguments->max_size;
+   values = arguments->value;
+   for (i = 0; i < count; i++)
+   {
+
+      if (!pb_encode_tag_for_field(stream, field))
+      {
+         return false;
+      }
+      if (!pb_encode_varint(stream, values[i]))
+      {
+         return false;
+      }
+   }
+   return true;
+}
+
+bool decode_repeatedInt(pb_istream_t *stream, const pb_field_t *field, void **arg)
+{
+   tRepeatedIntArguments* arguments = 0;
+   size_t i = 0;
+   uint64_t* values = 0;
+
+   if (arg == NULL || *arg == NULL)
+   {
+      return false;
+   }
+   arguments = (tRepeatedIntArguments*) *arg;
+
+   i = arguments->size;
+   values = (uint64_t*) arguments->value;
+   if (values == NULL)
+   {
+      return true;
+   }
+
+   if (!pb_decode_varint(stream, &values[i]))
+   {
+      return false;
+   }
+
+   arguments->size++;
+   if (arguments->size >= arguments->max_size)
+   {
+      arguments->size = 0;
+   }
+   return true;
+}
+
+void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedDoubleArguments *arg)
+{
+   // IMPORTANT: the callbacks are stored in a union, therefor a message object
+   // must be exclusive defined for transmission or reception
+   if (dir == FRI_MANAGER_NANOPB_ENCODE)
+   {
+      values->funcs.encode = &encode_repeatedDouble;
+   }
+   else
+   {
+      values->funcs.decode = &decode_repeatedDouble;
+   }
+   // map the local container data to the message data fields
+   arg->max_size = numDOF;
+   arg->size = 0;
+   if (numDOF > 0)
+   {
+      arg->value = (double*) malloc(numDOF * sizeof(double));
+
+   }
+   values->arg = arg;
+}
+
+void map_repeatedInt(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedIntArguments *arg)
+{
+   // IMPORTANT: the callbacks are stored in a union, therefor a message object
+   // must be exclusive defined for transmission or reception
+   if (dir == FRI_MANAGER_NANOPB_ENCODE)
+   {
+      // set the encode callback function
+      values->funcs.encode = &encode_repeatedInt;
+   }
+   else
+   {
+      // set the decode callback function
+      values->funcs.decode = &decode_repeatedInt;
+   }
+   // map the robot drive state from the container to message field
+   arg->max_size = numDOF;
+   arg->size = 0;
+   if (numDOF > 0)
+   {
+      arg->value = (int64_t*) malloc(numDOF * sizeof(int64_t));
+
+   }
+   values->arg = arg;
+}
+
+void init_repeatedDouble(tRepeatedDoubleArguments *arg)
+{
+   arg->size = 0;
+   arg->max_size = 0;
+   arg->value = NULL;
+}
+
+void init_repeatedInt(tRepeatedIntArguments *arg)
+{
+   arg->size = 0;
+   arg->max_size = 0;
+   arg->value = NULL;
+}
+
+void free_repeatedDouble(tRepeatedDoubleArguments *arg)
+{
+   if (arg->value != NULL)
+      free(arg->value);
+}
+
+void free_repeatedInt(tRepeatedIntArguments *arg)
+{
+   if (arg->value != NULL)
+      free(arg->value);
+}

src/iiwa_controller/external/libFRI/src/protobuf/pb_frimessages_callbacks.h

@@ -0,0 +1,121 @@
+/**
+
+The following license terms and conditions apply, unless a redistribution
+agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany.
+
+SCOPE
+
+The software “KUKA Sunrise.Connectivity FRI Client SDK” is targeted to work in
+conjunction with the “KUKA Sunrise.Connectivity FastRobotInterface” toolkit.
+In the following, the term “software” refers to all material directly
+belonging to the provided SDK “Software development kit”, particularly source
+code, libraries, binaries, manuals and technical documentation.
+
+COPYRIGHT
+
+All Rights Reserved
+Copyright (C)  2014-2018 
+KUKA Deutschland GmbH
+Augsburg, Germany
+
+LICENSE 
+
+Redistribution and use of the software in source and binary forms, with or
+without modification, are permitted provided that the following conditions are
+met:
+a) The software is used in conjunction with KUKA products only. 
+b) Redistributions of source code must retain the above copyright notice, this
+list of conditions and the disclaimer.
+c) Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the disclaimer in the documentation and/or other
+materials provided with the distribution. Altered source code of the
+redistribution must be made available upon request with the distribution.
+d) Modification and contributions to the original software provided by KUKA
+must be clearly marked and the authorship must be stated.
+e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+DISCLAIMER OF WARRANTY
+
+The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of
+any kind, including without limitation the warranties of merchantability,
+fitness for a particular purpose and non-infringement. 
+KUKA makes no warranty that the Software is free of defects or is suitable for
+any particular purpose. In no event shall KUKA be responsible for loss or
+damages arising from the installation or use of the Software, including but
+not limited to any indirect, punitive, special, incidental or consequential
+damages of any character including, without limitation, damages for loss of
+goodwill, work stoppage, computer failure or malfunction, or any and all other
+commercial damages or losses. 
+The entire risk to the quality and performance of the Software is not borne by
+KUKA. Should the Software prove defective, KUKA is not liable for the entire
+cost of any service and repair.
+
+
+
+\file
+\version {1.16}
+*/
+#ifndef _pb_frimessages_callbacks_H
+#define _pb_frimessages_callbacks_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "pb.h"
+#include "FRIMessages.pb.h"
+
+/** container for repeated double elements */
+typedef struct repeatedDoubleArguments {
+   size_t size;
+   size_t max_size;
+   double* value;
+} tRepeatedDoubleArguments;
+
+/** container for repeated integer elements */
+typedef struct repeatedIntArguments {
+   size_t size;
+   size_t max_size;
+   int64_t* value;
+} tRepeatedIntArguments;
+
+/** enumeration for direction (encoding/decoding) */
+typedef enum DIRECTION {
+   FRI_MANAGER_NANOPB_DECODE = 0, //!< Argument um eine 
+   FRI_MANAGER_NANOPB_ENCODE = 1  //!< 
+} eNanopbCallbackDirection;
+
+
+bool encode_repeatedDouble(pb_ostream_t *stream, const pb_field_t *field,
+      void * const *arg);
+
+bool decode_repeatedDouble(pb_istream_t *stream, const pb_field_t *field,
+      void **arg);
+
+bool encode_repeatedInt(pb_ostream_t *stream, const pb_field_t *field,
+      void * const *arg);
+
+bool decode_repeatedInt(pb_istream_t *stream, const pb_field_t *field,
+      void **arg);
+
+void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF,
+      pb_callback_t *values, tRepeatedDoubleArguments *arg);
+
+void map_repeatedInt(eNanopbCallbackDirection dir, int numDOF,
+      pb_callback_t *values, tRepeatedIntArguments *arg);
+
+void init_repeatedDouble(tRepeatedDoubleArguments *arg);
+
+void init_repeatedInt(tRepeatedIntArguments *arg);
+
+void free_repeatedDouble(tRepeatedDoubleArguments *arg);
+
+void free_repeatedInt(tRepeatedIntArguments *arg);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif

src/iiwa_controller/external/libFRI/src/protobuf_gen/FRIMessages.pb.c

@@ -0,0 +1,151 @@
+/* Automatically generated nanopb constant definitions */
+/* Generated by nanopb-0.2.8 at Tue Feb 14 12:42:03 2017. */
+
+#include "FRIMessages.pb.h"
+
+
+
+const pb_field_t JointValues_fields[2] = {
+    PB_FIELD2(  1, DOUBLE  , REPEATED, CALLBACK, FIRST, JointValues, value, value, 0),
+    PB_LAST_FIELD
+};
+
+const pb_field_t TimeStamp_fields[3] = {
+    PB_FIELD2(  1, UINT32  , REQUIRED, STATIC  , FIRST, TimeStamp, sec, sec, 0),
+    PB_FIELD2(  2, UINT32  , REQUIRED, STATIC  , OTHER, TimeStamp, nanosec, sec, 0),
+    PB_LAST_FIELD
+};
+
+const pb_field_t CartesianVector_fields[2] = {
+    PB_FIELD2(  1, DOUBLE  , REPEATED, STATIC  , FIRST, CartesianVector, element, element, 0),
+    PB_LAST_FIELD
+};
+
+const pb_field_t Checksum_fields[2] = {
+    PB_FIELD2(  1, INT32   , OPTIONAL, STATIC  , FIRST, Checksum, crc32, crc32, 0),
+    PB_LAST_FIELD
+};
+
+const pb_field_t Transformation_fields[4] = {
+    PB_FIELD2(  1, STRING  , REQUIRED, STATIC  , FIRST, Transformation, name, name, 0),
+    PB_FIELD2(  2, DOUBLE  , REPEATED, STATIC  , OTHER, Transformation, matrix, name, 0),
+    PB_FIELD2(  3, MESSAGE , OPTIONAL, STATIC  , OTHER, Transformation, timestamp, matrix, &TimeStamp_fields),
+    PB_LAST_FIELD
+};
+
+const pb_field_t FriIOValue_fields[6] = {
+    PB_FIELD2(  1, STRING  , REQUIRED, STATIC  , FIRST, FriIOValue, name, name, 0),
+    PB_FIELD2(  2, ENUM    , REQUIRED, STATIC  , OTHER, FriIOValue, type, name, 0),
+    PB_FIELD2(  3, ENUM    , REQUIRED, STATIC  , OTHER, FriIOValue, direction, type, 0),
+    PB_FIELD2(  4, UINT64  , OPTIONAL, STATIC  , OTHER, FriIOValue, digitalValue, direction, 0),
+    PB_FIELD2(  5, DOUBLE  , OPTIONAL, STATIC  , OTHER, FriIOValue, analogValue, digitalValue, 0),
+    PB_LAST_FIELD
+};
+
+const pb_field_t MessageHeader_fields[4] = {
+    PB_FIELD2(  1, UINT32  , REQUIRED, STATIC  , FIRST, MessageHeader, messageIdentifier, messageIdentifier, 0),
+    PB_FIELD2(  2, UINT32  , REQUIRED, STATIC  , OTHER, MessageHeader, sequenceCounter, messageIdentifier, 0),
+    PB_FIELD2(  3, UINT32  , REQUIRED, STATIC  , OTHER, MessageHeader, reflectedSequenceCounter, sequenceCounter, 0),
+    PB_LAST_FIELD
+};
+
+const pb_field_t ConnectionInfo_fields[5] = {
+    PB_FIELD2(  1, ENUM    , REQUIRED, STATIC  , FIRST, ConnectionInfo, sessionState, sessionState, 0),
+    PB_FIELD2(  2, ENUM    , REQUIRED, STATIC  , OTHER, ConnectionInfo, quality, sessionState, 0),
+    PB_FIELD2(  3, UINT32  , OPTIONAL, STATIC  , OTHER, ConnectionInfo, sendPeriod, quality, 0),
+    PB_FIELD2(  4, UINT32  , OPTIONAL, STATIC  , OTHER, ConnectionInfo, receiveMultiplier, sendPeriod, 0),
+    PB_LAST_FIELD
+};
+
+const pb_field_t RobotInfo_fields[6] = {
+    PB_FIELD2(  1, INT32   , OPTIONAL, STATIC  , FIRST, RobotInfo, numberOfJoints, numberOfJoints, 0),
+    PB_FIELD2(  2, ENUM    , OPTIONAL, STATIC  , OTHER, RobotInfo, safetyState, numberOfJoints, 0),
+    PB_FIELD2(  5, ENUM    , REPEATED, CALLBACK, OTHER, RobotInfo, driveState, safetyState, 0),
+    PB_FIELD2(  6, ENUM    , OPTIONAL, STATIC  , OTHER, RobotInfo, operationMode, driveState, 0),
+    PB_FIELD2(  7, ENUM    , OPTIONAL, STATIC  , OTHER, RobotInfo, controlMode, operationMode, 0),
+    PB_LAST_FIELD
+};
+
+const pb_field_t MessageMonitorData_fields[8] = {
+    PB_FIELD2(  1, MESSAGE , OPTIONAL, STATIC  , FIRST, MessageMonitorData, measuredJointPosition, measuredJointPosition, &JointValues_fields),
+    PB_FIELD2(  2, MESSAGE , OPTIONAL, STATIC  , OTHER, MessageMonitorData, measuredTorque, measuredJointPosition, &JointValues_fields),
+    PB_FIELD2(  3, MESSAGE , OPTIONAL, STATIC  , OTHER, MessageMonitorData, commandedJointPosition, measuredTorque, &JointValues_fields),
+    PB_FIELD2(  4, MESSAGE , OPTIONAL, STATIC  , OTHER, MessageMonitorData, commandedTorque, commandedJointPosition, &JointValues_fields),
+    PB_FIELD2(  5, MESSAGE , OPTIONAL, STATIC  , OTHER, MessageMonitorData, externalTorque, commandedTorque, &JointValues_fields),
+    PB_FIELD2(  8, MESSAGE , REPEATED, STATIC  , OTHER, MessageMonitorData, readIORequest, externalTorque, &FriIOValue_fields),
+    PB_FIELD2( 15, MESSAGE , OPTIONAL, STATIC  , OTHER, MessageMonitorData, timestamp, readIORequest, &TimeStamp_fields),
+    PB_LAST_FIELD
+};
+
+const pb_field_t MessageIpoData_fields[5] = {
+    PB_FIELD2(  1, MESSAGE , OPTIONAL, STATIC  , FIRST, MessageIpoData, jointPosition, jointPosition, &JointValues_fields),
+    PB_FIELD2( 10, ENUM    , OPTIONAL, STATIC  , OTHER, MessageIpoData, clientCommandMode, jointPosition, 0),
+    PB_FIELD2( 11, ENUM    , OPTIONAL, STATIC  , OTHER, MessageIpoData, overlayType, clientCommandMode, 0),
+    PB_FIELD2( 12, DOUBLE  , OPTIONAL, STATIC  , OTHER, MessageIpoData, trackingPerformance, overlayType, 0),
+    PB_LAST_FIELD
+};
+
+const pb_field_t MessageCommandData_fields[6] = {
+    PB_FIELD2(  1, MESSAGE , OPTIONAL, STATIC  , FIRST, MessageCommandData, jointPosition, jointPosition, &JointValues_fields),
+    PB_FIELD2(  2, MESSAGE , OPTIONAL, STATIC  , OTHER, MessageCommandData, cartesianWrenchFeedForward, jointPosition, &CartesianVector_fields),
+    PB_FIELD2(  3, MESSAGE , OPTIONAL, STATIC  , OTHER, MessageCommandData, jointTorque, cartesianWrenchFeedForward, &JointValues_fields),
+    PB_FIELD2(  4, MESSAGE , REPEATED, STATIC  , OTHER, MessageCommandData, commandedTransformations, jointTorque, &Transformation_fields),
+    PB_FIELD2(  5, MESSAGE , REPEATED, STATIC  , OTHER, MessageCommandData, writeIORequest, commandedTransformations, &FriIOValue_fields),
+    PB_LAST_FIELD
+};
+
+const pb_field_t MessageEndOf_fields[3] = {
+    PB_FIELD2(  1, INT32   , OPTIONAL, STATIC  , FIRST, MessageEndOf, messageLength, messageLength, 0),
+    PB_FIELD2(  2, MESSAGE , OPTIONAL, STATIC  , OTHER, MessageEndOf, messageChecksum, messageLength, &Checksum_fields),
+    PB_LAST_FIELD
+};
+
+const pb_field_t FRIMonitoringMessage_fields[8] = {
+    PB_FIELD2(  1, MESSAGE , REQUIRED, STATIC  , FIRST, FRIMonitoringMessage, header, header, &MessageHeader_fields),
+    PB_FIELD2(  2, MESSAGE , OPTIONAL, STATIC  , OTHER, FRIMonitoringMessage, robotInfo, header, &RobotInfo_fields),
+    PB_FIELD2(  3, MESSAGE , OPTIONAL, STATIC  , OTHER, FRIMonitoringMessage, monitorData, robotInfo, &MessageMonitorData_fields),
+    PB_FIELD2(  4, MESSAGE , OPTIONAL, STATIC  , OTHER, FRIMonitoringMessage, connectionInfo, monitorData, &ConnectionInfo_fields),
+    PB_FIELD2(  5, MESSAGE , OPTIONAL, STATIC  , OTHER, FRIMonitoringMessage, ipoData, connectionInfo, &MessageIpoData_fields),
+    PB_FIELD2(  6, MESSAGE , REPEATED, STATIC  , OTHER, FRIMonitoringMessage, requestedTransformations, ipoData, &Transformation_fields),
+    PB_FIELD2( 15, MESSAGE , OPTIONAL, STATIC  , OTHER, FRIMonitoringMessage, endOfMessageData, requestedTransformations, &MessageEndOf_fields),
+    PB_LAST_FIELD
+};
+
+const pb_field_t FRICommandMessage_fields[4] = {
+    PB_FIELD2(  1, MESSAGE , REQUIRED, STATIC  , FIRST, FRICommandMessage, header, header, &MessageHeader_fields),
+    PB_FIELD2(  2, MESSAGE , OPTIONAL, STATIC  , OTHER, FRICommandMessage, commandData, header, &MessageCommandData_fields),
+    PB_FIELD2( 15, MESSAGE , OPTIONAL, STATIC  , OTHER, FRICommandMessage, endOfMessageData, commandData, &MessageEndOf_fields),
+    PB_LAST_FIELD
+};
+
+
+/* Check that field information fits in pb_field_t */
+#if !defined(PB_FIELD_32BIT)
+/* If you get an error here, it means that you need to define PB_FIELD_32BIT
+ * compile-time option. You can do that in pb.h or on compiler command line.
+ * 
+ * The reason you need to do this is that some of your messages contain tag
+ * numbers or field sizes that are larger than what can fit in 8 or 16 bit
+ * field descriptors.
+ */
+STATIC_ASSERT((pb_membersize(Transformation, timestamp) < 65536 && pb_membersize(MessageMonitorData, measuredJointPosition) < 65536 && pb_membersize(MessageMonitorData, measuredTorque) < 65536 && pb_membersize(MessageMonitorData, commandedJointPosition) < 65536 && pb_membersize(MessageMonitorData, commandedTorque) < 65536 && pb_membersize(MessageMonitorData, externalTorque) < 65536 && pb_membersize(MessageMonitorData, readIORequest[0]) < 65536 && pb_membersize(MessageMonitorData, timestamp) < 65536 && pb_membersize(MessageIpoData, jointPosition) < 65536 && pb_membersize(MessageCommandData, jointPosition) < 65536 && pb_membersize(MessageCommandData, cartesianWrenchFeedForward) < 65536 && pb_membersize(MessageCommandData, jointTorque) < 65536 && pb_membersize(MessageCommandData, commandedTransformations[0]) < 65536 && pb_membersize(MessageCommandData, writeIORequest[0]) < 65536 && pb_membersize(MessageEndOf, messageChecksum) < 65536 && pb_membersize(FRIMonitoringMessage, header) < 65536 && pb_membersize(FRIMonitoringMessage, connectionInfo) < 65536 && pb_membersize(FRIMonitoringMessage, robotInfo) < 65536 && pb_membersize(FRIMonitoringMessage, monitorData) < 65536 && pb_membersize(FRIMonitoringMessage, ipoData) < 65536 && pb_membersize(FRIMonitoringMessage, requestedTransformations[0]) < 65536 && pb_membersize(FRIMonitoringMessage, endOfMessageData) < 65536 && pb_membersize(FRICommandMessage, header) < 65536 && pb_membersize(FRICommandMessage, commandData) < 65536 && pb_membersize(FRICommandMessage, endOfMessageData) < 65536), YOU_MUST_DEFINE_PB_FIELD_32BIT_FOR_MESSAGES_JointValues_TimeStamp_CartesianVector_Checksum_Transformation_FriIOValue_MessageHeader_ConnectionInfo_RobotInfo_MessageMonitorData_MessageIpoData_MessageCommandData_MessageEndOf_FRIMonitoringMessage_FRICommandMessage)
+#endif
+
+#if !defined(PB_FIELD_16BIT) && !defined(PB_FIELD_32BIT)
+/* If you get an error here, it means that you need to define PB_FIELD_16BIT
+ * compile-time option. You can do that in pb.h or on compiler command line.
+ * 
+ * The reason you need to do this is that some of your messages contain tag
+ * numbers or field sizes that are larger than what can fit in the default
+ * 8 bit descriptors.
+ */
+STATIC_ASSERT((pb_membersize(Transformation, timestamp) < 256 && pb_membersize(MessageMonitorData, measuredJointPosition) < 256 && pb_membersize(MessageMonitorData, measuredTorque) < 256 && pb_membersize(MessageMonitorData, commandedJointPosition) < 256 && pb_membersize(MessageMonitorData, commandedTorque) < 256 && pb_membersize(MessageMonitorData, externalTorque) < 256 && pb_membersize(MessageMonitorData, readIORequest[0]) < 256 && pb_membersize(MessageMonitorData, timestamp) < 256 && pb_membersize(MessageIpoData, jointPosition) < 256 && pb_membersize(MessageCommandData, jointPosition) < 256 && pb_membersize(MessageCommandData, cartesianWrenchFeedForward) < 256 && pb_membersize(MessageCommandData, jointTorque) < 256 && pb_membersize(MessageCommandData, commandedTransformations[0]) < 256 && pb_membersize(MessageCommandData, writeIORequest[0]) < 256 && pb_membersize(MessageEndOf, messageChecksum) < 256 && pb_membersize(FRIMonitoringMessage, header) < 256 && pb_membersize(FRIMonitoringMessage, connectionInfo) < 256 && pb_membersize(FRIMonitoringMessage, robotInfo) < 256 && pb_membersize(FRIMonitoringMessage, monitorData) < 256 && pb_membersize(FRIMonitoringMessage, ipoData) < 256 && pb_membersize(FRIMonitoringMessage, requestedTransformations[0]) < 256 && pb_membersize(FRIMonitoringMessage, endOfMessageData) < 256 && pb_membersize(FRICommandMessage, header) < 256 && pb_membersize(FRICommandMessage, commandData) < 256 && pb_membersize(FRICommandMessage, endOfMessageData) < 256), YOU_MUST_DEFINE_PB_FIELD_16BIT_FOR_MESSAGES_JointValues_TimeStamp_CartesianVector_Checksum_Transformation_FriIOValue_MessageHeader_ConnectionInfo_RobotInfo_MessageMonitorData_MessageIpoData_MessageCommandData_MessageEndOf_FRIMonitoringMessage_FRICommandMessage)
+#endif
+
+
+/* On some platforms (such as AVR), double is really float.
+ * These are not directly supported by nanopb, but see example_avr_double.
+ * To get rid of this error, remove any double fields from your .proto.
+ */
+STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES)
+

src/iiwa_controller/external/libFRI/src/protobuf_gen/FRIMessages.pb.h

@@ -0,0 +1,303 @@
+/* Automatically generated nanopb header */
+/* Generated by nanopb-0.2.8 at Tue Feb 14 12:42:03 2017. */
+
+#ifndef _PB_FRIMESSAGES_PB_H_
+#define _PB_FRIMESSAGES_PB_H_
+#include <pb.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Enum definitions */
+typedef enum _FRISessionState {
+    FRISessionState_IDLE = 0,
+    FRISessionState_MONITORING_WAIT = 1,
+    FRISessionState_MONITORING_READY = 2,
+    FRISessionState_COMMANDING_WAIT = 3,
+    FRISessionState_COMMANDING_ACTIVE = 4
+} FRISessionState;
+
+typedef enum _FRIConnectionQuality {
+    FRIConnectionQuality_POOR = 0,
+    FRIConnectionQuality_FAIR = 1,
+    FRIConnectionQuality_GOOD = 2,
+    FRIConnectionQuality_EXCELLENT = 3
+} FRIConnectionQuality;
+
+typedef enum _SafetyState {
+    SafetyState_NORMAL_OPERATION = 0,
+    SafetyState_SAFETY_STOP_LEVEL_0 = 1,
+    SafetyState_SAFETY_STOP_LEVEL_1 = 2,
+    SafetyState_SAFETY_STOP_LEVEL_2 = 3
+} SafetyState;
+
+typedef enum _OperationMode {
+    OperationMode_TEST_MODE_1 = 0,
+    OperationMode_TEST_MODE_2 = 1,
+    OperationMode_AUTOMATIC_MODE = 2
+} OperationMode;
+
+typedef enum _DriveState {
+    DriveState_OFF = 0,
+    DriveState_TRANSITIONING = 1,
+    DriveState_ACTIVE = 2
+} DriveState;
+
+typedef enum _ControlMode {
+    ControlMode_POSITION_CONTROLMODE = 0,
+    ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE = 1,
+    ControlMode_JOINT_IMPEDANCE_CONTROLMODE = 2,
+    ControlMode_NO_CONTROLMODE = 3
+} ControlMode;
+
+typedef enum _ClientCommandMode {
+    ClientCommandMode_NO_COMMAND_MODE = 0,
+    ClientCommandMode_POSITION = 1,
+    ClientCommandMode_WRENCH = 2,
+    ClientCommandMode_TORQUE = 3
+} ClientCommandMode;
+
+typedef enum _OverlayType {
+    OverlayType_NO_OVERLAY = 0,
+    OverlayType_JOINT = 1,
+    OverlayType_CARTESIAN = 2
+} OverlayType;
+
+typedef enum _FriIOType {
+    FriIOType_BOOLEAN = 0,
+    FriIOType_DIGITAL = 1,
+    FriIOType_ANALOG = 2
+} FriIOType;
+
+typedef enum _FriIODirection {
+    FriIODirection_INPUT = 0,
+    FriIODirection_OUTPUT = 1
+} FriIODirection;
+
+/* Struct definitions */
+typedef struct _CartesianVector {
+    size_t element_count;
+    double element[6];
+} CartesianVector;
+
+typedef struct _Checksum {
+    bool has_crc32;
+    int32_t crc32;
+} Checksum;
+
+typedef struct _ConnectionInfo {
+    FRISessionState sessionState;
+    FRIConnectionQuality quality;
+    bool has_sendPeriod;
+    uint32_t sendPeriod;
+    bool has_receiveMultiplier;
+    uint32_t receiveMultiplier;
+} ConnectionInfo;
+
+typedef struct _FriIOValue {
+    char name[64];
+    FriIOType type;
+    FriIODirection direction;
+    bool has_digitalValue;
+    uint64_t digitalValue;
+    bool has_analogValue;
+    double analogValue;
+} FriIOValue;
+
+typedef struct _JointValues {
+    pb_callback_t value;
+} JointValues;
+
+typedef struct _MessageHeader {
+    uint32_t messageIdentifier;
+    uint32_t sequenceCounter;
+    uint32_t reflectedSequenceCounter;
+} MessageHeader;
+
+typedef struct _RobotInfo {
+    bool has_numberOfJoints;
+    int32_t numberOfJoints;
+    bool has_safetyState;
+    SafetyState safetyState;
+    pb_callback_t driveState;
+    bool has_operationMode;
+    OperationMode operationMode;
+    bool has_controlMode;
+    ControlMode controlMode;
+} RobotInfo;
+
+typedef struct _TimeStamp {
+    uint32_t sec;
+    uint32_t nanosec;
+} TimeStamp;
+
+typedef struct _MessageEndOf {
+    bool has_messageLength;
+    int32_t messageLength;
+    bool has_messageChecksum;
+    Checksum messageChecksum;
+} MessageEndOf;
+
+typedef struct _MessageIpoData {
+    bool has_jointPosition;
+    JointValues jointPosition;
+    bool has_clientCommandMode;
+    ClientCommandMode clientCommandMode;
+    bool has_overlayType;
+    OverlayType overlayType;
+    bool has_trackingPerformance;
+    double trackingPerformance;
+} MessageIpoData;
+
+typedef struct _MessageMonitorData {
+    bool has_measuredJointPosition;
+    JointValues measuredJointPosition;
+    bool has_measuredTorque;
+    JointValues measuredTorque;
+    bool has_commandedJointPosition;
+    JointValues commandedJointPosition;
+    bool has_commandedTorque;
+    JointValues commandedTorque;
+    bool has_externalTorque;
+    JointValues externalTorque;
+    size_t readIORequest_count;
+    FriIOValue readIORequest[10];
+    bool has_timestamp;
+    TimeStamp timestamp;
+} MessageMonitorData;
+
+typedef struct _Transformation {
+    char name[64];
+    size_t matrix_count;
+    double matrix[12];
+    bool has_timestamp;
+    TimeStamp timestamp;
+} Transformation;
+
+typedef struct _FRIMonitoringMessage {
+    MessageHeader header;
+    bool has_robotInfo;
+    RobotInfo robotInfo;
+    bool has_monitorData;
+    MessageMonitorData monitorData;
+    bool has_connectionInfo;
+    ConnectionInfo connectionInfo;
+    bool has_ipoData;
+    MessageIpoData ipoData;
+    size_t requestedTransformations_count;
+    Transformation requestedTransformations[5];
+    bool has_endOfMessageData;
+    MessageEndOf endOfMessageData;
+} FRIMonitoringMessage;
+
+typedef struct _MessageCommandData {
+    bool has_jointPosition;
+    JointValues jointPosition;
+    bool has_cartesianWrenchFeedForward;
+    CartesianVector cartesianWrenchFeedForward;
+    bool has_jointTorque;
+    JointValues jointTorque;
+    size_t commandedTransformations_count;
+    Transformation commandedTransformations[5];
+    size_t writeIORequest_count;
+    FriIOValue writeIORequest[10];
+} MessageCommandData;
+
+typedef struct _FRICommandMessage {
+    MessageHeader header;
+    bool has_commandData;
+    MessageCommandData commandData;
+    bool has_endOfMessageData;
+    MessageEndOf endOfMessageData;
+} FRICommandMessage;
+
+/* Default values for struct fields */
+
+/* Field tags (for use in manual encoding/decoding) */
+#define CartesianVector_element_tag              1
+#define Checksum_crc32_tag                       1
+#define ConnectionInfo_sessionState_tag          1
+#define ConnectionInfo_quality_tag               2
+#define ConnectionInfo_sendPeriod_tag            3
+#define ConnectionInfo_receiveMultiplier_tag     4
+#define FriIOValue_name_tag                      1
+#define FriIOValue_type_tag                      2
+#define FriIOValue_direction_tag                 3
+#define FriIOValue_digitalValue_tag              4
+#define FriIOValue_analogValue_tag               5
+#define JointValues_value_tag                    1
+#define MessageHeader_messageIdentifier_tag      1
+#define MessageHeader_sequenceCounter_tag        2
+#define MessageHeader_reflectedSequenceCounter_tag 3
+#define RobotInfo_numberOfJoints_tag             1
+#define RobotInfo_safetyState_tag                2
+#define RobotInfo_driveState_tag                 5
+#define RobotInfo_operationMode_tag              6
+#define RobotInfo_controlMode_tag                7
+#define TimeStamp_sec_tag                        1
+#define TimeStamp_nanosec_tag                    2
+#define MessageEndOf_messageLength_tag           1
+#define MessageEndOf_messageChecksum_tag         2
+#define MessageIpoData_jointPosition_tag         1
+#define MessageIpoData_clientCommandMode_tag     10
+#define MessageIpoData_overlayType_tag           11
+#define MessageIpoData_trackingPerformance_tag   12
+#define MessageMonitorData_measuredJointPosition_tag 1
+#define MessageMonitorData_measuredTorque_tag    2
+#define MessageMonitorData_commandedJointPosition_tag 3
+#define MessageMonitorData_commandedTorque_tag   4
+#define MessageMonitorData_externalTorque_tag    5
+#define MessageMonitorData_readIORequest_tag     8
+#define MessageMonitorData_timestamp_tag         15
+#define Transformation_name_tag                  1
+#define Transformation_matrix_tag                2
+#define Transformation_timestamp_tag             3
+#define FRIMonitoringMessage_header_tag          1
+#define FRIMonitoringMessage_connectionInfo_tag  4
+#define FRIMonitoringMessage_robotInfo_tag       2
+#define FRIMonitoringMessage_monitorData_tag     3
+#define FRIMonitoringMessage_ipoData_tag         5
+#define FRIMonitoringMessage_requestedTransformations_tag 6
+#define FRIMonitoringMessage_endOfMessageData_tag 15
+#define MessageCommandData_jointPosition_tag     1
+#define MessageCommandData_cartesianWrenchFeedForward_tag 2
+#define MessageCommandData_jointTorque_tag       3
+#define MessageCommandData_commandedTransformations_tag 4
+#define MessageCommandData_writeIORequest_tag    5
+#define FRICommandMessage_header_tag             1
+#define FRICommandMessage_commandData_tag        2
+#define FRICommandMessage_endOfMessageData_tag   15
+
+/* Struct field encoding specification for nanopb */
+extern const pb_field_t JointValues_fields[2];
+extern const pb_field_t TimeStamp_fields[3];
+extern const pb_field_t CartesianVector_fields[2];
+extern const pb_field_t Checksum_fields[2];
+extern const pb_field_t Transformation_fields[4];
+extern const pb_field_t FriIOValue_fields[6];
+extern const pb_field_t MessageHeader_fields[4];
+extern const pb_field_t ConnectionInfo_fields[5];
+extern const pb_field_t RobotInfo_fields[6];
+extern const pb_field_t MessageMonitorData_fields[8];
+extern const pb_field_t MessageIpoData_fields[5];
+extern const pb_field_t MessageCommandData_fields[6];
+extern const pb_field_t MessageEndOf_fields[3];
+extern const pb_field_t FRIMonitoringMessage_fields[8];
+extern const pb_field_t FRICommandMessage_fields[4];
+
+/* Maximum encoded size of messages (where known) */
+#define TimeStamp_size                           12
+#define CartesianVector_size                     54
+#define Checksum_size                            11
+#define Transformation_size                      188
+#define FriIOValue_size                          98
+#define MessageHeader_size                       18
+#define ConnectionInfo_size                      24
+#define MessageEndOf_size                        24
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif

src/iiwa_controller/external/libFRI/src/protobuf_gen/Makefile

@@ -0,0 +1,15 @@
+BASE_DIR	= ../..
+include $(BASE_DIR)/build/GNUMake/paths.mak
+include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK)
+
+CC_SRC 		 = FRIMessages.pb.c
+
+INC_DIR		+= $(NANOPB_DIR)
+CFLAGS 		+= 
+LDFLAGS 		+= 
+
+################################################################################
+### Include general makefile (at the end)
+################################################################################
+
+include $(BASE_DIR)/build/GNUMake/rules.mak

src/iiwa_controller/iiwa_controller_plugin.xml

@@ -0,0 +1,6 @@
+<library path="iiwa_controller">
+    <class name="iiwa_controller/IIWAHardwareInterface"
+           type="iiwa_controller::IIWAHardwareInterface"
+           base_class_type="hardware_interface::SystemInterface"/>
+  </library>
+  

src/iiwa_controller/include/iiwa_controller/FRIClient.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <friLBRClient.h>
+#include <vector>
+#include <array>
+#include <cstring>
+
+class FRIClient : public KUKA::FRI::LBRClient {
+    
+    public:
+        FRIClient();
+
+        void monitor() override;
+        void waitForCommand() override;
+        void command() override;
+        void onStateChange(KUKA::FRI::ESessionState oldState,
+                           KUKA::FRI::ESessionState newState) override;
+
+        std::array<double, 7> getMeasuredJointPositions() const;
+        std::array<double, 7> getMeasuredTorque() const;
+        
+        void setTargetJointPositions(const std::array<double, 7> target_pos);
+    
+    private:
+        std::array<double, 7> measuredJointPositions_;
+        std::array<double, 7> measuredTorque_;
+        std::array<double, 7> targetJointPositions_;
+
+};

src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp

@@ -0,0 +1,57 @@
+#ifndef IIWA_HARDWARE_INTERFACE_HPP
+#define IIWA_HARDWARE_INTERFACE_HPP
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "rclcpp/macros.hpp"
+#include "FRIClient.h"
+#include "friUdpConnection.h"
+#include "friClientApplication.h"
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+using namespace KUKA::FRI;
+
+namespace iiwa_controller
+{
+
+    class IIWAHardwareInterface : public hardware_interface::SystemInterface {
+        
+        public:
+            CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
+            std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+            std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+            CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+            CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+            hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+            hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+        private:
+            // TODO: append robotClient FRI
+            std::unique_ptr<FRIClient> fri_client_;
+            std::unique_ptr<ClientApplication> app_;
+            std::unique_ptr<UdpConnection> connection_;
+
+            bool simulate_;
+            std::string hw_command_mode_;
+            std::vector<double> hw_commands_;
+            std::vector<double> hw_states_position_;
+            std::vector<double> hw_states_velocity_;
+            std::vector<double> hw_states_effort_;
+            std::vector<double> internal_command_position;
+            std::vector<double> prev_measured_pos_;
+            bool safety_override_active_ = true;
+    };
+
+
+}
+
+
+
+#endif

src/iiwa_controller/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>iiwa_controller</name>
+  <version>0.1.0</version>
+  <description>Own controller for controlling the KUKA aiwa 7 collaborative robot using the FRI library</description>
+  <maintainer email="daniell@example.com">Daniell</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/iiwa_controller/src/FRIClient.cpp

@@ -0,0 +1,79 @@
+#include "iiwa_controller/FRIClient.h"
+#include "rclcpp/rclcpp.hpp"
+
+using namespace KUKA::FRI;
+
+inline const char* to_string(KUKA::FRI::ESessionState s)
+{
+  using namespace KUKA::FRI;
+  switch (s)
+  {
+    case IDLE:               return "IDLE";
+    case MONITORING_WAIT:    return "MONITORING_WAIT";
+    case MONITORING_READY:   return "MONITORING_READY";
+    case COMMANDING_WAIT:    return "COMMANDING_WAIT";
+    case COMMANDING_ACTIVE:  return "COMMANDING_ACTIVE";
+    default:                 return "UNKNOWN";
+  }
+}
+
+FRIClient::FRIClient() {
+    targetJointPositions_.fill(0.0);
+    measuredJointPositions_.fill(0.0);
+    measuredTorque_.fill(0.0);
+};
+
+void FRIClient::monitor()
+{
+  std::memcpy(measuredJointPositions_.data(),
+              robotState().getMeasuredJointPosition(),
+              7 * sizeof(double));
+
+  std::memcpy(measuredTorque_.data(),
+              robotState().getMeasuredTorque(),
+              7 * sizeof(double));
+}
+
+void FRIClient::setTargetJointPositions(const std::array<double, 7> target_pos) {
+    targetJointPositions_ = target_pos;
+}
+
+std::array<double, 7> FRIClient::getMeasuredJointPositions() const {
+    return measuredJointPositions_;
+}
+
+std::array<double, 7> FRIClient::getMeasuredTorque() const {
+    return measuredTorque_;
+}
+
+void FRIClient::onStateChange(ESessionState oldState, ESessionState newState) {
+    RCLCPP_INFO_STREAM(
+        rclcpp::get_logger("FRIClient"),
+            "[FRI Client] FRI state: " << to_string(oldState) << " --> " << to_string(newState));
+}
+
+
+void FRIClient::waitForCommand()
+{
+    std::memcpy(targetJointPositions_.data(),
+              robotState().getMeasuredJointPosition(),
+              7 * sizeof(double));
+    
+    std::memcpy(measuredJointPositions_.data(),
+                robotState().getMeasuredJointPosition(),
+              7 * sizeof(double));
+
+    std::memcpy(measuredTorque_.data(),
+              robotState().getMeasuredTorque(),
+              7 * sizeof(double));
+
+  robotCommand().setJointPosition(targetJointPositions_.data());
+}
+
+void FRIClient::command() {
+    std::memcpy(measuredJointPositions_.data(),
+                robotState().getMeasuredJointPosition(),
+                7 * sizeof(double));
+
+    robotCommand().setJointPosition(targetJointPositions_.data());
+}

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -0,0 +1,318 @@
+#include "iiwa_controller/IIWAHardwareInterface.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+using namespace KUKA::FRI;
+
+namespace iiwa_controller {
+    
+    template<typename T>
+    constexpr const T& clamp(const T& v, const T& lo, const T& hi)
+    {
+        return (v < lo) ? lo : (hi < v) ? hi : v;
+    }
+
+    CallbackReturn IIWAHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) {
+
+        if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
+            return CallbackReturn::ERROR;
+
+        simulate_ = false;
+        hw_states_position_.resize(info_.joints.size(), 0.0);
+        hw_states_velocity_.resize(info_.joints.size(), 0.0);
+        hw_states_effort_.resize(info_.joints.size(), 0.0);
+        hw_commands_.resize(info_.joints.size(), 0.0);
+        prev_measured_pos_.resize(info_.joints.size(), 0.0);
+        internal_command_position.resize(info_.joints.size(), 0.0);
+
+        // пробегаемся по всем интерфейсам и смотрим какой режим управления установлен
+        for (const hardware_interface::ComponentInfo & joint : info_.joints) {
+            
+            // проверка, на то что все суставы используют один и тот же тип управления
+            if (joint.command_interfaces.size() != 1) {
+                RCLCPP_FATAL(
+                  rclcpp::get_logger("IiwaFRIHardwareInterface"),
+                  "Joint '%s' has %li command interfaces found. 1 expected.", joint.name.c_str(),
+                  joint.command_interfaces.size());
+                return CallbackReturn::ERROR;
+            }
+
+            // что у каждого сустава ровно 3 интерфейса состояния:
+            if (hw_command_mode_.empty()) {
+                hw_command_mode_ = joint.command_interfaces[0].name;
+          
+                if (hw_command_mode_ != hardware_interface::HW_IF_POSITION &&
+                  hw_command_mode_ != hardware_interface::HW_IF_VELOCITY &&
+                  hw_command_mode_ != hardware_interface::HW_IF_EFFORT)
+                {
+                  RCLCPP_FATAL(
+                    rclcpp::get_logger("IiwaFRIHardwareInterface"),
+                    "Joint '%s' have %s unknown command interfaces.", joint.name.c_str(),
+                    joint.command_interfaces[0].name.c_str());
+                  return CallbackReturn::ERROR;
+                }
+            }
+            
+            // 
+            if (hw_command_mode_ != joint.command_interfaces[0].name) {
+                RCLCPP_FATAL(
+                  rclcpp::get_logger("IiwaFRIHardwareInterface"),
+                  "Joint '%s' has %s command interfaces. Expected %s.", joint.name.c_str(),
+                  joint.command_interfaces[0].name.c_str(), hw_command_mode_.c_str());
+                return CallbackReturn::ERROR;
+            }
+          
+            if (joint.state_interfaces.size() != 3) {
+                RCLCPP_FATAL(
+                  rclcpp::get_logger("IiwaFRIHardwareInterface"),
+                  "Joint '%s' has %li state interface. 3 expected.", joint.name.c_str(),
+                  joint.state_interfaces.size());
+                return CallbackReturn::ERROR;
+            }
+          
+            if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
+                RCLCPP_FATAL(
+                  rclcpp::get_logger("IiwaFRIHardwareInterface"),
+                  "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
+                  joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
+                return CallbackReturn::ERROR;
+            }
+          
+            if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) {
+                RCLCPP_FATAL(
+                  rclcpp::get_logger("IiwaFRIHardwareInterface"),
+                  "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
+                  joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
+                return CallbackReturn::ERROR;
+            }
+          
+            if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) {
+                RCLCPP_FATAL(
+                  rclcpp::get_logger("IiwaFRIHardwareInterface"),
+                  "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
+                  joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT);
+                return CallbackReturn::ERROR;
+            }
+        
+        }
+
+        return CallbackReturn::SUCCESS;
+
+    }
+
+    std::vector<hardware_interface::StateInterface> IIWAHardwareInterface::export_state_interfaces() {
+        std::vector<hardware_interface::StateInterface> state_interfaces;
+        for (uint i = 0; i < info_.joints.size(); i++) {
+            state_interfaces.emplace_back(
+            hardware_interface::StateInterface(
+                info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_position_[i]));
+        }
+
+        for (uint i = 0; i < info_.joints.size(); i++) {
+            state_interfaces.emplace_back(
+            hardware_interface::StateInterface(
+                info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocity_[i]));
+        }
+
+        for (uint i = 0; i < info_.joints.size(); i++) {
+            state_interfaces.emplace_back(
+            hardware_interface::StateInterface(
+                info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_effort_[i]));
+        }
+
+        return state_interfaces;
+    }
+
+    std::vector<hardware_interface::CommandInterface> IIWAHardwareInterface::export_command_interfaces() {
+        std::vector<hardware_interface::CommandInterface> command_interfaces;
+
+        for (uint i = 0; i < info_.joints.size(); i++) {
+            if (hw_command_mode_ == hardware_interface::HW_IF_POSITION) {
+              command_interfaces.emplace_back(
+                hardware_interface::CommandInterface(
+                  info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
+            
+            } else if (hw_command_mode_ == hardware_interface::HW_IF_VELOCITY) {
+              command_interfaces.emplace_back(
+                hardware_interface::CommandInterface(
+                  info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i]));
+            
+            } else if (hw_command_mode_ == hardware_interface::HW_IF_EFFORT) {
+              command_interfaces.emplace_back(
+                hardware_interface::CommandInterface(
+                  info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_commands_[i]));
+            }
+
+        }
+        
+        return command_interfaces;
+
+    }
+
+    CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State& ) {
+        RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "Starting ...please wait...");
+
+        auto it = info_.hardware_parameters.find("simulate");
+        if (it != info_.hardware_parameters.end()) {
+            std::string sim_str = it->second;
+            std::transform(sim_str.begin(), sim_str.end(), sim_str.begin(), ::tolower);
+            simulate_ = (sim_str == "true");
+        }
+
+        if (!simulate_) {
+
+            std::string ip = info_.hardware_parameters.at("robot_ip");
+            int port = std::stoi(info_.hardware_parameters.at("robot_port"));
+            
+            fri_client_ = std::make_unique<FRIClient>();
+            connection_ = std::make_unique<UdpConnection>();
+            app_ = std::make_unique<ClientApplication>(*connection_, *fri_client_);
+            app_->connect(port, ip.c_str());
+
+            rclcpp::Time now = rclcpp::Clock().now();
+            rclcpp::Duration period = rclcpp::Duration::from_seconds(0.01);
+            this->read(now, period);
+
+            safety_override_active_ = true;
+            hw_commands_ = hw_states_position_;
+            RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "Connecting FRI to port= %i and ip= %s", port, ip.c_str());
+
+        }
+
+        RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "System Successfully started!");
+
+        return CallbackReturn::SUCCESS;
+
+    }
+
+    CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& ) {
+        
+        RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "Stopping ...please wait...");
+        
+        if (!simulate_) {
+            app_->disconnect();
+        }
+        
+        std::fill(hw_commands_.begin(), hw_commands_.end(), 0.0);
+        RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "hw_commands_ reset to zero.");
+
+
+        RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "System successfully stopped!");
+        
+        return CallbackReturn::SUCCESS;
+    }
+
+
+    hardware_interface::return_type IIWAHardwareInterface::read(const rclcpp::Time&, const rclcpp::Duration& period) {
+        if (!simulate_) {
+
+            if (!app_ || !app_->step())   // session порвалась?
+            {
+                RCLCPP_ERROR(rclcpp::get_logger("IIWAHardwareInterface"),
+                            "FRI session lost");
+                return hardware_interface::return_type::ERROR;
+            }
+
+            /* ---------- 2. Считываем измеренные данные ---------- */
+            const auto pos_meas = fri_client_->getMeasuredJointPositions();
+            const auto tau_meas = fri_client_->getMeasuredTorque();
+
+            /* ---------- 3. Копируем в ros2_control ---------- */
+            for (size_t i = 0; i < hw_states_position_.size(); ++i)
+            {
+                hw_states_position_[i] = pos_meas[i];
+
+                // простая численная производная = (dq) / dt
+                hw_states_velocity_[i] =
+                (pos_meas[i] - prev_measured_pos_[i]) / period.seconds();
+
+                hw_states_effort_[i] = tau_meas[i];
+                prev_measured_pos_[i] = pos_meas[i];
+            }
+
+            return hardware_interface::return_type::OK;
+        }
+
+        for (size_t i = 0; i < hw_states_position_.size(); ++i) {
+            hw_states_position_[i] = hw_commands_[i];
+            hw_states_velocity_[i] = 0.0;
+            hw_states_effort_[i] = 0.0;
+        }
+        return hardware_interface::return_type::OK;
+
+    }
+
+    hardware_interface::return_type IIWAHardwareInterface::write(const rclcpp::Time&, const rclcpp::Duration&)
+    {
+        if (simulate_)
+        {
+            RCLCPP_DEBUG(
+                rclcpp::get_logger("IIWAHardwareInterface"),
+                "Simulated write to robot (echo commands)");
+            return hardware_interface::return_type::OK;
+        }
+
+        // ---------- 1. Подготовка массивов команд ----------
+        std::array<double, 7> cmd_position{};
+        std::array<double, 7> cmd_torque{};
+
+        for (size_t i = 0; i < hw_commands_.size(); ++i)
+        {
+            if (hw_command_mode_ == hardware_interface::HW_IF_POSITION)
+                cmd_position[i] = hw_commands_[i];
+            else if (hw_command_mode_ == hardware_interface::HW_IF_EFFORT)
+                cmd_torque[i] = hw_commands_[i];
+        }
+
+        // ---------- 2. Проверка на "нулевые" команды ----------
+        double sum = std::accumulate(
+            hw_commands_.begin(), hw_commands_.end(), 0.0,
+            [](double a, double b) { return a + std::abs(b); });
+
+        if (sum > 1e-3 && safety_override_active_)
+        {
+            RCLCPP_WARN_ONCE(
+                rclcpp::get_logger("IIWAHardwareInterface"),
+                "Command ignored: hw_commands_ are effectively zero (likely startup or stale)");
+            return hardware_interface::return_type::OK;
+        }
+
+        safety_override_active_ = false;
+
+        // ---------- 3. Защита по лимитам углов ----------
+        const double joint_limits[7][2] = {
+            {-2.95, 2.95}, {-2.03, 2.03}, {-2.95, 2.95},
+            {-2.03, 2.03}, {-2.95, 2.95}, {-2.03, 2.03}, {-3.0, 3.0}};
+
+        if (hw_command_mode_ == hardware_interface::HW_IF_POSITION)
+        {
+            for (size_t i = 0; i < 7; ++i)
+            {
+                cmd_position[i] = clamp(cmd_position[i], joint_limits[i][0], joint_limits[i][1]);
+            }
+        }
+
+        // ---------- 4. Отправка команды в FRI-клиент ----------
+        if (hw_command_mode_ == hardware_interface::HW_IF_POSITION)
+        {
+            fri_client_->setTargetJointPositions(cmd_position);
+        }
+        else if (hw_command_mode_ == hardware_interface::HW_IF_EFFORT)
+        {
+            // TODO: реализовать setTargetTorque при необходимости
+        }
+        else if (hw_command_mode_ == hardware_interface::HW_IF_VELOCITY)
+        {
+            // Velocity mode не реализован в FRI
+        }
+
+        RCLCPP_DEBUG(
+            rclcpp::get_logger("IIWAHardwareInterface"),
+            "Command sent to FRI");
+        return hardware_interface::return_type::OK;
+    }
+
+}
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(iiwa_controller::IIWAHardwareInterface, hardware_interface::SystemInterface)

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
+}