Initial commit

.ci/check_format.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+set -ex
+
+cd /workspace/build/gen3_compliant_controllers/
+#Check whether files match format
+make check-format

.ci/generate_build.sh

@@ -0,0 +1,39 @@
+#!/bin/bash
+set -ex
+
+# setup ros environment in Docker Container
+source "/opt/ros/$ROS_DISTRO/setup.bash" --
+cd /
+# create workspace directory where package will be built
+mkdir workspace
+cd workspace
+cp -r ${GITHUB_WORKSPACE}/. src/
+# Update and install packages
+apt-get update
+apt-get -y install python3-pip
+# Install catkin build
+pip3 install -U catkin_tools
+# Install gen3_compliant_controllers package deps
+apt-get install python3-rosdep
+rosdep init
+rosdep update
+rosdep install --from-paths src --ignore-src -y
+# Install format checker
+apt-get install clang-format-10 -y
+apt-get update
+
+# Install pinocchio
+apt install -qqy lsb-release gnupg2 curl
+echo "deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" | sudo tee /etc/apt/sources.list.d/robotpkg.list
+curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
+apt-get update
+apt install -qqy robotpkg-py3*-pinocchio
+
+export PATH=/opt/openrobots/bin:$PATH
+export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH
+export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH
+export PYTHONPATH=/opt/openrobots/lib/python3.10/site-packages:$PYTHONPATH # Adapt your desired python version here
+export CMAKE_PREFIX_PATH=/opt/openrobots:$CMAKE_PREFIX_PATH
+
+#build
+catkin build 

.clang-format

@@ -0,0 +1,90 @@
+---
+BasedOnStyle:  Google
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+AlignEscapedNewlinesLeft: false
+AlignOperands:   true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: None
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakAfterDefinitionReturnType: false
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: true
+BinPackArguments: false
+BinPackParameters: false
+BraceWrapping:
+  AfterClass:      true
+  AfterControlStatement: true
+  AfterEnum:       true
+  AfterFunction:   true
+  AfterNamespace:  false
+  AfterObjCDeclaration: true
+  AfterStruct:     true
+  AfterUnion:      true
+  BeforeCatch:     true
+  BeforeElse:      true
+  IndentBraces:    false
+BreakBeforeBinaryOperators: All
+BreakBeforeBraces: Custom
+BreakBeforeInheritanceComma: true
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: true
+ColumnLimit:     80
+CommentPragmas:  ''
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 2
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: false
+DisableFormat:   false
+ExperimentalAutoDetectBinPacking: false
+ForEachMacros:   [ foreach, Q_FOREACH, BOOST_FOREACH ]
+# sort headers by main include file (implicit priority 0),
+# then project and private includes, then system headers
+IncludeBlocks: Regroup
+IncludeCategories:
+  - Regex:     '^<([a-z|_]+)>' # standard library headers
+    Priority:  1
+  - Regex:     '^(<|")aikido/' # Aikido headers
+    Priority:  3
+  - Regex:     '^(<[A-z]+)'    # dependency headers
+    Priority:  2
+  - Regex:     '^".*'          # headers relative to this project
+    Priority:  4
+IndentCaseLabels: true
+IndentFunctionDeclarationAfterType: false
+IndentWidth:     2
+IndentWrappedFunctionNames: false
+KeepEmptyLinesAtTheStartOfBlocks: true
+MacroBlockBegin: ''
+MacroBlockEnd: ''
+Language:        Cpp
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+SpaceAfterCStyleCast: false
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: ControlStatements
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 1
+SpacesInAngles: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard:        Auto
+TabWidth:        2
+UseTab:          Never
+...

.github/workflows/build-test.yml

@@ -0,0 +1,23 @@
+on: 
+  push:
+    branches:
+      - main
+  pull_request:
+    branches:
+      - main
+
+jobs:
+  build:
+    runs-on: ubuntu-20.04
+    container: 
+      image: ros:noetic-ros-core-focal
+    name: rosbuild
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v3
+
+      - name: gen_build
+        run: ./.ci/generate_build.sh
+
+      - name: check format
+        run: ./.ci/check_format.sh

.gitignore

@@ -0,0 +1,2 @@
+doc/
+build/

CMakeLists.txt

