initial commit

.clang-format

@@ -0,0 +1,4 @@
+BasedOnStyle: Chromium
+Standard: Cpp11
+SortIncludes: true
+ColumnLimit: 100

.clang-tidy

@@ -0,0 +1,54 @@
+Checks: 'cppcoreguidelines-*,-cppcoreguidelines-pro-type-vararg,-cppcoreguidelines-pro-bounds-array-to-pointer-decay,clang-diagnostic-*,clang-analyzer-*,-clang-analyzer-optin.cplusplus.VirtualCall,-clang-analyzer-alpha*,google-*,misc-*,readability-*,-readability-function-cognitive-complexity,modernize-*,-modernize-use-trailing-return-type,performance-*,-clang-diagnostic-deprecated-declarations,-modernize-pass-by-value,-clang-diagnostic-reinterpret-base-class,-clang-diagnostic-return-type,-clang-diagnostic-switch,-bugprone-lambda-function-name,-cppcoreguidelines-avoid-magic-numbers, -readability-magic-numbers'
+HeaderFilterRegex: '^franka_.*'
+CheckOptions:
+# Classes, structs, ...
+  - key:    readability-identifier-naming.NamespaceCase
+    value:  lower_case
+  - key:    readability-identifier-naming.ClassCase
+    value:  CamelCase
+  - key:    readability-identifier-naming.StructCase
+    value:  CamelCase
+  - key:    readability-identifier-naming.EnumCase
+    value:  CamelCase
+  - key:    readability-identifier-naming.UnionCase
+    value:  CamelCase
+  - key:    readability-identifier-naming.TypedefCase
+    value:  CamelCase
+
+# Variables, member variables, ...
+  - key:    readability-identifier-naming.ParameterCase
+    value:  lower_case
+  - key:    readability-identifier-naming.VariableCase
+    value:  lower_case
+  - key:    readability-identifier-naming.MemberCase
+    value:  lower_case
+  - key:    readability-identifier-naming.PublicMemberCase
+    value:  lower_case
+  - key:    readability-identifier-naming.ProtectedMemberCase
+    value:  lower_case
+  - key:    readability-identifier-naming.PrivateMemberCase
+    value:  lower_case
+  - key:    readability-identifier-naming.PrivateMemberSuffix
+    value:  _
+
+# Functions, methods, ...
+  - key:    readability-identifier-naming.FunctionCase
+    value:  camelBack
+  - key:    readability-identifier-naming.MethodCase
+    value:  camelBack
+
+# Constants
+  - key:    readability-identifier-naming.ConstantPrefix
+    value:  k
+  - key:    readability-identifier-naming.ConstantCase
+    value:  CamelCase
+  - key:    readability-identifier-naming.ConstantMemberPrefix
+    value:  ''
+  - key:    readability-identifier-naming.ConstantMemberCase
+    value:  lower_case
+  - key:    readability-identifier-naming.ConstantParameterCase
+    value:  lower_case
+  - key:    readability-identifier-naming.ConstantParameterPrefix
+    value:  ''
+
+

.devcontainer/Dockerfile

@@ -0,0 +1,66 @@
+FROM osrf/ros:humble-desktop
+
+ARG DEBIAN_FRONTEND=noninteractive
+
+ARG USER_UID=1001
+ARG USER_GID=1001
+ARG USERNAME=user
+
+WORKDIR /workspaces
+
+RUN groupadd --gid $USER_GID $USERNAME \
+    && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \
+    && mkdir -p -m 0700 /run/user/"${USER_UID}" \
+    && mkdir -p -m 0700 /run/user/"${USER_UID}"/gdm \
+    && chown user:user /run/user/"${USER_UID}" \
+    && chown user:user /workspaces \
+    && chown user:user /run/user/"${USER_UID}"/gdm \
+    # [Optional] Add sudo support. Omit if you don't need to install software after connecting.
+    && apt-get update \
+    && apt-get install -y sudo \
+    && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
+    && chmod 0440 /etc/sudoers.d/$USERNAME
+
+RUN apt-get update && \
+    DEBIAN_FRONTEND=noninteractive apt-get install -y --no-install-recommends \
+    ros-humble-hardware-interface \
+    ros-humble-generate-parameter-library \
+    ros-humble-xacro \
+    ros-humble-controller-interface \
+    ros-humble-realtime-tools \
+    ros-humble-ros2-control-test-assets \
+    ros-humble-controller-manager \
+    ros-humble-joint-state-publisher-gui \
+    ros-humble-control-msgs \
+    ros-humble-gazebo-ros \
+    ros-humble-control-toolbox \
+    ros-humble-ackermann-msgs \
+    ros-humble-ament-cmake \
+    ros-humble-ament-cmake-clang-format \
+    ros-humble-moveit \
+    ros-humble-joint-state-broadcaster \
+    libignition-gazebo6-dev \
+    ros-humble-joint-trajectory-controller \
+    libpoco-dev \
+    libeigen3-dev \
+    ament-cmake-clang-tidy \
+    && rm -rf /var/lib/apt/lists/*
+
+ENV XDG_RUNTIME_DIR=/run/user/"${USER_UID}"
+
+RUN mkdir ~/source_code
+RUN cd ~/source_code && git clone https://github.com/frankaemika/libfranka.git \
+    && cd libfranka \
+    && git switch fr3-develop \
+    && git submodule init \
+    && git submodule update \
+    && mkdir build && cd build \
+    && cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=OFF  .. \
+    && make franka -j$(nproc) \
+    && cpack -G DEB \
+    && sudo dpkg -i libfranka*.deb
+    
+# set the default user to the newly created user
+USER $USERNAME
+
+RUN echo 'source "/opt/ros/$ROS_DISTRO/setup.bash"' >>~/.bashrc

.devcontainer/devcontainer.json

@@ -0,0 +1,27 @@
+// For format details, see https://aka.ms/devcontainer.json. For config options, see the
+// README at: https://github.com/devcontainers/templates/tree/main/src/docker-existing-dockerfile
+{
+	"name": "ROS2 Humble",
+	"dockerComposeFile": "./docker-compose.yml",
+	"service": "ros2_control",
+	"workspaceFolder": "/workspaces",
+	"remoteUser": "user",
+	"customizations": {
+		"vscode": {
+			"extensions": [
+				"ms-python.python",
+				"bungcip.better-toml",
+				"ms-vscode.cpptools",
+				"ms-vscode.cpptools-extension-pack"
+			],
+			"settings": {
+				"terminal.integrated.defaultProfile.linux": "bash",
+				"terminal.integrated.profiles.linux": {
+					"bash": {
+						"path": "/bin/bash"
+					}
+				}
+			}
+		}
+	}
+}

.devcontainer/docker-compose.yml

@@ -0,0 +1,18 @@
+version: '2'
+
+services:
+  ros2_control:
+    build: .
+    network_mode: "host"
+    privileged: true
+    command: /bin/bash
+    tty: true
+    stdin_open: true
+    volumes:
+      - ../:/workspaces/src
+      - /tmp/.X11-unix:/tmp/.X11-unix
+      - $XAUTHORITY:$XAUTHORITY
+    environment:
+      QT_X11_NO_MITSHM: 1
+      DISPLAY: $DISPLAY
+    user: $UID:$UID

.github/workflows/ci.yml

@@ -0,0 +1,59 @@
+name: CI
+on:
+  push:
+    branches: [ develop ]
+  pull_request:
+    branches: [ develop ]
+jobs:
+  CI:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Prepare
+        run: |
+          mkdir -p ${{github.workspace}}/src
+
+      - uses: actions/checkout@v2
+        with:
+          path: src/franka_ros2
+
+      - name: Build Docker Image
+        uses: docker/build-push-action@v2
+        with:
+          tags: franka_ros2:humble
+          file: Dockerfile
+          push: false
+
+      - name: Build
+        uses: addnab/docker-run-action@v3
+        with:
+          image: franka_ros2:humble
+          options: -v ${{github.workspace}}/:/ros/
+          run: |
+            cd /ros
+            . /opt/ros/humble/setup.sh
+            colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCHECK_TIDY=ON
+
+      - name: Test
+        uses: addnab/docker-run-action@v3
+        with:
+          image: franka_ros2:humble
+          options: -v ${{github.workspace}}/:/ros/
+          run: |
+            cd /ros
+            . /opt/ros/humble/setup.sh
+            . install/setup.sh
+            colcon test
+            colcon test-result
+
+      - name: Upload Tests to Artifacts
+        uses: actions/upload-artifact@v2
+        if: always()
+        with:
+          name: test-results
+          path: build/*/test_results/*/*.xml
+
+      - name: Publish Unit Test Results
+        uses: EnricoMi/publish-unit-test-result-action@v1
+        if: always()
+        with:
+          files: build/*/test_results/*/*.xml

.gitignore

@@ -0,0 +1,9 @@
+.idea
+.vscode
+*.pyc
+COLCON_IGNORE
+AMENT_IGNORE
+cmake-build-*/
+build/
+install/
+log/

CHANGELOG.md

@@ -0,0 +1,29 @@
+# Changelog
+
+## 0.1.0 - 2023-07-28
+
+Requires libfranka >= 0.10.0, required ROS 2 Humble
+
+* franka\_bringup: franka_robot_state broadcaster added to franka.launch.py.
+* franka\_example\_controllers: model printing read onyl controller implemented
+* franka\_robot\_model: semantic component to access robot model parameters.
+* franka\_msgs: franka robot state msg added
+* franka\_robot\_state: broadcaster publishes robot state.
+
+### Added
+
+* CI tests in Jenkins.
+* joint\_effort\_trajectory\_controller package that contains a version of the
+ joint\_trajectory\_controller that can use the torque interface.
+ [See this PR](https://github.com/ros-controls/ros2_controllers/pull/225)
+* franka\_bringup package that contains various launch files to start controller examples or Moveit2.
+* franka\_moveit\_config package that contains a minimal moveit config to control the robot.
+* franka\_example\_controllers package that contains some example controllers to use.
+* franka\_hardware package that contains a plugin to access the robot.
+* franka\_msgs package that contains common message, service and action type definitions.
+* franka\_description package that contains all meshes and xacro files.
+* franka\_gripper package that offers action and service interfaces to use the Franka Hand gripper.
+
+### Fixed
+
+* franka\_hardware Fix the mismatched joint state interface type logger error message.

Dockerfile

@@ -0,0 +1,72 @@
+FROM ros:humble
+
+ARG DEBIAN_FRONTEND=noninteractive
+
+ARG USER_UID=1001
+ARG USER_GID=1001
+ARG USERNAME=user
+
+WORKDIR /tmp
+
+RUN groupadd --gid $USER_GID $USERNAME \
+    && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \
+    #
+    # [Optional] Add sudo support. Omit if you don't need to install software after connecting.
+    && apt-get update \
+    && apt-get install -y sudo \
+    && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
+    && chmod 0440 /etc/sudoers.d/$USERNAME
+
+RUN apt-get update -y && apt-get install -y --allow-unauthenticated \
+    clang-14 \
+    clang-format-14 \
+    clang-tidy-14 \
+    python3-pip \
+    libpoco-dev \
+    libeigen3-dev \
+    ros-humble-control-msgs \
+    ros-humble-xacro \
+    ros-humble-ament-cmake-clang-format \
+    ros-humble-ament-clang-format \
+    ros-humble-ament-flake8 \
+    ros-humble-ament-cmake-clang-tidy \
+    ros-humble-angles \
+    ros-humble-ros2-control \
+    ros-humble-realtime-tools \
+    ros-humble-control-toolbox \    
+    ros-humble-controller-manager \
+    ros-humble-hardware-interface \
+    ros-humble-generate-parameter-library \
+    ros-humble-controller-interface \
+    ros-humble-ros2-control-test-assets \
+    ros-humble-controller-manager \
+    && rm -rf /var/lib/apt/lists/*
+
+RUN python3 -m pip install -U \
+    argcomplete \
+    flake8-blind-except \
+    flake8-builtins \
+    flake8-class-newline \
+    flake8-comprehensions \
+    flake8-deprecated \
+    flake8-docstrings \
+    flake8-import-order \
+    flake8-quotes \
+    pytest-repeat \
+    pytest-rerunfailures \
+    pytest
+
+RUN mkdir ~/source_code    
+RUN cd ~/source_code && git clone https://github.com/frankaemika/libfranka.git \
+    && cd libfranka \
+    && git switch fr3-develop \
+    && git submodule init \
+    && git submodule update \
+    && mkdir build && cd build \
+    && cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=OFF  .. \
+    && make franka -j$(nproc) \
+    && cpack -G DEB \
+    && sudo dpkg -i libfranka*.deb
+
+# set the default user to the newly created user
+USER $USERNAME

Jenkinsfile

@@ -0,0 +1,49 @@
+pipeline {
+    agent {
+        dockerfile true 
+    }
+    triggers {
+        pollSCM('H/5 * * * *')
+    }
+    stages {
+        stage('Init') {
+            steps {
+                script {
+                    notifyBitbucket()
+                }
+                sh 'rm -rf build log install'
+            }
+        }
+        stage('Build') {
+            steps {
+                sh '''
+                    . /opt/ros/humble/setup.sh
+                    colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCHECK_TIDY=ON
+                '''
+            }
+        }
+        stage('Test') {
+            steps {
+                sh '''
+                    . /opt/ros/humble/setup.sh
+                    . install/setup.sh
+                    colcon test
+                    colcon test-result
+                '''
+            }
+            post {
+                always {
+                    junit 'build/**/test_results/**/*.xml'
+                }
+            }
+        }
+    }
+    post {
+        always {
+            cleanWs()
+            script {
+                notifyBitbucket()
+            }
+        }
+    }
+}

LICENSE

@@ -0,0 +1,176 @@
+                                 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

NOTICE

@@ -0,0 +1,15 @@
+franka_ros2
+
+Copyright 2021 Franka Emika GmbH
+
+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.

README.md

@@ -0,0 +1,13 @@
+# ROS 2 integration for Franka Emika research robots
+
+[![CI](https://github.com/frankaemika/franka_ros2/actions/workflows/ci.yml/badge.svg)](https://github.com/frankaemika/franka_ros2/actions/workflows/ci.yml)
+
+See the [Franka Control Interface (FCI) documentation][fci-docs] for more information.
+
+## License
+
+All packages of `franka_ros2` are licensed under the [Apache 2.0 license][apache-2.0].
+
+[apache-2.0]: https://www.apache.org/licenses/LICENSE-2.0.html
+
+[fci-docs]: https://frankaemika.github.io/docs

franka_bringup/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.5)
+project(franka_bringup)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY config launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

franka_bringup/config/controllers.yaml

@@ -0,0 +1,95 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 1000  # Hz
+
+    gravity_compensation_example_controller:
+      type: franka_example_controllers/GravityCompensationExampleController
+
+    joint_impedance_example_controller:
+      type: franka_example_controllers/JointImpedanceExampleController
+
+    move_to_start_example_controller:
+      type: franka_example_controllers/MoveToStartExampleController
+
+    model_example_controller:
+      type: franka_example_controllers/ModelExampleController
+
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    
+    franka_robot_state_broadcaster:
+      type: franka_robot_state_broadcaster/FrankaRobotStateBroadcaster
+
+franka_robot_state_broadcaster:
+  ros__parameters:
+    arm_id: panda
+
+model_example_controller:
+  ros__parameters:
+    arm_id: panda
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - panda_joint1
+      - panda_joint2
+      - panda_joint3
+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7
+    command_interfaces:
+      - effort
+    state_interfaces:
+      - position
+      - velocity
+    gains:
+      panda_joint1: { p: 600., d: 30., i: 0., i_clamp: 1. }
+      panda_joint2: { p: 600., d: 30., i: 0., i_clamp: 1. }
+      panda_joint3: { p: 600., d: 30., i: 0., i_clamp: 1. }
+      panda_joint4: { p: 600., d: 30., i: 0., i_clamp: 1. }
+      panda_joint5: { p: 250., d: 10., i: 0., i_clamp: 1. }
+      panda_joint6: { p: 150., d: 10., i: 0., i_clamp: 1. }
+      panda_joint7: { p: 50., d: 5., i: 0., i_clamp: 1. }
+
+joint_impedance_example_controller:
+  ros__parameters:
+    arm_id: panda
+    k_gains:
+      - 24.0
+      - 24.0
+      - 24.0
+      - 24.0
+      - 10.0
+      - 6.0
+      - 2.0
+    d_gains:
+      - 2.0
+      - 2.0
+      - 2.0
+      - 1.0
+      - 1.0
+      - 1.0
+      - 0.5
+move_to_start_example_controller:
+  ros__parameters:
+    arm_id: panda
+    k_gains:
+      - 600.0
+      - 600.0
+      - 600.0
+      - 600.0
+      - 250.0
+      - 150.0
+      - 50.0
+    d_gains:
+      - 30.0
+      - 30.0
+      - 30.0
+      - 30.0
+      - 10.0
+      - 10.0
+      - 5.

franka_bringup/launch/franka.launch.py

@@ -0,0 +1,136 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  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.
+
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, Shutdown
+from launch.conditions import IfCondition, UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    robot_ip_parameter_name = 'robot_ip'
+    load_gripper_parameter_name = 'load_gripper'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+    use_rviz_parameter_name = 'use_rviz'
+
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
+    use_rviz = LaunchConfiguration(use_rviz_parameter_name)
+
+    franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots',
+                                     'panda_arm.urdf.xacro')
+    robot_description = Command(
+        [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=', load_gripper,
+         ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware,
+         ' fake_sensor_commands:=', fake_sensor_commands])
+
+    rviz_file = os.path.join(get_package_share_directory('franka_description'), 'rviz',
+                             'visualize_franka.rviz')
+
+    franka_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare('franka_bringup'),
+            'config',
+            'controllers.yaml',
+        ]
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            robot_ip_parameter_name,
+            description='Hostname or IP address of the robot.'),
+        DeclareLaunchArgument(
+            use_rviz_parameter_name,
+            default_value='false',
+            description='Visualize the robot in Rviz'),
+        DeclareLaunchArgument(
+            use_fake_hardware_parameter_name,
+            default_value='false',
+            description='Use fake hardware'),
+        DeclareLaunchArgument(
+            fake_sensor_commands_parameter_name,
+            default_value='false',
+            description="Fake sensor commands. Only valid when '{}' is true".format(
+                use_fake_hardware_parameter_name)),
+        DeclareLaunchArgument(
+            load_gripper_parameter_name,
+            default_value='true',
+            description='Use Franka Gripper as an end-effector, otherwise, the robot is loaded '
+                        'without an end-effector.'),
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[{'robot_description': robot_description}],
+        ),
+        Node(
+            package='joint_state_publisher',
+            executable='joint_state_publisher',
+            name='joint_state_publisher',
+            parameters=[
+                {'source_list': ['franka/joint_states', 'panda_gripper/joint_states'],
+                 'rate': 30}],
+        ),
+        Node(
+            package='controller_manager',
+            executable='ros2_control_node',
+            parameters=[{'robot_description': robot_description}, franka_controllers],
+            remappings=[('joint_states', 'franka/joint_states')],
+            output={
+                'stdout': 'screen',
+                'stderr': 'screen',
+            },
+            on_exit=Shutdown(),
+        ),
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['joint_state_broadcaster'],
+            output='screen',
+        ),
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['franka_robot_state_broadcaster'],
+            output='screen',
+            condition=UnlessCondition(use_fake_hardware),
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([PathJoinSubstitution(
+                [FindPackageShare('franka_gripper'), 'launch', 'gripper.launch.py'])]),
+            launch_arguments={robot_ip_parameter_name: robot_ip,
+                              use_fake_hardware_parameter_name: use_fake_hardware}.items(),
+            condition=IfCondition(load_gripper)
+
+        ),
+
+        Node(package='rviz2',
+             executable='rviz2',
+             name='rviz2',
+             arguments=['--display-config', rviz_file],
+             condition=IfCondition(use_rviz)
+             )
+
+    ])

franka_bringup/launch/gravity_compensation_example_controller.launch.py

@@ -0,0 +1,76 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    robot_ip_parameter_name = 'robot_ip'
+    load_gripper_parameter_name = 'load_gripper'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+    use_rviz_parameter_name = 'use_rviz'
+
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
+    use_rviz = LaunchConfiguration(use_rviz_parameter_name)
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            robot_ip_parameter_name,
+            description='Hostname or IP address of the robot.'),
+        DeclareLaunchArgument(
+            use_rviz_parameter_name,
+            default_value='false',
+            description='Visualize the robot in Rviz'),
+        DeclareLaunchArgument(
+            use_fake_hardware_parameter_name,
+            default_value='false',
+            description='Use fake hardware'),
+        DeclareLaunchArgument(
+            fake_sensor_commands_parameter_name,
+            default_value='false',
+            description="Fake sensor commands. Only valid when '{}' is true".format(
+                use_fake_hardware_parameter_name)),
+        DeclareLaunchArgument(
+            load_gripper_parameter_name,
+            default_value='true',
+            description='Use Franka Gripper as an end-effector, otherwise, the robot is loaded '
+                        'without an end-effector.'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([PathJoinSubstitution(
+                [FindPackageShare('franka_bringup'), 'launch', 'franka.launch.py'])]),
+            launch_arguments={robot_ip_parameter_name: robot_ip,
+                              load_gripper_parameter_name: load_gripper,
+                              use_fake_hardware_parameter_name: use_fake_hardware,
+                              fake_sensor_commands_parameter_name: fake_sensor_commands,
+                              use_rviz_parameter_name: use_rviz
+                              }.items(),
+        ),
+
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['gravity_compensation_example_controller'],
+            output='screen',
+        ),
+    ])

franka_bringup/launch/joint_impedance_example_controller.launch.py

@@ -0,0 +1,77 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    robot_ip_parameter_name = 'robot_ip'
+    load_gripper_parameter_name = 'load_gripper'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+    use_rviz_parameter_name = 'use_rviz'
+
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
+    use_rviz = LaunchConfiguration(use_rviz_parameter_name)
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            robot_ip_parameter_name,
+            description='Hostname or IP address of the robot.'),
+        DeclareLaunchArgument(
+            use_rviz_parameter_name,
+            default_value='false',
+            description='Visualize the robot in Rviz'),
+        DeclareLaunchArgument(
+            use_fake_hardware_parameter_name,
+            default_value='false',
+            description='Use fake hardware'),
+        DeclareLaunchArgument(
+            fake_sensor_commands_parameter_name,
+            default_value='false',
+            description="Fake sensor commands. Only valid when '{}' is true".format(
+                use_fake_hardware_parameter_name)),
+        DeclareLaunchArgument(
+            load_gripper_parameter_name,
+            default_value='true',
+            description='Use Franka Gripper as an end-effector, otherwise, the robot is loaded '
+                        'without an end-effector.'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([PathJoinSubstitution(
+                [FindPackageShare('franka_bringup'), 'launch', 'franka.launch.py'])]),
+            launch_arguments={robot_ip_parameter_name: robot_ip,
+                              load_gripper_parameter_name: load_gripper,
+                              use_fake_hardware_parameter_name: use_fake_hardware,
+                              fake_sensor_commands_parameter_name: fake_sensor_commands,
+                              use_rviz_parameter_name: use_rviz
+                              }.items(),
+        ),
+
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['joint_impedance_example_controller'],
+            output='screen',
+        ),
+    ])

franka_bringup/launch/model_example_controller.launch.py

@@ -0,0 +1,77 @@
+#  Copyright (c) 2023 Franka Emika GmbH
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    robot_ip_parameter_name = 'robot_ip'
+    load_gripper_parameter_name = 'load_gripper'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+    use_rviz_parameter_name = 'use_rviz'
+
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
+    use_rviz = LaunchConfiguration(use_rviz_parameter_name)
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            robot_ip_parameter_name,
+            description='Hostname or IP address of the robot.'),
+        DeclareLaunchArgument(
+            use_rviz_parameter_name,
+            default_value='false',
+            description='Visualize the robot in Rviz'),
+        DeclareLaunchArgument(
+            use_fake_hardware_parameter_name,
+            default_value='false',
+            description='Use fake hardware'),
+        DeclareLaunchArgument(
+            fake_sensor_commands_parameter_name,
+            default_value='false',
+            description="Fake sensor commands. Only valid when '{}' is true".format(
+                use_fake_hardware_parameter_name)),
+        DeclareLaunchArgument(
+            load_gripper_parameter_name,
+            default_value='true',
+            description='Use Franka Gripper as an end-effector, otherwise, the robot is loaded '
+                        'without an end-effector.'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([PathJoinSubstitution(
+                [FindPackageShare('franka_bringup'), 'launch', 'franka.launch.py'])]),
+            launch_arguments={robot_ip_parameter_name: robot_ip,
+                              load_gripper_parameter_name: load_gripper,
+                              use_fake_hardware_parameter_name: use_fake_hardware,
+                              fake_sensor_commands_parameter_name: fake_sensor_commands,
+                              use_rviz_parameter_name: use_rviz
+                              }.items(),
+        ),
+
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['model_example_controller'],
+            output='screen',
+        ),
+    ])

franka_bringup/launch/move_to_start_example_controller.launch.py

@@ -0,0 +1,90 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    robot_ip_parameter_name = 'robot_ip'
+    load_gripper_parameter_name = 'load_gripper'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+    use_rviz_parameter_name = 'use_rviz'
+
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
+    use_rviz = LaunchConfiguration(use_rviz_parameter_name)
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                robot_ip_parameter_name, description='Hostname or IP address of the robot.'
+            ),
+            DeclareLaunchArgument(
+                use_rviz_parameter_name,
+                default_value='false',
+                description='Visualize the robot in Rviz',
+            ),
+            DeclareLaunchArgument(
+                use_fake_hardware_parameter_name,
+                default_value='false',
+                description='Use fake hardware',
+            ),
+            DeclareLaunchArgument(
+                fake_sensor_commands_parameter_name,
+                default_value='false',
+                description='Fake sensor commands. Only valid when "{}" is true'.format(
+                    use_fake_hardware_parameter_name
+                ),
+            ),
+            DeclareLaunchArgument(
+                load_gripper_parameter_name,
+                default_value='true',
+                description=(
+                    'Use Franka Gripper as an end-effector, otherwise, the robot is loaded '
+                    'without an end-effector.'
+                ),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    [
+                        PathJoinSubstitution(
+                            [FindPackageShare('franka_bringup'), 'launch', 'franka.launch.py']
+                        )
+                    ]
+                ),
+                launch_arguments={
+                    robot_ip_parameter_name: robot_ip,
+                    load_gripper_parameter_name: load_gripper,
+                    use_fake_hardware_parameter_name: use_fake_hardware,
+                    fake_sensor_commands_parameter_name: fake_sensor_commands,
+                    use_rviz_parameter_name: use_rviz,
+                }.items(),
+            ),
+            Node(
+                package='controller_manager',
+                executable='spawner',
+                arguments=['move_to_start_example_controller'],
+                output='screen',
+            ),
+        ]
+    )

franka_bringup/package.xml

@@ -0,0 +1,28 @@
+<?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>franka_bringup</name>
+  <version>0.1.0</version>
+  <description>Package with launch files and run-time configurations for using Franka Emika research robots with ros2_control</description>
+  <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>franka_description</exec_depend>
+  <exec_depend>franka_hardware</exec_depend>
+  <exec_depend>franka_robot_state_broadcaster</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

franka_description/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.5)
+project(franka_description)
+find_package(ament_cmake REQUIRED)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_add_pytest_test(${PROJECT_NAME}_urdf_tests test/urdf_tests.py)
+
+endif()
+
+# Install launch files.
+install(DIRECTORY
+  launch robots meshes rviz
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

franka_description/launch/visualize_franka.launch.py

@@ -0,0 +1,58 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    load_gripper_parameter_name = 'load_gripper'
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
+
+    franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots',
+                                     'panda_arm.urdf.xacro')
+    robot_description = Command(
+        [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=', load_gripper])
+
+    rviz_file = os.path.join(get_package_share_directory('franka_description'), 'rviz',
+                             'visualize_franka.rviz')
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            load_gripper_parameter_name,
+            default_value='true',
+            description='Use Franka Gripper as end-effector if true. Robot is loaded without '
+                        'end-effector otherwise'),
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[{'robot_description': robot_description}],
+        ),
+        Node(
+            package='joint_state_publisher_gui',
+            executable='joint_state_publisher_gui',
+            name='joint_state_publisher_gui'
+        ),
+        Node(package='rviz2',
+             executable='rviz2',
+             name='rviz2',
+             arguments=['--display-config', rviz_file])
+    ])

franka_description/package.xml

@@ -0,0 +1,24 @@
+<?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>franka_description</name>
+  <version>0.1.0</version>
+  <description>franka_description contains URDF files and meshes of Franka Emika robots</description>
+  <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
+  <license>Apache 2.0</license>
+  <author>Franka Emika GmbH</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+
+  <exec_depend>xacro</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

franka_description/robots/hand.urdf.xacro

@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">
+  <xacro:include filename="hand.xacro"/>
+  <xacro:hand ns="panda" safety_distance="0.03"/>
+</robot>

franka_description/robots/hand.xacro

@@ -0,0 +1,93 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">
+  <!-- safety_distance: Minimum safety distance in [m] by which the collision volumes are expanded and which is enforced during robot motions -->
+  <xacro:macro name="hand" params="connected_to:='' ns:='' rpy:='0 0 0' xyz:='0 0 0' safety_distance:=0">
+    <xacro:unless value="${connected_to == ''}">
+      <joint name="${ns}_hand_joint" type="fixed">
+        <parent link="${connected_to}"/>
+        <child link="${ns}_hand"/>
+        <origin xyz="${xyz}" rpy="${rpy}"/>
+      </joint>
+    </xacro:unless>
+    <link name="${ns}_hand">
+      <visual>
+        <geometry>
+          <mesh filename="package://franka_description/meshes/visual/hand.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0.04" rpy="0 ${pi/2} ${pi/2}"/>
+        <geometry>
+          <cylinder radius="${0.04+safety_distance}" length="0.1"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 -0.05 0.04" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.04+safety_distance}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0.05 0.04" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.04+safety_distance}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 0.1" rpy="0 ${pi/2} ${pi/2}"/>
+        <geometry>
+          <cylinder radius="${0.02+safety_distance}" length="0.1"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 -0.05 0.1" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.02+safety_distance}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0.05 0.1" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.02+safety_distance}"/>
+        </geometry>
+      </collision>
+    </link>
+    <!-- Define the hand_tcp frame -->
+    <link name="${ns}_hand_tcp"/>
+    <joint name="${ns}_hand_tcp_joint" type="fixed">
+      <origin xyz="0 0 0.1034" rpy="0 0 0"/>
+      <parent link="${ns}_hand"/>
+      <child link="${ns}_hand_tcp"/>
+    </joint>
+    <link name="${ns}_leftfinger">
+      <visual>
+        <geometry>
+          <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
+        </geometry>
+      </visual>
+    </link>
+    <link name="${ns}_rightfinger">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <geometry>
+          <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
+        </geometry>
+      </visual>
+    </link>
+    <joint name="${ns}_finger_joint1" type="prismatic">
+      <parent link="${ns}_hand"/>
+      <child link="${ns}_leftfinger"/>
+      <origin xyz="0 0 0.0584" rpy="0 0 0"/>
+      <axis xyz="0 1 0"/>
+      <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
+    </joint>
+    <joint name="${ns}_finger_joint2" type="prismatic">
+      <parent link="${ns}_hand"/>
+      <child link="${ns}_rightfinger"/>
+      <origin xyz="0 0 0.0584" rpy="0 0 0"/>
+      <axis xyz="0 -1 0"/>
+      <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
+      <mimic joint="${ns}_finger_joint1"/>
+    </joint>
+  </xacro:macro>
+</robot>

franka_description/robots/panda_arm.ros2_control.xacro

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="panda_arm_ros2_control" params="ns robot_ip use_fake_hardware:=^|false fake_sensor_commands:=^|false">
+    <ros2_control name="FrankaHardwareInterface" type="system">
+      <hardware>
+        <xacro:if value="${use_fake_hardware}">
+          <plugin>fake_components/GenericSystem</plugin>
+          <param name="fake_sensor_commands">${fake_sensor_commands}</param>
+          <param name="state_following_offset">0.0</param>
+        </xacro:if>
+        <xacro:unless value="${use_fake_hardware}">
+          <plugin>franka_hardware/FrankaHardwareInterface</plugin>
+          <param name="robot_ip">${robot_ip}</param>
+        </xacro:unless>
+      </hardware>
+
+      <xacro:macro name="configure_joint" params="joint_name initial_position">
+        <joint name="${joint_name}">
+          <param name="initial_position">${initial_position}</param>
+          <command_interface name="effort"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+          <state_interface name="effort"/>
+        </joint>
+      </xacro:macro>
+
+      <xacro:configure_joint joint_name="${ns}_joint1" initial_position="0.0"/>
+      <xacro:configure_joint joint_name="${ns}_joint2" initial_position="${-pi/4}"/>
+      <xacro:configure_joint joint_name="${ns}_joint3" initial_position="0.0"/>
+      <xacro:configure_joint joint_name="${ns}_joint4" initial_position="${-3*pi/4}"/>
+      <xacro:configure_joint joint_name="${ns}_joint5" initial_position="0.0"/>
+      <xacro:configure_joint joint_name="${ns}_joint6" initial_position="${pi/2}"/>
+      <xacro:configure_joint joint_name="${ns}_joint7" initial_position="${pi/4}"/>
+
+    </ros2_control>
+  </xacro:macro>
+</robot>

franka_description/robots/panda_arm.urdf.xacro

@@ -0,0 +1,18 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+  <xacro:arg name="arm_id" default="panda"/> <!-- Name of this panda -->
+  <xacro:arg name="hand" default="false"/> <!-- Should a franka_gripper be mounted at the flange?" -->
+  <xacro:arg name="robot_ip" default=""/> <!-- IP address or hostname of the robot" -->
+  <xacro:arg name="use_fake_hardware" default="false"/>
+  <xacro:arg name="fake_sensor_commands" default="false"/>
+
+  <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro"/>
+  <xacro:panda_arm arm_id="$(arg arm_id)" safety_distance="0.03"/>
+
+  <xacro:if value="$(arg hand)">
+    <xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
+    <xacro:hand ns="$(arg arm_id)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id)_link8" safety_distance="0.03"/>
+  </xacro:if>
+  <xacro:include filename="$(find franka_description)/robots/panda_arm.ros2_control.xacro"/>
+  <xacro:panda_arm_ros2_control ns="$(arg arm_id)" robot_ip="$(arg robot_ip)" use_fake_hardware="$(arg use_fake_hardware)" fake_sensor_commands="$(arg fake_sensor_commands)"/>
+</robot>

franka_description/robots/panda_arm.xacro

@@ -0,0 +1,314 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+  <!-- safety_distance: Minimum safety distance in [m] by which the collision volumes are expanded and which is enforced during robot motions -->
+  <!-- arm_id: Namespace of the panda arm. Serves to differentiate between arms in case of multiple instances. -->
+  <xacro:macro name="panda_arm" params="arm_id:='panda' description_pkg:='franka_description' connected_to:='' xyz:='0 0 0' rpy:='0 0 0' safety_distance:=0">
+    <xacro:unless value="${not connected_to}">
+      <joint name="${arm_id}_joint_${connected_to}" type="fixed">
+        <parent link="${connected_to}"/>
+        <child link="${arm_id}_link0"/>
+        <origin rpy="${rpy}" xyz="${xyz}"/>
+      </joint>
+    </xacro:unless>
+    <link name="${arm_id}_link0">
+      <visual>
+        <geometry>
+          <mesh filename="package://${description_pkg}/meshes/visual/link0.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="-0.075 0 0.06" rpy="0 ${pi/2} 0"/>
+        <geometry>
+          <cylinder radius="${0.06+safety_distance}" length="0.03" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="-0.06 0 0.06" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.06+safety_distance}" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="-0.09 0 0.06" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.06+safety_distance}" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="${arm_id}_link1">
+      <visual>
+        <geometry>
+          <mesh filename="package://${description_pkg}/meshes/visual/link1.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 -0.1915" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${0.06+safety_distance}" length="0.2830" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -0.333" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.06+safety_distance}" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -0.05" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.06+safety_distance}" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${arm_id}_joint1" type="revolute">
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+      <origin rpy="0 0 0" xyz="0 0 0.333"/>
+      <parent link="${arm_id}_link0"/>
+      <child link="${arm_id}_link1"/>
+      <axis xyz="0 0 1"/>
+      <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+    </joint>
+    <link name="${arm_id}_link2">
+      <visual>
+        <geometry>
+          <mesh filename="package://${description_pkg}/meshes/visual/link2.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${0.06+safety_distance}" length="0.12" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -0.06" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.06+safety_distance}" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 0.06" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.06+safety_distance}" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${arm_id}_joint2" type="revolute">
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
+      <origin rpy="${-pi/2} 0 0" xyz="0 0 0"/>
+      <parent link="${arm_id}_link1"/>
+      <child link="${arm_id}_link2"/>
+      <axis xyz="0 0 1"/>
+      <limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+    </joint>
+    <link name="${arm_id}_link3">
+      <visual>
+        <geometry>
+          <mesh filename="package://${description_pkg}/meshes/visual/link3.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 -0.145" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${0.06+safety_distance}" length="0.15" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -0.22" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.06+safety_distance}" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -0.07" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.06+safety_distance}" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${arm_id}_joint3" type="revolute">
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+      <origin rpy="${pi/2} 0 0" xyz="0 -0.316 0"/>
+      <parent link="${arm_id}_link2"/>
+      <child link="${arm_id}_link3"/>
+      <axis xyz="0 0 1"/>
+      <limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+    </joint>
+    <link name="${arm_id}_link4">
+      <visual>
+        <geometry>
+          <mesh filename="package://${description_pkg}/meshes/visual/link4.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${0.06+safety_distance}" length="0.12" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 0.06" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.06+safety_distance}" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -0.06" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.06+safety_distance}" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${arm_id}_joint4" type="revolute">
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
+      <origin rpy="${pi/2} 0 0" xyz="0.0825 0 0"/>
+      <parent link="${arm_id}_link3"/>
+      <child link="${arm_id}_link4"/>
+      <axis xyz="0 0 1"/>
+      <limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+    </joint>
+    <link name="${arm_id}_link5">
+      <visual>
+        <geometry>
+          <mesh filename="package://${description_pkg}/meshes/visual/link5.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 -0.26" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${0.06+safety_distance}" length="0.1" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -0.31" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.06+safety_distance}" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -0.21" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.06+safety_distance}" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0.08 -0.13" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${0.025+safety_distance}" length="0.14" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0.08 -0.06" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.025+safety_distance}" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0.08 -0.20" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.025+safety_distance}" />
+        </geometry>
+      </collision>
+
+    </link>
+    <joint name="${arm_id}_joint5" type="revolute">
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+      <origin rpy="${-pi/2} 0 0" xyz="-0.0825 0.384 0"/>
+      <parent link="${arm_id}_link4"/>
+      <child link="${arm_id}_link5"/>
+      <axis xyz="0 0 1"/>
+      <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+    </joint>
+    <link name="${arm_id}_link6">
+      <visual>
+        <geometry>
+          <mesh filename="package://${description_pkg}/meshes/visual/link6.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 -0.03" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${0.05+safety_distance}" length="0.08" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 0.01" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.05+safety_distance}" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -0.07" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.05+safety_distance}" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${arm_id}_joint6" type="revolute">
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
+      <origin rpy="${pi/2} 0 0" xyz="0 0 0"/>
+      <parent link="${arm_id}_link5"/>
+      <child link="${arm_id}_link6"/>
+      <axis xyz="0 0 1"/>
+      <limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
+    </joint>
+    <link name="${arm_id}_link7">
+      <visual>
+        <geometry>
+          <mesh filename="package://${description_pkg}/meshes/visual/link7.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0.01" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${0.04+safety_distance}" length="0.14" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 0.08" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.04+safety_distance}"  />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -0.06" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.04+safety_distance}"  />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${arm_id}_joint7" type="revolute">
+      <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
+      <origin rpy="${pi/2} 0 0" xyz="0.088 0 0"/>
+      <parent link="${arm_id}_link6"/>
+      <child link="${arm_id}_link7"/>
+      <axis xyz="0 0 1"/>
+      <limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+    </joint>
+    <link name="${arm_id}_link8">
+      <collision>
+        <origin xyz="0.0424 0.0424 -0.0250" rpy="${pi} ${pi/2} ${pi/2}"/>
+        <geometry>
+          <cylinder radius="${0.03+safety_distance}"  length="0.01" />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0.0424 0.0424 -0.02" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.03+safety_distance}"  />
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0.0424 0.0424 -0.03" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${0.03+safety_distance}"  />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${arm_id}_joint8" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 0.107"/>
+      <parent link="${arm_id}_link7"/>
+      <child link="${arm_id}_link8"/>
+    </joint>
+  </xacro:macro>
+</robot>

franka_description/rviz/visualize_franka.rviz

@@ -0,0 +1,208 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+        - /RobotModel1/Description Topic1
+        - /TF1
+      Splitter Ratio: 0.5
+    Tree Height: 814
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        panda_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: panda_link0
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 2.737480401992798
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.005491261370480061
+        Y: -0.07745686918497086
+        Z: 0.32312366366386414
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.23539823293685913
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.5790610313415527
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1043
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000003b9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003b9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003b9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003b9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c7000003b900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 0

franka_description/test/urdf_tests.py

@@ -0,0 +1,63 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+from os import path
+
+from ament_index_python.packages import get_package_share_directory
+import xacro
+
+panda_xacro_file_name = path.join(get_package_share_directory('franka_description'), 'robots',
+                                  'panda_arm.urdf.xacro')
+
+
+def test_load():
+    urdf = xacro.process_file(panda_xacro_file_name).toxml()
+    assert urdf.find('panda_finger_joint') == -1
+
+
+def test_load_with_gripper():
+    urdf = xacro.process_file(panda_xacro_file_name,
+                              mappings={'hand': 'true'}).toxml()
+    assert urdf.find('panda_finger_joint') != -1
+
+
+def test_load_with_fake_hardware():
+    urdf = xacro.process_file(panda_xacro_file_name,
+                              mappings={'use_fake_hardware': 'true'}).toxml()
+    assert urdf.find('fake_components/GenericSystem') != -1
+
+
+def test_load_with_robot_ip():
+    urdf = xacro.process_file(panda_xacro_file_name,
+                              mappings={'robot_ip': 'franka_ip_address'}).toxml()
+    assert urdf.find('franka_ip_address') != -1
+
+
+def test_load_with_arm_id():
+    urdf = xacro.process_file(panda_xacro_file_name,
+                              mappings={'arm_id': 'totally_different_arm'}).toxml()
+    assert urdf.find('totally_different_arm_joint1') != -1
+
+
+def test_check_interfaces():
+    urdf = xacro.process_file(panda_xacro_file_name).toxml()
+    assert urdf.find('state_interface') != -1
+    assert urdf.find('command_interface') != -1
+    assert urdf.find('position') != -1
+    assert urdf.find('velocity') != -1
+    assert urdf.find('effort') != -1
+
+
+if __name__ == '__main__':
+    pass

franka_example_controllers/CMakeLists.txt