@@ -0,0 +1,109 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(gen3_compliant_controllers)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+
+include_directories("include")
+
+#==============================================================================
+# Dependencies
+#
+find_package(catkin REQUIRED COMPONENTS
+  control_msgs
+  control_toolbox
+  controller_interface
+  hardware_interface
+  eigen_conversions
+  realtime_tools
+  eigen_conversions
+  dynamic_reconfigure
+)
+find_package(Threads REQUIRED)
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(PINOCCHIO pinocchio REQUIRED)
+
+include_directories(SYSTEM ${catkin_INCLUDE_DIRS})
+
+if ("${controller_interface_VERSION}" VERSION_LESS 0.10.0)
+  message(FATAL_ERROR "controller_interface package must be at least version 0.10.0, not packaged in Indigo. Please upgrade to Jade or Kinetic or build the ros_control packages from source.")
+endif()
+
+include_directories(
+  include/
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+  ${Boost_INCLUDE_DIRS}
+  ${PINOCCHIO_INCLUDE_DIRS}
+)
+
+generate_dynamic_reconfigure_options(
+  config/JointSpaceParams.cfg
+  config/TaskSpaceParams.cfg
+)
+
+#==============================================================================
+# Catkin Configuration
+#
+catkin_package(
+  CATKIN_DEPENDS
+    control_msgs
+    control_toolbox
+    controller_interface
+    eigen_conversions
+    hardware_interface
+    realtime_tools
+    dynamic_reconfigure
+    DEPENDS PINOCCHIO
+)
+
+link_libraries(
+  # ${catkin_LIBRARIES}
+  Threads::Threads
+  )
+#==============================================================================
+# Targets
+#
+add_library("${PROJECT_NAME}" SHARED
+  src/helpers.cpp
+  src/JointSpaceCompliantController.cpp
+  src/TaskSpaceCompliantController.cpp
+)
+target_link_libraries("${PROJECT_NAME}"
+  ${catkin_LIBRARIES}
+)
+
+# make sure configure headers are built before any node using them
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+
+target_compile_definitions(${PROJECT_NAME} PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
+#==============================================================================
+# Format
+#
+
+include(ClangFormat.cmake)
+file(GLOB_RECURSE FILES
+    ${PROJECT_SOURCE_DIR}/src/*.[ch]pp
+)
+set(FILES ${FILES} ${FILES_INCLUDE})
+
+
+clang_format_setup(VERSION 10)
+
+if (CLANG_FORMAT_EXECUTABLE)
+  clang_format_add_sources(${FILES})
+
+  clang_format_add_targets()
+endif()
+#==============================================================================
+# Installation
+#
+install(DIRECTORY "include"
+  DESTINATION "${CATKIN_PACKAGE_INCLUDE_DESTINATION}"
+)
+install(TARGETS "${PROJECT_NAME}"
+  ARCHIVE DESTINATION "${CATKIN_PACKAGE_LIB_DESTINATION}"
+  LIBRARY DESTINATION "${CATKIN_PACKAGE_LIB_DESTINATION}"
+  RUNTIME DESTINATION "${CATKIN_PACKAGE_BIN_DESTINATION}"
+)
+install(FILES "gen3_compliant_controllers_plugin.xml"
+  DESTINATION "${CATKIN_PACKAGE_SHARE_DESTINATION}"
+)

ClangFormat.cmake

@@ -0,0 +1,90 @@
+function(clang_format_setup)
+  cmake_parse_arguments(
+        CF_ARG # prefix 
+        "" # no boolean args
+        "VERSION" # clang-format version
+        "" # no multi-value args
+        ${ARGN} 
+    )
+  if(NOT CF_ARG_VERSION)
+    set(CF_NAME clang-format)
+  else()
+    set(CF_NAME clang-format-${CF_ARG_VERSION})
+  endif()
+  find_program(CLANG_FORMAT_EXECUTABLE NAMES ${CF_NAME})
+
+  if(NOT CLANG_FORMAT_EXECUTABLE)
+    message(STATUS "Looking for clang-format - NOT found, please install "
+      "${CF_NAME} to enable automatic code formatting."
+    )
+    return()
+  endif()
+
+  message(STATUS "Found ${CF_NAME}.")
+endfunction()
+
+#===============================================================================
+function(_property_add property_name)
+  get_property(is_defined GLOBAL PROPERTY ${property_name} DEFINED)
+  if(NOT is_defined)
+    define_property(GLOBAL PROPERTY ${property_name}
+      BRIEF_DOCS "${property_name}"
+      FULL_DOCS "Global properties for ${property_name}"
+    )
+  endif()
+  foreach(item ${ARGN})
+    set_property(GLOBAL APPEND PROPERTY ${property_name} "${item}")
+  endforeach()
+endfunction()
+
+#===============================================================================
+function(clang_format_add_sources)
+  foreach(source ${ARGN})
+    if(IS_ABSOLUTE "${source}")
+      set(source_abs "${source}")
+    else()
+      get_filename_component(source_abs
+        "${CMAKE_CURRENT_LIST_DIR}/${source}" ABSOLUTE)
+    endif()
+    if(EXISTS "${source_abs}")
+      _property_add(CLANG_FORMAT_FORMAT_FILES "${source_abs}")
+    else()
+      message(FATAL_ERROR
+        "Source file '${source}' does not exist at absolute path"
+        " '${source_abs}'. This should never happen. Did you recently delete"
+        " this file or modify 'CMAKE_CURRENT_LIST_DIR'")
+    endif()
+  endforeach()
+endfunction()
+
+#===============================================================================
+function(clang_format_add_targets)
+  get_property(formatting_files GLOBAL PROPERTY CLANG_FORMAT_FORMAT_FILES)
+  list(LENGTH formatting_files formatting_files_length)
+
+  if(formatting_files)
+    message(STATUS "Formatting on ${formatting_files_length} source files.")
+
+    add_custom_target(format
+      COMMAND ${CMAKE_COMMAND} -E echo "Formatting ${formatting_files_length} files..."
+      COMMAND ${CLANG_FORMAT_EXECUTABLE} -style=file -i ${formatting_files}
+      COMMAND ${CMAKE_COMMAND} -E echo "Done."
+      DEPENDS ${CLANG_FORMAT_EXECUTABLE}
+      WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+    )
+    add_custom_target(check-format
+      COMMAND ${CMAKE_COMMAND} -E echo "Checking ${formatting_files_length} files..."
+      COMMAND ${CLANG_FORMAT_EXECUTABLE} -style=file --dry-run --Werror ${formatting_files} >/dev/null
+      COMMAND ${CMAKE_COMMAND} -E echo "Done."
+      DEPENDS ${CLANG_FORMAT_EXECUTABLE}
+      WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+    )
+  else()
+    add_custom_target(format
+        COMMAND ${CMAKE_COMMAND} -E echo "No file to format code style."
+    )
+    add_custom_target(check-format
+        COMMAND ${CMAKE_COMMAND} -E echo "No file to check code style."
+    )
+  endif()
+endfunction()

LICENSE

@@ -0,0 +1,28 @@
+BSD 3-Clause License
+
+Copyright (c) 2023, EmPRISE Lab
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.