@@ -0,0 +1,138 @@
+cmake_minimum_required(VERSION 3.5)
+project(franka_example_controllers)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+    set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+option(CHECK_TIDY "Adds clang-tidy tests" OFF)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(franka_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(franka_semantic_components)
+find_package(generate_parameter_library)
+
+add_library(
+        ${PROJECT_NAME}
+        SHARED
+        src/gravity_compensation_example_controller.cpp
+        src/joint_impedance_example_controller.cpp
+        src/model_example_controller.cpp
+        src/move_to_start_example_controller.cpp
+        src/motion_generator.cpp)
+target_include_directories(
+        ${PROJECT_NAME}
+        PUBLIC
+        include
+        ${EIGEN3_INCLUDE_DIRS}
+)
+ament_target_dependencies(
+        ${PROJECT_NAME}
+        controller_interface
+        hardware_interface
+        pluginlib
+        rclcpp
+        rclcpp_lifecycle
+        franka_semantic_components
+)
+
+generate_parameter_library(franka_example_controllers_parameters src/model_example_controller_parameters.yaml)
+
+target_link_libraries(${PROJECT_NAME} franka_example_controllers_parameters)
+
+pluginlib_export_plugin_description_file(
+        controller_interface franka_example_controllers.xml)
+
+install(
+        TARGETS
+        ${PROJECT_NAME}
+        RUNTIME DESTINATION bin
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+)
+
+install(
+        DIRECTORY include/
+        DESTINATION include
+)
+
+if(BUILD_TESTING)
+    find_package(ament_cmake_clang_format REQUIRED)
+    find_package(ament_cmake_copyright REQUIRED)
+    find_package(ament_cmake_cppcheck REQUIRED)
+    find_package(ament_cmake_flake8 REQUIRED)
+    find_package(ament_cmake_lint_cmake REQUIRED)
+    find_package(ament_cmake_pep257 REQUIRED)
+    find_package(ament_cmake_xmllint REQUIRED)
+    find_package(ament_cmake_gmock REQUIRED)
+    find_package(controller_manager REQUIRED)
+    find_package(ros2_control_test_assets REQUIRED)
+
+    set(CPP_DIRECTORIES src include)
+    ament_clang_format(CONFIG_FILE ../.clang-format ${CPP_DIRECTORIES})
+    ament_copyright(src ${CPP_DIRECTORIES} package.xml)
+    ament_cppcheck(${CPP_DIRECTORIES})
+    ament_lint_cmake(CMakeLists.txt)
+    ament_flake8()
+    ament_pep257()
+    ament_xmllint()
+    ament_add_gmock(${PROJECT_NAME}_test
+        test/test_load_gravity_compensation_controller.cpp
+    )
+    target_include_directories(${PROJECT_NAME}_test PRIVATE include)
+    ament_target_dependencies(${PROJECT_NAME}_test
+        controller_manager
+        ros2_control_test_assets
+    )
+
+    ament_add_gmock(${PROJECT_NAME}_test_load_move_to_start
+        test/test_load_move_to_start_example_controller.cpp)
+    target_include_directories(${PROJECT_NAME}_test_load_move_to_start PRIVATE include)
+    ament_target_dependencies(${PROJECT_NAME}_test_load_move_to_start
+        controller_manager
+        ros2_control_test_assets
+    )
+
+
+    ament_add_gmock(${PROJECT_NAME}_gravity_test test/test_gravity_compensation_example.cpp)
+    target_include_directories(${PROJECT_NAME}_gravity_test PRIVATE include)
+    target_link_libraries(${PROJECT_NAME}_gravity_test ${PROJECT_NAME})
+
+    ament_add_gmock(${PROJECT_NAME}_move_to_start_test test/test_move_to_start_example_controller.cpp)
+    target_include_directories(${PROJECT_NAME}_move_to_start_test PRIVATE include)
+    target_link_libraries(${PROJECT_NAME}_move_to_start_test ${PROJECT_NAME})
+
+    if(CHECK_TIDY)
+        find_package(ament_cmake_clang_tidy REQUIRED)
+        set(ament_cmake_clang_tidy_CONFIG_FILE ../.clang-tidy)
+        ament_clang_tidy(${CMAKE_BINARY_DIR})
+    endif()
+endif()
+
+ament_export_include_directories(
+        include
+)
+ament_export_libraries(
+        ${PROJECT_NAME}
+)
+ament_export_dependencies(
+        controller_interface
+        pluginlib
+        rclcpp
+        rclcpp_lifecycle
+        hardware_interface
+)
+ament_package()

franka_example_controllers/franka_example_controllers.xml

@@ -0,0 +1,26 @@
+<library path="franka_example_controllers">
+    <class name="franka_example_controllers/GravityCompensationExampleController"
+           type="franka_example_controllers::GravityCompensationExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            The gravity compensation controller only sends zero torques so that the robot does gravity compensation
+        </description>
+    </class>
+    <class name="franka_example_controllers/JointImpedanceExampleController"
+           type="franka_example_controllers::JointImpedanceExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            The joint impedance example controller moves joint 4 and 5 in a very compliant periodic movement.
+        </description>
+    </class>
+    <class name="franka_example_controllers/MoveToStartExampleController"
+           type="franka_example_controllers::MoveToStartExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            The move to start example controller moves the robot into default pose.
+        </description>
+    </class>
+    <class name="franka_example_controllers/ModelExampleController"
+           type="franka_example_controllers::ModelExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            The franka model example controller evaluates and prints the dynamic model of Franka Emika Robots.
+        </description>
+    </class>
+</library>

franka_example_controllers/include/franka_example_controllers/gravity_compensation_example_controller.hpp

@@ -0,0 +1,57 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// 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.
+
+#pragma once
+
+#include <string>
+
+#include <controller_interface/controller_interface.hpp>
+#include "franka_example_controllers/visibility_control.h"
+
+#include <rclcpp/duration.hpp>
+#include <rclcpp/time.hpp>
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace franka_example_controllers {
+
+/**
+ * The gravity compensation controller only sends zero torques so that the robot does gravity
+ * compensation
+ */
+class GravityCompensationExampleController : public controller_interface::ControllerInterface {
+ public:
+  FRANKA_EXAMPLE_CONTROLLERS_PUBLIC
+  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+  FRANKA_EXAMPLE_CONTROLLERS_PUBLIC
+  CallbackReturn on_init() override;
+
+  FRANKA_EXAMPLE_CONTROLLERS_PUBLIC
+  [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration()
+      const override;
+
+  FRANKA_EXAMPLE_CONTROLLERS_PUBLIC
+  [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration()
+      const override;
+
+  FRANKA_EXAMPLE_CONTROLLERS_PUBLIC
+  controller_interface::return_type update(const rclcpp::Time& time,
+                                           const rclcpp::Duration& period) override;
+
+ private:
+  std::string arm_id_;
+  const int num_joints = 7;
+};
+}  // namespace franka_example_controllers

franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.hpp

@@ -0,0 +1,56 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// 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.
+
+#pragma once
+
+#include <string>
+
+#include <Eigen/Eigen>
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace franka_example_controllers {
+
+/**
+ * The joint impedance example controller moves joint 4 and 5 in a very compliant periodic movement.
+ */
+class JointImpedanceExampleController : public controller_interface::ControllerInterface {
+ public:
+  using Vector7d = Eigen::Matrix<double, 7, 1>;
+  [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration()
+      const override;
+  [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration()
+      const override;
+  controller_interface::return_type update(const rclcpp::Time& time,
+                                           const rclcpp::Duration& period) override;
+  CallbackReturn on_init() override;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+ private:
+  std::string arm_id_;
+  const int num_joints = 7;
+  Vector7d q_;
+  Vector7d initial_q_;
+  Vector7d dq_;
+  Vector7d dq_filtered_;
+  Vector7d k_gains_;
+  Vector7d d_gains_;
+  rclcpp::Time start_time_;
+  void updateJointStates();
+};
+
+}  // namespace franka_example_controllers

franka_example_controllers/include/franka_example_controllers/model_example_controller.hpp

@@ -0,0 +1,57 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// 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.
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include "franka_semantic_components/franka_robot_model.hpp"
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace franka_example_controllers {
+
+/// The model example controller prints robot model parameters.
+class ModelExampleController : public controller_interface::ControllerInterface {
+ public:
+  [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration()
+      const override;
+
+  [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration()
+      const override;
+
+  controller_interface::return_type update(const rclcpp::Time& time,
+                                           const rclcpp::Duration& period) override;
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::CallbackReturn on_configure(
+      const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::CallbackReturn on_activate(
+      const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::CallbackReturn on_deactivate(
+      const rclcpp_lifecycle::State& previous_state) override;
+
+ private:
+  std::string arm_id_;
+  std::unique_ptr<franka_semantic_components::FrankaRobotModel> franka_robot_model_;
+
+  const std::string k_robot_state_interface_name{"robot_state"};
+  const std::string k_robot_model_interface_name{"robot_model"};
+};
+}  // namespace franka_example_controllers

franka_example_controllers/include/franka_example_controllers/motion_generator.hpp

@@ -0,0 +1,73 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// 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.
+
+#pragma once
+
+#include <array>
+#include <utility>
+
+#include <Eigen/Core>
+#include <rclcpp/duration.hpp>
+/**
+ * An example showing how to generate a joint pose motion to a goal position. Adapted from:
+ * Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots
+ * (Kogan Page Science Paper edition).
+ */
+class MotionGenerator {
+ public:
+  using Vector7d = Eigen::Matrix<double, 7, 1>;
+  /**
+   * Creates a new MotionGenerator instance for a target q.
+   *
+   * @param[in] speed_factor General speed factor in range (0, 1].
+   * @param[in] q_start Start joint positions.
+   * @param[in] q_goal Target joint positions.
+   */
+  MotionGenerator(double speed_factor, const Vector7d& q_start, const Vector7d& q_goal);
+
+  /**
+   * Sends joint position calculations
+   *
+   * @param[in] robot_state Current state of the robot.
+   * @param[in] trajectory_time Amount of time, that has passed since the start of the trajectory.
+   *
+   * @return Joint positions to use inside a control loop and a boolean indicating whether the
+   * motion is finished.
+   */
+  std::pair<Vector7d, bool> getDesiredJointPositions(const rclcpp::Duration& trajectory_time);
+
+ private:
+  using Vector7i = Eigen::Matrix<int, 7, 1>;
+
+  bool calculateDesiredValues(double time, Vector7d* delta_q_d) const;
+  void calculateSynchronizedValues();
+
+  static constexpr double kDeltaQMotionFinished = 1e-6;
+  static const int kJoints = 7;
+
+  Vector7d q_start_;
+  Vector7d delta_q_;
+
+  Vector7d dq_max_sync_;
+  Vector7d t_1_sync_;
+  Vector7d t_2_sync_;
+  Vector7d t_f_sync_;
+  Vector7d q_1_;
+
+  double time_ = 0.0;
+
+  Vector7d dq_max_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished();  // in m/s
+  Vector7d ddq_max_start_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();         // in m/s^2
+  Vector7d ddq_max_goal_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();          // in m/s^2
+};

franka_example_controllers/include/franka_example_controllers/move_to_start_example_controller.hpp

@@ -0,0 +1,58 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// 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.
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <Eigen/Eigen>
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include "motion_generator.hpp"
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace franka_example_controllers {
+
+/// The move to start example controller moves the robot into default pose.
+class MoveToStartExampleController : public controller_interface::ControllerInterface {
+ public:
+  using Vector7d = Eigen::Matrix<double, 7, 1>;
+  [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration()
+      const override;
+  [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration()
+      const override;
+  controller_interface::return_type update(const rclcpp::Time& time,
+                                           const rclcpp::Duration& period) override;
+  CallbackReturn on_init() override;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+ private:
+  std::string arm_id_;
+  const int num_joints = 7;
+  Vector7d q_;
+  Vector7d q_goal_;
+  Vector7d dq_;
+  Vector7d dq_filtered_;
+  Vector7d k_gains_;
+  Vector7d d_gains_;
+  rclcpp::Time start_time_;
+  std::unique_ptr<MotionGenerator> motion_generator_;
+
+  void updateJointStates();
+};
+}  // namespace franka_example_controllers

franka_example_controllers/include/franka_example_controllers/visibility_control.h

@@ -0,0 +1,55 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// 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.
+
+/* This header must be included by all rclcpp headers which declare symbols
+ * which are defined in the rclcpp library. When not building the rclcpp
+ * library, i.e. when using the headers in other package's code, the contents
+ * of this header change the visibility of certain symbols which the rclcpp
+ * library cannot have, but the consuming code must have inorder to link.
+ */
+#ifndef FRANKA_EXAMPLE_CONTROLLERS__VISIBILITY_CONTROL_H_
+#define FRANKA_EXAMPLE_CONTROLLERS__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define FRANKA_EXAMPLE_CONTROLLERS_EXPORT __attribute__((dllexport))
+#define FRANKA_EXAMPLE_CONTROLLERS_IMPORT __attribute__((dllimport))
+#else
+#define FRANKA_EXAMPLE_CONTROLLERS_EXPORT __declspec(dllexport)
+#define FRANKA_EXAMPLE_CONTROLLERS_IMPORT __declspec(dllimport)
+#endif
+#ifdef FRANKA_EXAMPLE_CONTROLLERS_BUILDING_DLL
+#define FRANKA_EXAMPLE_CONTROLLERS_PUBLIC FRANKA_EXAMPLE_CONTROLLERS_EXPORT
+#else
+#define FRANKA_EXAMPLE_CONTROLLERS_PUBLIC FRANKA_EXAMPLE_CONTROLLERS_IMPORT
+#endif
+#define FRANKA_EXAMPLE_CONTROLLERS_PUBLIC_TYPE FRANKA_EXAMPLE_CONTROLLERS_PUBLIC
+#define FRANKA_EXAMPLE_CONTROLLERS_LOCAL
+#else
+#define FRANKA_EXAMPLE_CONTROLLERS_EXPORT __attribute__((visibility("default")))
+#define FRANKA_EXAMPLE_CONTROLLERS_IMPORT
+#if __GNUC__ >= 4
+#define FRANKA_EXAMPLE_CONTROLLERS_PUBLIC __attribute__((visibility("default")))
+#define FRANKA_EXAMPLE_CONTROLLERS_LOCAL __attribute__((visibility("hidden")))
+#else
+#define FRANKA_EXAMPLE_CONTROLLERS_PUBLIC
+#define FRANKA_EXAMPLE_CONTROLLERS_LOCAL
+#endif
+#define FRANKA_EXAMPLE_CONTROLLERS_PUBLIC_TYPE
+#endif
+
+#endif  // FRANKA_EXAMPLE_CONTROLLERS__VISIBILITY_CONTROL_H_

franka_example_controllers/package.xml

@@ -0,0 +1,36 @@
+<?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>franka_example_controllers</name>
+  <version>0.1.0</version>
+  <description>franka_example_controllers provides example code for controlling Franka Emika research robots with ros2_control</description>
+  <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>controller_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>franka_msgs</depend>
+  <depend>franka_semantic_components</depend>
+  
+
+  <test_depend>ament_cmake_clang_format</test_depend>
+  <test_depend>ament_cmake_copyright</test_depend>
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_cmake_flake8</test_depend>
+  <test_depend>ament_cmake_lint_cmake</test_depend>
+  <test_depend>ament_cmake_pep257</test_depend>
+  <test_depend>ament_cmake_xmllint</test_depend>
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ament_cmake_clang_tidy</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

franka_example_controllers/src/gravity_compensation_example_controller.cpp

@@ -0,0 +1,66 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// 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.
+
+#include <franka_example_controllers/gravity_compensation_example_controller.hpp>
+
+#include <exception>
+#include <string>
+
+namespace franka_example_controllers {
+
+controller_interface::InterfaceConfiguration
+GravityCompensationExampleController::command_interface_configuration() const {
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  for (int i = 1; i <= num_joints; ++i) {
+    config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/effort");
+  }
+  return config;
+}
+
+controller_interface::InterfaceConfiguration
+GravityCompensationExampleController::state_interface_configuration() const {
+  return {};
+}
+
+controller_interface::return_type GravityCompensationExampleController::update(
+    const rclcpp::Time& /*time*/,
+    const rclcpp::Duration& /*period*/) {
+  for (auto& command_interface : command_interfaces_) {
+    command_interface.set_value(0);
+  }
+  return controller_interface::return_type::OK;
+}
+
+CallbackReturn GravityCompensationExampleController::on_configure(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  arm_id_ = get_node()->get_parameter("arm_id").as_string();
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn GravityCompensationExampleController::on_init() {
+  try {
+    auto_declare<std::string>("arm_id", "panda");
+  } catch (const std::exception& e) {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    return CallbackReturn::ERROR;
+  }
+  return CallbackReturn::SUCCESS;
+}
+}  // namespace franka_example_controllers
+#include "pluginlib/class_list_macros.hpp"
+// NOLINTNEXTLINE
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::GravityCompensationExampleController,
+                       controller_interface::ControllerInterface)

franka_example_controllers/src/joint_impedance_example_controller.cpp

@@ -0,0 +1,136 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// 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.
+
+#include <franka_example_controllers/joint_impedance_example_controller.hpp>
+
+#include <cassert>
+#include <cmath>
+#include <exception>
+#include <string>
+
+#include <Eigen/Eigen>
+
+namespace franka_example_controllers {
+
+controller_interface::InterfaceConfiguration
+JointImpedanceExampleController::command_interface_configuration() const {
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  for (int i = 1; i <= num_joints; ++i) {
+    config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/effort");
+  }
+  return config;
+}
+
+controller_interface::InterfaceConfiguration
+JointImpedanceExampleController::state_interface_configuration() const {
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (int i = 1; i <= num_joints; ++i) {
+    config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/position");
+    config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/velocity");
+  }
+  return config;
+}
+
+controller_interface::return_type JointImpedanceExampleController::update(
+    const rclcpp::Time& /*time*/,
+    const rclcpp::Duration& /*period*/) {
+  updateJointStates();
+  Vector7d q_goal = initial_q_;
+  auto time = this->get_node()->now() - start_time_;
+  double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time.seconds()));
+  q_goal(3) += delta_angle;
+  q_goal(4) += delta_angle;
+
+  const double kAlpha = 0.99;
+  dq_filtered_ = (1 - kAlpha) * dq_filtered_ + kAlpha * dq_;
+  Vector7d tau_d_calculated =
+      k_gains_.cwiseProduct(q_goal - q_) + d_gains_.cwiseProduct(-dq_filtered_);
+  for (int i = 0; i < num_joints; ++i) {
+    command_interfaces_[i].set_value(tau_d_calculated(i));
+  }
+  return controller_interface::return_type::OK;
+}
+
+CallbackReturn JointImpedanceExampleController::on_init() {
+  try {
+    auto_declare<std::string>("arm_id", "panda");
+    auto_declare<std::vector<double>>("k_gains", {});
+    auto_declare<std::vector<double>>("d_gains", {});
+  } catch (const std::exception& e) {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    return CallbackReturn::ERROR;
+  }
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn JointImpedanceExampleController::on_configure(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  arm_id_ = get_node()->get_parameter("arm_id").as_string();
+  auto k_gains = get_node()->get_parameter("k_gains").as_double_array();
+  auto d_gains = get_node()->get_parameter("d_gains").as_double_array();
+  if (k_gains.empty()) {
+    RCLCPP_FATAL(get_node()->get_logger(), "k_gains parameter not set");
+    return CallbackReturn::FAILURE;
+  }
+  if (k_gains.size() != static_cast<uint>(num_joints)) {
+    RCLCPP_FATAL(get_node()->get_logger(), "k_gains should be of size %d but is of size %ld",
+                 num_joints, k_gains.size());
+    return CallbackReturn::FAILURE;
+  }
+  if (d_gains.empty()) {
+    RCLCPP_FATAL(get_node()->get_logger(), "d_gains parameter not set");
+    return CallbackReturn::FAILURE;
+  }
+  if (d_gains.size() != static_cast<uint>(num_joints)) {
+    RCLCPP_FATAL(get_node()->get_logger(), "d_gains should be of size %d but is of size %ld",
+                 num_joints, d_gains.size());
+    return CallbackReturn::FAILURE;
+  }
+  for (int i = 0; i < num_joints; ++i) {
+    d_gains_(i) = d_gains.at(i);
+    k_gains_(i) = k_gains.at(i);
+  }
+  dq_filtered_.setZero();
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn JointImpedanceExampleController::on_activate(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  updateJointStates();
+  initial_q_ = q_;
+  start_time_ = this->get_node()->now();
+  return CallbackReturn::SUCCESS;
+}
+
+void JointImpedanceExampleController::updateJointStates() {
+  for (auto i = 0; i < num_joints; ++i) {
+    const auto& position_interface = state_interfaces_.at(2 * i);
+    const auto& velocity_interface = state_interfaces_.at(2 * i + 1);
+
+    assert(position_interface.get_interface_name() == "position");
+    assert(velocity_interface.get_interface_name() == "velocity");
+
+    q_(i) = position_interface.get_value();
+    dq_(i) = velocity_interface.get_value();
+  }
+}
+
+}  // namespace franka_example_controllers
+#include "pluginlib/class_list_macros.hpp"
+// NOLINTNEXTLINE
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointImpedanceExampleController,
+                       controller_interface::ControllerInterface)

franka_example_controllers/src/model_example_controller.cpp

@@ -0,0 +1,124 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// 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.
+
+#include <franka_example_controllers/model_example_controller.hpp>
+
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
+namespace {
+template <class T, size_t N>
+std::ostream& operator<<(std::ostream& ostream, const std::array<T, N>& array) {
+  ostream << "[";
+  std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
+  std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
+  ostream << "]";
+  return ostream;
+}
+}  // anonymous namespace
+
+namespace franka_example_controllers {
+
+controller_interface::CallbackReturn ModelExampleController::on_init() {
+  try {
+    if (!get_node()->get_parameter("arm_id", arm_id_)) {
+      RCLCPP_FATAL(get_node()->get_logger(), "Failed to get arm_id parameter");
+      get_node()->shutdown();
+      return CallbackReturn::ERROR;
+    }
+  } catch (const std::exception& e) {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    return CallbackReturn::ERROR;
+  }
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration
+ModelExampleController::command_interface_configuration() const {
+  return controller_interface::InterfaceConfiguration{
+      controller_interface::interface_configuration_type::NONE};
+}
+
+controller_interface::InterfaceConfiguration ModelExampleController::state_interface_configuration()
+    const {
+  controller_interface::InterfaceConfiguration state_interfaces_config;
+  state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto& franka_robot_model_name : franka_robot_model_->get_state_interface_names()) {
+    state_interfaces_config.names.push_back(franka_robot_model_name);
+  }
+  return state_interfaces_config;
+}
+
+controller_interface::CallbackReturn ModelExampleController::on_configure(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  franka_robot_model_ = std::make_unique<franka_semantic_components::FrankaRobotModel>(
+      franka_semantic_components::FrankaRobotModel(arm_id_ + "/" + k_robot_model_interface_name,
+                                                   arm_id_ + "/" + k_robot_state_interface_name));
+
+  RCLCPP_DEBUG(get_node()->get_logger(), "configured successfully");
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn ModelExampleController::on_activate(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  franka_robot_model_->assign_loaned_state_interfaces(state_interfaces_);
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn ModelExampleController::on_deactivate(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  franka_robot_model_->release_interfaces();
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type ModelExampleController::update(
+    const rclcpp::Time& /*time*/,
+    const rclcpp::Duration& /*period*/) {
+  std::array<double, 49> mass = franka_robot_model_->getMassMatrix();
+  std::array<double, 7> coriolis = franka_robot_model_->getCoriolisForceVector();
+  std::array<double, 7> gravity = franka_robot_model_->getGravityForceVector();
+  std::array<double, 16> pose = franka_robot_model_->getPoseMatrix(franka::Frame::kJoint4);
+  std::array<double, 42> joint4_body_jacobian_wrt_joint4 =
+      franka_robot_model_->getBodyJacobian(franka::Frame::kJoint4);
+  std::array<double, 42> endeffector_jacobian_wrt_base =
+      franka_robot_model_->getZeroJacobian(franka::Frame::kEndEffector);
+
+  RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
+                       "-------------------------------------------------------------");
+  RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
+                              "mass :" << mass);
+  RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
+                              "coriolis :" << coriolis);
+  RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
+                              "gravity :" << gravity);
+  RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
+                              "joint_pose :" << pose);
+  RCLCPP_INFO_STREAM_THROTTLE(
+      get_node()->get_logger(), *get_node()->get_clock(), 1000,
+      "joint4_body_jacobian in joint4 frame :" << joint4_body_jacobian_wrt_joint4);
+  RCLCPP_INFO_STREAM_THROTTLE(
+      get_node()->get_logger(), *get_node()->get_clock(), 1000,
+      "end_effector_jacobian in base frame :" << endeffector_jacobian_wrt_base);
+  RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
+                       "-------------------------------------------------------------");
+
+  return controller_interface::return_type::OK;
+}
+
+}  // namespace franka_example_controllers
+
+#include "pluginlib/class_list_macros.hpp"
+// NOLINTNEXTLINE
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ModelExampleController,
+                       controller_interface::ControllerInterface)

franka_example_controllers/src/model_example_controller_parameters.yaml

@@ -0,0 +1,5 @@
+model_example_controller:
+  arm_id: {
+    type: string,
+    default_value: "panda",
+  }

franka_example_controllers/src/motion_generator.cpp

@@ -0,0 +1,127 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// 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.
+
+#include <franka_example_controllers/motion_generator.hpp>
+
+#include <algorithm>
+#include <array>
+#include <cassert>
+#include <cmath>
+#include <utility>
+
+#include <Eigen/Core>
+
+MotionGenerator::MotionGenerator(double speed_factor,
+                                 const Vector7d& q_start,
+                                 const Vector7d& q_goal)
+    : q_start_(q_start) {
+  assert(speed_factor > 0);
+  assert(speed_factor <= 1);
+  delta_q_ = q_goal - q_start;
+  dq_max_ *= speed_factor;
+  ddq_max_start_ *= speed_factor;
+  ddq_max_goal_ *= speed_factor;
+  calculateSynchronizedValues();
+}
+
+bool MotionGenerator::calculateDesiredValues(double time, Vector7d* delta_q_d) const {
+  Vector7i sign_delta_q;
+  sign_delta_q << delta_q_.cwiseSign().cast<int>();
+  Vector7d t_d = t_2_sync_ - t_1_sync_;
+  Vector7d delta_t_2_sync = t_f_sync_ - t_2_sync_;
+  std::array<bool, kJoints> joint_motion_finished{};
+
+  for (auto i = 0; i < kJoints; i++) {
+    if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) {
+      (*delta_q_d)[i] = 0;
+      joint_motion_finished.at(i) = true;
+    } else {
+      if (time < t_1_sync_[i]) {
+        (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] *
+                          (0.5 * time - t_1_sync_[i]) * std::pow(time, 3.0);
+      } else if (time >= t_1_sync_[i] && time < t_2_sync_[i]) {
+        (*delta_q_d)[i] = q_1_[i] + (time - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i];
+      } else if (time >= t_2_sync_[i] && time < t_f_sync_[i]) {
+        (*delta_q_d)[i] =
+            delta_q_[i] +
+            0.5 *
+                (1.0 / std::pow(delta_t_2_sync[i], 3.0) *
+                     (time - t_1_sync_[i] - 2.0 * delta_t_2_sync[i] - t_d[i]) *
+                     std::pow((time - t_1_sync_[i] - t_d[i]), 3.0) +
+                 (2.0 * time - 2.0 * t_1_sync_[i] - delta_t_2_sync[i] - 2.0 * t_d[i])) *
+                dq_max_sync_[i] * sign_delta_q[i];
+      } else {
+        (*delta_q_d)[i] = delta_q_[i];
+        joint_motion_finished.at(i) = true;
+      }
+    }
+  }
+  return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(),
+                     [](bool each_joint_finished) { return each_joint_finished; });
+}
+
+void MotionGenerator::calculateSynchronizedValues() {
+  Vector7d dq_max_reach(dq_max_);
+  Vector7d t_f = Vector7d::Zero();
+  Vector7d delta_t_2 = Vector7d::Zero();
+  Vector7d t_1 = Vector7d::Zero();
+  Vector7d delta_t_2_sync = Vector7d::Zero();
+  Vector7i sign_delta_q;
+  sign_delta_q << delta_q_.cwiseSign().cast<int>();
+
+  for (auto i = 0; i < kJoints; i++) {
+    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
+      if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_start_[i]) +
+                                   3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_goal_[i]))) {
+        dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] *
+                                    (ddq_max_start_[i] * ddq_max_goal_[i]) /
+                                    (ddq_max_start_[i] + ddq_max_goal_[i]));
+      }
+      t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i];
+      delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i];
+      t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i];
+    }
+  }
+  double max_t_f = t_f.maxCoeff();
+  for (auto i = 0; i < kJoints; i++) {
+    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
+      double param_a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]);
+      double param_b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i];
+      double param_c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i];
+      double delta = param_b * param_b - 4.0 * param_a * param_c;
+      if (delta < 0.0) {
+        delta = 0.0;
+      }
+      dq_max_sync_[i] = (-1.0 * param_b - std::sqrt(delta)) / (2.0 * param_a);
+      t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i];
+      delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i];
+      t_f_sync_[i] =
+          (t_1_sync_)[i] / 2.0 + delta_t_2_sync[i] / 2.0 + std::abs(delta_q_[i] / dq_max_sync_[i]);
+      t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i];
+      q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]);
+    }
+  }
+}
+
+std::pair<MotionGenerator::Vector7d, bool> MotionGenerator::getDesiredJointPositions(
+    const rclcpp::Duration& trajectory_time) {
+  time_ = trajectory_time.seconds();
+
+  Vector7d delta_q_d;
+  bool motion_finished = calculateDesiredValues(time_, &delta_q_d);
+
+  std::array<double, kJoints> joint_positions{};
+  Eigen::VectorXd::Map(joint_positions.data(), kJoints) = (q_start_ + delta_q_d);
+  return std::make_pair(q_start_ + delta_q_d, motion_finished);
+}

franka_example_controllers/src/move_to_start_example_controller.cpp

@@ -0,0 +1,142 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// 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.
+
+#include <franka_example_controllers/move_to_start_example_controller.hpp>
+
+#include <cassert>
+#include <cmath>
+#include <exception>
+
+#include <Eigen/Eigen>
+#include <controller_interface/controller_interface.hpp>
+
+namespace franka_example_controllers {
+
+controller_interface::InterfaceConfiguration
+MoveToStartExampleController::command_interface_configuration() const {
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  for (int i = 1; i <= num_joints; ++i) {
+    config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/effort");
+  }
+  return config;
+}
+
+controller_interface::InterfaceConfiguration
+MoveToStartExampleController::state_interface_configuration() const {
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (int i = 1; i <= num_joints; ++i) {
+    config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/position");
+    config.names.push_back(arm_id_ + "_joint" + std::to_string(i) + "/velocity");
+  }
+  return config;
+}
+
+controller_interface::return_type MoveToStartExampleController::update(
+    const rclcpp::Time& /*time*/,
+    const rclcpp::Duration& /*period*/) {
+  updateJointStates();
+  auto trajectory_time = this->get_node()->now() - start_time_;
+  auto motion_generator_output = motion_generator_->getDesiredJointPositions(trajectory_time);
+  Vector7d q_desired = motion_generator_output.first;
+  bool finished = motion_generator_output.second;
+  if (not finished) {
+    const double kAlpha = 0.99;
+    dq_filtered_ = (1 - kAlpha) * dq_filtered_ + kAlpha * dq_;
+    Vector7d tau_d_calculated =
+        k_gains_.cwiseProduct(q_desired - q_) + d_gains_.cwiseProduct(-dq_filtered_);
+    for (int i = 0; i < 7; ++i) {
+      command_interfaces_[i].set_value(tau_d_calculated(i));
+    }
+  } else {
+    for (auto& command_interface : command_interfaces_) {
+      command_interface.set_value(0);
+    }
+    this->get_node()->set_parameter({"process_finished", true});
+  }
+  return controller_interface::return_type::OK;
+}
+
+CallbackReturn MoveToStartExampleController::on_init() {
+  q_goal_ << 0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4;
+  try {
+    auto_declare<bool>("process_finished", false);
+    auto_declare<std::string>("arm_id", "panda");
+    auto_declare<std::vector<double>>("k_gains", {});
+    auto_declare<std::vector<double>>("d_gains", {});
+  } catch (const std::exception& e) {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    return CallbackReturn::ERROR;
+  }
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn MoveToStartExampleController::on_configure(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  arm_id_ = get_node()->get_parameter("arm_id").as_string();
+  auto k_gains = get_node()->get_parameter("k_gains").as_double_array();
+  auto d_gains = get_node()->get_parameter("d_gains").as_double_array();
+  if (k_gains.empty()) {
+    RCLCPP_FATAL(get_node()->get_logger(), "k_gains parameter not set");
+    return CallbackReturn::FAILURE;
+  }
+  if (k_gains.size() != static_cast<uint>(num_joints)) {
+    RCLCPP_FATAL(get_node()->get_logger(), "k_gains should be of size %d but is of size %ld",
+                 num_joints, k_gains.size());
+    return CallbackReturn::FAILURE;
+  }
+  if (d_gains.empty()) {
+    RCLCPP_FATAL(get_node()->get_logger(), "d_gains parameter not set");
+    return CallbackReturn::FAILURE;
+  }
+  if (d_gains.size() != static_cast<uint>(num_joints)) {
+    RCLCPP_FATAL(get_node()->get_logger(), "d_gains should be of size %d but is of size %ld",
+                 num_joints, d_gains.size());
+    return CallbackReturn::FAILURE;
+  }
+  for (int i = 0; i < num_joints; ++i) {
+    d_gains_(i) = d_gains.at(i);
+    k_gains_(i) = k_gains.at(i);
+  }
+  dq_filtered_.setZero();
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn MoveToStartExampleController::on_activate(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  updateJointStates();
+  motion_generator_ = std::make_unique<MotionGenerator>(0.2, q_, q_goal_);
+  start_time_ = this->get_node()->now();
+  return CallbackReturn::SUCCESS;
+}
+
+void MoveToStartExampleController::updateJointStates() {
+  for (auto i = 0; i < num_joints; ++i) {
+    const auto& position_interface = state_interfaces_.at(2 * i);
+    const auto& velocity_interface = state_interfaces_.at(2 * i + 1);
+
+    assert(position_interface.get_interface_name() == "position");
+    assert(velocity_interface.get_interface_name() == "velocity");
+
+    q_(i) = position_interface.get_value();
+    dq_(i) = velocity_interface.get_value();
+  }
+}
+}  // namespace franka_example_controllers
+#include "pluginlib/class_list_macros.hpp"
+// NOLINTNEXTLINE
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::MoveToStartExampleController,
+                       controller_interface::ControllerInterface)

franka_example_controllers/test/test_gravity_compensation_example.cpp

@@ -0,0 +1,136 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// 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.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <vector>
+
+#include "franka_example_controllers/gravity_compensation_example_controller.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+
+using hardware_interface::CommandInterface;
+using hardware_interface::HW_IF_EFFORT;
+using hardware_interface::LoanedCommandInterface;
+
+class TestGravityCompensationExample : public ::testing::Test {
+ public:
+  static void SetUpTestSuite();
+  static void TearDownTestSuite();
+
+  void SetUp();
+  void TearDown();
+
+  void SetUpController();
+
+ protected:
+  std::unique_ptr<franka_example_controllers::GravityCompensationExampleController> controller_;
+
+  // dummy joint state values used for tests
+  const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3", "joint4",
+                                                 "joint5", "joint6", "joint7"};
+  std::vector<double> joint_commands_ = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
+
+  CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]};
+  CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]};
+  CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]};
+  CommandInterface joint_4_pos_cmd_{joint_names_[3], HW_IF_EFFORT, &joint_commands_[3]};
+  CommandInterface joint_5_pos_cmd_{joint_names_[4], HW_IF_EFFORT, &joint_commands_[4]};
+  CommandInterface joint_6_pos_cmd_{joint_names_[5], HW_IF_EFFORT, &joint_commands_[5]};
+  CommandInterface joint_7_pos_cmd_{joint_names_[6], HW_IF_EFFORT, &joint_commands_[6]};
+};
+
+void TestGravityCompensationExample::SetUpTestSuite() {
+  rclcpp::init(0, nullptr);
+}
+
+void TestGravityCompensationExample::TearDownTestSuite() {
+  rclcpp::shutdown();
+}
+
+void TestGravityCompensationExample::SetUp() {
+  controller_ =
+      std::make_unique<franka_example_controllers::GravityCompensationExampleController>();
+}
+
+void TestGravityCompensationExample::TearDown() {
+  controller_.reset(nullptr);
+}
+
+void TestGravityCompensationExample::SetUpController() {
+  const auto result = controller_->init("test_gravitiy_compensation_example");
+  ASSERT_EQ(result, controller_interface::return_type::OK);
+  std::vector<LoanedCommandInterface> command_ifs;
+  command_ifs.emplace_back(joint_1_pos_cmd_);
+  command_ifs.emplace_back(joint_2_pos_cmd_);
+  command_ifs.emplace_back(joint_3_pos_cmd_);
+  command_ifs.emplace_back(joint_4_pos_cmd_);
+  command_ifs.emplace_back(joint_5_pos_cmd_);
+  command_ifs.emplace_back(joint_6_pos_cmd_);
+  command_ifs.emplace_back(joint_7_pos_cmd_);
+
+  controller_->assign_interfaces(std::move(command_ifs), {});
+}
+
+TEST_F(TestGravityCompensationExample, JointsParameterNotSet) {
+  GTEST_SKIP() << "Skipping joint parameters not set test";
+  SetUpController();
+
+  // TODO(baris) configure must fail, 'joints' parameter not set
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
+}
+
+TEST_F(TestGravityCompensationExample, JointsParameterIsEmpty) {
+  GTEST_SKIP() << "Skipping joints parameter is empty test";
+  SetUpController();
+  controller_->get_node()->set_parameter({"joints", std::vector<std::string>()});
+
+  // Should return ERROR!!
+  // TODO(baris) params_.joints can't be empty add a check
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
+}
+
+TEST_F(TestGravityCompensationExample, given_correct_number_of_joints_configure_returns_success) {
+  SetUpController();
+  controller_->get_node()->set_parameter({"joints", joint_names_});
+
+  // configure successful
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+}
+
+TEST_F(TestGravityCompensationExample, given_joints_and_interface_when_update_expect_zero_values) {
+  SetUpController();
+  controller_->get_node()->set_parameter({"joints", joint_names_});
+
+  // configure successful
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+  // update successful though no command has been send yet
+  ASSERT_EQ(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+            controller_interface::return_type::OK);
+
+  // check joint commands are updated to zero torque value
+  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 0.0);
+  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 0.0);
+  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 0.0);
+  ASSERT_EQ(joint_4_pos_cmd_.get_value(), 0.0);
+  ASSERT_EQ(joint_5_pos_cmd_.get_value(), 0.0);
+  ASSERT_EQ(joint_6_pos_cmd_.get_value(), 0.0);
+  ASSERT_EQ(joint_7_pos_cmd_.get_value(), 0.0);
+}

franka_example_controllers/test/test_load_gravity_compensation_controller.cpp

@@ -0,0 +1,40 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// 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.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "controller_manager/controller_manager.hpp"
+#include "hardware_interface/resource_manager.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "ros2_control_test_assets/descriptions.hpp"
+
+TEST(TestLoadGravityCompensationExampleController, load_controller) {
+  rclcpp::init(0, nullptr);
+
+  std::shared_ptr<rclcpp::Executor> executor =
+      std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+  controller_manager::ControllerManager cm(std::make_unique<hardware_interface::ResourceManager>(
+                                               ros2_control_test_assets::minimal_robot_urdf),
+                                           executor, "test_controller_manager");
+
+  auto response =
+      cm.load_controller("test_gravity_compensation_example_controller",
+                         "franka_example_controllers/GravityCompensationExampleController");
+
+  ASSERT_NE(response, nullptr);
+
+  rclcpp::shutdown();
+}

franka_example_controllers/test/test_load_move_to_start_example_controller.cpp

@@ -0,0 +1,39 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// 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.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "controller_manager/controller_manager.hpp"
+#include "hardware_interface/resource_manager.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "ros2_control_test_assets/descriptions.hpp"
+
+TEST(TestLoadMoveToStartExampleController, load_controller) {
+  rclcpp::init(0, nullptr);
+
+  std::shared_ptr<rclcpp::Executor> executor =
+      std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+  controller_manager::ControllerManager cm(std::make_unique<hardware_interface::ResourceManager>(
+                                               ros2_control_test_assets::minimal_robot_urdf),
+                                           executor, "test_controller_manager");
+
+  auto response = cm.load_controller("test_move_to_start_example_controller",
+                                     "franka_example_controllers/MoveToStartExampleController");
+
+  ASSERT_NE(response, nullptr);
+
+  rclcpp::shutdown();
+}

franka_example_controllers/test/test_move_to_start_example_controller.cpp

@@ -0,0 +1,137 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// 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.
+
+#include <memory>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "test_move_to_start_example_controller.hpp"
+
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+
+const double k_EPS = 1e-5;
+
+void MoveToStartExampleControllerTest::SetUpTestSuite() {
+  rclcpp::init(0, nullptr);
+}
+
+void MoveToStartExampleControllerTest::TearDownTestSuite() {
+  rclcpp::shutdown();
+}
+
+void MoveToStartExampleControllerTest::SetUp() {
+  controller_ = std::make_unique<franka_example_controllers::MoveToStartExampleController>();
+}
+
+void MoveToStartExampleControllerTest::TearDown() {
+  controller_.reset(nullptr);
+}
+
+void MoveToStartExampleControllerTest::SetUpController() {
+  const auto result = controller_->init("test_move_to_start_example");
+  ASSERT_EQ(result, controller_interface::return_type::OK);
+  std::vector<LoanedCommandInterface> command_ifs;
+  std::vector<LoanedStateInterface> state_ifs;
+
+  command_ifs.emplace_back(joint_1_pos_cmd_);
+  command_ifs.emplace_back(joint_2_pos_cmd_);
+  command_ifs.emplace_back(joint_3_pos_cmd_);
+  command_ifs.emplace_back(joint_4_pos_cmd_);
+  command_ifs.emplace_back(joint_5_pos_cmd_);
+  command_ifs.emplace_back(joint_6_pos_cmd_);
+  command_ifs.emplace_back(joint_7_pos_cmd_);
+
+  state_ifs.emplace_back(joint_1_pos_state_);
+  state_ifs.emplace_back(joint_1_vel_state_);
+  state_ifs.emplace_back(joint_2_pos_state_);
+  state_ifs.emplace_back(joint_2_vel_state_);
+  state_ifs.emplace_back(joint_3_pos_state_);
+  state_ifs.emplace_back(joint_3_vel_state_);
+  state_ifs.emplace_back(joint_4_pos_state_);
+  state_ifs.emplace_back(joint_4_vel_state_);
+  state_ifs.emplace_back(joint_5_pos_state_);
+  state_ifs.emplace_back(joint_5_vel_state_);
+  state_ifs.emplace_back(joint_6_pos_state_);
+  state_ifs.emplace_back(joint_6_vel_state_);
+  state_ifs.emplace_back(joint_7_pos_state_);
+  state_ifs.emplace_back(joint_7_vel_state_);
+
+  controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
+}
+
+TEST_F(MoveToStartExampleControllerTest, controller_gains_not_set_failure) {
+  SetUpController();
+
+  // Failure due to not set K, D gains
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::FAILURE);
+}
+
+TEST_F(MoveToStartExampleControllerTest, contoller_gain_empty) {
+  SetUpController();
+  controller_->get_node()->set_parameter({"k_gains", std::vector<double>()});
+
+  // Failure due empty k gain parameter
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::FAILURE);
+}
+
+TEST_F(MoveToStartExampleControllerTest, contoller_damping_gain_empty) {
+  SetUpController();
+  controller_->get_node()->set_parameter({"k_gains", K_gains_});
+  controller_->get_node()->set_parameter({"d_gains", std::vector<double>()});
+
+  // Failure due to empty d gain parameter
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::FAILURE);
+}
+
+TEST_F(MoveToStartExampleControllerTest, correct_controller_gains_success) {
+  SetUpController();
+  controller_->get_node()->set_parameter({"k_gains", K_gains_});
+  controller_->get_node()->set_parameter({"d_gains", D_gains_});
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+}
+
+TEST_F(MoveToStartExampleControllerTest, correct_setup_on_activate_expect_success) {
+  SetUpController();
+  controller_->get_node()->set_parameter({"k_gains", K_gains_});
+  controller_->get_node()->set_parameter({"d_gains", D_gains_});
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+}
+
+TEST_F(MoveToStartExampleControllerTest, correct_setup_on_update_expect_ok) {
+  SetUpController();
+  controller_->get_node()->set_parameter({"k_gains", K_gains_});
+  controller_->get_node()->set_parameter({"d_gains", D_gains_});
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+
+  auto time = rclcpp::Time(0);
+  auto duration = rclcpp::Duration(0, 0);
+
+  ASSERT_EQ(controller_->update(time, duration), controller_interface::return_type::OK);
+
+  EXPECT_NEAR(joint_1_pos_cmd_.get_value(), 0.0, k_EPS);
+  EXPECT_NEAR(joint_2_pos_cmd_.get_value(), 0.0, k_EPS);
+  EXPECT_NEAR(joint_3_pos_cmd_.get_value(), 0.0, k_EPS);
+  EXPECT_NEAR(joint_4_pos_cmd_.get_value(), 0.0, k_EPS);
+  EXPECT_NEAR(joint_5_pos_cmd_.get_value(), 0.0, k_EPS);
+  EXPECT_NEAR(joint_6_pos_cmd_.get_value(), 0.0, k_EPS);
+  EXPECT_NEAR(joint_7_pos_cmd_.get_value(), 0.0, k_EPS);
+}

franka_example_controllers/test/test_move_to_start_example_controller.hpp

@@ -0,0 +1,85 @@
+// Copyright 2023 Franka Emika GmbH.
+//
+// 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.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+
+#include "franka_example_controllers/move_to_start_example_controller.hpp"
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
+using hardware_interface::CommandInterface;
+using hardware_interface::HW_IF_EFFORT;
+using hardware_interface::HW_IF_POSITION;
+using hardware_interface::HW_IF_VELOCITY;
+using hardware_interface::LoanedCommandInterface;
+using hardware_interface::LoanedStateInterface;
+using hardware_interface::StateInterface;
+
+class MoveToStartExampleControllerTest : public ::testing::Test {
+ public:
+  static void SetUpTestSuite();
+  static void TearDownTestSuite();
+
+  void SetUp();
+  void TearDown();
+
+  void SetUpController();
+
+ protected:
+  std::unique_ptr<franka_example_controllers::MoveToStartExampleController> controller_;
+
+  // dummy joint state values used for tests
+  const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3", "joint4",
+                                                 "joint5", "joint6", "joint7"};
+  std::vector<double> joint_commands_ = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
+
+  // The joint goal pose for the move to start controller
+  std::vector<double> joint_q_state_ = {0, -0.785398, 0, -2.35619, 0, 1.5708, 0.785398};
+  std::vector<double> joint_dq_state_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+
+  std::vector<double> K_gains_ = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
+  std::vector<double> D_gains_ = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
+
+  CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]};
+  CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]};
+  CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]};
+  CommandInterface joint_4_pos_cmd_{joint_names_[3], HW_IF_EFFORT, &joint_commands_[3]};
+  CommandInterface joint_5_pos_cmd_{joint_names_[4], HW_IF_EFFORT, &joint_commands_[4]};
+  CommandInterface joint_6_pos_cmd_{joint_names_[5], HW_IF_EFFORT, &joint_commands_[5]};
+  CommandInterface joint_7_pos_cmd_{joint_names_[6], HW_IF_EFFORT, &joint_commands_[6]};
+
+  // Position state interfaces
+  StateInterface joint_1_pos_state_{joint_names_[0], HW_IF_POSITION, &joint_q_state_[0]};
+  StateInterface joint_2_pos_state_{joint_names_[1], HW_IF_POSITION, &joint_q_state_[1]};
+  StateInterface joint_3_pos_state_{joint_names_[2], HW_IF_POSITION, &joint_q_state_[2]};
+  StateInterface joint_4_pos_state_{joint_names_[3], HW_IF_POSITION, &joint_q_state_[3]};
+  StateInterface joint_5_pos_state_{joint_names_[4], HW_IF_POSITION, &joint_q_state_[4]};
+  StateInterface joint_6_pos_state_{joint_names_[5], HW_IF_POSITION, &joint_q_state_[5]};
+  StateInterface joint_7_pos_state_{joint_names_[6], HW_IF_POSITION, &joint_q_state_[6]};
+
+  // Velocity state interfaces
+  StateInterface joint_1_vel_state_{joint_names_[0], HW_IF_VELOCITY, &joint_dq_state_[0]};
+  StateInterface joint_2_vel_state_{joint_names_[1], HW_IF_VELOCITY, &joint_dq_state_[1]};
+  StateInterface joint_3_vel_state_{joint_names_[2], HW_IF_VELOCITY, &joint_dq_state_[2]};
+  StateInterface joint_4_vel_state_{joint_names_[3], HW_IF_VELOCITY, &joint_dq_state_[3]};
+  StateInterface joint_5_vel_state_{joint_names_[4], HW_IF_VELOCITY, &joint_dq_state_[4]};
+  StateInterface joint_6_vel_state_{joint_names_[5], HW_IF_VELOCITY, &joint_dq_state_[5]};
+  StateInterface joint_7_vel_state_{joint_names_[6], HW_IF_VELOCITY, &joint_dq_state_[6]};
+};

franka_gripper/CMakeLists.txt

@@ -0,0 +1,76 @@
+cmake_minimum_required(VERSION 3.5)
+project(franka_gripper)
+
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+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(ament_cmake_python REQUIRED)
+find_package(franka_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(control_msgs REQUIRED)
+
+find_package(Franka REQUIRED)
+
+add_library(gripper_server SHARED
+        src/gripper_action_server.cpp)
+target_link_libraries(gripper_server Franka::Franka)
+target_include_directories(gripper_server PRIVATE
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>)
+
+ament_target_dependencies(gripper_server
+        franka_msgs
+        rclcpp
+        rclcpp_action
+        rclcpp_components
+        Franka
+        sensor_msgs
+        std_srvs
+        control_msgs)
+rclcpp_components_register_node(gripper_server PLUGIN "franka_gripper::GripperActionServer" EXECUTABLE franka_gripper_node)
+install(TARGETS
+        gripper_server
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin)
+
+install(DIRECTORY
+        launch config
+        DESTINATION share/${PROJECT_NAME}/)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(PROGRAMS
+        scripts/fake_gripper_state_publisher.py
+        DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+    find_package(ament_cmake_clang_format REQUIRED)
+    find_package(ament_cmake_copyright REQUIRED)
+    find_package(ament_cmake_cppcheck REQUIRED)
+    find_package(ament_cmake_flake8 REQUIRED)
+    find_package(ament_cmake_lint_cmake REQUIRED)
+    find_package(ament_cmake_pep257 REQUIRED)
+    find_package(ament_cmake_xmllint REQUIRED)
+
+    set(CPP_DIRECTORIES src include)
+    ament_clang_format(CONFIG_FILE ../.clang-format ${CPP_DIRECTORIES})
+    ament_copyright(launch ${CPP_DIRECTORIES} package.xml)
+    ament_cppcheck(${CPP_DIRECTORIES})
+    ament_lint_cmake(CMakeLists.txt)
+    ament_flake8()
+    ament_pep257()
+    ament_xmllint()
+endif()
+
+ament_package()

franka_gripper/config/franka_gripper_node.yaml

@@ -0,0 +1,8 @@
+panda_gripper:
+  ros__parameters:
+    state_publish_rate: 50  # [Hz]
+    feedback_publish_rate: 30 # [Hz]
+    default_speed: 0.1  # [m/s]
+    default_grasp_epsilon:
+      inner: 0.005 # [m]
+      outer: 0.005 # [m]

franka_gripper/franka_gripper/__init__.py

@@ -0,0 +1,13 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  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.

franka_gripper/include/franka_gripper/gripper_action_server.hpp

@@ -0,0 +1,221 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// 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.
+
+#pragma once
+
+#include <chrono>
+#include <functional>
+#include <future>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include <franka/exception.h>
+#include <franka/gripper.h>
+#include <franka/gripper_state.h>
+#include <control_msgs/action/gripper_command.hpp>
+#include <franka_msgs/action/grasp.hpp>
+#include <franka_msgs/action/homing.hpp>
+#include <franka_msgs/action/move.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <std_srvs/srv/trigger.hpp>
+
+namespace franka_gripper {
+
+/// checks whether an asynchronous command has finished
+/// @tparam T the expected return type of the future
+/// @param t the future which should be checked
+/// @param future_wait_timeout how long to wait for the result before returning false
+/// @return whether the asynchronous function has already finished
+template <typename T>
+bool resultIsReady(std::future<T>& t, std::chrono::nanoseconds future_wait_timeout) {
+  return t.wait_for(future_wait_timeout) == std::future_status::ready;
+}
+
+/// ROS node that offers multiple actions to use the gripper.
+class GripperActionServer : public rclcpp::Node {
+ public:
+  using Homing = franka_msgs::action::Homing;
+  using GoalHandleHoming = rclcpp_action::ServerGoalHandle<Homing>;
+
+  using Move = franka_msgs::action::Move;
+  using GoalHandleMove = rclcpp_action::ServerGoalHandle<Move>;
+
+  using Grasp = franka_msgs::action::Grasp;
+  using GoalHandleGrasp = rclcpp_action::ServerGoalHandle<Grasp>;
+
+  using GripperCommand = control_msgs::action::GripperCommand;
+  using GoalHandleGripperCommand = rclcpp_action::ServerGoalHandle<GripperCommand>;
+
+  using Trigger = std_srvs::srv::Trigger;
+
+  /// creates an instance of a GripperActionServer
+  /// @param options options for node initialization
+  explicit GripperActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+
+ private:
+  /// describes the different tasks. Each task corresponds to one action server
+  enum class Task { kHoming, kMove, kGrasp, kGripperCommand };
+
+  /// returns a string version of the Task enum
+  static std::string getTaskName(Task task) {
+    switch (task) {
+      case Task::kHoming:
+        return {"Homing"};
+      case Task::kMove:
+        return {"Moving"};
+      case Task::kGrasp:
+        return {"Grasping"};
+      case Task::kGripperCommand:
+        return {"GripperCommand"};
+      default:
+        throw std::invalid_argument("getTaskName is not implemented for this case");
+    }
+  };
+
+  const double k_default_grasp_epsilon = 0.005;    // default inner and outer grasp epsilon in meter
+  const double k_default_speed = 0.1;              // default gripper speed in m/s
+  const int k_default_state_publish_rate = 30;     // default gripper state publish rate
+  const int k_default_feedback_publish_rate = 10;  // default action feedback publish rate
+
+  std::unique_ptr<franka::Gripper> gripper_;
+  rclcpp_action::Server<Homing>::SharedPtr homing_server_;
+  rclcpp_action::Server<Move>::SharedPtr move_server_;
+  rclcpp_action::Server<Grasp>::SharedPtr grasp_server_;
+  rclcpp_action::Server<GripperCommand>::SharedPtr gripper_command_server_;
+  rclcpp::Service<Trigger>::SharedPtr stop_service_;
+  std::mutex gripper_state_mutex_;
+  franka::GripperState current_gripper_state_;
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_states_publisher_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  double default_speed_;          // default gripper speed parameter value in m/s
+  double default_epsilon_inner_;  // default gripper inner epsilon parameter value in m
+  double default_epsilon_outer_;  //  default gripper outer epsilon parameter value in m
+  std::vector<std::string> joint_names_;
+  std::chrono::nanoseconds future_wait_timeout_{0};
+
+  void publishGripperState();
+
+  /// stops the gripper and writes the result into the response
+  /// @param[out] response  will be updated with the success status and error message
+  void stopServiceCallback(const std::shared_ptr<Trigger::Response>& response);
+
+  /// accepts any cancel request
+  rclcpp_action::CancelResponse handleCancel(Task task);
+
+  /// accepts any goal request
+  rclcpp_action::GoalResponse handleGoal(Task task);
+
+  /// performs homing
+  void executeHoming(const std::shared_ptr<GoalHandleHoming>& goal_handle);
+
+  /// performs move
+  void executeMove(const std::shared_ptr<GoalHandleMove>& goal_handle);
+
+  /// performs grasp
+  void executeGrasp(const std::shared_ptr<GoalHandleGrasp>& goal_handle);
+
+  /// Performs the moveit grasp command
+  /// @param goal_handle
+  /// @param command_handler eiter a grasp or move command defined by the onExecuteGripperCommand
+  /// method
+  void executeGripperCommand(const std::shared_ptr<GoalHandleGripperCommand>& goal_handle,
+                             const std::function<bool()>& command_handler);
+
+  /// Defines a function for either grasping or moving the gripper, depending on the current gripper
+  /// state and the commanded goal. Then it calls executeGripperCommand to execute that function
+  void onExecuteGripperCommand(const std::shared_ptr<GoalHandleGripperCommand>& goal_handle);
+
+  /// Executes a gripper command
+  /// @tparam T A gripper action message type (Move, Grasp, Homing)
+  /// @param[in] goal_handle The goal handle from the action server
+  /// @param[in] task The type of the Task
+  /// @param[in] command_handler a function that performs the the task. Returns true on success.
+  /// This function is allowed to throw a franka::Exception
+  template <typename T>
+  void executeCommand(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T>>& goal_handle,
+                      Task task,
+                      const std::function<bool()>& command_handler) {
+    const auto kTaskName = getTaskName(task);
+    RCLCPP_INFO(this->get_logger(), "Gripper %s...", kTaskName.c_str());
+
+    auto command_execution_thread = withResultGenerator<T>(command_handler);
+
+    std::future<std::shared_ptr<typename T::Result>> result_future =
+        std::async(std::launch::async, command_execution_thread);
+
+    while (!resultIsReady(result_future, future_wait_timeout_) && rclcpp::ok()) {
+      if (goal_handle->is_canceling()) {
+        gripper_->stop();
+        auto result = result_future.get();
+        RCLCPP_INFO(get_logger(), "Gripper %s canceled", kTaskName.c_str());
+        goal_handle->canceled(result);
+        return;
+      }
+      publishGripperWidthFeedback(goal_handle);
+    }
+    if (rclcpp::ok()) {
+      const auto kResult = result_future.get();
+      if (kResult->success) {
+        RCLCPP_INFO(get_logger(), "Gripper %s succeeded", kTaskName.c_str());
+        goal_handle->succeed(kResult);
+      } else {
+        RCLCPP_INFO(get_logger(), "Gripper %s failed", kTaskName.c_str());
+        goal_handle->abort(kResult);
+      }
+    }
+  }
+
+  /// Creates a function that catches exceptions for the gripper command function and returns a
+  /// result
+  /// @tparam T A gripper action message type (Move, Grasp, Homing)
+  /// @param[in] command_handler a function that performs the the task. Returns true on success.
+  /// This function is allowed to throw a franka::Exception
+  /// @return[in] enhanced command_handler that now returns a result an does not throw a
+  /// franka::exception anymore
+  template <typename T>
+  auto withResultGenerator(const std::function<bool()>& command_handler)
+      -> std::function<std::shared_ptr<typename T::Result>()> {
+    return [command_handler]() {
+      auto result = std::make_shared<typename T::Result>();
+      try {
+        result->success = command_handler();
+      } catch (const franka::Exception& e) {
+        result->success = false;
+        result->error = e.what();
+      }
+      return result;
+    };
+  }
+
+  /// Publishes the gripper width as feedback for actions
+  /// @tparam T A gripper action message type (Move, Grasp, Homing)
+  /// @param[in] goal_handle The goal handle from the action server
+  template <typename T>
+  void publishGripperWidthFeedback(
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<T>>& goal_handle) {
+    auto gripper_feedback = std::make_shared<typename T::Feedback>();
+    std::lock_guard<std::mutex> guard(gripper_state_mutex_);
+    gripper_feedback->current_width = current_gripper_state_.width;
+    goal_handle->publish_feedback(gripper_feedback);
+  }
+
+  /// Publishes the gripper width as feedback for the GripperCommand action
+  void publishGripperCommandFeedback(
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<GripperCommand>>& goal_handle);
+};
+}  // namespace franka_gripper

franka_gripper/launch/gripper.launch.py

@@ -0,0 +1,92 @@
+# Copyright (c) 2021 Franka Emika GmbH
+#
+#  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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    robot_ip_parameter_name = 'robot_ip'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    arm_parameter_name = 'arm_id'
+    joint_names_parameter_name = 'joint_names'
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+    arm_id = LaunchConfiguration(arm_parameter_name)
+    joint_names = LaunchConfiguration(joint_names_parameter_name)
+
+    gripper_config = os.path.join(
+        get_package_share_directory('franka_gripper'), 'config', 'franka_gripper_node.yaml'
+    )
+
+    default_joint_name_postfix = '_finger_joint'
+    arm_default_argument = [
+        '[',
+        arm_id,
+        default_joint_name_postfix,
+        '1',
+        ',',
+        arm_id,
+        default_joint_name_postfix,
+        '2',
+        ']',
+    ]
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                robot_ip_parameter_name, description='Hostname or IP address of the robot.'
+            ),
+            DeclareLaunchArgument(
+                use_fake_hardware_parameter_name,
+                default_value='false',
+                description=(
+                    'Publish fake gripper joint states without connecting to a real gripper'
+                ),
+            ),
+            DeclareLaunchArgument(
+                arm_parameter_name,
+                default_value='panda',
+                description=(
+                    'Name of the arm in the URDF file. This is used to generate the joint '
+                    'names of the gripper.'
+                ),
+            ),
+            DeclareLaunchArgument(
+                joint_names_parameter_name,
+                default_value=arm_default_argument,
+                description='Names of the gripper joints in the URDF',
+            ),
+            Node(
+                package='franka_gripper',
+                executable='franka_gripper_node',
+                name=[arm_id, '_gripper'],
+                parameters=[{'robot_ip': robot_ip, 'joint_names': joint_names}, gripper_config],
+                condition=UnlessCondition(use_fake_hardware),
+            ),
+            Node(
+                package='franka_gripper',
+                executable='fake_gripper_state_publisher.py',
+                name=[arm_id, '_gripper'],
+                parameters=[{'robot_ip': robot_ip, 'joint_names': joint_names}, gripper_config],
+                condition=IfCondition(use_fake_hardware),
+            ),
+        ]
+    )

franka_gripper/package.xml

@@ -0,0 +1,34 @@
+<?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>franka_gripper</name>
+  <version>0.1.0</version>
+  <description>This package implements the franka gripper of type Franka Hand for the use in ROS2</description>
+  <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>franka_msgs</depend>
+  <depend>rclpy</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>control_msgs</depend>
+  <depend>libfranka</depend>
+
+  <test_depend>ament_cmake_clang_format</test_depend>
+  <test_depend>ament_cmake_copyright</test_depend>
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_cmake_flake8</test_depend>
+  <test_depend>ament_cmake_lint_cmake</test_depend>
+  <test_depend>ament_cmake_pep257</test_depend>
+  <test_depend>ament_cmake_xmllint</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

franka_gripper/scripts/fake_gripper_state_publisher.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  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.
+
+import rclpy
+from rclpy import Parameter
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+
+class FakeGripperStatePublisher(Node):
+
+    def __init__(self):
+        super().__init__('fake_gripper_state_publisher')
+        self.publisher_ = self.create_publisher(JointState, '~/joint_states', 1)
+        timer_period = 0.1  # seconds
+
+        self.declare_parameter('joint_names', Parameter.Type.STRING_ARRAY)
+        self.joint_names = (
+            self.get_parameter('joint_names').get_parameter_value().string_array_value
+        )
+
+        assert len(self.joint_names) == 2
+        self.timer = self.create_timer(timer_period, self.publish_state)
+
+    def publish_state(self):
+        joint_states = JointState()
+        joint_states.header.stamp = self.get_clock().now().to_msg()
+        joint_states.name = self.joint_names
+        joint_states.position = [0.035, 0.035]
+        joint_states.velocity = [0.0, 0.0]
+        joint_states.effort = [0.0, 0.0]
+        self.publisher_.publish(joint_states)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    state_publisher = FakeGripperStatePublisher()
+
+    rclpy.spin(state_publisher)
+    state_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

franka_gripper/src/gripper_action_server.cpp

@@ -0,0 +1,290 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// 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.
+
+#include <functional>
+#include <future>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include <franka/exception.h>
+#include <franka/gripper.h>
+#include <franka/gripper_state.h>
+#include <control_msgs/action/gripper_command.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <rclcpp_components/register_node_macro.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <std_srvs/srv/trigger.hpp>
+
+#include <franka_gripper/gripper_action_server.hpp>
+
+namespace franka_gripper {
+GripperActionServer::GripperActionServer(const rclcpp::NodeOptions& options)
+    : Node("franka_gripper_node", options) {
+  this->declare_parameter("robot_ip", std::string());
+  this->declare_parameter("default_grasp_epsilon.inner", k_default_grasp_epsilon);
+  this->declare_parameter("default_grasp_epsilon.outer", k_default_grasp_epsilon);
+  this->declare_parameter("default_speed", k_default_speed);
+  this->declare_parameter("joint_names", std::vector<std::string>());
+  this->declare_parameter("state_publish_rate", k_default_state_publish_rate);
+  this->declare_parameter("feedback_publish_rate", k_default_feedback_publish_rate);
+  std::string robot_ip;
+  if (!this->get_parameter<std::string>("robot_ip", robot_ip)) {
+    RCLCPP_FATAL(this->get_logger(), "Parameter 'robot_ip' not set");
+    throw std::invalid_argument("Parameter 'robot_ip' not set");
+  }
+
+  this->default_speed_ = this->get_parameter("default_speed").as_double();
+  this->default_epsilon_inner_ = this->get_parameter("default_grasp_epsilon.inner").as_double();
+  this->default_epsilon_outer_ = this->get_parameter("default_grasp_epsilon.outer").as_double();
+  if (!this->get_parameter("joint_names", this->joint_names_)) {
+    RCLCPP_WARN(this->get_logger(), "Parameter 'joint_names' not set");
+    this->joint_names_ = {"", ""};
+  }
+
+  if (this->joint_names_.size() != 2) {
+    RCLCPP_FATAL(this->get_logger(),
+                 "Parameter 'joint_names' needs exactly two arguments, got %ld instead",
+                 this->joint_names_.size());
+    throw std::invalid_argument("Parameter 'joint_names' has wrong number of arguments");
+  }
+
+  const double kStatePublishRate =
+      static_cast<double>(this->get_parameter("state_publish_rate").as_int());
+  const double kFeedbackPublishRate =
+      static_cast<double>(this->get_parameter("feedback_publish_rate").as_int());
+  this->future_wait_timeout_ = rclcpp::WallRate(kFeedbackPublishRate).period();
+
+  RCLCPP_INFO(this->get_logger(), "Trying to establish a connection with the gripper");
+  try {
+    this->gripper_ = std::make_unique<franka::Gripper>(robot_ip);
+  } catch (const franka::Exception& exception) {
+    RCLCPP_FATAL(this->get_logger(), exception.what());
+    throw exception;
+  }
+  RCLCPP_INFO(this->get_logger(), "Connected to gripper");
+  current_gripper_state_ = gripper_->readOnce();
+  const auto kHomingTask = Task::kHoming;
+  this->stop_service_ =  // NOLINTNEXTLINE
+      create_service<Trigger>("~/stop",
+                              [this](std::shared_ptr<Trigger::Request> /*request*/,  // NOLINT
+                                     std::shared_ptr<Trigger::Response> response) {  // NOLINT
+                                return stopServiceCallback(std::move(response));     // NOLINT
+                              });
+
+  this->homing_server_ = rclcpp_action::create_server<Homing>(
+      this, "~/homing",
+      [this, kHomingTask](auto /*uuid*/, auto /*goal*/) { return handleGoal(kHomingTask); },
+      [this, kHomingTask](const auto& /*goal_handle*/) { return handleCancel(kHomingTask); },
+      [this](const auto& goal_handle) {
+        return std::thread{[goal_handle, this]() { executeHoming(goal_handle); }}.detach();
+      });
+  const auto kMoveTask = Task::kMove;
+  this->move_server_ = rclcpp_action::create_server<Move>(
+      this, "~/move",
+      [this, kMoveTask](auto /*uuid*/, auto /*goal*/) { return handleGoal(kMoveTask); },
+      [this, kMoveTask](const auto& /*goal_handle*/) { return handleCancel(kMoveTask); },
+      [this](const auto& goal_handle) {
+        return std::thread{[goal_handle, this]() { executeMove(goal_handle); }}.detach();
+      });
+
+  const auto kGraspTask = Task::kGrasp;
+  this->grasp_server_ = rclcpp_action::create_server<Grasp>(
+      this, "~/grasp",
+      [this, kGraspTask](auto /*uuid*/, auto /*goal*/) { return handleGoal(kGraspTask); },
+      [this, kGraspTask](const auto& /*goal_handle*/) { return handleCancel(kGraspTask); },
+      [this](const auto& goal_handle) {
+        return std::thread{[goal_handle, this]() { executeGrasp(goal_handle); }}.detach();
+      });
+
+  const auto kGripperCommandTask = Task::kGripperCommand;
+  this->gripper_command_server_ = rclcpp_action::create_server<GripperCommand>(
+      this, "~/gripper_action",
+      [this, kGripperCommandTask](auto /*uuid*/, auto /*goal*/) {
+        return handleGoal(kGripperCommandTask);
+      },
+      [this, kGripperCommandTask](const auto& /*goal_handle*/) {
+        return handleCancel(kGripperCommandTask);
+      },
+      [this](const auto& goal_handle) {
+        return std::thread{[goal_handle, this]() { onExecuteGripperCommand(goal_handle); }}
+            .detach();
+      });
+
+  this->joint_states_publisher_ =
+      this->create_publisher<sensor_msgs::msg::JointState>("~/joint_states", 1);
+  this->timer_ = this->create_wall_timer(rclcpp::WallRate(kStatePublishRate).period(),
+                                         [this]() { return publishGripperState(); });
+}
+
+rclcpp_action::CancelResponse GripperActionServer::handleCancel(Task task) {
+  RCLCPP_INFO(this->get_logger(), "Received request to handleCancel %s", getTaskName(task).c_str());
+  return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+rclcpp_action::GoalResponse GripperActionServer::handleGoal(Task task) {
+  RCLCPP_INFO(this->get_logger(), "Received %s request", getTaskName(task).c_str());
+  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+void GripperActionServer::executeHoming(const std::shared_ptr<GoalHandleHoming>& goal_handle) {
+  const auto kCommand = [this]() { return gripper_->homing(); };
+  executeCommand(goal_handle, Task::kHoming, kCommand);
+}
+
+void GripperActionServer::executeMove(const std::shared_ptr<GoalHandleMove>& goal_handle) {
+  auto command = [goal_handle, this]() {
+    const auto kGoal = goal_handle->get_goal();
+    return gripper_->move(kGoal->width, kGoal->speed);
+  };
+  executeCommand(goal_handle, Task::kMove, command);
+}
+
+void GripperActionServer::executeGrasp(const std::shared_ptr<GoalHandleGrasp>& goal_handle) {
+  auto command = [goal_handle, this]() {
+    const auto kGoal = goal_handle->get_goal();
+    return gripper_->grasp(kGoal->width, kGoal->speed, kGoal->force, kGoal->epsilon.inner,
+                           kGoal->epsilon.outer);
+  };
+  executeCommand(goal_handle, Task::kGrasp, command);
+}
+
+void GripperActionServer::onExecuteGripperCommand(
+    const std::shared_ptr<GoalHandleGripperCommand>& goal_handle) {
+  const auto kGoal = goal_handle->get_goal();
+  const double kTargetWidth = 2 * kGoal->command.position;
+
+  std::unique_lock<std::mutex> guard(gripper_state_mutex_);
+  constexpr double kSamePositionThreshold = 1e-4;
+  auto result = std::make_shared<control_msgs::action::GripperCommand::Result>();
+  const double kCurrentWidth = current_gripper_state_.width;
+  if (kTargetWidth > current_gripper_state_.max_width || kTargetWidth < 0) {
+    RCLCPP_ERROR(this->get_logger(),
+                 "GripperServer: Commanding out of range width! max_width = %f command = %f",
+                 current_gripper_state_.max_width, kTargetWidth);
+    goal_handle->abort(result);
+    return;
+  }
+  if (std::abs(kTargetWidth - kCurrentWidth) < kSamePositionThreshold) {
+    result->effort = 0;
+    result->position = kCurrentWidth;
+    result->reached_goal = true;
+    result->stalled = false;
+    goal_handle->succeed(result);
+    return;
+  }
+  guard.unlock();
+  auto command = [kTargetWidth, kCurrentWidth, kGoal, this]() {
+    if (kTargetWidth >= kCurrentWidth) {
+      return gripper_->move(kTargetWidth, default_speed_);
+    }
+    return gripper_->grasp(kTargetWidth, default_speed_, kGoal->command.max_effort,
+                           default_epsilon_inner_, default_epsilon_outer_);
+  };
+
+  executeGripperCommand(goal_handle, command);
+}
+
+void GripperActionServer::executeGripperCommand(
+    const std::shared_ptr<GoalHandleGripperCommand>& goal_handle,
+    const std::function<bool()>& command_handler) {
+  const auto kTaskName = getTaskName(Task::kGripperCommand);
+  RCLCPP_INFO(this->get_logger(), "Gripper %s...", kTaskName.c_str());
+
+  auto command_execution_thread = [command_handler, this]() {
+    auto result = std::make_shared<GripperCommand::Result>();
+    try {
+      result->reached_goal = command_handler();
+    } catch (const franka::Exception& e) {
+      result->reached_goal = false;
+      RCLCPP_ERROR(this->get_logger(), e.what());
+    }
+    return result;
+  };
+
+  std::future<std::shared_ptr<typename GripperCommand ::Result>> result_future =
+      std::async(std::launch::async, command_execution_thread);
+
+  while (!resultIsReady(result_future, future_wait_timeout_) && rclcpp::ok()) {
+    if (goal_handle->is_canceling()) {
+      gripper_->stop();
+      auto result = result_future.get();
+      RCLCPP_INFO(get_logger(), "Gripper %s canceled", kTaskName.c_str());
+      goal_handle->canceled(result);
+      return;
+    }
+    publishGripperCommandFeedback(goal_handle);
+  }
+  if (rclcpp::ok()) {
+    const auto kResult = result_future.get();
+    std::lock_guard<std::mutex> guard(gripper_state_mutex_);
+    kResult->position = current_gripper_state_.width;
+    kResult->effort = 0.;
+    if (kResult->reached_goal) {
+      RCLCPP_INFO(get_logger(), "Gripper %s succeeded", kTaskName.c_str());
+      goal_handle->succeed(kResult);
+    } else {
+      RCLCPP_INFO(get_logger(), "Gripper %s failed", kTaskName.c_str());
+      goal_handle->abort(kResult);
+    }
+  }
+}
+
+void GripperActionServer::stopServiceCallback(const std::shared_ptr<Trigger::Response>& response) {
+  RCLCPP_INFO(this->get_logger(), "Stopping gripper_...");
+  auto action_result = withResultGenerator<Homing>([this]() { return gripper_->stop(); })();
+  response->success = action_result->success;
+  response->message = action_result->error;
+  if (response->success) {
+    RCLCPP_INFO(this->get_logger(), "Gripper stopped");
+  } else {
+    RCLCPP_INFO(this->get_logger(), "Gripper could not be stopped");
+  }
+  if (!response->message.empty()) {
+    RCLCPP_ERROR(this->get_logger(), response->message.c_str());
+  }
+}
+
+void GripperActionServer::publishGripperState() {
+  std::lock_guard<std::mutex> lock(gripper_state_mutex_);
+  try {
+    current_gripper_state_ = gripper_->readOnce();
+  } catch (const franka::Exception& e) {
+    RCLCPP_ERROR(this->get_logger(), e.what());
+  }
+  sensor_msgs::msg::JointState joint_states;
+  joint_states.header.stamp = this->now();
+  joint_states.name.push_back(this->joint_names_[0]);
+  joint_states.name.push_back(this->joint_names_[1]);
+  joint_states.position.push_back(current_gripper_state_.width / 2);
+  joint_states.position.push_back(current_gripper_state_.width / 2);
+  joint_states.velocity.push_back(0.0);
+  joint_states.velocity.push_back(0.0);
+  joint_states.effort.push_back(0.0);
+  joint_states.effort.push_back(0.0);
+  joint_states_publisher_->publish(joint_states);
+}
+
+void GripperActionServer::publishGripperCommandFeedback(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<GripperCommand>>& goal_handle) {
+  auto gripper_feedback = std::make_shared<GripperCommand::Feedback>();
+  std::lock_guard<std::mutex> guard(gripper_state_mutex_);
+  gripper_feedback->position = current_gripper_state_.width;
+  gripper_feedback->effort = 0.;
+  goal_handle->publish_feedback(gripper_feedback);
+}
+}  // namespace franka_gripper
+
+RCLCPP_COMPONENTS_REGISTER_NODE(franka_gripper::GripperActionServer)  // NOLINT

franka_hardware/CMakeLists.txt

@@ -0,0 +1,92 @@
+cmake_minimum_required(VERSION 3.5)
+project(franka_hardware)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+option(CHECK_TIDY "Adds clang-tidy tests" OFF)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(franka_msgs REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(Franka REQUIRED)
+
+
+add_library(franka_hardware
+        SHARED
+        src/franka_hardware_interface.cpp
+        src/robot.cpp)
+target_include_directories(
+        franka_hardware
+        PRIVATE
+        include
+)
+ament_target_dependencies(
+        franka_hardware
+        hardware_interface
+        Franka
+        pluginlib
+        rclcpp
+        franka_msgs
+)
+pluginlib_export_plugin_description_file(hardware_interface franka_hardware.xml)
+
+
+install(
+        TARGETS franka_hardware
+        DESTINATION lib
+)
+install(
+        DIRECTORY include/
+        DESTINATION include
+)
+
+
+if(BUILD_TESTING)
+    find_package(ament_cmake_clang_format REQUIRED)
+    find_package(ament_cmake_copyright REQUIRED)
+    find_package(ament_cmake_cppcheck REQUIRED)
+    find_package(ament_cmake_flake8 REQUIRED)
+    find_package(ament_cmake_lint_cmake REQUIRED)
+    find_package(ament_cmake_pep257 REQUIRED)
+    find_package(ament_cmake_xmllint REQUIRED)
+
+    set(CPP_DIRECTORIES src include)
+    ament_clang_format(CONFIG_FILE ../.clang-format ${CPP_DIRECTORIES})
+    ament_copyright(${CPP_DIRECTORIES} package.xml)
+    ament_cppcheck(${CPP_DIRECTORIES})
+    ament_lint_cmake(CMakeLists.txt)
+    ament_flake8()
+    ament_pep257()
+    ament_xmllint()
+
+    add_subdirectory(test)
+
+    if(CHECK_TIDY)
+        find_package(ament_cmake_clang_tidy REQUIRED)
+        set(ament_cmake_clang_tidy_CONFIG_FILE ../.clang-tidy)
+        ament_clang_tidy(${CMAKE_BINARY_DIR})
+    endif()
+endif()
+
+ament_export_include_directories(
+        include
+)
+ament_export_libraries(
+        franka_hardware
+)
+ament_export_dependencies(
+        hardware_interface
+        pluginlib
+        rclcpp
+)
+ament_package()

franka_hardware/franka_hardware.xml

@@ -0,0 +1,10 @@
+<library path="franka_hardware">
+    <class name="franka_hardware/FrankaHardwareInterface"
+           type="franka_hardware::FrankaHardwareInterface"
+           base_class_type="hardware_interface::SystemInterface">
+
+        <description>
+            Hardware interface for Franka Emika research robots
+        </description>
+    </class>
+</library>

franka_hardware/include/franka_hardware/franka_hardware_interface.hpp

@@ -0,0 +1,76 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// 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.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <hardware_interface/visibility_control.h>
+#include <franka_hardware/robot.hpp>
+
+#include <hardware_interface/hardware_info.hpp>
+#include <hardware_interface/system_interface.hpp>
+#include <hardware_interface/types/hardware_interface_return_values.hpp>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/macros.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/state.hpp>
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace franka_hardware {
+
+class FrankaHardwareInterface : public hardware_interface::SystemInterface {
+ public:
+  explicit FrankaHardwareInterface(std::unique_ptr<Robot> robot);
+  FrankaHardwareInterface() = default;
+
+  hardware_interface::return_type prepare_command_mode_switch(
+      const std::vector<std::string>& start_interfaces,
+      const std::vector<std::string>& stop_interfaces) override;
+  hardware_interface::return_type perform_command_mode_switch(
+      const std::vector<std::string>& start_interfaces,
+      const std::vector<std::string>& stop_interfaces) 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;
+  CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
+  static const size_t kNumberOfJoints = 7;
+
+ private:
+  std::unique_ptr<Robot> robot_;
+  std::array<double, kNumberOfJoints> hw_commands_{0, 0, 0, 0, 0, 0, 0};
+  std::array<double, kNumberOfJoints> hw_positions_{0, 0, 0, 0, 0, 0, 0};
+  std::array<double, kNumberOfJoints> hw_velocities_{0, 0, 0, 0, 0, 0, 0};
+  std::array<double, kNumberOfJoints> hw_efforts_{0, 0, 0, 0, 0, 0, 0};
+  franka::RobotState hw_franka_robot_state_;
+  franka::RobotState* hw_franka_robot_state_addr_ = &hw_franka_robot_state_;
+  Model* hw_franka_model_ptr_ = nullptr;
+
+  bool effort_interface_claimed_ = false;
+  bool effort_interface_running_ = false;
+  static rclcpp::Logger getLogger();
+
+  const std::string k_robot_name{"panda"};
+  const std::string k_robot_state_interface_name{"robot_state"};
+  const std::string k_robot_model_interface_name{"robot_model"};
+};
+}  // namespace franka_hardware

franka_hardware/include/franka_hardware/model.hpp

@@ -0,0 +1,290 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// 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.
+
+#pragma once
+
+#include <array>
+
+#include <franka/model.h>
+
+namespace franka_hardware {
+
+/**
+ * This class is a thin wrapper around a @ref franka::Model and delegates all calls to
+ * that
+ */
+class Model {  // NOLINT(cppcoreguidelines-pro-type-member-init,
+               // cppcoreguidelines-special-member-functions)
+ public:
+  /**
+   * Create a new Model instance wrapped around a franka::Model
+   */
+  explicit Model(franka::Model* model) : model_(model) {}
+  virtual ~Model() = default;
+
+  /**
+   * Gets the 4x4 pose matrix for the given frame in base frame.
+   *
+   * The pose is represented as a 4x4 matrix in column-major format.
+   *
+   * @param[in] frame The desired frame.
+   * @param[in] q Joint position.
+   * @param[in] F_T_EE End effector in flange frame.
+   * @param[in] EE_T_K Stiffness frame K in the end effector frame.
+   *
+   * @return Vectorized 4x4 pose matrix, column-major.
+   */
+  [[nodiscard]] std::array<double, 16> pose(
+      franka::Frame frame,
+      const std::array<double, 7>& q,        // NOLINT(readability-identifier-length)
+      const std::array<double, 16>& F_T_EE,  // NOLINT(readability-identifier-naming)
+      const std::array<double, 16>& EE_T_K)  // NOLINT(readability-identifier-naming)
+      const {
+    return model_->pose(frame, q, F_T_EE, EE_T_K);
+  }
+
+  /**
+   * Gets the 6x7 Jacobian for the given frame, relative to that frame.
+   *
+   * The Jacobian is represented as a 6x7 matrix in column-major format.
+   *
+   * @param[in] frame The desired frame.
+   * @param[in] q Joint position.
+   * @param[in] F_T_EE End effector in flange frame.
+   * @param[in] EE_T_K Stiffness frame K in the end effector frame.
+   *
+   * @return Vectorized 6x7 Jacobian, column-major.
+   */
+  [[nodiscard]] std::array<double, 42> bodyJacobian(
+      franka::Frame frame,
+      const std::array<double, 7>& q,        // NOLINT(readability-identifier-length)
+      const std::array<double, 16>& F_T_EE,  // NOLINT(readability-identifier-naming)
+      const std::array<double, 16>& EE_T_K)  // NOLINT(readability-identifier-naming)
+      const {
+    return model_->bodyJacobian(frame, q, F_T_EE, EE_T_K);
+  }
+
+  /**
+   * Gets the 6x7 Jacobian for the given joint relative to the base frame.
+   *
+   * The Jacobian is represented as a 6x7 matrix in column-major format.
+   *
+   * @param[in] frame The desired frame.
+   * @param[in] robot_state State from which the pose should be calculated.
+   *
+   * @return Vectorized 6x7 Jacobian, column-major.
+   */
+  [[nodiscard]] std::array<double, 42> zeroJacobian(
+      franka::Frame frame,
+      const std::array<double, 7>& q,        // NOLINT(readability-identifier-length)
+      const std::array<double, 16>& F_T_EE,  // NOLINT(readability-identifier-naming)
+      const std::array<double, 16>& EE_T_K)  // NOLINT(readability-identifier-naming)
+      const {
+    return model_->zeroJacobian(frame, q, F_T_EE, EE_T_K);
+  }
+
+  /**
+   * Calculates the 7x7 mass matrix. Unit: \f$[kg \times m^2]\f$.
+   *
+   * @param[in] q Joint position.
+   * @param[in] I_total Inertia of the attached total load including end effector, relative to
+   * center of mass, given as vectorized 3x3 column-major matrix. Unit: \f$[kg \times m^2]\f$.
+   * @param[in] m_total Weight of the attached total load including end effector.
+   * Unit: \f$[kg]\f$.
+   * @param[in] F_x_Ctotal Translation from flange to center of mass of the attached total load.
+   * Unit: \f$[m]\f$.
+   *
+   * @return Vectorized 7x7 mass matrix, column-major.
+   */
+  [[nodiscard]] std::array<double, 49> mass(
+      const std::array<double, 7>& q,        // NOLINT(readability-identifier-length)
+      const std::array<double, 9>& I_total,  // NOLINT(readability-identifier-naming)
+      double m_total,
+      const std::array<double, 3>& F_x_Ctotal)  // NOLINT(readability-identifier-naming)
+      const noexcept {
+    return model_->mass(q, I_total, m_total, F_x_Ctotal);
+  }
+
+  /**
+   * Calculates the Coriolis force vector (state-space equation): \f$ c= C \times
+   * dq\f$, in \f$[Nm]\f$.
+   *
+   * @param[in] q Joint position.
+   * @param[in] dq Joint velocity.
+   * @param[in] I_total Inertia of the attached total load including end effector, relative to
+   * center of mass, given as vectorized 3x3 column-major matrix. Unit: \f$[kg \times m^2]\f$.
+   * @param[in] m_total Weight of the attached total load including end effector.
+   * Unit: \f$[kg]\f$.
+   * @param[in] F_x_Ctotal Translation from flange to center of mass of the attached total load.
+   * Unit: \f$[m]\f$.
+   *
+   * @return Coriolis force vector.
+   */
+  [[nodiscard]] std::array<double, 7> coriolis(
+      const std::array<double, 7>& q,        // NOLINT(readability-identifier-length)
+      const std::array<double, 7>& dq,       // NOLINT(readability-identifier-length)
+      const std::array<double, 9>& I_total,  // NOLINT(readability-identifier-naming)
+      double m_total,
+      const std::array<double, 3>& F_x_Ctotal)  // NOLINT(readability-identifier-naming)
+      const noexcept {
+    return model_->coriolis(q, dq, I_total, m_total, F_x_Ctotal);
+  }
+
+  /**
+   * Calculates the gravity vector. Unit: \f$[Nm]\f$.
+   *
+   * @param[in] q Joint position.
+   * @param[in] m_total Weight of the attached total load including end effector.
+   * Unit: \f$[kg]\f$.
+   * @param[in] F_x_Ctotal Translation from flange to center of mass of the attached total load.
+   * Unit: \f$[m]\f$.
+   * @param[in] gravity_earth Earth's gravity vector. Unit: \f$\frac{m}{s^2}\f$.
+   *
+   * @return Gravity vector.
+   */
+  [[nodiscard]] std::array<double, 7> gravity(
+      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
+      double m_total,
+      const std::array<double, 3>& F_x_Ctotal,  // NOLINT(readability-identifier-naming)
+      const std::array<double, 3>& gravity_earth) const noexcept {
+    return model_->gravity(q, m_total, F_x_Ctotal, gravity_earth);
+  }
+  /**
+   * Gets the 4x4 pose matrix for the given frame in base frame.
+   *
+   * The pose is represented as a 4x4 matrix in column-major format.
+   *
+   * @param[in] frame The desired frame.
+   * @param[in] robot_state State from which the pose should be calculated.
+   *
+   * @return Vectorized 4x4 pose matrix, column-major.
+   */
+  [[nodiscard]] virtual std::array<double, 16> pose(franka::Frame frame,
+                                                    const franka::RobotState& robot_state) const {
+    return pose(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
+  }
+
+  /**
+   * Gets the 6x7 Jacobian for the given frame, relative to that frame.
+   *
+   * The Jacobian is represented as a 6x7 matrix in column-major format.
+   *
+   * @param[in] frame The desired frame.
+   * @param[in] robot_state State from which the pose should be calculated.
+   *
+   * @return Vectorized 6x7 Jacobian, column-major.
+   */
+  [[nodiscard]] virtual std::array<double, 42> bodyJacobian(
+      franka::Frame frame,
+      const franka::RobotState& robot_state) const {
+    return bodyJacobian(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
+  }
+
+  /**
+   * Gets the 6x7 Jacobian for the given joint relative to the base frame.
+   *
+   * The Jacobian is represented as a 6x7 matrix in column-major format.
+   *
+   * @param[in] frame The desired frame.
+   * @param[in] robot_state State from which the pose should be calculated.
+   *
+   * @return Vectorized 6x7 Jacobian, column-major.
+   */
+  [[nodiscard]] virtual std::array<double, 42> zeroJacobian(
+      franka::Frame frame,
+      const franka::RobotState& robot_state) const {
+    return zeroJacobian(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
+  }
+
+  /**
+   * Calculates the 7x7 mass matrix. Unit: \f$[kg \times m^2]\f$.
+   *
+   * @param[in] robot_state State from which the pose should be calculated.
+   *
+   * @return Vectorized 7x7 mass matrix, column-major.
+   */
+  [[nodiscard]] virtual std::array<double, 49> mass(const franka::RobotState& robot_state) const {
+    return mass(robot_state.q, robot_state.I_total, robot_state.m_total, robot_state.F_x_Ctotal);
+  }
+
+  /**
+   * Calculates the Coriolis force vector (state-space equation): \f$ c= C \times
+   * dq\f$, in \f$[Nm]\f$.
+   *
+   * @param[in] robot_state State from which the Coriolis force vector should be calculated.
+   *
+   * @return Coriolis force vector.
+   */
+  [[nodiscard]] virtual std::array<double, 7> coriolis(
+      const franka::RobotState& robot_state) const {
+    return coriolis(robot_state.q, robot_state.dq, robot_state.I_total, robot_state.m_total,
+                    robot_state.F_x_Ctotal);
+  }
+
+  /**
+   * Calculates the gravity vector. Unit: \f$[Nm]\f$. Assumes default gravity vector of -9.81 m/s^2
+   *
+   * @param[in] q Joint position.
+   * @param[in] m_total Weight of the attached total load including end effector.
+   * Unit: \f$[kg]\f$.
+   * @param[in] F_x_Ctotal Translation from flange to center of mass of the attached total load.
+   * Unit: \f$[m]\f$.
+   *
+   * @return Gravity vector.
+   */
+  [[nodiscard]] std::array<double, 7> gravity(
+      const std::array<double, 7>& q,  // NOLINT(readability-identifier-length)
+      double m_total,
+      const std::array<double, 3>& F_x_Ctotal  // NOLINT(readability-identifier-naming)
+  ) const {
+    return gravity(q, m_total, F_x_Ctotal, {0, 0, -9.81});
+  }
+
+  /**
+   * Calculates the gravity vector. Unit: \f$[Nm]\f$.
+   *
+   * @param[in] robot_state State from which the gravity vector should be calculated.
+   * @param[in] gravity_earth Earth's gravity vector. Unit: \f$\frac{m}{s^2}\f$.
+   *
+   * @return Gravity vector.
+   */
+  [[nodiscard]] virtual std::array<double, 7> gravity(
+      const franka::RobotState& robot_state,
+      const std::array<double, 3>& gravity_earth) const {
+    return gravity(robot_state.q, robot_state.m_total, robot_state.F_x_Ctotal, gravity_earth);
+  }
+
+  /**
+   * Calculates the gravity vector. Unit: \f$[Nm]\f$. Assumes default gravity vector of -9.81 m/s^2
+   *
+   * @param[in] robot_state State from which the gravity vector should be calculated.
+   *
+   * @return Gravity vector.
+   */
+  [[nodiscard]] virtual std::array<double, 7> gravity(const franka::RobotState& robot_state) const {
+#ifdef ENABLE_BASE_ACCELERATION
+    return gravity(robot_state.q, robot_state.m_total, robot_state.F_x_Ctotal, robot_state.O_ddP_O);
+#else
+    return gravity(robot_state.q, robot_state.m_total, robot_state.F_x_Ctotal, {0, 0, -9.81});
+#endif
+  }
+
+ protected:
+  Model() = default;
+
+ private:
+  franka::Model* model_;
+};
+
+}  // namespace franka_hardware

franka_hardware/include/franka_hardware/robot.hpp

@@ -0,0 +1,104 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// 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.
+
+#pragma once
+
+#include <array>
+#include <atomic>
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+
+#include <franka/model.h>
+#include <franka/robot.h>
+#include <franka_hardware/model.hpp>
+#include <rclcpp/logger.hpp>
+
+namespace franka_hardware {
+
+class Robot {
+ public:
+  /**
+   * Connects to the robot. This method can block for up to one minute if the robot is not
+   * responding. An exception will be thrown if the connection cannot be established.
+   *
+   * @param[in] robot_ip IP address or hostname of the robot.
+   * @param[im] logger ROS Logger to print eventual warnings.
+   */
+  explicit Robot(const std::string& robot_ip, const rclcpp::Logger& logger);
+  Robot(const Robot&) = delete;
+  Robot& operator=(const Robot& other) = delete;
+  Robot& operator=(Robot&& other) = delete;
+  Robot(Robot&& other) = delete;
+
+  /// Stops the currently running loop and closes the connection with the robot.
+  virtual ~Robot();
+
+  /**
+   * Starts a torque control loop. Before using this method make sure that no other
+   * control or reading loop is currently active.
+   */
+  virtual void initializeTorqueControl();
+
+  /**
+   * Starts a reading loop of the robot state. Before using this method make sure that no other
+   * control or reading loop is currently active.
+   */
+  virtual void initializeContinuousReading();
+
+  /// stops the control or reading loop of the robot.
+  virtual void stopRobot();
+
+  /**
+   * Get the current robot state in a thread-safe way.
+   * @return current robot state.
+   */
+  virtual franka::RobotState read();
+
+  /**
+   * Return pointer to the franka robot model object .
+   * @return pointer to the current robot model.
+   */
+  virtual franka_hardware::Model* getModel();
+
+  /**
+   * Sends new desired torque commands to the control loop in a thread-safe way.
+   * The robot will use these torques until a different set of torques are commanded.
+   * @param[in] efforts torque command for each joint.
+   */
+  virtual void write(const std::array<double, 7>& efforts);
+
+  /// @return true if there is no control or reading loop running.
+  [[nodiscard]] virtual bool isStopped() const;
+
+ protected:
+  Robot() = default;
+
+ private:
+  std::unique_ptr<std::thread> control_thread_;
+  std::unique_ptr<franka::Robot> robot_;
+  std::unique_ptr<franka::Model> model_;
+  std::unique_ptr<Model> franka_hardware_model_;
+
+  std::mutex read_mutex_;
+  std::mutex write_mutex_;
+  std::atomic_bool finish_{false};
+  bool stopped_ = true;
+  franka::RobotState current_state_;
+
+  std::array<double, 7> tau_command_{};
+};
+}  // namespace franka_hardware

franka_hardware/package.xml

@@ -0,0 +1,30 @@
+<?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>franka_hardware</name>
+  <version>0.1.0</version>
+  <description>franka_hardware provides hardware interfaces for using Franka Emika research robots with ros2_control</description>
+  <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>franka_msgs</depend>
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+
+  <test_depend>ament_cmake_clang_format</test_depend>
+  <test_depend>ament_cmake_copyright</test_depend>
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_cmake_flake8</test_depend>
+  <test_depend>ament_cmake_lint_cmake</test_depend>
+  <test_depend>ament_cmake_pep257</test_depend>
+  <test_depend>ament_cmake_xmllint</test_depend>
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ament_cmake_clang_tidy</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

franka_hardware/src/franka_hardware_interface.cpp

@@ -0,0 +1,231 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// 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.
+
+#include <franka_hardware/franka_hardware_interface.hpp>
+
+#include <algorithm>
+#include <cmath>
+#include <exception>
+
+#include <franka/exception.h>
+#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/macros.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace franka_hardware {
+
+using StateInterface = hardware_interface::StateInterface;
+using CommandInterface = hardware_interface::CommandInterface;
+
+FrankaHardwareInterface::FrankaHardwareInterface(std::unique_ptr<Robot> robot)
+    : robot_{std::move(robot)} {}
+
+std::vector<StateInterface> FrankaHardwareInterface::export_state_interfaces() {
+  std::vector<StateInterface> state_interfaces;
+  for (auto i = 0U; i < info_.joints.size(); i++) {
+    state_interfaces.emplace_back(StateInterface(
+        info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_.at(i)));
+    state_interfaces.emplace_back(StateInterface(
+        info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_.at(i)));
+    state_interfaces.emplace_back(
+        StateInterface(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_efforts_.at(i)));
+  }
+
+  state_interfaces.emplace_back(StateInterface(
+      k_robot_name, k_robot_state_interface_name,
+      reinterpret_cast<double*>(  // NOLINT(cppcoreguidelines-pro-type-reinterpret-cast)
+          &hw_franka_robot_state_addr_)));
+  state_interfaces.emplace_back(StateInterface(
+      k_robot_name, k_robot_model_interface_name,
+      reinterpret_cast<double*>(  // NOLINT(cppcoreguidelines-pro-type-reinterpret-cast)
+          &hw_franka_model_ptr_)));
+  return state_interfaces;
+}
+
+std::vector<CommandInterface> FrankaHardwareInterface::export_command_interfaces() {
+  std::vector<CommandInterface> command_interfaces;
+  command_interfaces.reserve(info_.joints.size());
+  for (auto i = 0U; i < info_.joints.size(); i++) {
+    command_interfaces.emplace_back(CommandInterface(
+        info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_commands_.at(i)));
+  }
+  return command_interfaces;
+}
+
+CallbackReturn FrankaHardwareInterface::on_activate(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  robot_->initializeContinuousReading();
+  hw_commands_.fill(0);
+  read(rclcpp::Time(0),
+       rclcpp::Duration(0, 0));  // makes sure that the robot state is properly initialized.
+  RCLCPP_INFO(getLogger(), "Started");
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn FrankaHardwareInterface::on_deactivate(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  RCLCPP_INFO(getLogger(), "trying to Stop...");
+  robot_->stopRobot();
+  RCLCPP_INFO(getLogger(), "Stopped");
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::return_type FrankaHardwareInterface::read(const rclcpp::Time& /*time*/,
+                                                              const rclcpp::Duration& /*period*/) {
+  if (hw_franka_model_ptr_ == nullptr) {
+    hw_franka_model_ptr_ = robot_->getModel();
+  }
+  hw_franka_robot_state_ = robot_->read();
+  hw_positions_ = hw_franka_robot_state_.q;
+  hw_velocities_ = hw_franka_robot_state_.dq;
+  hw_efforts_ = hw_franka_robot_state_.tau_J;
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type FrankaHardwareInterface::write(const rclcpp::Time& /*time*/,
+                                                               const rclcpp::Duration& /*period*/) {
+  if (std::any_of(hw_commands_.begin(), hw_commands_.end(),
+                  [](double hw_command) { return !std::isfinite(hw_command); })) {
+    return hardware_interface::return_type::ERROR;
+  }
+
+  robot_->write(hw_commands_);
+  return hardware_interface::return_type::OK;
+}
+
+CallbackReturn FrankaHardwareInterface::on_init(const hardware_interface::HardwareInfo& info) {
+  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
+    return CallbackReturn::ERROR;
+  }
+  if (info_.joints.size() != kNumberOfJoints) {
+    RCLCPP_FATAL(getLogger(), "Got %ld joints. Expected %ld.", info_.joints.size(),
+                 kNumberOfJoints);
+    return CallbackReturn::ERROR;
+  }
+
+  for (const auto& joint : info_.joints) {
+    if (joint.command_interfaces.size() != 1) {
+      RCLCPP_FATAL(getLogger(), "Joint '%s' has %zu command interfaces found. 1 expected.",
+                   joint.name.c_str(), joint.command_interfaces.size());
+      return CallbackReturn::ERROR;
+    }
+    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_EFFORT) {
+      RCLCPP_FATAL(getLogger(), "Joint '%s' has unexpected command interface '%s'. Expected '%s'",
+                   joint.name.c_str(), joint.command_interfaces[0].name.c_str(),
+                   hardware_interface::HW_IF_EFFORT);
+      return CallbackReturn::ERROR;
+    }
+    if (joint.state_interfaces.size() != 3) {
+      RCLCPP_FATAL(getLogger(), "Joint '%s' has %zu state interfaces found. 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(getLogger(), "Joint '%s' has unexpected state interface '%s'. Expected '%s'",
+                   joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
+                   hardware_interface::HW_IF_POSITION);
+    }
+    if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) {
+      RCLCPP_FATAL(getLogger(), "Joint '%s' has unexpected state interface '%s'. Expected '%s'",
+                   joint.name.c_str(), joint.state_interfaces[1].name.c_str(),
+                   hardware_interface::HW_IF_VELOCITY);
+    }
+    if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) {
+      RCLCPP_FATAL(getLogger(), "Joint '%s' has unexpected state interface '%s'. Expected '%s'",
+                   joint.name.c_str(), joint.state_interfaces[2].name.c_str(),
+                   hardware_interface::HW_IF_EFFORT);
+    }
+  }
+  if (!robot_) {
+    std::string robot_ip;
+    try {
+      robot_ip = info_.hardware_parameters.at("robot_ip");
+    } catch (const std::out_of_range& ex) {
+      RCLCPP_FATAL(getLogger(), "Parameter 'robot_ip' is not set");
+      return CallbackReturn::ERROR;
+    }
+    try {
+      RCLCPP_INFO(getLogger(), "Connecting to robot at \"%s\" ...", robot_ip.c_str());
+      robot_ = std::make_unique<Robot>(robot_ip, getLogger());
+    } catch (const franka::Exception& e) {
+      RCLCPP_FATAL(getLogger(), "Could not connect to robot");
+      RCLCPP_FATAL(getLogger(), "%s", e.what());
+      return CallbackReturn::ERROR;
+    }
+    RCLCPP_INFO(getLogger(), "Successfully connected to robot");
+  }
+  return CallbackReturn::SUCCESS;
+}
+
+rclcpp::Logger FrankaHardwareInterface::getLogger() {
+  return rclcpp::get_logger("FrankaHardwareInterface");
+}
+
+hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_switch(
+    const std::vector<std::string>& /*start_interfaces*/,
+    const std::vector<std::string>& /*stop_interfaces*/) {
+  if (!effort_interface_running_ && effort_interface_claimed_) {
+    robot_->stopRobot();
+    robot_->initializeTorqueControl();
+    effort_interface_running_ = true;
+  } else if (effort_interface_running_ && !effort_interface_claimed_) {
+    robot_->stopRobot();
+    robot_->initializeContinuousReading();
+    effort_interface_running_ = false;
+  }
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type FrankaHardwareInterface::prepare_command_mode_switch(
+    const std::vector<std::string>& start_interfaces,
+    const std::vector<std::string>& stop_interfaces) {
+  auto is_effort_interface = [](const std::string& interface) {
+    return interface.find(hardware_interface::HW_IF_EFFORT) != std::string::npos;
+  };
+
+  int64_t num_stop_effort_interfaces =
+      std::count_if(stop_interfaces.begin(), stop_interfaces.end(), is_effort_interface);
+  if (num_stop_effort_interfaces == kNumberOfJoints) {
+    effort_interface_claimed_ = false;
+  } else if (num_stop_effort_interfaces != 0) {
+    RCLCPP_FATAL(this->getLogger(), "Expected %ld effort interfaces to stop, but got %ld instead.",
+                 kNumberOfJoints, num_stop_effort_interfaces);
+    std::string error_string = "Invalid number of effort interfaces to stop. Expected ";
+    error_string += std::to_string(kNumberOfJoints);
+    throw std::invalid_argument(error_string);
+  }
+
+  int64_t num_start_effort_interfaces =
+      std::count_if(start_interfaces.begin(), start_interfaces.end(), is_effort_interface);
+  if (num_start_effort_interfaces == kNumberOfJoints) {
+    effort_interface_claimed_ = true;
+  } else if (num_start_effort_interfaces != 0) {
+    RCLCPP_FATAL(this->getLogger(), "Expected %ld effort interfaces to start, but got %ld instead.",
+                 kNumberOfJoints, num_start_effort_interfaces);
+    std::string error_string = "Invalid number of effort interfaces to start. Expected ";
+    error_string += std::to_string(kNumberOfJoints);
+    throw std::invalid_argument(error_string);
+  }
+  return hardware_interface::return_type::OK;
+}
+}  // namespace franka_hardware
+
+#include "pluginlib/class_list_macros.hpp"
+// NOLINTNEXTLINE
+PLUGINLIB_EXPORT_CLASS(franka_hardware::FrankaHardwareInterface,
+                       hardware_interface::SystemInterface)

franka_hardware/src/robot.cpp

@@ -0,0 +1,104 @@
+// Copyright (c) 2021 Franka Emika GmbH
+//
+// 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.
+
+#include <franka_hardware/robot.hpp>
+
+#include <cassert>
+#include <mutex>
+
+#include <franka/control_tools.h>
+#include <rclcpp/logging.hpp>
+
+namespace franka_hardware {
+
+Robot::Robot(const std::string& robot_ip, const rclcpp::Logger& logger) {
+  tau_command_.fill(0.);
+  franka::RealtimeConfig rt_config = franka::RealtimeConfig::kEnforce;
+  if (!franka::hasRealtimeKernel()) {
+    rt_config = franka::RealtimeConfig::kIgnore;
+    RCLCPP_WARN(
+        logger,
+        "You are not using a real-time kernel. Using a real-time kernel is strongly recommended!");
+  }
+  robot_ = std::make_unique<franka::Robot>(robot_ip, rt_config);
+  model_ = std::make_unique<franka::Model>(robot_->loadModel());
+  franka_hardware_model_ = std::make_unique<Model>(model_.get());
+}
+
+void Robot::write(const std::array<double, 7>& efforts) {
+  std::lock_guard<std::mutex> lock(write_mutex_);
+  tau_command_ = efforts;
+}
+
+franka::RobotState Robot::read() {
+  std::lock_guard<std::mutex> lock(read_mutex_);
+  return {current_state_};
+}
+
+franka_hardware::Model* Robot::getModel() {
+  return franka_hardware_model_.get();
+}
+
+void Robot::stopRobot() {
+  if (!stopped_) {
+    finish_ = true;
+    control_thread_->join();
+    finish_ = false;
+    stopped_ = true;
+  }
+}
+
+void Robot::initializeTorqueControl() {
+  assert(isStopped());
+  stopped_ = false;
+  const auto kTorqueControl = [this]() {
+    robot_->control(
+        [this](const franka::RobotState& state, const franka::Duration& /*period*/) {
+          {
+            std::lock_guard<std::mutex> lock(read_mutex_);
+            current_state_ = state;
+          }
+          std::lock_guard<std::mutex> lock(write_mutex_);
+          franka::Torques out(tau_command_);
+          out.motion_finished = finish_;
+          return out;
+        },
+        true, franka::kMaxCutoffFrequency);
+  };
+  control_thread_ = std::make_unique<std::thread>(kTorqueControl);
+}
+
+void Robot::initializeContinuousReading() {
+  assert(isStopped());
+  stopped_ = false;
+  const auto kReading = [this]() {
+    robot_->read([this](const franka::RobotState& state) {
+      {
+        std::lock_guard<std::mutex> lock(read_mutex_);
+        current_state_ = state;
+      }
+      return !finish_;
+    });
+  };
+  control_thread_ = std::make_unique<std::thread>(kReading);
+}
+
+Robot::~Robot() {
+  stopRobot();
+}
+
+bool Robot::isStopped() const {
+  return stopped_;
+}
+}  // namespace franka_hardware

franka_hardware/test/CMakeLists.txt

@@ -0,0 +1,6 @@
+find_package(ament_cmake_gmock REQUIRED)
+
+ament_add_gmock(${PROJECT_NAME}_test franka_hardware_interface_test.cpp)
+target_include_directories(${PROJECT_NAME}_test PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include)
+
+target_link_libraries(${PROJECT_NAME}_test  ${PROJECT_NAME})

franka_hardware/test/franka_hardware_interface_test.cpp

@@ -0,0 +1,400 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// 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.
+
+#include <gmock/gmock.h>
+#include <exception>
+#include <rclcpp/rclcpp.hpp>
+
+#include <franka_hardware/franka_hardware_interface.hpp>
+#include <franka_hardware/model.hpp>
+#include <franka_hardware/robot.hpp>
+
+#include <hardware_interface/hardware_info.hpp>
+#include <hardware_interface/types/hardware_interface_return_values.hpp>
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+
+const std::string k_position_controller{"position"};
+const std::string k_velocity_controller{"velocity"};
+const std::string k_effort_controller{"effort"};
+const std::string k_joint_name{"joint"};
+const size_t k_number_of_joints{7};
+const double k_EPS{1e-5};
+
+class MockModel : public franka_hardware::Model {};
+
+class MockRobot : public franka_hardware::Robot {
+ public:
+  MOCK_METHOD(void, initializeContinuousReading, (), (override));
+  MOCK_METHOD(void, stopRobot, (), (override));
+  MOCK_METHOD(void, initializeTorqueControl, (), (override));
+  MOCK_METHOD(franka::RobotState, read, (), (override));
+  MOCK_METHOD(MockModel*, getModel, (), (override));
+};
+
+auto createHardwareInfo() -> hardware_interface::HardwareInfo {
+  hardware_interface::HardwareInfo info;
+  std::unordered_map<std::string, std::string> hw_params;
+  hw_params["robot_ip"] = "dummy_ip";
+
+  info.hardware_parameters = hw_params;
+  hardware_interface::InterfaceInfo command_interface, effort_state_interface,
+      position_state_interface, velocity_state_interface;
+
+  effort_state_interface.name = hardware_interface::HW_IF_EFFORT;
+  position_state_interface.name = hardware_interface::HW_IF_POSITION;
+  velocity_state_interface.name = hardware_interface::HW_IF_VELOCITY;
+
+  std::vector<hardware_interface::InterfaceInfo> state_interfaces = {
+      position_state_interface, velocity_state_interface, effort_state_interface};
+
+  for (auto i = 0U; i < k_number_of_joints; i++) {
+    hardware_interface::ComponentInfo joint;
+
+    joint.name = k_joint_name + std::to_string(i + 1);
+
+    command_interface.name = k_effort_controller;
+
+    joint.command_interfaces.push_back(command_interface);
+    joint.state_interfaces = state_interfaces;
+
+    info.joints.push_back(joint);
+  }
+
+  return info;
+}
+
+TEST(FrankaHardwareInterfaceTest, when_on_init_called_expect_success) {
+  auto mock_robot = std::make_unique<MockRobot>();
+  const hardware_interface::HardwareInfo info = createHardwareInfo();
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+  auto return_type = franka_hardware_interface.on_init(info);
+
+  EXPECT_EQ(return_type,
+            rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS);
+}
+
+TEST(FrankaHardwareInterfaceTest, given_that_the_robot_interfaces_set_when_read_called_return_ok) {
+  franka::RobotState robot_state;
+  MockModel mock_model;
+  MockModel* model_address = &mock_model;
+
+  auto mock_robot = std::make_unique<MockRobot>();
+  EXPECT_CALL(*mock_robot, read()).WillOnce(testing::Return(robot_state));
+  EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));
+
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+
+  auto time = rclcpp::Time(0, 0);
+  auto duration = rclcpp::Duration(0, 0);
+  auto return_type = franka_hardware_interface.read(time, duration);
+  EXPECT_EQ(return_type, hardware_interface::return_type::OK);
+}
+
+TEST(
+    FrankaHardwareInterfaceTest,
+    given_that_the_robot_interfaces_are_set_when_call_export_state_return_zero_values_and_correct_interface_names) {
+  franka::RobotState robot_state;
+  const size_t state_interface_size =
+      23;  // position, effort and velocity states for every joint + robot state and model
+  auto mock_robot = std::make_unique<MockRobot>();
+  MockModel mock_model;
+  MockModel* model_address = &mock_model;
+
+  EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));
+  EXPECT_CALL(*mock_robot, read()).WillOnce(testing::Return(robot_state));
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+
+  const auto hardware_info = createHardwareInfo();
+  franka_hardware_interface.on_init(hardware_info);
+  auto time = rclcpp::Time(0);
+  auto duration = rclcpp::Duration(0, 0);
+  auto return_type = franka_hardware_interface.read(time, duration);
+  EXPECT_EQ(return_type, hardware_interface::return_type::OK);
+  auto states = franka_hardware_interface.export_state_interfaces();
+  size_t joint_index = 0;
+
+  // Get all the states except the last two reserved for robot state
+  for (size_t i = 0; i < states.size() - 2; i++) {
+    if (i % 3 == 0) {
+      joint_index++;
+    }
+    const std::string joint_name = k_joint_name + std::to_string(joint_index);
+    if (i % 3 == 0) {
+      EXPECT_EQ(states[i].get_name(), joint_name + "/" + k_position_controller);
+    } else if (i % 3 == 1) {
+      EXPECT_EQ(states[i].get_name(), joint_name + "/" + k_velocity_controller);
+    } else {
+      EXPECT_EQ(states[i].get_name(), joint_name + "/" + k_effort_controller);
+    }
+    EXPECT_EQ(states[i].get_value(), 0.0);
+  }
+
+  EXPECT_EQ(states.size(), state_interface_size);
+}
+
+TEST(
+    FrankaHardwareInterfaceTest,
+    given_that_the_robot_interfaces_are_set_when_call_export_state_interface_robot_model_interface_exists) {
+  franka::RobotState robot_state;
+  const size_t state_interface_size =
+      23;  // position, effort and velocity states for every joint + robot state and model
+  auto mock_robot = std::make_unique<MockRobot>();
+
+  MockModel mock_model;
+  MockModel* model_address = &mock_model;
+
+  EXPECT_CALL(*mock_robot, read()).WillOnce(testing::Return(robot_state));
+  EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+
+  const auto hardware_info = createHardwareInfo();
+  franka_hardware_interface.on_init(hardware_info);
+  auto time = rclcpp::Time(0);
+  auto duration = rclcpp::Duration(0, 0);
+  auto return_type = franka_hardware_interface.read(time, duration);
+  EXPECT_EQ(return_type, hardware_interface::return_type::OK);
+  auto states = franka_hardware_interface.export_state_interfaces();
+  EXPECT_EQ(states[state_interface_size - 1].get_name(),
+            "panda/robot_model");  // Last state interface is the robot model state
+  EXPECT_NEAR(states[state_interface_size - 1].get_value(),
+              *reinterpret_cast<double*>(&model_address),
+              k_EPS);  // testing that the casted mock_model ptr
+                       // is correctly pushed to state interface
+}
+
+TEST(
+    FrankaHardwareInterfaceTest,
+    given_that_the_robot_interfaces_are_set_when_call_export_state_interface_robot_state_interface_exists) {
+  const size_t state_interface_size =
+      23;  // position, effort and velocity states for every joint + robot state and model
+  auto mock_robot = std::make_unique<MockRobot>();
+
+  franka::RobotState robot_state;
+  franka::RobotState* robot_state_address = &robot_state;
+
+  MockModel mock_model;
+  MockModel* model_address = &mock_model;
+
+  EXPECT_CALL(*mock_robot, read()).WillOnce(testing::Return(robot_state));
+  EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+
+  const auto hardware_info = createHardwareInfo();
+  franka_hardware_interface.on_init(hardware_info);
+  auto time = rclcpp::Time(0);
+  auto duration = rclcpp::Duration(0, 0);
+  auto return_type = franka_hardware_interface.read(time, duration);
+  EXPECT_EQ(return_type, hardware_interface::return_type::OK);
+  auto states = franka_hardware_interface.export_state_interfaces();
+  EXPECT_EQ(states[state_interface_size - 2].get_name(),
+            "panda/robot_state");  // Last state interface is the robot model state
+  EXPECT_NEAR(states[state_interface_size - 2].get_value(),
+              *reinterpret_cast<double*>(&robot_state_address),
+              k_EPS);  // testing that the casted robot state ptr
+                       // is correctly pushed to state interface
+}
+
+TEST(FrankaHardwareInterfaceTest,
+     when_prepare_command_mode_interface_for_stop_effort_interfaces_expect_ok) {
+  auto mock_robot = std::make_unique<MockRobot>();
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+
+  const auto hardware_info = createHardwareInfo();
+  franka_hardware_interface.on_init(hardware_info);
+  std::vector<std::string> stop_interface;
+
+  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
+    const std::string joint_name = k_joint_name + std::to_string(i);
+    stop_interface.push_back(joint_name + "/" + k_effort_controller);
+  }
+  std::vector<std::string> start_interface = {};
+  EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
+            hardware_interface::return_type::OK);
+}
+
+TEST(
+    FrankaHardwareInterfaceTest,
+    when_prepare_command_mode_interface_is_called_with_invalid_start_interface_number_expect_throw) {
+  auto mock_robot = std::make_unique<MockRobot>();
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+
+  const auto hardware_info = createHardwareInfo();
+  franka_hardware_interface.on_init(hardware_info);
+  std::vector<std::string> stop_interface;
+
+  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
+    const std::string joint_name = k_joint_name + std::to_string(i);
+    stop_interface.push_back(joint_name + "/" + k_effort_controller);
+  }
+  std::vector<std::string> start_interface = {"fr3_joint1/effort"};
+  EXPECT_THROW(
+      franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
+      std::invalid_argument);
+}
+
+TEST(FrankaHardwareInterfaceTest,
+     when_prepare_command_mode_interface_for_start_effort_interfaces_expect_ok) {
+  auto mock_robot = std::make_unique<MockRobot>();
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+
+  const auto hardware_info = createHardwareInfo();
+  franka_hardware_interface.on_init(hardware_info);
+  std::vector<std::string> start_interface;
+
+  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
+    const std::string joint_name = k_joint_name + std::to_string(i);
+    start_interface.push_back(joint_name + "/" + k_effort_controller);
+  }
+
+  std::vector<std::string> stop_interface = {};
+
+  EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
+            hardware_interface::return_type::OK);
+}
+
+TEST(
+    FrankaHardwareInterfaceTest,
+    when_prepare_command_mode_interface_is_called_with_invalid_stop_interface_number_expect_throw) {
+  auto mock_robot = std::make_unique<MockRobot>();
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+
+  const auto hardware_info = createHardwareInfo();
+  franka_hardware_interface.on_init(hardware_info);
+  std::vector<std::string> start_interface;
+
+  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
+    const std::string joint_name = k_joint_name + std::to_string(i);
+    start_interface.push_back(joint_name + "/" + k_effort_controller);
+  }
+
+  std::vector<std::string> stop_interface = {"fr3_joint1/effort"};
+
+  EXPECT_THROW(
+      franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
+      std::invalid_argument);
+}
+
+TEST(FrankaHardwareIntefaceTest, when_write_called_expect_ok) {
+  auto mock_robot = std::make_unique<MockRobot>();
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+
+  const auto hardware_info = createHardwareInfo();
+  franka_hardware_interface.on_init(hardware_info);
+
+  const auto time = rclcpp::Time(0, 0);
+  const auto duration = rclcpp::Duration(0, 0);
+
+  EXPECT_EQ(franka_hardware_interface.write(time, duration), hardware_interface::return_type::OK);
+}
+
+TEST(FrankaHardwareInterfaceTest, when_on_activate_called_expect_success) {
+  franka::RobotState robot_state;
+
+  MockModel mock_model;
+  MockModel* model_address = &mock_model;
+
+  auto mock_robot = std::make_unique<MockRobot>();
+  EXPECT_CALL(*mock_robot, read()).WillOnce(testing::Return(robot_state));
+  EXPECT_CALL(*mock_robot, getModel()).WillOnce(testing::Return(model_address));
+
+  EXPECT_CALL(*mock_robot, initializeContinuousReading());
+
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+
+  EXPECT_EQ(franka_hardware_interface.on_activate(rclcpp_lifecycle::State()),
+            rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS);
+}
+
+TEST(FrankaHardwareInterfaceTest, when_on_deactivate_called_expect_success) {
+  franka::RobotState robot_state;
+
+  auto mock_robot = std::make_unique<MockRobot>();
+  EXPECT_CALL(*mock_robot, stopRobot());
+
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+
+  EXPECT_EQ(franka_hardware_interface.on_deactivate(rclcpp_lifecycle::State()),
+            rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS);
+}
+
+TEST(FrankaHardwareInterfaceTest,
+     given_start_effort_interface_prepared_when_perform_comamnd_mode_switch_called_expect_ok) {
+  auto mock_robot = std::make_unique<MockRobot>();
+  EXPECT_CALL(*mock_robot, stopRobot());
+  EXPECT_CALL(*mock_robot, initializeTorqueControl());
+
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+
+  const auto hardware_info = createHardwareInfo();
+  franka_hardware_interface.on_init(hardware_info);
+  std::vector<std::string> start_interface;
+
+  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
+    const std::string joint_name = k_joint_name + std::to_string(i);
+    start_interface.push_back(joint_name + "/" + k_effort_controller);
+  }
+
+  std::vector<std::string> stop_interface = {};
+
+  EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
+            hardware_interface::return_type::OK);
+
+  EXPECT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface),
+            hardware_interface::return_type::OK);
+}
+
+TEST(FrankaHardwareInterfaceTest,
+     given_that_effort_control_started_perform_command_mode_switch_stop_expect_ok) {
+  auto mock_robot = std::make_unique<MockRobot>();
+  EXPECT_CALL(*mock_robot, stopRobot()).Times(2).WillRepeatedly(testing::Return());
+  EXPECT_CALL(*mock_robot, initializeTorqueControl());
+  EXPECT_CALL(*mock_robot, initializeContinuousReading());
+
+  franka_hardware::FrankaHardwareInterface franka_hardware_interface(std::move(mock_robot));
+
+  const auto hardware_info = createHardwareInfo();
+  franka_hardware_interface.on_init(hardware_info);
+  std::vector<std::string> start_interface;
+
+  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
+    const std::string joint_name = k_joint_name + std::to_string(i);
+    start_interface.push_back(joint_name + "/" + k_effort_controller);
+  }
+
+  std::vector<std::string> stop_interface = {};
+
+  EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
+            hardware_interface::return_type::OK);
+
+  EXPECT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface),
+            hardware_interface::return_type::OK);
+
+  for (size_t i = 0; i < hardware_info.joints.size(); i++) {
+    const std::string joint_name = k_joint_name + std::to_string(i);
+    stop_interface.push_back(joint_name + "/" + k_effort_controller);
+  }
+
+  start_interface.clear();
+
+  EXPECT_EQ(franka_hardware_interface.prepare_command_mode_switch(start_interface, stop_interface),
+            hardware_interface::return_type::OK);
+
+  EXPECT_EQ(franka_hardware_interface.perform_command_mode_switch(start_interface, stop_interface),
+            hardware_interface::return_type::OK);
+}
+
+int main(int argc, char** argv) {
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

franka_moveit_config/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.5)
+project(franka_moveit_config)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY config launch srdf rviz
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_add_pytest_test(${PROJECT_NAME}_srdf_tests test/srdf_tests.py)
+endif()
+
+ament_package()

franka_moveit_config/config/kinematics.yaml

@@ -0,0 +1,4 @@
+panda_arm:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05

franka_moveit_config/config/ompl_planning.yaml

@@ -0,0 +1,152 @@
+planner_configs:
+  SBLkConfigDefault:
+    type: geometric::SBL
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  ESTkConfigDefault:
+    type: geometric::EST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+  LBKPIECEkConfigDefault:
+    type: geometric::LBKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  BKPIECEkConfigDefault:
+    type: geometric::BKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  KPIECEkConfigDefault:
+    type: geometric::KPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  RRTkConfigDefault:
+    type: geometric::RRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+  RRTConnectkConfigDefault:
+    type: geometric::RRTConnect
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  RRTstarkConfigDefault:
+    type: geometric::RRTstar
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
+  TRRTkConfigDefault:
+    type: geometric::TRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    max_states_failed: 10  # when to start increasing temp. default: 10
+    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
+    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
+    init_temperature: 10e-6  # initial temperature. default: 10e-6
+    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
+  PRMkConfigDefault:
+    type: geometric::PRM
+    max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
+  PRMstarkConfigDefault:
+    type: geometric::PRMstar
+  FMTkConfigDefault:
+    type: geometric::FMT
+    num_samples: 1000  # number of states that the planner should sample. default: 1000
+    radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1
+    nearest_k: 1  # use Knearest strategy. default: 1
+    cache_cc: 1  # use collision checking cache. default: 1
+    heuristics: 0  # activate cost to go heuristics. default: 0
+    extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+  BFMTkConfigDefault:
+    type: geometric::BFMT
+    num_samples: 1000  # number of states that the planner should sample. default: 1000
+    radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0
+    nearest_k: 1  # use the Knearest strategy. default: 1
+    balanced: 0  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
+    optimality: 1  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
+    heuristics: 1  # activates cost to go heuristics. default: 1
+    cache_cc: 1  # use the collision checking cache. default: 1
+    extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+  PDSTkConfigDefault:
+    type: geometric::PDST
+  STRIDEkConfigDefault:
+    type: geometric::STRIDE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    use_projected_distance: 0  # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
+    degree: 16  # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
+    max_degree: 18  # max degree of a node in the GNAT. default: 12
+    min_degree: 12  # min degree of a node in the GNAT. default: 12
+    max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6
+    estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0
+    min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2
+  BiTRRTkConfigDefault:
+    type: geometric::BiTRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
+    init_temperature: 100  # initial temperature. default: 100
+    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+    frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
+  LBTRRTkConfigDefault:
+    type: geometric::LBTRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    epsilon: 0.4  # optimality approximation factor. default: 0.4
+  BiESTkConfigDefault:
+    type: geometric::BiEST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  ProjESTkConfigDefault:
+    type: geometric::ProjEST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+  LazyPRMkConfigDefault:
+    type: geometric::LazyPRM
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  LazyPRMstarkConfigDefault:
+    type: geometric::LazyPRMstar
+  SPARSkConfigDefault:
+    type: geometric::SPARS
+    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
+    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
+    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
+    max_failures: 1000  # maximum consecutive failure limit. default: 1000
+  SPARStwokConfigDefault:
+    type: geometric::SPARStwo
+    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
+    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
+    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
+    max_failures: 5000  # maximum consecutive failure limit. default: 5000
+  TrajOptDefault:
+    type: geometric::TrajOpt
+
+panda_arm:
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+    - FMTkConfigDefault
+    - BFMTkConfigDefault
+    - PDSTkConfigDefault
+    - STRIDEkConfigDefault
+    - BiTRRTkConfigDefault
+    - LBTRRTkConfigDefault
+    - BiESTkConfigDefault
+    - ProjESTkConfigDefault
+    - LazyPRMkConfigDefault
+    - LazyPRMstarkConfigDefault
+    - SPARSkConfigDefault
+    - SPARStwokConfigDefault
+    - TrajOptDefault
+

franka_moveit_config/config/panda_controllers.yaml

@@ -0,0 +1,15 @@
+controller_names:
+  - panda_arm_controller
+
+panda_arm_controller:
+  action_ns: follow_joint_trajectory
+  type: FollowJointTrajectory
+  default: true
+  joints:
+    - panda_joint1
+    - panda_joint2
+    - panda_joint3
+    - panda_joint4
+    - panda_joint5
+    - panda_joint6
+    - panda_joint7

franka_moveit_config/config/panda_ros_controllers.yaml

@@ -0,0 +1,40 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 1000  # Hz
+
+    panda_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    franka_robot_state_broadcaster:
+      type: franka_robot_state_broadcaster/FrankaRobotStateBroadcaster
+
+franka_robot_state_broadcaster:
+  ros__parameters:
+    arm_id: panda
+
+panda_arm_controller:
+  ros__parameters:
+    command_interfaces:
+      - effort
+    state_interfaces:
+      - position
+      - velocity
+    joints:
+      - panda_joint1
+      - panda_joint2
+      - panda_joint3
+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7
+    gains:
+      panda_joint1: { p: 600., d: 30., i: 0., i_clamp: 1., ff_velocity_scale: 1.}
+      panda_joint2: { p: 600., d: 30., i: 0., i_clamp: 1., ff_velocity_scale: 1. }
+      panda_joint3: { p: 600., d: 30., i: 0., i_clamp: 1., ff_velocity_scale: 1. }
+      panda_joint4: { p: 600., d: 30., i: 0., i_clamp: 1., ff_velocity_scale: 1. }
+      panda_joint5: { p: 250., d: 10., i: 0., i_clamp: 1., ff_velocity_scale: 1. }
+      panda_joint6: { p: 150., d: 10., i: 0., i_clamp: 1., ff_velocity_scale: 1. }
+      panda_joint7: { p: 50., d: 5., i: 0., i_clamp: 1., ff_velocity_scale: 1. }

franka_moveit_config/launch/moveit.launch.py

@@ -0,0 +1,244 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  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.
+
+# This file is an adapted version of
+# https://github.com/ros-planning/moveit_resources/blob/ca3f7930c630581b5504f3b22c40b4f82ee6369d/panda_moveit_config/launch/demo.launch.py
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription,
+                            Shutdown)
+from launch.conditions import UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+import yaml
+
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return yaml.safe_load(file)
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def generate_launch_description():
+    robot_ip_parameter_name = 'robot_ip'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
+
+    # Command-line arguments
+
+    db_arg = DeclareLaunchArgument(
+        'db', default_value='False', description='Database flag'
+    )
+
+    # planning_context
+    franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots',
+                                     'panda_arm.urdf.xacro')
+    robot_description_config = Command(
+        [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=true',
+         ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware,
+         ' fake_sensor_commands:=', fake_sensor_commands])
+
+    robot_description = {'robot_description': robot_description_config}
+
+    franka_semantic_xacro_file = os.path.join(get_package_share_directory('franka_moveit_config'),
+                                              'srdf',
+                                              'panda_arm.srdf.xacro')
+    robot_description_semantic_config = Command(
+        [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=true']
+    )
+    robot_description_semantic = {
+        'robot_description_semantic': robot_description_semantic_config
+    }
+
+    kinematics_yaml = load_yaml(
+        'franka_moveit_config', 'config/kinematics.yaml'
+    )
+
+    # Planning Functionality
+    ompl_planning_pipeline_config = {
+        'move_group': {
+            'planning_plugin': 'ompl_interface/OMPLPlanner',
+            'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization '
+                                'default_planner_request_adapters/ResolveConstraintFrames '
+                                'default_planner_request_adapters/FixWorkspaceBounds '
+                                'default_planner_request_adapters/FixStartStateBounds '
+                                'default_planner_request_adapters/FixStartStateCollision '
+                                'default_planner_request_adapters/FixStartStatePathConstraints',
+            'start_state_max_bounds_error': 0.1,
+        }
+    }
+    ompl_planning_yaml = load_yaml(
+        'franka_moveit_config', 'config/ompl_planning.yaml'
+    )
+    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
+
+    # Trajectory Execution Functionality
+    moveit_simple_controllers_yaml = load_yaml(
+        'franka_moveit_config', 'config/panda_controllers.yaml'
+    )
+    moveit_controllers = {
+        'moveit_simple_controller_manager': moveit_simple_controllers_yaml,
+        'moveit_controller_manager': 'moveit_simple_controller_manager'
+                                     '/MoveItSimpleControllerManager',
+    }
+
+    trajectory_execution = {
+        'moveit_manage_controllers': True,
+        'trajectory_execution.allowed_execution_duration_scaling': 1.2,
+        'trajectory_execution.allowed_goal_duration_margin': 0.5,
+        'trajectory_execution.allowed_start_tolerance': 0.01,
+    }
+
+    planning_scene_monitor_parameters = {
+        'publish_planning_scene': True,
+        'publish_geometry_updates': True,
+        'publish_state_updates': True,
+        'publish_transforms_updates': True,
+    }
+
+    # Start the actual move_group node/action server
+    run_move_group_node = Node(
+        package='moveit_ros_move_group',
+        executable='move_group',
+        output='screen',
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            kinematics_yaml,
+            ompl_planning_pipeline_config,
+            trajectory_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+        ],
+    )
+
+    # RViz
+    rviz_base = os.path.join(get_package_share_directory('franka_moveit_config'), 'rviz')
+    rviz_full_config = os.path.join(rviz_base, 'moveit.rviz')
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='log',
+        arguments=['-d', rviz_full_config],
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            ompl_planning_pipeline_config,
+            kinematics_yaml,
+        ],
+    )
+
+    # Publish TF
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='both',
+        parameters=[robot_description],
+    )
+
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory('franka_moveit_config'),
+        'config',
+        'panda_ros_controllers.yaml',
+    )
+    ros2_control_node = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        parameters=[robot_description, ros2_controllers_path],
+        remappings=[('joint_states', 'franka/joint_states')],
+        output={
+            'stdout': 'screen',
+            'stderr': 'screen',
+        },
+        on_exit=Shutdown(),
+    )
+
+    # Load controllers
+    load_controllers = []
+    for controller in ['panda_arm_controller', 'joint_state_broadcaster']:
+        load_controllers += [
+            ExecuteProcess(
+                cmd=['ros2 run controller_manager spawner {}'.format(controller)],
+                shell=True,
+                output='screen',
+            )
+        ]
+
+    joint_state_publisher = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        parameters=[
+            {'source_list': ['franka/joint_states', 'panda_gripper/joint_states'], 'rate': 30}],
+    )
+
+    franka_robot_state_broadcaster = Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['franka_robot_state_broadcaster'],
+            output='screen',
+            condition=UnlessCondition(use_fake_hardware),
+    )
+
+    robot_arg = DeclareLaunchArgument(
+        robot_ip_parameter_name,
+        description='Hostname or IP address of the robot.')
+
+    use_fake_hardware_arg = DeclareLaunchArgument(
+        use_fake_hardware_parameter_name,
+        default_value='false',
+        description='Use fake hardware')
+    fake_sensor_commands_arg = DeclareLaunchArgument(
+        fake_sensor_commands_parameter_name,
+        default_value='false',
+        description="Fake sensor commands. Only valid when '{}' is true".format(
+            use_fake_hardware_parameter_name))
+    gripper_launch_file = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([PathJoinSubstitution(
+            [FindPackageShare('franka_gripper'), 'launch', 'gripper.launch.py'])]),
+        launch_arguments={'robot_ip': robot_ip,
+                          use_fake_hardware_parameter_name: use_fake_hardware}.items(),
+    )
+    return LaunchDescription(
+        [robot_arg,
+         use_fake_hardware_arg,
+         fake_sensor_commands_arg,
+         db_arg,
+         rviz_node,
+         robot_state_publisher,
+         run_move_group_node,
+         ros2_control_node,
+         joint_state_publisher,
+         franka_robot_state_broadcaster,
+         gripper_launch_file
+         ]
+        + load_controllers
+    )

franka_moveit_config/launch/moveit_without_hand.launch.py

@@ -0,0 +1,237 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  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.
+
+# This file is an adapted version of
+# https://github.com/ros-planning/moveit_resources/blob/ca3f7930c630581b5504f3b22c40b4f82ee6369d/panda_moveit_config/launch/demo.launch.py
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription,
+                            Shutdown)
+from launch.conditions import UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+import yaml
+
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return yaml.safe_load(file)
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def generate_launch_description():
+    robot_ip_parameter_name = 'robot_ip'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
+
+    # Command-line arguments
+
+    db_arg = DeclareLaunchArgument(
+        'db', default_value='False', description='Database flag'
+    )
+
+    # planning_context
+    franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots',
+                                     'panda_arm.urdf.xacro')
+    robot_description_config = Command(
+        [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=false',
+         ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware,
+         ' fake_sensor_commands:=', fake_sensor_commands])
+
+    robot_description = {'robot_description': robot_description_config}
+
+    franka_semantic_xacro_file = os.path.join(get_package_share_directory('franka_moveit_config'),
+                                              'srdf',
+                                              'panda_arm.srdf.xacro')
+    robot_description_semantic_config = Command(
+        [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=false']
+    )
+    robot_description_semantic = {
+        'robot_description_semantic': robot_description_semantic_config
+    }
+
+    kinematics_yaml = load_yaml(
+        'franka_moveit_config', 'config/kinematics.yaml'
+    )
+
+    # Planning Functionality
+    ompl_planning_pipeline_config = {
+        'move_group': {
+            'planning_plugin': 'ompl_interface/OMPLPlanner',
+            'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization '
+                                'default_planner_request_adapters/ResolveConstraintFrames '
+                                'default_planner_request_adapters/FixWorkspaceBounds '
+                                'default_planner_request_adapters/FixStartStateBounds '
+                                'default_planner_request_adapters/FixStartStateCollision '
+                                'default_planner_request_adapters/FixStartStatePathConstraints',
+            'start_state_max_bounds_error': 0.1,
+        }
+    }
+    ompl_planning_yaml = load_yaml(
+        'franka_moveit_config', 'config/ompl_planning.yaml'
+    )
+    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
+
+    # Trajectory Execution Functionality
+    moveit_simple_controllers_yaml = load_yaml(
+        'franka_moveit_config', 'config/panda_controllers.yaml'
+    )
+    moveit_controllers = {
+        'moveit_simple_controller_manager': moveit_simple_controllers_yaml,
+        'moveit_controller_manager': 'moveit_simple_controller_manager'
+                                     '/MoveItSimpleControllerManager',
+    }
+
+    trajectory_execution = {
+        'moveit_manage_controllers': True,
+        'trajectory_execution.allowed_execution_duration_scaling': 1.2,
+        'trajectory_execution.allowed_goal_duration_margin': 0.5,
+        'trajectory_execution.allowed_start_tolerance': 0.01,
+    }
+
+    planning_scene_monitor_parameters = {
+        'publish_planning_scene': True,
+        'publish_geometry_updates': True,
+        'publish_state_updates': True,
+        'publish_transforms_updates': True,
+    }
+
+    # Start the actual move_group node/action server
+    run_move_group_node = Node(
+        package='moveit_ros_move_group',
+        executable='move_group',
+        output='screen',
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            kinematics_yaml,
+            ompl_planning_pipeline_config,
+            trajectory_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+        ],
+    )
+
+    # RViz
+    rviz_base = os.path.join(get_package_share_directory('franka_moveit_config'), 'rviz')
+    rviz_full_config = os.path.join(rviz_base, 'moveit.rviz')
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='log',
+        arguments=['-d', rviz_full_config],
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            ompl_planning_pipeline_config,
+            kinematics_yaml,
+        ],
+    )
+
+    # Publish TF
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='both',
+        parameters=[robot_description],
+    )
+
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory('franka_moveit_config'),
+        'config',
+        'panda_ros_controllers.yaml',
+    )
+    ros2_control_node = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        parameters=[robot_description, ros2_controllers_path],
+        remappings=[('joint_states', 'franka/joint_states')],
+        output={
+            'stdout': 'screen',
+            'stderr': 'screen',
+        },
+        on_exit=Shutdown(),
+    )
+
+    # Load controllers
+    load_controllers = []
+    for controller in ['panda_arm_controller', 'joint_state_broadcaster']:
+        load_controllers += [
+            ExecuteProcess(
+                cmd=['ros2 run controller_manager spawner {}'.format(controller)],
+                shell=True,
+                output='screen',
+            )
+        ]
+
+    joint_state_publisher = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        parameters=[
+            {'source_list': ['franka/joint_states'], 'rate': 30}],
+    )
+
+    franka_robot_state_broadcaster = Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['franka_robot_state_broadcaster'],
+            output='screen',
+            condition=UnlessCondition(use_fake_hardware),
+    )
+
+    robot_arg = DeclareLaunchArgument(
+        robot_ip_parameter_name,
+        description='Hostname or IP address of the robot.')
+
+    use_fake_hardware_arg = DeclareLaunchArgument(
+        use_fake_hardware_parameter_name,
+        default_value='false',
+        description='Use fake hardware')
+    fake_sensor_commands_arg = DeclareLaunchArgument(
+        fake_sensor_commands_parameter_name,
+        default_value='false',
+        description="Fake sensor commands. Only valid when '{}' is true".format(
+            use_fake_hardware_parameter_name))
+    return LaunchDescription(
+        [robot_arg,
+         use_fake_hardware_arg,
+         fake_sensor_commands_arg,
+         db_arg,
+         rviz_node,
+         robot_state_publisher,
+         run_move_group_node,
+         ros2_control_node,
+         joint_state_publisher,
+         franka_robot_state_broadcaster
+         ]
+        + load_controllers
+    )

franka_moveit_config/package.xml

@@ -0,0 +1,33 @@
+<?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>franka_moveit_config</name>
+  <version>0.1.0</version>
+  <description>Contains Moveit2 configuration files for Franka Emika research robots</description>
+  <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>franka_description</exec_depend>
+  <exec_depend>franka_hardware</exec_depend>
+  <exec_depend>franka_gripper</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_kinematics</exec_depend>
+  <exec_depend>moveit_planners_ompl</exec_depend>
+  <exec_depend>moveit_ros_visualization</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

franka_moveit_config/rviz/moveit.rviz

@@ -0,0 +1,359 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 814
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 100
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /rviz_visual_tools
+      Value: true
+    - Class: moveit_rviz_plugin/Trajectory
+      Color Enabled: false
+      Enabled: true
+      Interrupt Display: false
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        panda_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Loop Animation: false
+      Name: Trajectory
+      Robot Alpha: 0.5
+      Robot Color: 150; 50; 150
+      Robot Description: robot_description
+      Show Robot Collision: false
+      Show Robot Visual: true
+      Show Trail: false
+      State Display Time: 0.05 s
+      Trail Step Size: 1
+      Trajectory Topic: /display_planned_path
+      Value: true
+    - Class: moveit_rviz_plugin/PlanningScene
+      Enabled: true
+      Move Group Namespace: ""
+      Name: PlanningScene
+      Planning Scene Topic: /monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+    - Acceleration_Scaling_Factor: 0.1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: false
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Warehouse_Host: 127.0.0.1
+      MoveIt_Warehouse_Port: 33829
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+        Loop Animation: false
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 0.05 s
+        Trail Step Size: 1
+        Trajectory Topic: /display_planned_path
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: panda_manipulator
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: /monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: false
+      Velocity_Scaling_Factor: 0.1
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: panda_link0
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 3.119211196899414
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.02386285550892353
+        Y: 0.15478567779064178
+        Z: 0.039489321410655975
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5953981876373291
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 5.958578109741211
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1043
+  Hide Left Dock: false
+  Hide Right Dock: true
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Trajectory - Trajectory Slider:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1848
+  X: 72
+  Y: 454

franka_moveit_config/srdf/group_definition.xacro

@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+  <xacro:macro name="group_definition" params="group_name arm_id tip_link">
+    <group name="${group_name}">
+      <chain base_link="${arm_id}_link0" tip_link="${tip_link}"/>
+    </group>
+
+    <group_state name="ready" group="${group_name}">
+      <joint name="${arm_id}_joint1" value="0"/>
+      <joint name="${arm_id}_joint2" value="${-pi/4}"/>
+      <joint name="${arm_id}_joint3" value="0"/>
+      <joint name="${arm_id}_joint4" value="${-3*pi/4}"/>
+      <joint name="${arm_id}_joint5" value="0"/>
+      <joint name="${arm_id}_joint6" value="${pi/2}"/>
+      <joint name="${arm_id}_joint7" value="${pi/4}"/>
+    </group_state>
+    <group_state name="extended" group="${group_name}">
+      <joint name="${arm_id}_joint1" value="0"/>
+      <joint name="${arm_id}_joint2" value="0"/>
+      <joint name="${arm_id}_joint3" value="0"/>
+      <joint name="${arm_id}_joint4" value="-0.1"/>
+      <joint name="${arm_id}_joint5" value="0"/>
+      <joint name="${arm_id}_joint6" value="${pi/2}"/>
+      <joint name="${arm_id}_joint7" value="${pi/4}"/>
+    </group_state>
+  </xacro:macro>
+</robot>

franka_moveit_config/srdf/hand.xacro

@@ -0,0 +1,16 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+  <xacro:macro name="hand" params="arm_id:='panda' ">
+
+    <group name="hand">
+      <link name="${arm_id}_hand"/>
+      <link name="${arm_id}_leftfinger"/>
+      <link name="${arm_id}_rightfinger"/>
+      <joint name="${arm_id}_finger_joint1"/>
+      <passive_joint name="${arm_id}_finger_joint2"/>
+    </group>
+    <disable_collisions link1="${arm_id}_hand" link2="${arm_id}_leftfinger" reason="Adjacent"/>
+    <disable_collisions link1="${arm_id}_hand" link2="${arm_id}_rightfinger" reason="Adjacent"/>
+    <disable_collisions link1="${arm_id}_leftfinger" link2="${arm_id}_rightfinger" reason="Default"/>
+  </xacro:macro>
+</robot>

franka_moveit_config/srdf/panda_arm.srdf.xacro

@@ -0,0 +1,19 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+
+  <xacro:arg name="arm_id" default="panda"/> <!-- Name of this panda -->
+  <xacro:arg name="hand" default="true"/> <!-- Should a franka_gripper be mounted at the flange? (Currently does not work without it) -->
+
+  <xacro:property name="arm_id" value="$(arg arm_id)"/>
+
+  <xacro:include filename="$(find franka_moveit_config)/srdf/panda_arm.xacro"/>
+  <xacro:panda_arm arm_id="${arm_id}"/>
+
+  <xacro:if value="$(arg hand)">
+    <xacro:include filename="$(find franka_moveit_config)/srdf/hand.xacro"/>
+    <xacro:hand arm_id="${arm_id}"/>
+
+    <xacro:include filename="$(find franka_moveit_config)/srdf/panda_arm_hand.xacro"/>
+    <xacro:panda_arm_hand arm_id="${arm_id}"/>
+  </xacro:if>
+</robot>

franka_moveit_config/srdf/panda_arm.xacro

@@ -0,0 +1,32 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+  <xacro:macro name="panda_arm" params="arm_id:='panda' ">
+
+    <xacro:include filename="$(find franka_moveit_config)/srdf/group_definition.xacro"/>
+    <xacro:group_definition arm_id="${arm_id}" group_name="${arm_id}_arm" tip_link="${arm_id}_link8"/>
+
+    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="${arm_id}_link0"/>
+    <disable_collisions link1="${arm_id}_link0" link2="${arm_id}_link1" reason="Adjacent"/>
+    <disable_collisions link1="${arm_id}_link0" link2="${arm_id}_link2" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link0" link2="${arm_id}_link3" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link0" link2="${arm_id}_link4" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link1" link2="${arm_id}_link2" reason="Adjacent"/>
+    <disable_collisions link1="${arm_id}_link1" link2="${arm_id}_link3" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link1" link2="${arm_id}_link4" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link2" link2="${arm_id}_link3" reason="Adjacent"/>
+    <disable_collisions link1="${arm_id}_link2" link2="${arm_id}_link4" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link2" link2="${arm_id}_link6" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link3" link2="${arm_id}_link4" reason="Adjacent"/>
+    <disable_collisions link1="${arm_id}_link3" link2="${arm_id}_link5" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link3" link2="${arm_id}_link6" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link3" link2="${arm_id}_link7" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link4" link2="${arm_id}_link5" reason="Adjacent"/>
+    <disable_collisions link1="${arm_id}_link4" link2="${arm_id}_link6" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link4" link2="${arm_id}_link7" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link4" link2="${arm_id}_link8" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link5" link2="${arm_id}_link6" reason="Adjacent"/>
+    <disable_collisions link1="${arm_id}_link6" link2="${arm_id}_link7" reason="Adjacent"/>
+    <disable_collisions link1="${arm_id}_link6" link2="${arm_id}_link8" reason="Default"/>
+    <disable_collisions link1="${arm_id}_link7" link2="${arm_id}_link8" reason="Adjacent"/>
+  </xacro:macro>
+</robot>

franka_moveit_config/srdf/panda_arm_hand.xacro

@@ -0,0 +1,31 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+  <xacro:macro name="panda_arm_hand" params="arm_id:='panda' ">
+
+    <group_state name="open" group="hand">
+      <joint name="${arm_id}_finger_joint1" value="0.035"/>
+      <joint name="${arm_id}_finger_joint2" value="0.035"/>
+    </group_state>
+    <group_state name="close" group="hand">
+      <joint name="${arm_id}_finger_joint1" value="0"/>
+      <joint name="${arm_id}_finger_joint2" value="0"/>
+    </group_state>
+    <end_effector name="hand_tcp" parent_link="${arm_id}_hand_tcp" group="hand" parent_group="${arm_id}_manipulator"/>
+    <end_effector name="hand" parent_link="${arm_id}_link8" group="hand" parent_group="${arm_id}_arm"/>
+    <disable_collisions link1="${arm_id}_hand" link2="${arm_id}_link3" reason="Never"/>
+    <disable_collisions link1="${arm_id}_hand" link2="${arm_id}_link4" reason="Never"/>
+    <disable_collisions link1="${arm_id}_hand" link2="${arm_id}_link6" reason="Never"/>
+    <disable_collisions link1="${arm_id}_hand" link2="${arm_id}_link7" reason="Adjacent"/>
+    <disable_collisions link1="${arm_id}_hand" link2="${arm_id}_link8" reason="Adjacent"/>
+    <disable_collisions link1="${arm_id}_leftfinger" link2="${arm_id}_link3" reason="Never"/>
+    <disable_collisions link1="${arm_id}_leftfinger" link2="${arm_id}_link4" reason="Never"/>
+    <disable_collisions link1="${arm_id}_leftfinger" link2="${arm_id}_link6" reason="Never"/>
+    <disable_collisions link1="${arm_id}_leftfinger" link2="${arm_id}_link7" reason="Never"/>
+    <disable_collisions link1="${arm_id}_leftfinger" link2="${arm_id}_link8" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link3" link2="${arm_id}_rightfinger" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link4" link2="${arm_id}_rightfinger" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link6" link2="${arm_id}_rightfinger" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link7" link2="${arm_id}_rightfinger" reason="Never"/>
+    <disable_collisions link1="${arm_id}_link8" link2="${arm_id}_rightfinger" reason="Never"/>
+  </xacro:macro>
+</robot>

franka_moveit_config/test/srdf_tests.py

@@ -0,0 +1,42 @@
+#  Copyright (c) 2021 Franka Emika GmbH
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+from os import path
+
+from ament_index_python.packages import get_package_share_directory
+import xacro
+
+panda_xacro_file_name = path.join(get_package_share_directory('franka_moveit_config'), 'srdf',
+                                  'panda_arm.srdf.xacro')
+
+
+def test_load():
+    urdf = xacro.process_file(panda_xacro_file_name).toxml()
+    assert urdf.find('panda_rightfinger') != -1
+
+
+def test_load_without_gripper():
+    urdf = xacro.process_file(panda_xacro_file_name,
+                              mappings={'hand': 'false'}).toxml()
+    assert urdf.find('panda_rightfinger') == -1
+
+
+def test_load_with_arm_id():
+    urdf = xacro.process_file(panda_xacro_file_name,
+                              mappings={'arm_id': 'totally_different_arm'}).toxml()
+    assert urdf.find('totally_different_arm_joint1') != -1
+
+
+if __name__ == '__main__':
+    pass

franka_msgs/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.5)
+project(franka_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "action/Grasp.action"
+  "action/Homing.action"
+  "action/Move.action"
+  "msg/Errors.msg"
+  "msg/GraspEpsilon.msg"
+  "msg/FrankaRobotState.msg"
+  DEPENDENCIES std_msgs builtin_interfaces
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

franka_msgs/action/Grasp.action

@@ -0,0 +1,9 @@
+float64 width # [m]
+GraspEpsilon epsilon
+float64 speed # [m/s]
+float64 force # [N]
+---
+bool success
+string error
+---
+float64 current_width # [m]

franka_msgs/action/Homing.action

@@ -0,0 +1,5 @@
+---
+bool success
+string error
+---
+float64 current_width # [m]

franka_msgs/action/Move.action

@@ -0,0 +1,7 @@
+float64 width  # [m]
+float64 speed  # [m/s]
+---
+bool success
+string error
+---
+float64 current_width # [m]

franka_msgs/msg/Errors.msg

@@ -0,0 +1,36 @@
+bool joint_position_limits_violation
+bool cartesian_position_limits_violation
+bool self_collision_avoidance_violation
+bool joint_velocity_violation
+bool cartesian_velocity_violation
+bool force_control_safety_violation
+bool joint_reflex
+bool cartesian_reflex
+bool max_goal_pose_deviation_violation
+bool max_path_pose_deviation_violation
+bool cartesian_velocity_profile_safety_violation
+bool joint_position_motion_generator_start_pose_invalid
+bool joint_motion_generator_position_limits_violation
+bool joint_motion_generator_velocity_limits_violation
+bool joint_motion_generator_velocity_discontinuity
+bool joint_motion_generator_acceleration_discontinuity
+bool cartesian_position_motion_generator_start_pose_invalid
+bool cartesian_motion_generator_elbow_limit_violation
+bool cartesian_motion_generator_velocity_limits_violation
+bool cartesian_motion_generator_velocity_discontinuity
+bool cartesian_motion_generator_acceleration_discontinuity
+bool cartesian_motion_generator_elbow_sign_inconsistent
+bool cartesian_motion_generator_start_elbow_invalid
+bool cartesian_motion_generator_joint_position_limits_violation
+bool cartesian_motion_generator_joint_velocity_limits_violation
+bool cartesian_motion_generator_joint_velocity_discontinuity
+bool cartesian_motion_generator_joint_acceleration_discontinuity
+bool cartesian_position_motion_generator_invalid_frame
+bool force_controller_desired_force_tolerance_violation
+bool controller_torque_discontinuity
+bool start_elbow_sign_inconsistent
+bool communication_constraints_violation
+bool power_limit_violation
+bool joint_p2p_insufficient_torque_for_planning
+bool tau_j_range_violation
+bool instability_detected

franka_msgs/msg/FrankaRobotState.msg

@@ -0,0 +1,52 @@
+std_msgs/Header header
+float64[6] cartesian_collision
+float64[6] cartesian_contact
+float64[7] q
+float64[7] q_d
+float64[7] dq
+float64[7] dq_d
+float64[7] ddq_d
+float64[7] theta
+float64[7] dtheta
+float64[7] tau_j
+float64[7] dtau_j
+float64[7] tau_j_d
+float64[6] k_f_ext_hat_k
+float64[2] elbow
+float64[2] elbow_d
+float64[2] elbow_c
+float64[2] delbow_c
+float64[2] ddelbow_c
+float64[7] joint_collision
+float64[7] joint_contact
+float64[6] o_f_ext_hat_k
+float64[6] o_dp_ee_d
+float64[6] o_dp_ee_c
+float64[6] o_ddp_ee_c
+float64[7] tau_ext_hat_filtered
+float64 m_ee
+float64[3] f_x_cee
+float64[9] i_ee
+float64 m_load
+float64[3] f_x_cload
+float64[9] i_load
+float64 m_total
+float64[3] f_x_ctotal
+float64[9] i_total
+float64[16] o_t_ee
+float64[16] o_t_ee_d
+float64[16] o_t_ee_c
+float64[16] f_t_ee
+float64[16] ee_t_k
+float64 time
+float64 control_command_success_rate
+uint8 ROBOT_MODE_OTHER=0
+uint8 ROBOT_MODE_IDLE=1
+uint8 ROBOT_MODE_MOVE=2
+uint8 ROBOT_MODE_GUIDING=3
+uint8 ROBOT_MODE_REFLEX=4
+uint8 ROBOT_MODE_USER_STOPPED=5
+uint8 ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY=6
+uint8 robot_mode
+Errors current_errors
+Errors last_motion_errors