├── .gitignore ├── 2_RIDE ├── services │ ├── msg │ │ ├── SocketInfo.ev │ │ └── GetSocketPose.op │ ├── CMakeLists.txt │ └── src │ │ └── socket_service.cpp ├── statemachines │ ├── bundles │ │ └── ride_tutorial │ │ │ ├── manifest.json │ │ │ ├── resources │ │ │ ├── plug-inserted.svg │ │ │ ├── logo.svg │ │ │ ├── input.svg │ │ │ ├── plug-backup.svg │ │ │ └── plug.svg │ │ │ └── sources │ │ │ ├── convert_safety.lf │ │ │ ├── plug_in_vision.lf │ │ │ ├── plug_in.lf │ │ │ ├── plug_in_app.lf │ │ │ └── plug_in_wrapper.lf │ ├── cleanup.sh │ ├── CMakeLists.txt │ └── Makefile ├── CMakeLists.txt ├── fig │ └── plug_in_vision_structure_bundles.png └── README.md ├── 3_FCI ├── ride_integration │ ├── statemachines │ │ ├── bundles │ │ │ └── fci_tutorial │ │ │ │ ├── manifest.json │ │ │ │ ├── resources │ │ │ │ ├── plug-inserted.svg │ │ │ │ ├── logo.svg │ │ │ │ ├── input.svg │ │ │ │ ├── plug-backup.svg │ │ │ │ └── plug.svg │ │ │ │ └── sources │ │ │ │ └── plug_in_fci.lf │ │ ├── cleanup.sh │ │ ├── CMakeLists.txt │ │ └── Makefile │ └── services │ │ ├── msg │ │ └── PlugIn.op │ │ ├── CMakeLists.txt │ │ └── src │ │ └── fci_service.cpp ├── fig │ └── fci_service_structure_bundles.png ├── CMakeLists.txt ├── standalone │ ├── CMakeLists.txt │ ├── src │ │ ├── plug_in_controller_standalone.cpp │ │ └── plug_in_controller.cpp │ └── include │ │ └── plug_in_controller.h └── README.md ├── 4_Learning ├── statemachines │ ├── bundles │ │ ├── fci_learning_tutorial │ │ │ ├── manifest.json │ │ │ └── resources │ │ │ │ ├── plug-inserted.svg │ │ │ │ ├── logo.svg │ │ │ │ ├── input.svg │ │ │ │ ├── plug-backup.svg │ │ │ │ └── plug.svg │ │ └── ride_learning_tutorial │ │ │ ├── manifest.json │ │ │ ├── resources │ │ │ ├── plug-inserted.svg │ │ │ ├── logo.svg │ │ │ ├── input.svg │ │ │ ├── plug-backup.svg │ │ │ └── plug.svg │ │ │ └── sources │ │ │ └── plug_in_learning_ride.lf │ ├── cleanup.sh │ ├── CMakeLists.txt │ └── Makefile ├── services │ ├── msg │ │ ├── GetParams.op │ │ ├── SetResult.op │ │ └── Init.op │ ├── CMakeLists.txt │ ├── src │ │ ├── random_search.cpp │ │ └── learning_service.cpp │ └── include │ │ └── random_search.h ├── CMakeLists.txt ├── fig │ ├── fci_learning_structure_bundles.png │ └── ride_learning_structure_bundles.png └── README.md ├── fig ├── overview_system_architecture.png └── panda_programming_interfaces_gs.png ├── cmake ├── FindEigen3.cmake └── ClangTools.cmake ├── CMakeLists.txt ├── 1_Desk └── README.md ├── README.md └── LICENSE /.gitignore: -------------------------------------------------------------------------------- 1 | build/ -------------------------------------------------------------------------------- /2_RIDE/services/msg/SocketInfo.ev: -------------------------------------------------------------------------------- 1 | {int number_of_sockets;}; 2 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/bundles/ride_tutorial/manifest.json: -------------------------------------------------------------------------------- 1 | {"version":"1.0"} 2 | -------------------------------------------------------------------------------- /3_FCI/ride_integration/statemachines/bundles/fci_tutorial/manifest.json: -------------------------------------------------------------------------------- 1 | {"version":"1.0"} 2 | -------------------------------------------------------------------------------- /4_Learning/statemachines/bundles/fci_learning_tutorial/manifest.json: -------------------------------------------------------------------------------- 1 | {"version":"1.0"} 2 | -------------------------------------------------------------------------------- /4_Learning/statemachines/bundles/ride_learning_tutorial/manifest.json: -------------------------------------------------------------------------------- 1 | {"version":"1.0"} 2 | -------------------------------------------------------------------------------- /4_Learning/services/msg/GetParams.op: -------------------------------------------------------------------------------- 1 | {} request; 2 | { 3 | []float parameters; 4 | } result; 5 | string error; 6 | -------------------------------------------------------------------------------- /2_RIDE/services/msg/GetSocketPose.op: -------------------------------------------------------------------------------- 1 | {} request; 2 | { 3 | [16]float socket_pose; 4 | } result; 5 | string error; 6 | -------------------------------------------------------------------------------- /4_Learning/services/msg/SetResult.op: -------------------------------------------------------------------------------- 1 | { 2 | float cost; 3 | bool success; 4 | } request; 5 | bool result; 6 | string error; 7 | -------------------------------------------------------------------------------- /fig/overview_system_architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/the_reference_platform/HEAD/fig/overview_system_architecture.png -------------------------------------------------------------------------------- /fig/panda_programming_interfaces_gs.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/the_reference_platform/HEAD/fig/panda_programming_interfaces_gs.png -------------------------------------------------------------------------------- /3_FCI/fig/fci_service_structure_bundles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/the_reference_platform/HEAD/3_FCI/fig/fci_service_structure_bundles.png -------------------------------------------------------------------------------- /2_RIDE/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(ride_tutorial) 4 | 5 | add_subdirectory(services) 6 | add_subdirectory(statemachines) 7 | -------------------------------------------------------------------------------- /2_RIDE/fig/plug_in_vision_structure_bundles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/the_reference_platform/HEAD/2_RIDE/fig/plug_in_vision_structure_bundles.png -------------------------------------------------------------------------------- /4_Learning/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(learning_tutorial) 4 | 5 | add_subdirectory(services) 6 | add_subdirectory(statemachines) 7 | -------------------------------------------------------------------------------- /4_Learning/fig/fci_learning_structure_bundles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/the_reference_platform/HEAD/4_Learning/fig/fci_learning_structure_bundles.png -------------------------------------------------------------------------------- /4_Learning/fig/ride_learning_structure_bundles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/the_reference_platform/HEAD/4_Learning/fig/ride_learning_structure_bundles.png -------------------------------------------------------------------------------- /4_Learning/services/msg/Init.op: -------------------------------------------------------------------------------- 1 | { 2 | int max_iteration; 3 | int param_count; 4 | []float lower_bounds; 5 | []float upper_bounds; 6 | } request; 7 | {} result; 8 | string error; 9 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/cleanup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | BUNDLE_NAME="ride_tutorial" 4 | 5 | GREP_RESULT=$(ride bundle list | grep -i $BUNDLE_NAME) 6 | if [ ! -z "$GREP_RESULT" ] 7 | then 8 | echo "'$BUNDLE_NAME' already installed." 9 | echo "Removing the bundle before installing the new one..." 10 | ride bundle remove $BUNDLE_NAME 11 | fi 12 | -------------------------------------------------------------------------------- /3_FCI/ride_integration/statemachines/cleanup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | BUNDLE_NAME="fci_tutorial" 4 | 5 | GREP_RESULT=$(ride bundle list | grep -i $BUNDLE_NAME) 6 | if [ ! -z "$GREP_RESULT" ] 7 | then 8 | echo "'$BUNDLE_NAME' already installed." 9 | echo "Removing the bundle before installing the new one..." 10 | ride bundle remove $BUNDLE_NAME 11 | fi 12 | -------------------------------------------------------------------------------- /4_Learning/statemachines/cleanup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | BUNDLES="ride_learning_tutorial fci_learning_tutorial" 4 | 5 | for BUNDLE in $BUNDLES; do 6 | GREP_RESULT=$(ride bundle list | grep -i $BUNDLE) 7 | if [ ! -z "$GREP_RESULT" ] 8 | then 9 | echo "'$BUNDLE' already installed." 10 | echo "Removing the bundle before installing the new one..." 11 | ride bundle remove $BUNDLE 12 | fi 13 | done 14 | -------------------------------------------------------------------------------- /3_FCI/ride_integration/services/msg/PlugIn.op: -------------------------------------------------------------------------------- 1 | { 2 | { 3 | float translational_stiffness; 4 | float rotational_stiffness; 5 | float desired_force; 6 | float wiggle_frequency_x; 7 | float wiggle_frequency_y; 8 | float wiggle_amplitude_x; 9 | float wiggle_amplitude_y; 10 | } plug_in_params; 11 | float duration; 12 | []float target_position; 13 | []float tolerance; 14 | } request; 15 | {} result; 16 | string error; 17 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(ride_tutorial_statemachines) 4 | 5 | add_custom_target( 6 | remove_ride_tutorial_statemachines 7 | WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/ 8 | COMMAND ${CMAKE_CURRENT_LIST_DIR}/cleanup.sh 9 | ) 10 | 11 | add_custom_target( 12 | ride_tutorial_statemachines 13 | WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/ 14 | COMMAND ${CMAKE_CURRENT_LIST_DIR}/cleanup.sh 15 | COMMAND make 16 | ) 17 | -------------------------------------------------------------------------------- /3_FCI/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(fci_tutorial) 4 | 5 | option(BUILD_FCI_STANDALONE "Build fci standalone tutorial" ON) 6 | option(BUILD_FCI_INTEGRATION "Build fci integration tutorial" ON) 7 | 8 | if(BUILD_FCI_STANDALONE OR BUILD_FCI_INTEGRATION) 9 | add_subdirectory(standalone) 10 | endif() 11 | 12 | if(BUILD_FCI_INTEGRATION) 13 | add_subdirectory(ride_integration/services) 14 | add_subdirectory(ride_integration/statemachines) 15 | endif() 16 | -------------------------------------------------------------------------------- /3_FCI/ride_integration/statemachines/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(fci_tutorial_statemachines) 4 | 5 | add_custom_target( 6 | remove_fci_tutorial_statemachines 7 | WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/ 8 | COMMAND ${CMAKE_CURRENT_LIST_DIR}/cleanup.sh 9 | ) 10 | 11 | add_custom_target( 12 | fci_tutorial_statemachines 13 | WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/ 14 | COMMAND ${CMAKE_CURRENT_LIST_DIR}/cleanup.sh 15 | COMMAND make 16 | ) 17 | -------------------------------------------------------------------------------- /4_Learning/statemachines/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(learning_tutorial_statemachines) 4 | 5 | add_custom_target( 6 | remove_learning_tutorial_statemachines 7 | WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/ 8 | COMMAND ${CMAKE_CURRENT_LIST_DIR}/cleanup.sh 9 | ) 10 | 11 | add_custom_target( 12 | learning_tutorial_statemachines 13 | WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/ 14 | COMMAND ${CMAKE_CURRENT_LIST_DIR}/cleanup.sh 15 | COMMAND make 16 | ) 17 | -------------------------------------------------------------------------------- /cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | find_package(Eigen3 CONFIG) 2 | mark_as_advanced(FORCE Eigen3_DIR) 3 | 4 | include(FindPackageHandleStandardArgs) 5 | find_package_handle_standard_args(Eigen3 6 | FOUND_VAR Eigen3_FOUND 7 | REQUIRED_VARS EIGEN3_INCLUDE_DIRS 8 | ) 9 | 10 | if(NOT TARGET Eigen3::Eigen3) 11 | add_library(Eigen3::Eigen3 INTERFACE IMPORTED) 12 | set_target_properties(Eigen3::Eigen3 PROPERTIES 13 | INTERFACE_INCLUDE_DIRECTORIES ${EIGEN3_INCLUDE_DIRS} 14 | INTERFACE_COMPILE_DEFINITIONS "${EIGEN3_DEFINITIONS}" 15 | ) 16 | endif() 17 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(tutorial) 4 | 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 8 | 9 | option(BUILD_RIDE "Build RIDE tutorial" ON) 10 | option(BUILD_FCI "Build FCI tutorial" ON) 11 | option(BUILD_LEARNING "Build LEARNING tutorial" ON) 12 | 13 | if(BUILD_RIDE) 14 | add_subdirectory(2_RIDE) 15 | endif() 16 | 17 | if(BUILD_FCI) 18 | add_subdirectory(3_FCI) 19 | endif() 20 | 21 | if(BUILD_LEARNING) 22 | add_subdirectory(4_Learning) 23 | endif() 24 | -------------------------------------------------------------------------------- /3_FCI/ride_integration/services/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(fci_tutorial_services) 4 | 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 8 | 9 | find_package(RaceCom) 10 | 11 | racecom_message_library( 12 | TARGET fci_messages 13 | DIRECTORY msg 14 | STATIC) 15 | 16 | add_executable(fci_service src/fci_service.cpp) 17 | target_include_directories( 18 | fci_service 19 | PRIVATE 20 | ${RACECOM_INCLUDE_DIR} 21 | ) 22 | target_link_libraries( 23 | fci_service 24 | PRIVATE 25 | ${RACECOM_LIBRARY} 26 | plug_in_controller 27 | fci_messages 28 | ) 29 | 30 | # Tools 31 | include(${CMAKE_CURRENT_LIST_DIR}/../../../cmake/ClangTools.cmake OPTIONAL RESULT_VARIABLE CLANG_TOOLS) 32 | if(CLANG_TOOLS) 33 | file( 34 | GLOB_RECURSE 35 | SOURCES 36 | ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp 37 | ) 38 | file( 39 | GLOB_RECURSE 40 | HEADERS 41 | ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h 42 | ${CMAKE_CURRENT_SOURCE_DIR}/include/*/*.h 43 | ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h 44 | ) 45 | add_format_target(${PROJECT_NAME} FILES ${SOURCES} ${HEADERS}) 46 | add_tidy_target(${PROJECT_NAME} FILES ${SOURCES}) 47 | endif() 48 | -------------------------------------------------------------------------------- /2_RIDE/services/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(ride_tutorial_services) 4 | 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 8 | 9 | find_package(RaceCom) 10 | 11 | racecom_message_library( 12 | TARGET ride_tutorial_messages 13 | DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/msg 14 | STATIC) 15 | 16 | add_executable(socket_service ${CMAKE_CURRENT_LIST_DIR}/src/socket_service.cpp) 17 | target_include_directories( 18 | socket_service 19 | PRIVATE 20 | ${RACECOM_INCLUDE_DIR} 21 | ) 22 | target_link_libraries( 23 | socket_service 24 | PRIVATE 25 | ${RACECOM_LIBRARY} 26 | ride_tutorial_messages 27 | ) 28 | 29 | # Tools 30 | include(${CMAKE_CURRENT_LIST_DIR}/../../cmake/ClangTools.cmake OPTIONAL RESULT_VARIABLE CLANG_TOOLS) 31 | if(CLANG_TOOLS) 32 | file( 33 | GLOB_RECURSE 34 | SOURCES 35 | ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp 36 | ) 37 | file( 38 | GLOB_RECURSE 39 | HEADERS 40 | ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h 41 | ${CMAKE_CURRENT_SOURCE_DIR}/include/*/*.h 42 | ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h 43 | ) 44 | add_format_target(${PROJECT_NAME} FILES ${SOURCES} ${HEADERS}) 45 | add_tidy_target(${PROJECT_NAME} FILES ${SOURCES}) 46 | endif() 47 | -------------------------------------------------------------------------------- /4_Learning/services/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(learning_tutorial_services) 4 | 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 8 | 9 | find_package(RaceCom) 10 | 11 | racecom_message_library( 12 | TARGET learning_messages 13 | DIRECTORY msg 14 | STATIC) 15 | 16 | add_library(optimizer) 17 | target_include_directories( 18 | optimizer 19 | PUBLIC 20 | include/ 21 | ) 22 | target_sources( 23 | optimizer 24 | PRIVATE 25 | src/random_search.cpp 26 | ) 27 | 28 | add_executable(learning_service src/learning_service.cpp) 29 | target_include_directories( 30 | learning_service 31 | PRIVATE 32 | ${RACECOM_INCLUDE_DIR} 33 | ) 34 | target_link_libraries( 35 | learning_service 36 | PRIVATE 37 | ${RACECOM_LIBRARY} 38 | optimizer 39 | learning_messages 40 | ) 41 | 42 | # Tools 43 | include(${CMAKE_CURRENT_LIST_DIR}/../../cmake/ClangTools.cmake OPTIONAL RESULT_VARIABLE CLANG_TOOLS) 44 | if(CLANG_TOOLS) 45 | file( 46 | GLOB_RECURSE 47 | SOURCES 48 | ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp 49 | ) 50 | file( 51 | GLOB_RECURSE 52 | HEADERS 53 | ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h 54 | ${CMAKE_CURRENT_SOURCE_DIR}/include/*/*.h 55 | ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h 56 | ) 57 | add_format_target(${PROJECT_NAME} FILES ${SOURCES} ${HEADERS}) 58 | add_tidy_target(${PROJECT_NAME} FILES ${SOURCES}) 59 | endif() 60 | -------------------------------------------------------------------------------- /3_FCI/standalone/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(fci_standalone_tutorial) 4 | 5 | list(INSERT CMAKE_MODULE_PATH 0 ${CMAKE_CURRENT_LIST_DIR}/../../cmake) 6 | 7 | set(CMAKE_CXX_STANDARD 14) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 10 | 11 | find_package(Eigen3 REQUIRED) 12 | find_package(Franka) 13 | 14 | add_library(plug_in_controller) 15 | target_include_directories( 16 | plug_in_controller 17 | PUBLIC 18 | include 19 | ${Franka_INCLUDE_DIRS} 20 | ) 21 | target_link_libraries( 22 | plug_in_controller 23 | PRIVATE 24 | Eigen3::Eigen3 25 | ${Franka_LIBRARIES} 26 | ) 27 | target_sources(plug_in_controller PRIVATE src/plug_in_controller) 28 | 29 | add_executable(plug_in_controller_standalone src/plug_in_controller_standalone.cpp) 30 | target_link_libraries(plug_in_controller_standalone PRIVATE plug_in_controller) 31 | 32 | # Tools 33 | include(${CMAKE_CURRENT_LIST_DIR}/../../cmake/ClangTools.cmake OPTIONAL RESULT_VARIABLE CLANG_TOOLS) 34 | if(CLANG_TOOLS) 35 | file( 36 | GLOB_RECURSE 37 | SOURCES 38 | ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp 39 | ) 40 | file( 41 | GLOB_RECURSE 42 | HEADERS 43 | ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h 44 | ${CMAKE_CURRENT_SOURCE_DIR}/include/*/*.h 45 | ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h 46 | ) 47 | add_format_target(${PROJECT_NAME} FILES ${SOURCES} ${HEADERS}) 48 | add_tidy_target(${PROJECT_NAME} FILES ${SOURCES}) 49 | endif() 50 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/Makefile: -------------------------------------------------------------------------------- 1 | # Configurable variables 2 | SOURCE_DIR ?= bundles 3 | BUILDDIR ?= build 4 | DISTDIR ?= $(BUILDDIR)/dist 5 | 6 | # File handling variables 7 | BUNDLE_LF=$(foreach d,$(SOURCE_DIR), $(wildcard $(d)/*/sources/*.lf)) 8 | PARSED_STATEMACHINES=$(foreach f,$(BUNDLE_LF), $(BUILDDIR)/$(f:.lf=.json)) 9 | 10 | all: upload-bundles 11 | 12 | BUNDLES=$(shell find $(SOURCE_DIR) -type f -name 'manifest.json' | sed -r 's|/[^/]+$$||' | sed -r 's|^$(SOURCE_DIR)/||' | sort | uniq) 13 | bundles: parse $(DISTDIR) 14 | ride bundle compile $(BUNDLES) -B $(BUILDDIR)/bundles --ignore-dependencies -o $(DISTDIR)/$@.zip 15 | .PHONY: bundles 16 | 17 | upload-bundles: bundles 18 | ride bundle install $(DISTDIR)/bundles.zip 19 | 20 | bundle-%: parse $(DISTDIR) 21 | ride bundle compile $(@:bundle-%=%) -B $(BUILDDIR)/bundles --ignore-dependencies -o $(DISTDIR)/$(patsubst bundle-%,%,$(@)).zip $(patsubst bundle-%,%,$(@)) 22 | 23 | clean: 24 | rm -rf $(BUILDDIR) $(DISTDIR) 25 | 26 | parse: $(PARSED_STATEMACHINES) 27 | for dir in $(SOURCE_DIR); do \ 28 | mkdir -p $(BUILDDIR)/$$dir; \ 29 | bundles=$$(find $$dir -type f -name 'manifest.json' | sed -r 's|/[^/]+$$||' | sort | uniq); \ 30 | for bundle in $$bundles; do \ 31 | mkdir -p $(BUILDDIR)/$$bundle; \ 32 | cp $$bundle/manifest.json $(BUILDDIR)/$$bundle; \ 33 | if [ -d $$bundle/resources ]; then cp -r $$bundle/resources $(BUILDDIR)/$$bundle; fi; \ 34 | ln -fs sources $(BUILDDIR)/$$bundle/statemachines; \ 35 | done; \ 36 | done 37 | 38 | $(BUILDDIR)/%.json: %.lf 39 | @mkdir -p $(shell dirname $@) 40 | ride cli parse -o $@ -f $< 41 | 42 | $(DISTDIR): 43 | mkdir -p $(DISTDIR) 44 | -------------------------------------------------------------------------------- /4_Learning/statemachines/Makefile: -------------------------------------------------------------------------------- 1 | # Configurable variables 2 | SOURCE_DIR ?= bundles 3 | BUILDDIR ?= build 4 | DISTDIR ?= $(BUILDDIR)/dist 5 | 6 | # File handling variables 7 | BUNDLE_LF=$(foreach d,$(SOURCE_DIR), $(wildcard $(d)/*/sources/*.lf)) 8 | PARSED_STATEMACHINES=$(foreach f,$(BUNDLE_LF), $(BUILDDIR)/$(f:.lf=.json)) 9 | 10 | all: upload-bundles 11 | 12 | BUNDLES=$(shell find $(SOURCE_DIR) -type f -name 'manifest.json' | sed -r 's|/[^/]+$$||' | sed -r 's|^$(SOURCE_DIR)/||' | sort | uniq) 13 | bundles: parse $(DISTDIR) 14 | ride bundle compile $(BUNDLES) -B $(BUILDDIR)/bundles --ignore-dependencies -o $(DISTDIR)/$@.zip 15 | .PHONY: bundles 16 | 17 | upload-bundles: bundles 18 | ride bundle install $(DISTDIR)/bundles.zip 19 | 20 | bundle-%: parse $(DISTDIR) 21 | ride bundle compile $(@:bundle-%=%) -B $(BUILDDIR)/bundles --ignore-dependencies -o $(DISTDIR)/$(patsubst bundle-%,%,$(@)).zip $(patsubst bundle-%,%,$(@)) 22 | 23 | clean: 24 | rm -rf $(BUILDDIR) $(DISTDIR) 25 | 26 | parse: $(PARSED_STATEMACHINES) 27 | for dir in $(SOURCE_DIR); do \ 28 | mkdir -p $(BUILDDIR)/$$dir; \ 29 | bundles=$$(find $$dir -type f -name 'manifest.json' | sed -r 's|/[^/]+$$||' | sort | uniq); \ 30 | for bundle in $$bundles; do \ 31 | mkdir -p $(BUILDDIR)/$$bundle; \ 32 | cp $$bundle/manifest.json $(BUILDDIR)/$$bundle; \ 33 | if [ -d $$bundle/resources ]; then cp -r $$bundle/resources $(BUILDDIR)/$$bundle; fi; \ 34 | ln -fs sources $(BUILDDIR)/$$bundle/statemachines; \ 35 | done; \ 36 | done 37 | 38 | $(BUILDDIR)/%.json: %.lf 39 | @mkdir -p $(shell dirname $@) 40 | ride cli parse -o $@ -f $< 41 | 42 | $(DISTDIR): 43 | mkdir -p $(DISTDIR) 44 | -------------------------------------------------------------------------------- /3_FCI/ride_integration/statemachines/Makefile: -------------------------------------------------------------------------------- 1 | # Configurable variables 2 | SOURCE_DIR ?= bundles 3 | BUILDDIR ?= build 4 | DISTDIR ?= $(BUILDDIR)/dist 5 | 6 | # File handling variables 7 | BUNDLE_LF=$(foreach d,$(SOURCE_DIR), $(wildcard $(d)/*/sources/*.lf)) 8 | PARSED_STATEMACHINES=$(foreach f,$(BUNDLE_LF), $(BUILDDIR)/$(f:.lf=.json)) 9 | 10 | all: upload-bundles 11 | 12 | BUNDLES=$(shell find $(SOURCE_DIR) -type f -name 'manifest.json' | sed -r 's|/[^/]+$$||' | sed -r 's|^$(SOURCE_DIR)/||' | sort | uniq) 13 | bundles: parse $(DISTDIR) 14 | ride bundle compile $(BUNDLES) -B $(BUILDDIR)/bundles --ignore-dependencies -o $(DISTDIR)/$@.zip 15 | .PHONY: bundles 16 | 17 | upload-bundles: bundles 18 | ride bundle install $(DISTDIR)/bundles.zip 19 | 20 | bundle-%: parse $(DISTDIR) 21 | ride bundle compile $(@:bundle-%=%) -B $(BUILDDIR)/bundles --ignore-dependencies -o $(DISTDIR)/$(patsubst bundle-%,%,$(@)).zip $(patsubst bundle-%,%,$(@)) 22 | 23 | clean: 24 | rm -rf $(BUILDDIR) $(DISTDIR) 25 | 26 | parse: $(PARSED_STATEMACHINES) 27 | for dir in $(SOURCE_DIR); do \ 28 | mkdir -p $(BUILDDIR)/$$dir; \ 29 | bundles=$$(find $$dir -type f -name 'manifest.json' | sed -r 's|/[^/]+$$||' | sort | uniq); \ 30 | for bundle in $$bundles; do \ 31 | mkdir -p $(BUILDDIR)/$$bundle; \ 32 | cp $$bundle/manifest.json $(BUILDDIR)/$$bundle; \ 33 | if [ -d $$bundle/resources ]; then cp -r $$bundle/resources $(BUILDDIR)/$$bundle; fi; \ 34 | ln -fs sources $(BUILDDIR)/$$bundle/statemachines; \ 35 | done; \ 36 | done 37 | 38 | $(BUILDDIR)/%.json: %.lf 39 | @mkdir -p $(shell dirname $@) 40 | ride cli parse -o $@ -f $< 41 | 42 | $(DISTDIR): 43 | mkdir -p $(DISTDIR) 44 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/bundles/ride_tutorial/resources/plug-inserted.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /3_FCI/ride_integration/statemachines/bundles/fci_tutorial/resources/plug-inserted.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /4_Learning/statemachines/bundles/fci_learning_tutorial/resources/plug-inserted.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /4_Learning/statemachines/bundles/ride_learning_tutorial/resources/plug-inserted.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /3_FCI/standalone/src/plug_in_controller_standalone.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | int main(int argc, char** argv) { 9 | // Check whether the required arguments were passed 10 | if (argc != 2) { 11 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 12 | return EXIT_FAILURE; 13 | } 14 | std::cout << "WARNING: Collision thresholds are set to high values. " 15 | << "Make sure you have the user stop at hand!" << std::endl 16 | << "WARNING: Make sure that end effector is in contact with a horizontal rigid surface " 17 | "before starting." 18 | << std::endl 19 | << "Press Enter to continue..." << std::endl; 20 | std::cin.ignore(); 21 | try { 22 | { 23 | franka::Robot robot(argv[1]); 24 | robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, 25 | {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, 26 | {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, 27 | {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}); 28 | } 29 | // some hardcode parameters 30 | plug_in_controller::Parameters params = {1000.0, 30.0, 3.0, 0.8, 0.5, 0.5, 0.8}; 31 | double duration = 5.0; // in [s] 32 | std::array target_position_array = {0., 0., 0.}; 33 | std::array tolerance_position_array = {0., 0., 0.}; 34 | plug_in_controller::run(argv[1], params, duration, target_position_array, 35 | tolerance_position_array); 36 | } catch (const franka::Exception& ex) { 37 | std::cerr << ex.what() << std::endl; 38 | return EXIT_FAILURE; 39 | } 40 | return EXIT_SUCCESS; 41 | } 42 | -------------------------------------------------------------------------------- /3_FCI/standalone/include/plug_in_controller.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace plug_in_controller { 5 | 6 | /** 7 | * @brief This struct defines the parameters of the plug in controller. 8 | */ 9 | struct Parameters { 10 | /**Stiffness in x,y,z direction of the end effector [N/m], Bounded: [0, 2000] N/m */ 11 | double translational_stiffness; 12 | /**Stiffness around x,y,z axis of the end effector [N/rad], Bounded: [0, 300] N/m */ 13 | double rotational_stiffness; 14 | /**Forces applied in the end effectors z direction [N] */ 15 | double desired_force; 16 | /** Frequency of the wiggle in end effectors x direction [Hz] */ 17 | double wiggle_frequency_x; 18 | /** Frequency of the wiggle in end effectors y direction [Hz] */ 19 | double wiggle_frequency_y; 20 | /** Amplitude of the wiggle in end effectors x direction [m] */ 21 | double wiggle_amplitude_x; 22 | /** Amplitude of the wiggle in end effectors y direction [m] */ 23 | double wiggle_amplitude_y; 24 | }; 25 | 26 | /** 27 | * @brief This function runs the plug in controller 28 | * 29 | * @param robot_ip IP of the robot 30 | * @param plug_in_param Parameters of the plug in controller 31 | * @param duration Amount of time the plug in controller should run. [s] 32 | * @param target_position_array Array defining the target position. When this position is reached 33 | * within an tolerance the controller stops. [m] 34 | * @param target_tolerance_array Array specifying the maximum tolerance. If the end effector reaches 35 | * the target position within this tolerance the controller stops. [m] 36 | */ 37 | void run(const std::string& robot_ip, 38 | const Parameters plug_in_params, 39 | const double duration, // in seconds 40 | const std::array target_position_array, 41 | const std::array target_tolerance_array); 42 | } // namespace plug_in_controller 43 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/bundles/ride_tutorial/resources/logo.svg: -------------------------------------------------------------------------------- 1 | Plug In -------------------------------------------------------------------------------- /3_FCI/ride_integration/statemachines/bundles/fci_tutorial/resources/logo.svg: -------------------------------------------------------------------------------- 1 | Plug In -------------------------------------------------------------------------------- /4_Learning/statemachines/bundles/fci_learning_tutorial/resources/logo.svg: -------------------------------------------------------------------------------- 1 | Plug In -------------------------------------------------------------------------------- /4_Learning/statemachines/bundles/ride_learning_tutorial/resources/logo.svg: -------------------------------------------------------------------------------- 1 | Plug In -------------------------------------------------------------------------------- /4_Learning/services/src/random_search.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "random_search.h" 5 | 6 | RandomSearch::RandomSearch() = default; 7 | 8 | void RandomSearch::init(const Hyperparameters& hyperparams) { 9 | hyperparams_ = hyperparams; 10 | // reset optimizer 11 | current_iteration_ = 0; 12 | samples_.clear(); 13 | costs_.clear(); 14 | constraints_.clear(); 15 | best_sample_.clear(); 16 | best_cost_ = std::numeric_limits::max(); 17 | best_iter_ = 0; 18 | } 19 | 20 | std::vector RandomSearch::getNextSample() { 21 | std::vector next_sample; 22 | // generate random number between lower and upper_bound for every parameter 23 | next_sample.reserve(hyperparams_.param_count); 24 | for (int i = 0; i < hyperparams_.param_count; i++) { 25 | next_sample.push_back( 26 | generateRandomNumber(hyperparams_.lower_bounds[i], hyperparams_.upper_bounds[i])); 27 | } 28 | // store sample 29 | samples_.push_back(next_sample); 30 | return next_sample; 31 | } 32 | 33 | void RandomSearch::setResult(const double cost, const bool constraint_satisfied) { 34 | costs_.push_back(cost); 35 | constraints_.push_back(constraint_satisfied); 36 | // check if cost is smaller 37 | if (cost < best_cost_ && constraint_satisfied) { 38 | best_sample_ = samples_.back(); // last added element 39 | best_cost_ = cost; 40 | best_iter_ = current_iteration_; 41 | } 42 | // print result of exploration and best parameters 43 | std::cout << "Iteration: " << current_iteration_ << std::endl; 44 | std::cout << "Parameters: "; 45 | for (const auto& item : samples_.back()) { 46 | std::cout << item << ' '; 47 | } 48 | std::cout << "Cost: " << cost << " Success:" << static_cast(constraint_satisfied) 49 | << std::endl; 50 | std::cout << "Best Parameters: "; 51 | for (const auto& item : best_sample_) { 52 | std::cout << item << ' '; 53 | } 54 | std::cout << "Best Cost: " << best_cost_ << std::endl; 55 | std::cout << "=======" << std::endl; 56 | 57 | current_iteration_++; 58 | } 59 | 60 | bool RandomSearch::hasFinished() { 61 | // stop after certain number of iteration 62 | return current_iteration_ >= hyperparams_.max_iterations; 63 | } 64 | 65 | double RandomSearch::generateRandomNumber(const double lower_bound, const double upper_bound) { 66 | std::random_device rd; 67 | std::mt19937 gen(rd()); 68 | std::uniform_real_distribution dist(lower_bound, upper_bound); 69 | double random_number = dist(gen); 70 | return random_number; 71 | } 72 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/bundles/ride_tutorial/sources/convert_safety.lf: -------------------------------------------------------------------------------- 1 | convert_safety { 2 | port success result~=nil 3 | port error result==nil 4 | 5 | clientData { 6 | description: @{ 7 | This statemachine converts the safety parameters (collision thresholds) 8 | received from timeline to the format required by the state machines. 9 | }@; 10 | } 11 | 12 | parameterType { 13 | { 14 | { 15 | float x; 16 | float y; 17 | float z; 18 | float x_rot; 19 | float y_rot; 20 | float z_rot; 21 | } force_thresholds; 22 | { 23 | float j0; 24 | float j1; 25 | float j2; 26 | float j3; 27 | float j4; 28 | float j5; 29 | float j6; 30 | } torque_thresholds; 31 | } safety; 32 | } 33 | 34 | resultType { 35 | { 36 | [7]float lower_torque_thresholds_nominal; 37 | [7]float upper_torque_thresholds_nominal; 38 | [7]float lower_torque_thresholds_acceleration; 39 | [7]float upper_torque_thresholds_acceleration; 40 | [6]float lower_force_thresholds_nominal; 41 | [6]float upper_force_thresholds_nominal; 42 | [6]float lower_force_thresholds_acceleration; 43 | [6]float upper_force_thresholds_acceleration; 44 | } safety; 45 | } 46 | 47 | entry @{ 48 | local result = {} 49 | local torque_thresholds = { 50 | parameter.safety.torque_thresholds.j0, parameter.safety.torque_thresholds.j1, 51 | parameter.safety.torque_thresholds.j2, parameter.safety.torque_thresholds.j3, 52 | parameter.safety.torque_thresholds.j4, parameter.safety.torque_thresholds.j5, 53 | parameter.safety.torque_thresholds.j6 54 | } 55 | local force_thresholds = { 56 | parameter.safety.force_thresholds.x, parameter.safety.force_thresholds.y, 57 | parameter.safety.force_thresholds.z, parameter.safety.force_thresholds.x_rot, 58 | parameter.safety.force_thresholds.y_rot, parameter.safety.force_thresholds.z_rot 59 | } 60 | result.safety = { 61 | lower_torque_thresholds_nominal = torque_thresholds; 62 | upper_torque_thresholds_nominal = torque_thresholds; 63 | lower_torque_thresholds_acceleration = torque_thresholds; 64 | upper_torque_thresholds_acceleration = torque_thresholds; 65 | lower_force_thresholds_nominal = force_thresholds; 66 | upper_force_thresholds_nominal = force_thresholds; 67 | lower_force_thresholds_acceleration = force_thresholds; 68 | upper_force_thresholds_acceleration = force_thresholds; 69 | } 70 | setResult(result) 71 | }@ 72 | 73 | } 74 | -------------------------------------------------------------------------------- /4_Learning/services/include/random_search.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | /** 7 | * @brief A structure representing hyperparameters for random search 8 | */ 9 | struct Hyperparameters { 10 | /** Maximum number of iterations*/ 11 | int max_iterations; 12 | /** Number of parameters being optimized*/ 13 | int param_count; 14 | /** Lower boundaries for each parameter being optimized*/ 15 | std::vector lower_bounds; 16 | /** Upper boundaries for each parameter being optimized*/ 17 | std::vector upper_bounds; 18 | }; 19 | 20 | /** 21 | * @brief This class implements random search. 22 | */ 23 | class RandomSearch { 24 | public: 25 | RandomSearch(); 26 | /** 27 | * @brief This method initializes random search by setting hyperparameters and 28 | * resetting the optimization. 29 | * @param hyperparams Hyperparmeters of random search 30 | */ 31 | void init(const Hyperparameters& hyperparams); 32 | /** 33 | * @brief This method returns a vector of random sampled parameters. 34 | * @return parameters for the next experimential trial 35 | */ 36 | std::vector getNextSample(); 37 | /** 38 | * @brief This method sets a cost value and an indicator if the execution was 39 | * successfull. 40 | * @param cost value of the cost function 41 | * @param contraint_statisfied boolean specifying if the constraint has been satisfied 42 | */ 43 | void setResult(const double cost, const bool constraint_satisfied); 44 | /** 45 | * @brief This method returns true when the maximum number of iteration are reached. 46 | */ 47 | bool hasFinished(); 48 | /** 49 | * @brief This method returns the so far best sample 50 | * @return so far best sample minimizing the cost function and satisfying the constraint 51 | * (successful execution). 52 | */ 53 | std::vector getBestSample() const { return best_sample_; }; 54 | /** 55 | * @brief This method returns the hyperparameters of random search. 56 | * @return hyperparameters of random search 57 | */ 58 | Hyperparameters hyperParameters() const { return hyperparams_; } 59 | 60 | private: 61 | /** 62 | * @brief This method returns a random scalar between lower and upper bound 63 | * @param lower_bound 64 | * @param upper_bound 65 | */ 66 | double generateRandomNumber(const double lower_bound, const double upper_bound); 67 | 68 | Hyperparameters hyperparams_; 69 | std::vector> samples_; 70 | std::vector costs_; 71 | std::vector constraints_; 72 | std::vector best_sample_; 73 | double best_cost_; 74 | int current_iteration_; 75 | int best_iter_; 76 | }; 77 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/bundles/ride_tutorial/resources/input.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /4_Learning/statemachines/bundles/fci_learning_tutorial/resources/input.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /4_Learning/statemachines/bundles/ride_learning_tutorial/resources/input.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /3_FCI/ride_integration/statemachines/bundles/fci_tutorial/resources/input.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /3_FCI/ride_integration/services/src/fci_service.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | 11 | struct FCIParams { 12 | std::string robot_ip; 13 | }; 14 | 15 | void plugInCb(CallHandle h, struct PlugIn_request* req, void* hint) { 16 | auto* fci_params = static_cast(hint); 17 | 18 | // required convertion 19 | std::array target_position, tolerance; 20 | std::copy(req->target_position, req->target_position + 3, target_position.begin()); 21 | std::copy(req->tolerance, req->tolerance + 3, tolerance.begin()); 22 | 23 | plug_in_controller::Parameters plug_in_params = { 24 | req->plug_in_params.translational_stiffness, req->plug_in_params.rotational_stiffness, 25 | req->plug_in_params.desired_force, req->plug_in_params.wiggle_frequency_x, 26 | req->plug_in_params.wiggle_frequency_y, req->plug_in_params.wiggle_amplitude_x, 27 | req->plug_in_params.wiggle_amplitude_y}; 28 | 29 | // run the plug in controller with the desired parameters 30 | try { 31 | plug_in_controller::run(fci_params->robot_ip, plug_in_params, req->duration, target_position, 32 | tolerance); 33 | 34 | } catch (const franka::Exception& ex) { 35 | std::cerr << ex.what() << std::endl; 36 | PlugIn_error error = strdup(ex.what()); 37 | failOperation(h, &error); 38 | free_PlugIn_request(req); 39 | return; 40 | } 41 | succeedOperation(h, {}); 42 | free_PlugIn_request(req); 43 | } 44 | 45 | int main(int argc, char* argv[]) { 46 | if (argc != 4) { 47 | std::cerr << "Usage: " << argv[0] 48 | << " " << std::endl; 49 | return EXIT_FAILURE; 50 | } 51 | 52 | FCIParams fci_params{argv[1]}; 53 | 54 | installSigIntHandler(); 55 | Service* race_service = 56 | registerService(("tcp://" + std::string(argv[1]) + ":" + std::string(argv[2])).c_str(), 57 | ("tcp://" + std::string(argv[3])).c_str(), "fci"); 58 | if (race_service == nullptr) { 59 | std::cerr << "Unable to register RaceCom Service: fci" << std::endl; 60 | return EXIT_FAILURE; 61 | } 62 | 63 | try { 64 | int rc = registerOperation(race_service, "plugIn", PlugInDescriptor(), 65 | reinterpret_cast(plugInCb), &fci_params); 66 | if (rc != 0) { 67 | throw std::runtime_error("Could not register operation 'fci.plugIn'."); 68 | } 69 | 70 | } catch (std::exception const& e) { 71 | std::cerr << e.what() << std::endl; 72 | unregisterService(race_service); 73 | return EXIT_FAILURE; 74 | } 75 | 76 | spin(race_service); 77 | 78 | unregisterService(race_service); 79 | 80 | return EXIT_SUCCESS; 81 | } 82 | -------------------------------------------------------------------------------- /2_RIDE/services/src/socket_service.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | /** Callback for getSocketPose racecom operation. It returns the socket pose. 11 | */ 12 | void getSocketPoseCb(CallHandle h, struct GetSocketPose_request* req, void* /*hint*/) { 13 | /*Assume that a vision component determines the socket pose and returns this to the 14 | plug_in_vision statemachine. However, this example only shows how to connect a RaceCom service 15 | to a RIDE statemachine, the socket pose is hardcoded and no vision is performed here. To 16 | successfully run this demo, the user must manually obtain and copy actual socket information 17 | here. The information consists of the cartesian pose which can be obtained by 18 | moving the robot's end-effector to the socket pose and running following command: `ride service 19 | echo robot sensor_data` in a terminal. The cartesian pose is located in the O_T_EE entry. 20 | */ 21 | // pose: column major 22 | std::array socket_pose = { 23 | -0.05183746976227782, 0.9983504730999704, -0.024289019966200448, 0., 24 | 0.9982147812036288, 0.052514346441068005, 0.028111223837941478, 0., 25 | 0.02934094053127373, -0.02278888279214321, -0.9993096497231606, 0., 26 | 0.6370806581504417, 0.06103184325272377, 0.24091866348352692, 1.}; 27 | 28 | // Prepare result 29 | // First allocate array for pose 30 | auto* socket_pose_c_array = static_cast(allocateArray(16, sizeof(double))); 31 | // ... then copy content from pose to this allocated C-array 32 | std::copy(socket_pose.begin(), socket_pose.end(), socket_pose_c_array); 33 | GetSocketPose_result result = {socket_pose_c_array}; 34 | 35 | // Return result 36 | succeedOperation(h, &result); 37 | free_GetSocketPose_request(req); 38 | } 39 | 40 | int main(int argc, char* argv[]) { 41 | if (argc != 4) { 42 | std::cerr << "Usage: " << argv[0] 43 | << " " << std::endl; 44 | return EXIT_FAILURE; 45 | } 46 | 47 | // Register Service 48 | installSigIntHandler(); 49 | Service* race_service = 50 | registerService(("tcp://" + std::string(argv[1]) + ":" + std::string(argv[2])).c_str(), 51 | ("tcp://" + std::string(argv[3])).c_str(), "socket-pose"); 52 | if (race_service == nullptr) { 53 | std::cerr << "Could not register service 'socket-pose'." << std::endl; 54 | return EXIT_FAILURE; 55 | } 56 | 57 | Publisher* pub = nullptr; 58 | try { 59 | // Register operation 60 | int rc = registerOperation(race_service, "getSocketPose", GetSocketPoseDescriptor(), 61 | reinterpret_cast(getSocketPoseCb), nullptr); 62 | if (rc != 0) { 63 | throw std::runtime_error("Could not register operation 'socket-pose.getSocketPose'."); 64 | } 65 | 66 | // Register event 67 | pub = registerEvent(race_service, "socket_info", SocketInfoDescriptor()); 68 | 69 | if (pub == nullptr) { 70 | throw std::runtime_error("Could not register event 'socket-pose.socket_info'."); 71 | } 72 | } catch (const std::runtime_error& e) { 73 | std::cerr << e.what() << std::endl; 74 | unregisterService(race_service); 75 | return EXIT_FAILURE; 76 | } 77 | 78 | while (!shutdownSignalled()) { 79 | spinOnce(race_service, 1000); 80 | // Determine number of sockets, e.g. by a vision component. 81 | // As we don't do any vision here, the number of sockets is hardcoded. 82 | int number_of_sockets = 1; 83 | // create message 84 | SocketInfo socket_info{.number_of_sockets = number_of_sockets}; 85 | publish(pub, &socket_info); 86 | } 87 | 88 | unregisterService(race_service); 89 | 90 | return EXIT_SUCCESS; 91 | } 92 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/bundles/ride_tutorial/resources/plug-backup.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /4_Learning/statemachines/bundles/fci_learning_tutorial/resources/plug-backup.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /4_Learning/statemachines/bundles/ride_learning_tutorial/resources/plug-backup.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /3_FCI/ride_integration/statemachines/bundles/fci_tutorial/resources/plug-backup.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/bundles/ride_tutorial/resources/plug.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /3_FCI/ride_integration/statemachines/bundles/fci_tutorial/resources/plug.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /4_Learning/statemachines/bundles/fci_learning_tutorial/resources/plug.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /4_Learning/statemachines/bundles/ride_learning_tutorial/resources/plug.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /1_Desk/README.md: -------------------------------------------------------------------------------- 1 | # *Desk* 2 | This tutorial illustrates how to program the *Franka Emika AIR Platform* with *Desk*. 3 | 4 | ## Getting started 5 | Make sure you are connected to the robot, either to the *Arm* base or to *Control*. Then enter the robot ip in a web browser to open *Desk*. Your robot ip will be: 6 | 7 | * `robot.franka.de` if you are connected to the *Arm* base, which is running a DHCP server. 8 | * The already configured shopfloor IP if you are connected to *Control*. If you don't know your shopfloor IP you can always connect your PC to the *Arm* base first and configure it in the Admin menu of *Desk*. 9 | 10 | ## Programming with Desk 11 | This tutorial implements the plug in task using *Desk* as described in Section III-A in the [Tutorial Paper](../README.md#tutorial-paper).
12 | The plug in task is programmed using the following three basic Apps included in the Research Apps bundle: 13 | 14 | * The *Cartesian Motion App* will position the robot in its initial pose. 15 | * The *Move to Contact App* will move the robot towards the socket until it detects a contact. 16 | * The *Lissajous* will regulate a constant force in z-axis and produces a wiggle motion in the `(x,y)` -plane for `n` seconds to insert the plug. 17 | 18 | Drag and drop the three Apps from the App pane to the timeline one after another. 19 | 20 | With *Desk* you can also **download** and **import** your tasks. The `plug-in.task` file included in this folder is a preprogrammed timeline for the task described in the tutorial. 21 | To import it, just drag the `plug-in.task` file and drop it into the task area in *Desk*. Please make sure that *CART MOTION*, *MOVE TO CONTACT* and *LISSAJOUS* apps are already available in desk. 22 | 23 | ## Parameterizing the task 24 | In order to run the plug in task in your custom setup you will have to first parameterize each App. If you imported the task file included in this repo, the parameters will not fit your setup so you will also require a reparameterization. You can do it using the web interface and your mouse but we recommend using the *Pilot* buttons: 25 | 26 | 1. Click on the *Cartesian Motion App* and guide the robot to a position above the hole while the robot is holding the plug between its fingers. Press the O button in the *Pilot* to store poses or the X button to delete them. To set the velocity and acceleration factors you can navigate the menus with the directional pad 27 | ➕ and validate with the ✔ . 28 | 29 | 2. Click on the *Move to Contact App* and guide the robot to the socket pose by inserting the grasped plug into the socket. The context menu of this App is very similar to the previous one. 30 | 31 | 3. Click on the *Lissajous App* and set the wiggle and the force until the plug is successfully inserted. To set the desired wiggle motion and force/torque values you can navigate the menus with the directional pad ➕ and validate with the ✔ . For the wiggle motion you need to configure the number of loops and size of each axis. A good starting point is 3 loops in one axis and 2 in the other one. Try to keep the size as small as possible! Start here with a size of 1 and increase the values carefully if required. In a table-top configuration select the `(x,y)` - plane in which to move. 32 | Moreover you will only need positive force values in the `z` axis. Try to keep the force as small as possible! Start with 5 N and increase the values carefully if needed. Also increase the collision thresholds accordingly to avoid any undesired errors. If after trying high values for the forces the robot is still not able to insert the plug, try teaching again the wiggle motion and/or the poses from the first and the second App. 33 | 34 | ## Running the task 35 | 36 | You can run the task in two different ways: 37 | 38 | 1. **Using *Desk* (web-based interface):**
39 | First open *Desk* in a web browser, activate the external activation device (robot lights should be blue) and press the run button on the side bar. 40 | 2. **Using the `ride-cli` command-line tool:**
41 | You can also use the command-line tool `ride-cli` to start your task from a Linux terminal. To do so, open a terminal and connect to the *Core* by using the following command: 42 | 43 | ```sh 44 | ride login 45 | ``` 46 | 47 | To run the task activate the external activation device, stop any execution currently running (e.g. idle state machine) by 48 | 49 | ```sh 50 | ride cli stop 51 | ``` 52 | 53 | and then enter 54 | 55 | ```sh 56 | ride cli start 0_plug-in 57 | ``` 58 | -------------------------------------------------------------------------------- /3_FCI/ride_integration/statemachines/bundles/fci_tutorial/sources/plug_in_fci.lf: -------------------------------------------------------------------------------- 1 | plug_in_fci { 2 | port success child("plug_in_controller").port("success") 3 | port error child("move_to_socket_pose").port("error") or 4 | child("set_safety").port("error") or 5 | child("plug_in_controller").port("error") 6 | 7 | clientData{ 8 | description: @{ 9 | This statemachine inserts a plug into a hole. Therefore it first moves to a socket pose. 10 | Then it runs the plug in controller implemented using fci. It is assumed that the plug is 11 | inserted when the robot pose reaches the hole pose within a tolerance. 12 | }@; 13 | } 14 | 15 | parameterType { 16 | -- parameter for socket motion 17 | { 18 | []{ 19 | {[16]float pose;} pose; 20 | bool point2point; 21 | float cartesian_velocity_factor; 22 | float cartesian_acceleration_factor; 23 | float cartesian_deceleration_factor; 24 | float q3; 25 | } poses; 26 | { 27 | [6]int K_x; 28 | } cartesian_impedance; 29 | { 30 | [7]int K_theta; 31 | } joint_impedance; 32 | { 33 | [7]float lower_torque_thresholds_nominal; 34 | [7]float upper_torque_thresholds_nominal; 35 | [7]float lower_torque_thresholds_acceleration; 36 | [7]float upper_torque_thresholds_acceleration; 37 | [6]float lower_force_thresholds_nominal; 38 | [6]float upper_force_thresholds_nominal; 39 | [6]float lower_force_thresholds_acceleration; 40 | [6]float upper_force_thresholds_acceleration; 41 | } safety; 42 | int controller_mode; 43 | } socket_motion; 44 | -- safety parameter for plug_in 45 | { 46 | [7]float lower_torque_thresholds_nominal; 47 | [7]float upper_torque_thresholds_nominal; 48 | [7]float lower_torque_thresholds_acceleration; 49 | [7]float upper_torque_thresholds_acceleration; 50 | [6]float lower_force_thresholds_nominal; 51 | [6]float upper_force_thresholds_nominal; 52 | [6]float lower_force_thresholds_acceleration; 53 | [6]float upper_force_thresholds_acceleration; 54 | } insertion_safety; 55 | -- end effector pose when plug is inserted 56 | [16]float hole_pose; 57 | -- plug in parameter 58 | { 59 | float translational_stiffness; 60 | float rotational_stiffness; 61 | float desired_force; 62 | float wiggle_frequency_x; 63 | float wiggle_frequency_y; 64 | float wiggle_amplitude_x; 65 | float wiggle_amplitude_y; 66 | } plug_in_params; 67 | -- tolerance between the end effector pose 68 | -- and hole pose to check if plug is successfully inserted [m] 69 | [3]float tolerance; 70 | -- maximum insertion time [s] 71 | float duration; 72 | } 73 | 74 | resultType { 75 | string error_cause; 76 | } 77 | 78 | --> move_to_socket_pose <- move_via_with_move_configuration { 79 | port success -> set_safety 80 | } where parameter.socket_motion 81 | 82 | -- set collisions thresholds 83 | set_safety <- set_safety { 84 | port success -> plug_in_controller 85 | } where parameter.insertion_safety 86 | 87 | -- Run the plug_in fci controller. If service returns success, we assume plug has been inserted. 88 | -- Otherwise we assume failure 89 | plug_in_controller { 90 | port success service("fci").operation("plugIn").status == "success" 91 | port error service("fci").operation("plugIn").status == "error" 92 | 93 | parameterType { 94 | { 95 | float translational_stiffness; 96 | float rotational_stiffness; 97 | float desired_force; 98 | float wiggle_frequency_x; 99 | float wiggle_frequency_y; 100 | float wiggle_amplitude_x; 101 | float wiggle_amplitude_y; 102 | } plug_in_params; 103 | float duration; 104 | [3]float tolerance; 105 | [3]float target_position; 106 | } 107 | 108 | resultType { 109 | string error_cause; 110 | } 111 | 112 | entry @{ 113 | service("fci").operation("plugIn").call({ 114 | plug_in_params = parameter.plug_in_params; 115 | duration = parameter.duration; 116 | tolerance = parameter.tolerance; 117 | target_position = parameter.target_position; 118 | }) 119 | }@ 120 | 121 | action service("fci").operation("plugIn").status == "error" @{ 122 | setResult({error_cause = service("fci").operation("plugIn").error}) 123 | }@ 124 | 125 | } where { 126 | plug_in_params: parameter.plug_in_params; 127 | duration: parameter.duration; 128 | tolerance: parameter.tolerance; 129 | target_position: [parameter.hole_pose[13], parameter.hole_pose[14], parameter.hole_pose[15]]; 130 | } 131 | 132 | -- Check for triggered ports and set result accordingly. 133 | 134 | action child("move_to_socket_pose").port("error") @{ 135 | setResult({error_cause = child("move_to_socket_pose").result.error_cause}) 136 | }@ 137 | 138 | action child("set_safety").port("error") @{ 139 | setResult({error_cause = child("set_safety").result.error_cause}) 140 | }@ 141 | 142 | action child("plug_in_controller").port("error") @{ 143 | setResult({error_cause = child("plug_in_controller").result.error_cause}) 144 | }@ 145 | 146 | } 147 | -------------------------------------------------------------------------------- /4_Learning/services/src/learning_service.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | /** 12 | * @brief Helper function to convert a c_array to a vector 13 | */ 14 | template 15 | std::vector cArrayToVector(int size, T* c_array) { 16 | return std::vector(c_array, c_array + size); 17 | } 18 | 19 | /** 20 | * @brief Helper function to convert a vector to a c_array 21 | */ 22 | template 23 | T* vectorToCArray(std::vector vec) { 24 | auto* c_array = static_cast(allocateArray(static_cast(vec.size()), sizeof(double))); 25 | std::copy(vec.begin(), vec.end(), c_array); 26 | return c_array; 27 | } 28 | 29 | /** 30 | * @brief Callback for init racecom operation. It calls init method of random search 31 | * class. 32 | */ 33 | void initCb(CallHandle h, struct Init_request* req, void* hint) { 34 | auto* random_search = static_cast(hint); 35 | Hyperparameters hyperparams; 36 | hyperparams.max_iterations = req->max_iteration; 37 | hyperparams.param_count = req->param_count; 38 | hyperparams.lower_bounds = cArrayToVector(req->param_count, req->lower_bounds); 39 | hyperparams.upper_bounds = cArrayToVector(req->param_count, req->upper_bounds); 40 | random_search->init(hyperparams); 41 | succeedOperation(h, {}); 42 | free_Init_request(req); 43 | } 44 | 45 | /** 46 | * @brief Callback for get_param racecom operation. It returns an array of 47 | * random sampled parameters by calling getNextSample method of random search class. 48 | */ 49 | void getParamsCb(CallHandle h, struct GetParams_request* req, void* hint) { 50 | auto* random_search = static_cast(hint); 51 | std::vector next_sample = random_search->getNextSample(); 52 | GetParams_result result{vectorToCArray(next_sample)}; 53 | succeedOperation(h, &result); 54 | free_GetParams_request(req); 55 | } 56 | 57 | /** 58 | * @brief Callback for set_result racecom operation. It sets the result of 59 | * the execution for the random search class. 60 | */ 61 | void setResultCb(CallHandle h, struct SetResult_request* req, void* hint) { 62 | auto* random_search = static_cast(hint); 63 | random_search->setResult(req->cost, req->success); 64 | bool stop = random_search->hasFinished(); 65 | succeedOperation(h, &stop); 66 | free_SetResult_request(req); 67 | } 68 | 69 | /** 70 | * @brief Callback for get_best_params operation. It returns an array of the best 71 | * observed parameters. 72 | */ 73 | void getBestParamsCb(CallHandle h, struct GetParams_request* req, void* hint) { 74 | auto* random_search = static_cast(hint); 75 | std::vector best_params = random_search->getBestSample(); 76 | if (best_params.empty()) { 77 | GetParams_error error = strdup("App hasn't been learned so far!"); 78 | failOperation(h, &error); 79 | free_GetParams_request(req); 80 | return; 81 | } 82 | GetParams_result result{vectorToCArray(best_params)}; 83 | succeedOperation(h, &result); 84 | free_GetParams_request(req); 85 | } 86 | 87 | int main(int argc, char* argv[]) { 88 | if (argc != 4) { 89 | std::cerr << "Usage: " << argv[0] 90 | << " " << std::endl; 91 | return EXIT_FAILURE; 92 | } 93 | 94 | RandomSearch random_search; 95 | 96 | // Register Service 97 | installSigIntHandler(); 98 | Service* race_service = 99 | registerService(("tcp://" + std::string(argv[1]) + ":" + std::string(argv[2])).c_str(), 100 | ("tcp://" + std::string(argv[3])).c_str(), "learning"); 101 | if (race_service == nullptr) { 102 | std::cerr << "Unable to register RaceCom Service: learning" << std::endl; 103 | return EXIT_FAILURE; 104 | } 105 | 106 | // Register all operations 107 | try { 108 | int rc = registerOperation(race_service, "init", InitDescriptor(), 109 | reinterpret_cast(initCb), &random_search); 110 | if (rc != 0) { 111 | throw std::runtime_error("Could not register operation 'learning.init'."); 112 | } 113 | 114 | rc = registerOperation(race_service, "getParams", GetParamsDescriptor(), 115 | reinterpret_cast(getParamsCb), &random_search); 116 | if (rc != 0) { 117 | throw std::runtime_error("Could not register operation 'learning.getParams'."); 118 | } 119 | 120 | rc = registerOperation(race_service, "setResult", SetResultDescriptor(), 121 | reinterpret_cast(setResultCb), &random_search); 122 | if (rc != 0) { 123 | throw std::runtime_error("Could not register operation 'learning.setResult'."); 124 | } 125 | 126 | rc = registerOperation(race_service, "getBestParams", GetParamsDescriptor(), 127 | reinterpret_cast(getBestParamsCb), &random_search); 128 | 129 | if (rc != 0) { 130 | throw std::runtime_error("Could not register operation 'learning.getBestParams'."); 131 | } 132 | 133 | } catch (std::exception const& e) { 134 | std::cerr << e.what() << std::endl; 135 | unregisterService(race_service); 136 | return EXIT_FAILURE; 137 | } 138 | 139 | // Start event loop 140 | spin(race_service); 141 | 142 | unregisterService(race_service); 143 | 144 | return EXIT_SUCCESS; 145 | } 146 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/bundles/ride_tutorial/sources/plug_in_vision.lf: -------------------------------------------------------------------------------- 1 | plug_in_vision { 2 | port success child("plug_in").port("success") 3 | port error child("check_for_sockets").port("error") or 4 | child("get_socket_pose").port("error") or 5 | child("plug_in").port("error") 6 | 7 | clientData { 8 | description: @{ 9 | This statemachine implements the vision-based plug-in statemachine as described in the tutorial paper. 10 | It basically shows how to connect a service with a statemachine. Therefore it first subscribes to the 11 | `socket-pose.socket_info` event and checks the number of detected sockets. If the number of sockets 12 | are larger than 0, this statemachine calls the `socket-pose.getSocketPose` operation to get the pose 13 | of the socket and continue with the already implemented plug_in statemachine. 14 | }@; 15 | } 16 | 17 | parameterType { 18 | { 19 | [7]float lower_torque_thresholds_nominal; 20 | [7]float upper_torque_thresholds_nominal; 21 | [7]float lower_torque_thresholds_acceleration; 22 | [7]float upper_torque_thresholds_acceleration; 23 | [6]float lower_force_thresholds_nominal; 24 | [6]float upper_force_thresholds_nominal; 25 | [6]float lower_force_thresholds_acceleration; 26 | [6]float upper_force_thresholds_acceleration; 27 | } safety; 28 | { 29 | -- parameter for wiggle motion 30 | float amplitude_factor_translation; 31 | float amplitude_factor_rotation; 32 | -- stiffness of robot during insertion 33 | float impedance_factor; 34 | -- insertion force in z direction of robot's end-effector frame [N] 35 | float F_z; 36 | } plug_in_params; 37 | -- tolerance between the end effector pose 38 | -- and hole pose to check if plug is successfully inserted [m] 39 | [3]float tolerance; 40 | -- maximum insertion time [s] 41 | float duration; 42 | } 43 | 44 | resultType { 45 | string error_cause; 46 | } 47 | 48 | -- Check number of detected sockets. The amount of detected sockets `number_of_sockets` 49 | -- will be published by the event `sockets-info` of the `socket-pose` service. 50 | -- If `number_of_sockets` is larger than 0 continue to the next step, 51 | -- otherwise fail the plug_in_vision. 52 | --> check_for_sockets { 53 | port success service("socket-pose").event("socket_info")~=nil and 54 | service("socket-pose").event("socket_info").number_of_sockets > 0 -> get_socket_pose 55 | port error service("socket-pose").event("socket_info")~=nil and 56 | service("socket-pose").event("socket_info").number_of_sockets == 0 57 | } 58 | 59 | -- Get the socket pose. 60 | -- It is received from the `getSocketPose` operation of the `socket-pose` operation. 61 | get_socket_pose { 62 | port success service("socket-pose").operation("getSocketPose") 63 | .status == "success" -> get_q3 64 | port error service("socket-pose").operation("getSocketPose").status == "error" 65 | 66 | resultType 67 | { 68 | [16]float socket_pose; 69 | } 70 | 71 | entry @{ 72 | service("socket-pose").operation("getSocketPose").call({}) 73 | }@ 74 | 75 | exit @{ 76 | setResult(service("socket-pose").operation("getSocketPose").result) 77 | }@ 78 | } 79 | 80 | -- For the socket motion in the plug_in statemachine, a desired angle of joint 3 (q3) 81 | -- for the nullspace is required. This q3 will be set to the current q3. 82 | get_q3 { 83 | port done result~=nil -> compute_hole_pose 84 | 85 | resultType float 86 | 87 | action service("robot").event("sensor_data")~=nil @{ 88 | setResult(service("robot").event("sensor_data").q[3]) 89 | }@ 90 | } 91 | 92 | -- Compute the hole_pose. 93 | -- The hole pose is required to check if plug is inserted. 94 | compute_hole_pose { 95 | port success result~=nil -> plug_in 96 | 97 | parameterType { 98 | [16]float socket_pose; 99 | } 100 | 101 | resultType { 102 | [16]float hole_pose; 103 | } 104 | 105 | entry @{ 106 | -- It is assumed that the `hole_pose` is equal to the `socket_pose` 107 | -- minus the plug length in z direction of the base frame. 108 | -- NOTE: If you change the plug, update the plug_length accordingly. 109 | local plug_length = 0.01 -- [m] valid for HDMI plug 110 | local hole_pose = parameter.socket_pose 111 | hole_pose[15] = hole_pose[15] - plug_length 112 | setResult{hole_pose = hole_pose} 113 | }@ 114 | } where child("get_socket_pose").result 115 | 116 | -- Plug in the plug 117 | plug_in <- plug_in_wrapper { 118 | } where { 119 | socket_pose: { 120 | pose: child("get_socket_pose").result.socket_pose; 121 | q3: child("get_q3").result; 122 | }; 123 | hole_pose: child("compute_hole_pose").result.hole_pose; 124 | plug_in_params: parameter.plug_in_params; 125 | duration: parameter.duration; 126 | tolerance: parameter.tolerance; 127 | safety: parameter.safety; 128 | } 129 | 130 | -- check for triggered ports and set result accordingly 131 | 132 | action child("check_for_sockets").port("error") @{ 133 | setResult({error_cause = "Could not detect any socket."}) 134 | }@ 135 | 136 | action child("get_socket_pose").port("error") @{ 137 | setResult({error_cause = "Get_socket_pose: Operation call failed."}) 138 | }@ 139 | 140 | action child("plug_in").port("error") @{ 141 | setResult(child("plug_in").result) 142 | }@ 143 | 144 | action child("plug_in").port("success") @{ 145 | print("Plug-in-vision ended successfully!") 146 | }@ 147 | 148 | } where { 149 | safety: { 150 | lower_torque_thresholds_nominal: [60., 60., 60., 60., 50., 40., 30.]; 151 | upper_torque_thresholds_nominal: [60., 60., 60., 60., 50., 40., 30.]; 152 | lower_torque_thresholds_acceleration: [60., 60., 60., 60., 50., 40., 30.]; 153 | upper_torque_thresholds_acceleration: [60., 60., 60., 60., 50., 40., 30.]; 154 | lower_force_thresholds_nominal: [60., 60., 60., 30., 30., 30.]; 155 | upper_force_thresholds_nominal: [60., 60., 60., 30., 30., 30.]; 156 | lower_force_thresholds_acceleration: [60., 60., 60., 30., 30., 30.]; 157 | upper_force_thresholds_acceleration: [60., 60., 60., 30., 30., 30.]; 158 | }; 159 | plug_in_params: { 160 | amplitude_factor_translation: 1.0; 161 | amplitude_factor_rotation: 1.0; 162 | impedance_factor: 4.0; 163 | F_z: 10.0; 164 | }; 165 | duration: 15.0; 166 | tolerance: [0.015, 0.015, 0.001]; 167 | } 168 | -------------------------------------------------------------------------------- /3_FCI/standalone/src/plug_in_controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace plug_in_controller { 15 | 16 | void run(const std::string& robot_ip, 17 | const Parameters plug_in_params, 18 | const double duration, // in seconds 19 | const std::array target_position_array, 20 | const std::array target_tolerance_array) { 21 | // Parameters for stopping criterion 22 | Eigen::Vector3d target_position(Eigen::Vector3d::Map(target_position_array.data())); 23 | Eigen::Vector3d target_tolerance(Eigen::Vector3d::Map(target_tolerance_array.data())); 24 | 25 | // Compliance parameters 26 | Eigen::MatrixXd cartesian_stiffness(6, 6), cartesian_damping(6, 6); 27 | cartesian_stiffness.setZero(); 28 | cartesian_stiffness.topLeftCorner(3, 3) 29 | << plug_in_params.translational_stiffness * Eigen::MatrixXd::Identity(3, 3); 30 | cartesian_stiffness.bottomRightCorner(3, 3) 31 | << plug_in_params.rotational_stiffness * Eigen::MatrixXd::Identity(3, 3); 32 | cartesian_damping.setZero(); 33 | cartesian_damping.topLeftCorner(3, 3) 34 | << 2.0 * sqrt(plug_in_params.translational_stiffness) * Eigen::MatrixXd::Identity(3, 3); 35 | cartesian_damping.bottomRightCorner(3, 3) 36 | << 2.0 * sqrt(plug_in_params.rotational_stiffness) * Eigen::MatrixXd::Identity(3, 3); 37 | 38 | // Force parameters 39 | Eigen::VectorXd force_error_integral(6); 40 | // force control P, I gain 41 | double k_p{0.0}; 42 | double k_i{1.0}; 43 | 44 | franka::Robot robot(robot_ip); 45 | // load the kinematics and dynamics model 46 | franka::Model model = robot.loadModel(); 47 | 48 | // initial robot state 49 | franka::RobotState initial_state = robot.readOnce(); 50 | Eigen::Map> force_initial(initial_state.O_F_ext_hat_K.data()); 51 | force_error_integral.setZero(); 52 | 53 | // equilibrium point is the initial position 54 | Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data())); 55 | Eigen::Vector3d position_d(initial_transform.translation()); 56 | Eigen::Quaterniond orientation_initial(initial_transform.linear()); 57 | 58 | double time = 0.0; 59 | 60 | // define callback for the torque control loop 61 | std::function 62 | plug_in_control_callback = 63 | [&](const franka::RobotState& robot_state, franka::Duration period) -> franka::Torques { 64 | time += period.toSec(); 65 | 66 | // get state variables 67 | std::array coriolis_array = model.coriolis(robot_state); 68 | std::array jacobian_array = 69 | model.zeroJacobian(franka::Frame::kEndEffector, robot_state); 70 | 71 | // convert to Eigen 72 | Eigen::Map> coriolis(coriolis_array.data()); 73 | Eigen::Map> jacobian(jacobian_array.data()); 74 | Eigen::Map> force(robot_state.O_F_ext_hat_K.data()); 75 | Eigen::Map> dq(robot_state.dq.data()); 76 | Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); 77 | Eigen::Vector3d position(transform.translation()); 78 | Eigen::Quaterniond orientation(transform.linear()); 79 | 80 | // Compute desired orientation of equilibrium pose and desired forces 81 | // wiggle motion 82 | Eigen::AngleAxisd angle_axis_wiggle_x; 83 | angle_axis_wiggle_x.axis() << 1, 0, 0; 84 | angle_axis_wiggle_x.angle() = sin(2.0 * M_PI * time * plug_in_params.wiggle_frequency_x) * 85 | plug_in_params.wiggle_amplitude_x; 86 | Eigen::AngleAxisd angle_axis_wiggle_y; 87 | angle_axis_wiggle_y.axis() << 0, 1, 0; 88 | angle_axis_wiggle_y.angle() = sin(2.0 * M_PI * time * plug_in_params.wiggle_frequency_y) * 89 | plug_in_params.wiggle_amplitude_y; 90 | Eigen::Quaterniond wiggle_x(angle_axis_wiggle_x); 91 | Eigen::Quaterniond wiggle_y(angle_axis_wiggle_y); 92 | Eigen::Quaterniond orientation_d(wiggle_y * (wiggle_x * orientation_initial)); 93 | 94 | // desired forces 95 | Eigen::VectorXd desired_force(6); 96 | desired_force.setZero(); 97 | desired_force(2) = -plug_in_params.desired_force; 98 | 99 | // Compute error to desired equilibrium pose 100 | // position error 101 | Eigen::Matrix error; 102 | error.head(3) << position - position_d; 103 | 104 | // orientation error 105 | if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) { 106 | orientation.coeffs() << -orientation.coeffs(); 107 | } 108 | // "difference" quaternion 109 | Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d); 110 | error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z(); 111 | // Transform to base frame 112 | error.tail(3) << -transform.linear() * error.tail(3); 113 | 114 | // Compute error to desired force removing initial bias 115 | Eigen::VectorXd force_error; 116 | force_error = desired_force - force + force_initial; 117 | force_error_integral = force_error_integral + period.toSec() * force_error; 118 | 119 | // Compute control 120 | Eigen::VectorXd force_control(6), tau_force(7), tau_cart(7), tau_cmd(7); 121 | 122 | // Force control term 123 | force_control = desired_force + k_p * force_error + k_i * force_error_integral; 124 | force_control << 0, 0, force_control(2), 0, 0, 0; 125 | tau_force = jacobian.transpose() * force_control; 126 | 127 | // Cartesian control term 128 | tau_cart << jacobian.transpose() * 129 | (-cartesian_stiffness * error - cartesian_damping * (jacobian * dq)); 130 | 131 | // Commanded torque 132 | tau_cmd << tau_cart + tau_force + coriolis; 133 | 134 | std::array tau_d_array{}; 135 | Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_cmd; 136 | 137 | // check if motion is completed 138 | Eigen::Matrix target_error((target_position - position).cwiseAbs()); 139 | bool is_inserted = 140 | (target_error[0] <= target_tolerance[0] && target_error[1] <= target_tolerance[1] && 141 | target_error[2] <= target_tolerance[2]); 142 | if (time <= duration && is_inserted) { 143 | return franka::MotionFinished(franka::Torques(tau_d_array)); 144 | } 145 | if (time > duration) { 146 | throw franka::Exception("Timeout: Plug in controller failed!"); 147 | } 148 | return tau_d_array; 149 | }; 150 | 151 | robot.control(plug_in_control_callback); 152 | } 153 | 154 | } // namespace plug_in_controller 155 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/bundles/ride_tutorial/sources/plug_in.lf: -------------------------------------------------------------------------------- 1 | plug_in { 2 | port success child("monitor_pose").port("is_inserted") 3 | port error child("move_to_socket_pose").port("error") or 4 | child("set_move_configuration").port("error") or 5 | child("wiggle").port("error") or 6 | child("wiggle").port("success") or 7 | child("press").port("error") or 8 | child("press").port("success") 9 | 10 | clientData { 11 | description: @{ 12 | This statemachine inserts a plug into a hole. Therefore it first moves to a socket pose. 13 | Then the robot executes a wiggle motion and presses in the end effector`s z-direction 14 | to insert the plug. We assume that the plug is inserted when 15 | the robot pose reaches the hole pose within an tolerance. 16 | }@; 17 | } 18 | 19 | parameterType { 20 | -- parameter for socket motion 21 | { 22 | []{ 23 | {[16]float pose;} pose; 24 | bool point2point; 25 | float cartesian_velocity_factor; 26 | float cartesian_acceleration_factor; 27 | float cartesian_deceleration_factor; 28 | float q3; 29 | } poses; 30 | { 31 | [6]int K_x; 32 | } cartesian_impedance; 33 | { 34 | [7]int K_theta; 35 | } joint_impedance; 36 | { 37 | [7]float lower_torque_thresholds_nominal; 38 | [7]float upper_torque_thresholds_nominal; 39 | [7]float lower_torque_thresholds_acceleration; 40 | [7]float upper_torque_thresholds_acceleration; 41 | [6]float lower_force_thresholds_nominal; 42 | [6]float upper_force_thresholds_nominal; 43 | [6]float lower_force_thresholds_acceleration; 44 | [6]float upper_force_thresholds_acceleration; 45 | } safety; 46 | int controller_mode; 47 | } socket_motion; 48 | -- wiggle motion for insertion 49 | { 50 | [6]{ 51 | int type; 52 | float a_0; 53 | [5]float a_n; 54 | [5]float b_n; 55 | float T_fourier; 56 | } profiles; 57 | float duration; 58 | int frame_selection; 59 | float safety_distance_x; 60 | float safety_distance_y; 61 | float safety_distance_z; 62 | float safety_distance_theta; 63 | } wiggle_motion; 64 | -- force profile for insertion 65 | { 66 | [6]float F_d; 67 | float duration; 68 | float safety_distance_x; 69 | float safety_distance_y; 70 | float safety_distance_z; 71 | float safety_distance_theta; 72 | float force_tolerance; 73 | } force; 74 | -- move configuration during the actual insertion: high collision thresholds, 75 | -- low stiffness (depends on high-level parameter impedance_factor) 76 | { 77 | -- cartesian impedance during insertion 78 | { 79 | [6]int K_x; 80 | } cartesian_impedance; 81 | -- joint impedance during insertion, affecting nullspace if a cartesian controller is used 82 | { 83 | [7]int K_theta; 84 | } joint_impedance; 85 | { 86 | [7]float lower_torque_thresholds_nominal; 87 | [7]float upper_torque_thresholds_nominal; 88 | [7]float lower_torque_thresholds_acceleration; 89 | [7]float upper_torque_thresholds_acceleration; 90 | [6]float lower_force_thresholds_nominal; 91 | [6]float upper_force_thresholds_nominal; 92 | [6]float lower_force_thresholds_acceleration; 93 | [6]float upper_force_thresholds_acceleration; 94 | } safety; 95 | } insertion_move_configuration; 96 | -- end effector pose when plug is inserted 97 | [16]float hole_pose; 98 | -- tolerance between the end effector pose 99 | -- and hole pose to check if plug is successfully inserted [m] 100 | [3]float tolerance; 101 | } 102 | 103 | resultType { 104 | string error_cause; 105 | } 106 | 107 | --> move_to_socket_pose <- move_via_with_move_configuration { 108 | port success -> set_move_configuration 109 | } where parameter.socket_motion 110 | 111 | -- set collisions thresholds and stiffness for the actual insertion 112 | set_move_configuration <- set_move_configuration { 113 | port success -> insert 114 | } where parameter.insertion_move_configuration 115 | 116 | -- Execute the wiggle motion, apply force in end effector's z direction 117 | -- and monitor end effector pose in parallel. If hole pose is not reached 118 | -- after a certain amount of time, the plug in state machine failed. Otherwise 119 | -- we assume the plug is inserted and the state machine finishes with success. 120 | barrier insert { 121 | -> wiggle 122 | -> press 123 | -> monitor_pose 124 | } 125 | 126 | -- Execution of the wiggle motion. The robot follows a fourier profile. 127 | wiggle <- move_velocity { 128 | } where parameter.wiggle_motion 129 | 130 | -- Apply force in end effector's z-direction for a certain amount of time. 131 | press <- apply_force { 132 | } where parameter.force 133 | 134 | -- Monitor current end effector pose and check if hole pose is reached within an tolerance. 135 | -- If hole pose is reached we assume that the plug is successfully inserted 136 | monitor_pose { 137 | port is_inserted result~=nil and result 138 | 139 | parameterType{ 140 | [16]float target_pose; 141 | [3]float tolerance; 142 | } 143 | 144 | resultType bool 145 | 146 | action service("robot").event("sensor_data") ~= nil @{ 147 | local current = service("robot").event("sensor_data").O_T_EE.pose 148 | local x = abs(parameter.target_pose[13] - current[13]) 149 | local y = abs(parameter.target_pose[14] - current[14]) 150 | local z = abs(parameter.target_pose[15] - current[15]) 151 | if x < parameter.tolerance[1] and y < parameter.tolerance[2] and z < parameter.tolerance[3] then 152 | setResult(true) 153 | end 154 | }@ 155 | } where { 156 | target_pose: parameter.hole_pose; 157 | tolerance: parameter.tolerance; 158 | } 159 | 160 | -- Check for triggered ports and set result accordingly. 161 | 162 | action child("move_to_socket_pose").port("error") @{ 163 | setResult({error_cause = child("move_to_socket_pose").result.error_cause}) 164 | }@ 165 | 166 | action child("set_move_configuration").port("error") @{ 167 | setResult({error_cause = child("set_move_configuration").result.error_cause}) 168 | }@ 169 | 170 | action child("wiggle").port("error") @{ 171 | setResult({error_cause = child("wiggle").result.error_cause}) 172 | }@ 173 | 174 | action child("press").port("success") @{ 175 | setResult({error_cause = "Insertion failed: Timeout."}) 176 | }@ 177 | 178 | action child("wiggle").port("success") @{ 179 | setResult({error_cause = "Insertion failed: Timeout."}) 180 | }@ 181 | 182 | action child("press").port("error") @{ 183 | setResult({error_cause = child("press").result.error_cause}) 184 | }@ 185 | 186 | } 187 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # A Tutorial on the Reference Platform for AI and Robotics (AIR) from Franka Emika 2 | 3 | The Franka Emika Panda Series is the first series of industrial AI-enabled tactile robot platforms. Beyond industrial use, the systems can be seamlessly expanded to fulfill the demands of *research and education* across all robotics and AI disciplines. 4 | 5 | In this repository you will find the accompanying code of the Reference Platform Paper. If this code helps you in your research, please cite. 6 | 7 | In addition, the researchers at Munich School of Robotics and Machine Learning (MSRM) have prepared a series of comprehensive tutorial videos which can be found [here](https://www.youtube.com/playlist?list=PLeVqiD9lLCrlxLJI3Eibi-je2fNqwt1am). 8 | 9 | ## Three programming interfaces 10 | 11 | ![](fig/panda_programming_interfaces_gs.png "Three programming interfaces to cover the needs of robotics and AI research.") 12 | 13 | 14 | The AIR Platfrom provides three different programming interfaces: 15 | 16 | * **Desk**, a high level App-based user interface for easy and fast task programming. *Desk*, together with the Research Apps is conceptualized to be the right interface for high-level programs, such as human-robot interaction studies or for rapid prototyping quick ideas. 17 | * **RIDE**, a command-based programming environment to create high performance robot skills that enables programming custom Apps and integrating external sensors. *RIDE* is the most suitable programming interface for research that does not require per-millisecond reactions, such as task and path planning or perception-based manipulation. 18 | * The **FCI**, a 1 kHz low-level torque and position control interface that exploits the also available Lagrangian dynamic robot model. It is the right 19 | interface to explore new low-level planning and control schemes. 20 | 21 | #### System Architecture 22 | 23 | Before getting started, get familiar with the system architecture shown in the following figure: 24 | 25 | ![](fig/overview_system_architecture.png "System architecture.") 26 | 27 | The *Franka Operating System* running on Franka Emika robots has the following main components: 28 | 29 | * *State machines*, which are the basic units of execution in the system. They call operations from services, execute scripts, and may contain other state machines. All Apps as seen in *Desk* are state machines. They are grouped together into *bundles*, which also store related resources such as icons. State machines are programmed in a domain-specific language called *Lingua Franka* and stored in .lf files. 30 | 31 | * *Services* are nodes which run on *Control* (internal services) or any external computer (external services). They communicate with the core via the *race-com* library and can provide operations and publish events periodically. The most relevant service is the *robot service*, which provides all operations to command the robot such as move or applyForce. 32 | 33 | * The *Core* orchestrates the execution of state machines, stores installed bundles and ensures that newly uploaded bundles are consistent with existing ones. It also implements three system interfaces: 34 | * The *Franka Application Interface* (FAI), the interface used by *Desk* to command and monitor the system. 35 | * The *Franka Professional Interface* (FPI), the interface used by *ride-cli* to manipulate, run and install state machines and Apps in the system. 36 | * The *Franka Service Interface* (FSI), the interface used by the *race-com* library to connect external services to the system. 37 | 38 | The *Application Software* represent the access point for users. There are currently two clients in the system: 39 | 40 | * *Desk*, which can start the execution of timelines~(which are sequences of state machines) and trigger other specific functionalities such as opening the brakes or turning the power on or off. 41 | * *ride-cli*, the main App development tool, which provides a command-line interface to start, compile, install, monitor and debug the execution of state machines. 42 | 43 | 44 | Note that the *FCI*, the low-level 1 kHz interface to the robot bypasses the Franka Operating System and represents a direct UDP-based connection to the real-time robot control. 45 | 46 | ## The tutorial 47 | This tutorial showcases the wide range of programming possibilities that the Franka Emika's AIR Platform offers. We strongly recommend to read first the *Reference Platform Paper* before continuing. The tutorial is divided in 4 lessons and built around a plug in task, solved with all three programming interfaces: 48 | 49 | * The [first lesson](1_Desk/), [`1_Desk`](1_Desk/), shows how to quickly prototype the plug in task in *Desk* and how to start tasks from the web interface or the command line tool. 50 | * The [second lesson](2_RIDE/), [`2_RIDE`](2_RIDE/), implements a custom and more capable plug in App with *RIDE*. It then adds an external service simulating the integration of a camera and incorporates it into the App. 51 | * The [third lesson](3_FCI/), [`3_FCI`](3_FCI/), implements a custom plug in controller as a Cartesian impedance controller with an additional term regulating the contact force. It then integrates this custom controller into an App. 52 | * The [fourth lesson](4_Learning/), [`4_Learning`](4_Learning/), implements an on robot learning algorithm that learns the optimal control parameters for the custom App and the custom controller presented in the second and third lesson respectively. 53 | 54 | 55 | ## RIDE CLI 56 | The following table lists some helpful commands that might be used during the tutorial. 57 | 58 | | Command | Description | 59 | |----------------------------------------|---------------------------------------------| 60 | | `ride login ` | Connect to the core. | 61 | | `ride bundle list` | List installed bundles. | 62 | | `ride node list` | List root nodes (installed state machines). | 63 | | `ride service list` | List connected services. | 64 | | `ride execution start ` | Start execution of a state machine. | 65 | | `ride execution stop` | Stop execution gracefully. | 66 | | `ride log` | Display the log message feed from the core. | 67 | | `ride service echo ` | Print service events relayed by the core. | 68 | | `ride execution trace` | Trace execution of a state machine. | 69 | 70 | ## Requirements 71 | 72 | * Ubuntu with PREEMP_RT patch according to [Minimum System and Network Requirements](https://frankaemika.github.io/docs/requirements.html) 73 | * minumum cmake version: cmake >= 3.5 74 | * Robot System version 4.X 75 | * RIDE version 3.X 76 | * RaceCom version 14.X 77 | * libfranka version 0.8.X 78 | 79 | ## Tutorial paper 80 | 81 | **The Franka Emika Robot: A Reference Platform for Robotics Research and Education** paper is located [here](https://ieeexplore.ieee.org/document/9721535). 82 | 83 | ## License 84 | 85 | `the_reference_platform` is licensed under the [Apache 2.0 license][apache-2.0]. 86 | 87 | [apache-2.0]: https://www.apache.org/licenses/LICENSE-2.0.html 88 | -------------------------------------------------------------------------------- /3_FCI/README.md: -------------------------------------------------------------------------------- 1 | # *FCI* 2 | This tutorial shows how to program with the *FCI* as described in the tutorial paper in Section III-C. 3 | The *FCI* offers a 1kHz real-time transparent interface to the robot that allows to run custom controllers on an external computer running Linux with the PREEMPT RT patch. 4 | It can either fully circumvent *Control* or rely on the internal controllers to follow joint or Cartesian space trajectories at 1 kHz specified with the motion interfaces. Specifically, the *FCI* also provides joint position, joint velocity, Cartesian pose and Cartesian velocity interfaces. *libfranka*, the open source C++ library for the client-side of the interface, provides also access to all sensor measurements and the robot model library, which includes the kinematic and Lagrangian dynamic model parameters. This tutorial will show how to implement a standalone custom controller and then how to embed this custom controller into an App. 5 | 6 | **Note:** For more information about the *FCI* check out its [documentation](https://frankaemika.github.io/docs/index.html). 7 | 8 | ## Installation 9 | Make sure that libfranka, *RIDE* and *RaceCom* are installed on your computer. To install libfranka follow these [instructions](https://frankaemika.github.io/docs/installation_linux.html). To install the necessary state machines for the *RIDE* integration, make sure that you are connected to *Control* (The *FCI* will not work if you are connected to the *Arm* base!) and logged into the *Core*: 10 | 11 | ```sh 12 | ride login 13 | ``` 14 | 15 | Create your build folder by running the following command on the root of the repository (not in the `3_FCI` folder!): 16 | 17 | ```sh 18 | mkdir build && cd build 19 | ``` 20 | 21 | Run cmake with the following flags in order to build the *FCI* tutorial 22 | 23 | ```sh 24 | cmake -DBUILD_RIDE=OFF -DBUILD_FCI=ON -DBUILD_LEARNING=OFF -DCMAKE_BUILD_TYPE=Release -DFranka_DIR= .. 25 | ``` 26 | 27 | and finally compile the *FCI* tutorial with 28 | 29 | ```sh 30 | make 31 | ``` 32 | 33 | To compile the state machines and Apps and upload them to your robot run 34 | 35 | ```sh 36 | make fci_tutorial_statemachines 37 | ``` 38 | 39 | After installing the bundle the `Plug In FCI` App should appear in the app pane. Note that the App is grey shaded as long as the `fci_service` is not running. 40 | 41 | ## Standalone 42 | To program a plug in task with the *FCI* we implement a Cartesian impedance controller with an additional term that regulates the desired contact force. For details on the mathematical background, please check the [Tutorial Paper](../README.md#tutorial-paper). The controller is implemented in `plug_in_controller.cpp/.h` as a reusable function which is used in `plug_in_controller_standalone.cpp` to run the controller. The controller finishes when the `hole_pose` is reached within an certain tolerance and time. Otherwise it throws an `franka_exception` which stop the robot control. For more information on the controller have look at the paper in Section III-C. 43 | 44 | #### Running the custom plug in controller 45 | 46 | To run any *FCI* controller first make sure that your PC is running a real-time kernel and that it's connected to *Control*.
47 | 48 | **Warning:** Make sure that end effector is in contact with a horizontal surface before running this code. The controller regulates the contact force along the z-axis of the robot's end-effector and if no object is holding it, the robot will move down! 49 | 50 | To run the plug in controller go to the root directory of the repository, activate the external activation device and run 51 | 52 | ```sh 53 | ./build/3_FCI/standalone/plug_in_controller_standalone 54 | ``` 55 | 56 | **Note:** You can change the control parameters in `plug_in_controller_standalone.cpp` and observe its impact to the behavior of the robot. Make sure you rebuild the plug in controller every time you make a change. The parameters are described in the header `plug_in_controller.h`. 57 | 58 | ## *RIDE* integration 59 | An *FCI* program can be also integrated into a state machine or an App. To do so we will need (1) an external *RIDE* service with an operation that runs the *FCI* controller and (2) a state machine that calls the operation. The following figure illustrates the interplay of interfaces in the system architecture and the involved bundles. 60 | 61 | ![](fig/fci_service_structure_bundles.png "Interplay of components and bundles of the RIDE integration of the FCI service.") 62 | 63 | You can find the corresponding files in the `ride_integration` directory. 64 | 65 | 1. ***FCI* service:** 66 | The implementation of the *RaceCom* service `fci` is located in `services/src/fci_service.cpp`. It has an operation `plugIn` that runs the *FCI* plug in controller described in the standalone section of this tutorial. The operation is defined in the `services/msg/PlugIn.op` file. It specifies an operation that accepts the plug in controller parameters and returns nothing in case of success and a string including the error message in case of any errors. 67 | 68 | 2. **State machine:** 69 | The state machines are very similar to the plug in state machines of the `ride_tutorial` bundle. It also follows the same multi-layered implementation, but in this case the wrapper layer is not necessary because we have to convert less parameters from the context menu. Specifically, the `fci_tutorial` bundle located in the `statemachines/bundles` folder consists of the following state machines: 70 | 71 | 1. *The App layer `plug_in_fci_app.lf`:*
72 | This state machine implements the *context menu* and handles all parameters such as the socket pose, the hole pose and the plug in controller parameters. It then performs the plug in by calling the `plug_in_fci` statemachine. 73 | 74 | 2. *The execution layer `plug_in_fci.lf`:*
75 | This statemachine executes the plug in. It's the same code as in the `plug_in` state machine of the `ride_tutorial` bundle, but instead of the `insert` state, the operation `plugIn` of the `fci` service is called to run the controller. 76 | 77 | 78 | ### Running the Plug in *FCI* App 79 | If you successfully installed the `fci_tutorial` bundle you should see the `Plug In FCI` App (most probably grey shaded) in the App area of *Desk*.
80 | Make sure you are connected to *Control* and do the following: 81 | 82 | 1. Run the `fci` service:
83 | Open a terminal, go to the root of the repository and run 84 | 85 | ```sh 86 | ./build/3_FCI/ride_integration/services/fci_service 11511 87 | ``` 88 | 89 | with the following arguments: 90 | 91 | * ``: is IP address of the robot (Your preconfigured shopfloor IP). 92 | * ``: is the network interface used by your computer to reach the robot. You can check your network interfaces with the `ip a` command. If you are using an ethernet adapter, it will be named `enpXXXX`. Wireless adapters are denoted as `wlpXXX`. Ask your system administrator if you can't figure out your network interface. 93 | 94 | If everything went well, the service should be available and will appear listed when you enter the following command 95 | 96 | ```sh 97 | ride cli services 98 | ``` 99 | 100 | In addition, the `Plug In FCI` App should now appear colored in Desk. The reason for that is that, for each App, Desk checks if the services that it requires are connected to the robot. If a service is missing, the App will appear grey shaded and will not be executable. In our example, after connecting the `fci` service the App should fulfill all requirements and should appear colored. 101 | 102 | 2. Run the `Plug In FCI` App in Desk: 103 | 104 | 1. Create a new Task 105 | 106 | 2. *Program the task:* Drag the `Plug In FCI` App into the Timeline of the Task 107 | 108 | 3. *Parameterize the task:* Click on the `Plug In FCI` App and follow the instruction to teach the robot.
109 | **Note:** The expert parameters are preconfigured and needn't to be changed. Anyway you can play around with them and observe how the behavior of the robot changes. 110 | 111 | 4. **Optional:** Add a `Cartesian Motion` App before the `Plug in FCI` App to allow for the experiment to be repeated. Teach the `Cartesian Motion` in a way that it unplugs the plug. 112 | 113 | 5. Activate the external activation device and click on the run button to start the task. **This will finally make the robot move!** 114 | -------------------------------------------------------------------------------- /cmake/ClangTools.cmake: -------------------------------------------------------------------------------- 1 | include(CMakeParseArguments) 2 | 3 | find_program(CLANG_FORMAT_PROG clang-format-6.0 DOC "'clang-format' executable") 4 | if(CLANG_FORMAT_PROG AND NOT TARGET format) 5 | add_custom_target(format) 6 | add_custom_target(check-format) 7 | endif() 8 | find_program(CLANG_TIDY_PROG clang-tidy-6.0 DOC "'clang-tidy' executable") 9 | if(CLANG_TIDY_PROG AND NOT TARGET tidy) 10 | if(NOT CMAKE_EXPORT_COMPILE_COMMANDS) 11 | message(WARNING "Invoke Catkin/CMake with '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON' 12 | to generate compilation database for 'clang-tidy'.") 13 | endif() 14 | 15 | add_custom_target(tidy) 16 | add_custom_target(check-tidy) 17 | endif() 18 | find_program(AUTOPEP8_PROG autopep8 DOC "'autopep8' executable") 19 | if(AUTOPEP8_PROG AND NOT TARGET pep8) 20 | add_custom_target(pep8) 21 | endif() 22 | find_package(Git QUIET) 23 | if(NOT GIT_FOUND) 24 | message(WARNING "Git cannot be found. ") 25 | endif() 26 | find_program(BASENAME_PROG basename DOC "'basename' executable") 27 | if(NOT BASENAME_PROG) 28 | message(WARNING "basename executable cannot be found. ") 29 | endif() 30 | find_program(XARGS_PROG xargs DOC "'xargs' executable") 31 | if(NOT XARGS_PROG) 32 | message(WARNING "xargs executable cannot be found. ") 33 | endif() 34 | find_program(PARALLEL_PROG parallel DOC "'parallel' executable") 35 | if(PARALLEL_PROG) 36 | set(PARALLEL_DELIMITER ":::") 37 | else() 38 | message(WARNING "parallel executable cannot be found. Try with 'sudo apt install parallel'.") 39 | set(PARALLEL_PROG "") 40 | endif() 41 | 42 | # Starting at SEARCH_DIR, look up directory hierarchy for directory containing NAME. 43 | # SEARCH_DIR is a directory. Stop at the first parent directory containing a folder NAME, 44 | # and return the directory in the parent scope in FOUND_DOMINATING_DIR. Return empty if not found. 45 | # 46 | # USAGE: 47 | # locate_dominating_dir( 48 | # NAME 49 | # SEARCH_DIR 50 | # ) 51 | # 52 | # ARGUMENTS: 53 | # NAME 54 | # name of the directory to be found 55 | # SEARCH_DIR 56 | # directory path where the search starts 57 | # 58 | # OUTPUT VARIABLES: 59 | # FOUND_DOMINATING_DIR 60 | # path to the directory where NAME is first found 61 | # 62 | # EXAMPLE: 63 | # locate_dominating_dir( 64 | # NAME .git 65 | # SEARCH_URL ${CMAKE_CURRENT_LIST_DIR} 66 | # ) 67 | # 68 | function(locate_dominating_dir) 69 | cmake_parse_arguments(ARG "" "NAME;SEARCH_DIR" "" ${ARGN}) 70 | if(IS_DIRECTORY ${ARG_SEARCH_DIR}/${ARG_NAME}) 71 | set(FOUND_DOMINATING_DIR "${ARG_SEARCH_DIR}" PARENT_SCOPE) 72 | elseif(EXISTS ${ARG_SEARCH_DIR}/..) 73 | locate_dominating_dir( 74 | NAME ${ARG_NAME} 75 | SEARCH_DIR ${ARG_SEARCH_DIR}/.. 76 | ) 77 | set(FOUND_DOMINATING_DIR ${FOUND_DOMINATING_DIR} PARENT_SCOPE) 78 | endif() 79 | endfunction() 80 | 81 | # Return the git repository name of the calling CMakeLists.txt. 82 | # 83 | # USAGE: 84 | # get_git_repo_name() 85 | # 86 | # OUTPUT VARIABLES: 87 | # GIT_REPO_NAME 88 | # git repository name of the calling CMakeLists.txt 89 | # 90 | function(get_git_repo_name) 91 | if(NOT GIT_FOUND OR NOT BASENAME_PROG OR NOT XARGS_PROG) 92 | return() 93 | endif() 94 | execute_process( 95 | COMMAND ${GIT_EXECUTABLE} config --get remote.origin.url 96 | COMMAND xargs ${BASENAME_PROG} -s .git 97 | WORKING_DIRECTORY "${CMAKE_CURRENT_LIST_DIR}" 98 | OUTPUT_VARIABLE GIT_REPO_NAME 99 | RESULT_VARIABLE GIT_GET_NAME_RESULT 100 | ERROR_QUIET 101 | OUTPUT_STRIP_TRAILING_WHITESPACE) 102 | if(NOT GIT_GET_NAME_RESULT EQUAL "0") 103 | unset(GIT_REPO_NAME) 104 | message(WARNING "${CMAKE_CURRENT_LIST_FILE} is probably not inside git repository.") 105 | endif() 106 | set(GIT_REPO_NAME ${GIT_REPO_NAME} PARENT_SCOPE) 107 | endfunction() 108 | 109 | # Use clang-format to format c/c++/Objective-C code. 110 | # 111 | # USAGE: 112 | # add_format_target(_target 113 | # FILES 114 | # ) 115 | # 116 | # ARGUMENTS: 117 | # _target 118 | # name of the target 119 | # FILES 120 | # group of paths to header and source files 121 | # 122 | # EXAMPLE: 123 | # add_format_target(${PROJECT_NAME} 124 | # FILES ${SOURCES} ${HEADERS} 125 | # ) 126 | # 127 | function(add_format_target _target) 128 | if(NOT CLANG_FORMAT_PROG) 129 | return() 130 | endif() 131 | cmake_parse_arguments(ARG "" "" "FILES" ${ARGN}) 132 | locate_dominating_dir( 133 | NAME .git 134 | SEARCH_DIR ${CMAKE_CURRENT_LIST_DIR} 135 | ) 136 | if (NOT FOUND_DOMINATING_DIR) 137 | set(WORKING_DIR ${CMAKE_CURRENT_LIST_DIR}/..) 138 | else() 139 | set(WORKING_DIR ${FOUND_DOMINATING_DIR}) 140 | endif() 141 | 142 | add_custom_target(format-${_target} 143 | COMMAND ${CLANG_FORMAT_PROG} -i ${ARG_FILES} 144 | WORKING_DIRECTORY ${WORKING_DIR} 145 | COMMENT "Formatting ${_target} source code with clang-format" 146 | VERBATIM 147 | ) 148 | add_dependencies(format format-${_target}) 149 | 150 | add_custom_target(check-format-${_target} 151 | COMMAND ${CLANG_FORMAT_PROG} -output-replacements-xml ${ARG_FILES} | grep " /dev/null && exit 1 || exit 0 152 | WORKING_DIRECTORY ${WORKING_DIR} 153 | COMMENT "Checking ${_target} code formatting with clang-format" 154 | VERBATIM 155 | ) 156 | add_dependencies(check-format check-format-${_target}) 157 | 158 | get_git_repo_name() 159 | if(GIT_REPO_NAME) 160 | if(NOT TARGET format-${GIT_REPO_NAME}) 161 | add_custom_target(format-${GIT_REPO_NAME}) 162 | add_custom_target(check-format-${GIT_REPO_NAME}) 163 | endif() 164 | add_dependencies(format-${GIT_REPO_NAME} format-${_target}) 165 | add_dependencies(check-format-${GIT_REPO_NAME} check-format-${_target}) 166 | endif() 167 | 168 | endfunction() 169 | 170 | # Use clang-tidy to tidy c/c++ code. 171 | # 172 | # USAGE: 173 | # add_tidy_target(_target 174 | # FILES 175 | # DEPENDS 176 | # ) 177 | # 178 | # ARGUMENTS: 179 | # _target 180 | # name of the target 181 | # FILES 182 | # group of paths to c/c++ source files 183 | # DEPENDS 184 | # group of dependency targets 185 | # 186 | # EXAMPLE: 187 | # add_tidy_target(${PROJECT_NAME} 188 | # FILES ${SOURCES} 189 | # DEPENDS dep_target_1 dep_target_2 190 | # ) 191 | # 192 | function(add_tidy_target _target) 193 | if(NOT CLANG_TIDY_PROG) 194 | return() 195 | endif() 196 | 197 | cmake_parse_arguments(ARG "" "" "FILES;DEPENDS" ${ARGN}) 198 | locate_dominating_dir( 199 | NAME .git 200 | SEARCH_DIR ${CMAKE_CURRENT_LIST_DIR}) 201 | if (NOT FOUND_DOMINATING_DIR) 202 | set(WORKING_DIR ${CMAKE_CURRENT_LIST_DIR}/..) 203 | else() 204 | set(WORKING_DIR ${FOUND_DOMINATING_DIR}) 205 | endif() 206 | 207 | add_custom_target(tidy-${_target} 208 | COMMAND ${PARALLEL_PROG} ${CLANG_TIDY_PROG} -fix -p=${CMAKE_BINARY_DIR} ${PARALLEL_DELIMITER} ${ARG_FILES} 209 | WORKING_DIRECTORY ${WORKING_DIR} 210 | DEPENDS ${ARG_DEPENDS} 211 | COMMENT "Running clang-tidy for ${_target}" 212 | VERBATIM) 213 | add_dependencies(tidy tidy-${_target}) 214 | 215 | add_custom_target(check-tidy-${_target} 216 | COMMAND ${PARALLEL_PROG} ${CLANG_TIDY_PROG} -p=${CMAKE_BINARY_DIR} ${PARALLEL_DELIMITER} ${ARG_FILES} | grep . && exit 1 || exit 0 217 | WORKING_DIRECTORY ${WORKING_DIR} 218 | DEPENDS ${ARG_DEPENDS} 219 | COMMENT "Running clang-tidy for ${_target}" 220 | VERBATIM) 221 | add_dependencies(check-tidy check-tidy-${_target}) 222 | 223 | get_git_repo_name() 224 | if(GIT_REPO_NAME) 225 | if(NOT TARGET tidy-${GIT_REPO_NAME}) 226 | add_custom_target(tidy-${GIT_REPO_NAME}) 227 | add_custom_target(check-tidy-${GIT_REPO_NAME}) 228 | endif() 229 | add_dependencies(tidy-${GIT_REPO_NAME} tidy-${_target}) 230 | add_dependencies(check-tidy-${GIT_REPO_NAME} check-tidy-${_target}) 231 | endif() 232 | endfunction() 233 | 234 | # Use autopep8 to format python code with pep8. 235 | # 236 | # USAGE: 237 | # add_pep8_target(_target 238 | # FILES 239 | # ) 240 | # 241 | # ARGUMENTS: 242 | # _target 243 | # name of the target 244 | # FILES 245 | # group of paths to python files 246 | # 247 | # EXAMPLE: 248 | # add_pep8_target(${PROJECT_NAME} 249 | # FILES ${PY_SOURCES} 250 | # ) 251 | # 252 | function(add_pep8_target _target) 253 | if(NOT AUTOPEP8_PROG) 254 | return() 255 | endif() 256 | cmake_parse_arguments(ARG "" "" "FILES" ${ARGN}) 257 | 258 | add_custom_target(pep8-${_target} 259 | COMMAND ${AUTOPEP8_PROG} -i ${ARG_FILES} 260 | WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/.. 261 | COMMENT "Formatting ${_target} source code with autopep8" 262 | VERBATIM) 263 | add_dependencies(pep8 pep8-${_target}) 264 | endfunction() 265 | -------------------------------------------------------------------------------- /2_RIDE/README.md: -------------------------------------------------------------------------------- 1 | # *RIDE* 2 | This tutorial illustrates how to program with *RIDE*, as described in the [Tutorial Paper](../README.md#tutorial-paper) in Section III-B. It consists of the implementation of the `plug_in` and `plug_in_vision` state machines, their corresponding Apps and services. 3 | 4 | ## Installation 5 | Make sure that *RIDE* and *RaceCom* is installed in your computer, you are connected to the *Arm* base or to *Control* and logged into the *Core*: 6 | 7 | ```sh 8 | ride login 9 | ``` 10 | 11 | Create your build folder by running the following command on the root of this repository (not in the `2_RIDE` folder!): 12 | 13 | ```sh 14 | mkdir build && cd build 15 | ``` 16 | 17 | Run cmake with the following flags in order to build the RIDE tutorials 18 | 19 | ```sh 20 | cmake -DBUILD_RIDE=ON -DBUILD_FCI=OFF -DBUILD_LEARNING=OFF .. 21 | ``` 22 | 23 | and finally compile the *RIDE* tutorial with 24 | 25 | ```sh 26 | make 27 | ``` 28 | 29 | To upload the state machines and Apps to the robot run 30 | 31 | ```sh 32 | make ride_tutorial_statemachines 33 | ``` 34 | 35 | After installing the bundle the Apps should appear in the app pane. In addition to that, the bundle should appear listed after entering the following command 36 | 37 | ```sh 38 | ride bundle list 39 | ``` 40 | 41 | Furthermore the individual state machines and Apps should be listed when you enter 42 | 43 | ```sh 44 | ride node list 45 | ``` 46 | 47 | ## Plug in App 48 | The Plug in App is located in the `ride-tutorial` bundle in the `statemachines` directory. Every bundle consists of a `manifest.json` defining the bundle, a resource folder that stores all the icons for the context menu and a source folder, containing the state machines and App files. 49 | The Plug in App of this tutorial is implemented in three different state machine files that can also be seen as layers: 50 | 51 | 1. *The App layer `plug_in_app.lf`:*
52 | This state machine implements the *context menu* where the user can set required parameters in *Desk* like the socket and hole pose. It is also the 'main' execution file: after getting the context menu parameters, it runs the `plug_in_wrapper` state machine which first converts the collected parameters to state machine parameters and then performs the plug insertion. 53 | 2. *The wrapper layer `plug_in_wrapper.lf`:*
54 | This statemachine receives the high-level parameters from the context menus and converts them to the actual parameters required for plug in and then runs the `plug_in` state machine. 55 | 3. *The execution layer `plug_in.lf`:*
56 | This state machine implements the execution of the plug in task given by the following state flow:
57 | 58 | 1. `move_to_socket_pose` - a cartesian motion to the socket_pose with high stiffness for high accuracy and sensitivity 59 | 2. `set_move_configuration` - set collision thresholds and stiffness for the insertion 60 | 3. `insert` - a *barrier* executing the following states in parallel: 61 | 62 | * `wiggle` - execute a wiggle motion in the (x,y) plane 63 | * `press` - apply and regulate a constant force along the z-axis 64 | * `monitor_pose` - monitor the current end-effector pose and check if the hole pose has been reached within a certain tolerance 65 | 66 | This implementation assumes that a plug in is successful when the robot reaches the hole pose within a certain tolerance and maximum time. 67 | 68 | The advantage of such a multi-layer implementation is on the one hand the reusability of individual state machines (e.g. `plug_in` and `plug_in_wrapper`) and the clarity (implementation of context menu, parameter conversion and execution are split in three different files). 69 | 70 | 71 | #### Running the Plug in App 72 | If you installed successfully the `ride_tutorial` the `Plug in` App will appear in the App pane of *Desk*.
73 | To run it follow the subsequent instructions: 74 | 75 | 1. Create a new Task 76 | 2. Program the task: Drag the `Plug in` App into the Timeline of the Task 77 | 3. Parameterize the task: Click on the `Plug in` App and follow the instructions to teach the robot. Note that the context menu that appear in this step are defined in the `plug_in_app.lf` file.
78 | **Note:** The expert parameters are preconfigured and don't need to be changed. However, you can play around with them and see how the behavior of the robot changes. 79 | 4. **Optional:** Add a `Cartesian Motion` App before the `Plug in` App to allow for the experiment to be repeated. Teach the `Cartesian Motion` in a way that it unplugs the plug. 80 | 5. Activate the external activation device and click on the run button to start the task. **This will make the robot move!** 81 | 82 | You can trace the execution in a linux terminal with 83 | 84 | ```sh 85 | ride execution trace 86 | ``` 87 | 88 | and check the logs with 89 | 90 | ```sh 91 | ride log 92 | ``` 93 | 94 | ## Plug in App with vision 95 | The Plug in App described above is capable of performing an insertion for a socket and hole pose taught with *Desk*. Let us assume now that a computer vision module is estimating the socket pose and we would like to use it in our `Plug in` state machine. To realize this we need: (1) a state machine executing the plug in and (2) a service providing the socket pose. 96 | The following figure illustrates the interplay of interfaces in the system architecture and the involved bundles. 97 | 98 | ![](fig/plug_in_vision_structure_bundles.png "Interplay of components and bundles of the RIDE integration of the FCI service.") 99 | 100 | 1. **Socket-pose service**
101 | The `socket-pose` service is located in `services/src/socket_service.cpp`. It implements the `getSocketPose` operation that returns the socket pose and periodically publishes the `socket-info` event with the number of sockets detected. The operation type is defined in the `GetSocketPose.op` file in `services/msg` directory. It specifies an empty operation call that returns a 16-dimensional float array in case of success and a string in case of an error. Similarly, the event type is defined in the `SocketInfo.ev` file which specifies a message given by an integer.
102 | **Note:** The socket-pose and number of sockets are hardcoded. Please adapt this to your setup. 103 | 104 | 2. **State machine**
105 | The state machine is implemented in the `plug_in_vision.lf` file located in the `ride_tutorial` bundle. It consists of the following state flow: 106 | 107 | 1. `check_for_sockets` - subscribes to the event `socket-info` and checks if any sockets are detected. 108 | 2. `get_socket_pose` - calls the operation `getSocketPose` which returns the pose. 109 | 3. `compute_hole_pose` - computes `hole_pose` from `socket_pose` 110 | 4. `plug_in` - converts parameters and inserts the plug into the detected socket by calling `plug_in_wrapper` from above 111 | 112 | Note that this example focuses on how to connect an external service with a state machine. All hypothetical vision related results such as the socket pose are hardcoded. 113 | 114 | 115 | #### Running the Plug in App with vision 116 | Before running the demo you probably want to adapt the hardcoded pose of the `socket-pose` service to your setup. To do so modify the hardcoded `socket_pose` in `services/src/socket_service.cpp` (l. 22) and compile the service again as described in the installation instructions. To adapt the socket pose to your setup, guide the robot to the desired pose and run the following command in a terminal 117 | 118 | ```sh 119 | ride service echo robot sensor_data 120 | ``` 121 | 122 | which echoes all robot sensor data. Copy the output of the `O_T_EE` (end effector pose) and paste it in the `services/src/socket_service.cpp` file (l. 22). After recompiling as described in the installation instructions the service should be providing the new hardcoded pose. 123 | 124 | To run the App do the following: 125 | 126 | 1. Run the service:
127 | Open a terminal, go to the root of the repository and run 128 | 129 | ```sh 130 | ./build/2_RIDE/services/socket_service 11511 131 | ``` 132 | 133 | with the following arguments: 134 | 135 | * ``: is IP address of the robot (`robot.franka.de` if you are connected to the base or your preconfigured robot ip otherwise). 136 | * ``: is the network interface used by your computer to reach the robot. You can check your network interfaces with the `ip a` command. If you are using an ethernet adapter, it will be named `enpXXXX`. Wireless adapters are denoted as `wlpXXX`. Ask your system administrator if you can't figure out your network interface. 137 | 138 | After running the service, it should appear listed when you enter the following command 139 | 140 | ```sh 141 | ride service list 142 | ``` 143 | 144 | 2. Run the `plug_in_vision` state machine:
145 | To run the `plug_in_vision` state machine first activate the external activation device, stop any execution currently running (e.g. idle state machine) by 146 | 147 | ```sh 148 | ride execution stop 149 | ``` 150 | and then 151 | 152 | ```sh 153 | ride execution start plug_in_vision 154 | ``` 155 | 156 | **This will make the robot move!** 157 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/bundles/ride_tutorial/sources/plug_in_app.lf: -------------------------------------------------------------------------------- 1 | plug_in_app { 2 | port Success child("plug_in").port("success") 3 | port Error child("plug_in").port("error") or 4 | child("convert").port("error") 5 | 6 | clientData { 7 | type : "app"; 8 | name : "Plug In"; 9 | color : "hsl(230, 55%, 82%)"; 10 | description: @{ 11 | This app inserts a plug into a hole. It gets high-level parameter from desk 12 | and calls the plug-in-wrapper for converting parameters and inserting the plug. 13 | }@; 14 | requiredParameters: [ 15 | { source: safety; localParameter: safety; } 16 | ]; 17 | image: @{ 18 | 19 | }@; 20 | contextMenu: @{ 21 | 22 | 32 |
33 | 36 | 46 |
47 | 48 | 49 | 50 | 51 |
52 | 53 | 54 | 56 | 68 |
69 | 70 | 71 | 72 | 73 | 74 | 75 |
76 | 77 | 78 |
79 | 80 | 81 | 82 |

83 | 90 | 91 | 92 |

93 | 102 | 103 | 104 | 106 | 111 | 112 |

113 | 121 | 122 | 123 |

124 | 131 | 132 | 133 |

134 | 141 | 142 | 143 |

144 | 151 | 152 | 153 | }@; 154 | } 155 | 156 | parameterType { 157 | -- safety parameters, received from timeline 158 | { 159 | { 160 | float x; 161 | float y; 162 | float z; 163 | float x_rot; 164 | float y_rot; 165 | float z_rot; 166 | } force_thresholds; 167 | { 168 | float j0; 169 | float j1; 170 | float j2; 171 | float j3; 172 | float j4; 173 | float j5; 174 | float j6; 175 | } torque_thresholds; 176 | } safety; 177 | -- pose of socket 178 | { 179 | [16]float pose; 180 | []float joint_angles; 181 | } socket_pose; 182 | -- end effector pose when plug is inserted 183 | { 184 | [16]float pose; 185 | []float joint_angles; 186 | } hole_pose; 187 | { 188 | -- parameter for wiggle motion 189 | float amplitude_factor_translation; 190 | float amplitude_factor_rotation; 191 | -- stiffness of robot during insertion 192 | float impedance_factor; 193 | -- insertion force in z direction of robot's end-effector frame [N] 194 | float F_z; 195 | } plug_in_params; 196 | -- tolerance between the end effector pose 197 | -- and hole pose to check if plug is successfully inserted [m] 198 | [3]float tolerance; 199 | -- maximum insertion time [s] 200 | float duration; 201 | } 202 | 203 | resultType { 204 | string error_cause; 205 | } 206 | 207 | -- convert timeline parameters 208 | --> convert <- convert_safety { 209 | port success -> plug_in 210 | } where { 211 | safety: parameter.safety; 212 | } 213 | 214 | -- plug_in plug 215 | plug_in <- plug_in_wrapper { 216 | } where { 217 | socket_pose: { 218 | pose: parameter.socket_pose.pose; 219 | q3: parameter.socket_pose.joint_angles[3]; 220 | }; 221 | hole_pose: parameter.hole_pose.pose; 222 | plug_in_params: parameter.plug_in_params; 223 | duration: parameter.duration; 224 | tolerance: parameter.tolerance; 225 | safety: child("convert").result.safety; 226 | } 227 | 228 | action child("plug_in").port("error") @{ 229 | setResult(child("plug_in").result) 230 | }@ 231 | 232 | } where { 233 | safety: nil; 234 | socket_pose: nil; 235 | hole_pose: nil; 236 | plug_in_params: { 237 | amplitude_factor_translation: 1.0; 238 | amplitude_factor_rotation: 1.0; 239 | impedance_factor: 7.0; 240 | F_z: 7.0; 241 | }; 242 | duration: 5.0; 243 | tolerance: [0.015, 0.015, 0.0007]; 244 | } 245 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | -------------------------------------------------------------------------------- /4_Learning/README.md: -------------------------------------------------------------------------------- 1 | # Learning 2 | This tutorial shows how to apply *on robot* learning with the Franka Emika AIR Platform as described in the [Tutorial Paper](../README.md#tutorial-paper) in Section IV. 3 | 4 | For both the `plug_in` ([RIDE tutorial](../2_RIDE)) and `plug_in_fci` ([FCI tutorial](../3_FCI)) examples, we implement a procedure that explores and optimizes the control parameters that succeed and at the same time minimize the interaction forces. 5 | 6 | ## Installation 7 | 8 | Make sure that libfranka, *RIDE* and *RaceCom* are installed on your computer. To install libfranka follow these [instructions](https://frankaemika.github.io/docs/installation_linux.html). To install the necessary state machines for the *RIDE* integration, make sure that you are connected to *Control* (The *FCI* will not work if you are connected to the *Arm* base!) and logged into the *Core*: 9 | 10 | ```sh 11 | ride login 12 | ``` 13 | 14 | The learning tutorial depends on services, state machines and controllers from the *RIDE* and *FCI* tutorials. 15 | To build all of them, run in the repository root (not in the `4_Learning` folder!): 16 | 17 | ```sh 18 | mkdir build && cd build 19 | ``` 20 | 21 | Run cmake with the following flags in order to build the learning tutorial 22 | 23 | ```sh 24 | cmake -DBUILD_RIDE=ON -DBUILD_FCI=ON -DBUILD_LEARNING=ON -DCMAKE_BUILD_TYPE=Release -DFranka_Dir= .. 25 | ``` 26 | 27 | and compile with 28 | 29 | ```sh 30 | make 31 | ``` 32 | 33 | To compile all state machines and Apps and upload them to your robot run 34 | 35 | ``` 36 | make ride_tutorial_statemachines && 37 | make fci_tutorial_statemachines && 38 | make learning_tutorial_statemachines 39 | ``` 40 | 41 | After installing the bundles the Apps should appear in the app pane. Note that the Apps are grey shaded as long as the `fci` and the `learning` service are not running. 42 | 43 | ## Learning service 44 | Regardless of the method used, e.g. reinforcement learning or stochastic optimization, all *on robot* learning methods follow the same underlying operational loop where a learning service generate parameters that are then tested and evaluated on the robot. The result is used by the learning service to generate the next parameter set and this procedure is iterated until convergence. 45 | 46 | The implementation of the `learning` service is located in `services/src/learning_service.cpp`. 47 | In our prototypical implementation, the `learning` service provides the following 4 operations: 48 | 49 | * `init` initializes the learning algorithm. This operation is defined by the `Init.op` and receives the hyperparameters of the learning algorithm. 50 | * `getParams` computes and returns the parameters for the next experimental trial. This operation is defined in the `GetParam.op` file. 51 | * `setResult` receives the cost function value and a boolean representing the insertion success. This operation forwards the result to the learning algorithm. In addition, the operation returns a boolean indicating if the learning process has finished/converged. This operation is defined in the `SetResult.op` file. 52 | * `getBestParams` returns the best set of parameters that has been experienced so far by the learning algorithm. More precisely, the best parameters are the ones that led to a successful execution task and also minimize the interaction forces. This operation is defined by the `GetBestParams.op` file. 53 | 54 | #### Learning algorithm 55 | This tutorial provides the skeleton for the implementation of on robot learning approaches. For the sake of simplicity, the learning algorithm implemented is *random search*, which *randomly* selects parameters, tests and evaluates them.
56 | It is implemented as a C++ class that is located in the `service` directory. The methods of this class are used in the `learning` service to implement the operations. The `learning` service is implemented in such a way that it can learn any state machine or App as it interacts with the state machine through an array of doubles, i.e. it searches for the optimal array of parameters with no prior knowledge at all. 57 | Adapting the implementation to a more capable algorithm should be straightforward. 58 | 59 | ## Plug in learning 60 | This examples shows how to learn the parameters of a state machine, in this case the `plug_in` state machine of the `ride_tutorial` bundle. 61 | The learning loop is implemented in a state machine. The state machine uses the `learning` service for learning and `RIDE` to test and evaluate parameters on the robot. 62 | The following figure illustrates the interplay of interfaces in the system architecture and the involved bundles. 63 | 64 | ![](fig/ride_learning_structure_bundles.png "Interplay of components and bundles of the RIDE learning example with the external learning service.") 65 | 66 | Specifically, the `ride_learning_tutorial` bundle located in the `statemachine/source` directory is composed by the following state machines: 67 | 68 | * *The App layer `plug_in_learning_ride_app.lf`:*
69 | This state machine implements the *context_menu*, gathers all required parameters from *Desk* and runs the App. It consists of the following two states: 70 | 71 | 1. `learn_or_run`: This state evaluates a boolean that indicates if the user wants to learn or just run the plug in statemachine. The user can toggle this variable in the context menu. 72 | 73 | 2. Depending on the selected mode in the previous step, it will: 74 | 75 | a) **Learning:** 76 | Run the `plug_in_learning_ride` state machine explained below.
77 | 78 | b) **Execution:** 79 | Run the following two states: 80 | 81 | 1. `get_best_params`: This state requests the `getBestParams` operation to get best previously learned parameters currently stored in the `learning` service. 82 | 83 | 2. `plug_in`: This runs the `plug_in_wrapper` state machine from the `ride_tutorial` bundle with the acquired parameters. 84 | 85 | * *The learning layer: `plug_in_learning_ride.lf`:* 86 | This state machine implements the learning loop, given by the following states: 87 | 88 | 1. `init`: The first state initializes the learning algorithm with the hyperparameters using the `init` operation of the `learning` service. 89 | 2. *Learning loop:* The following states will be sequentially executed 90 | 91 | 1. `reset`: This state resets the robot and its environment. It moves the robot to a home pose and clears all errors. 92 | 2. `get_params`: This state request the `getParams` operation for getting the parameters for the next experimental trial. 93 | 3. `explore`: This state runs the plug in App with the parameters from the `get_params` state. It executes the `plug_in_wrapper` state machine of the `ride_tutorial` bundle using these parameters and at the same time the robot state is observed to compute the cost function (here the interaction forces are cumulated). 94 | 4. `set_result`: Regardless of the outcome of the `plug_in_wrapper` state machine, this state returns the result of the cost function and an boolean representing the insertion success by calling the `setResult` operation of the `learning` service. Depending on the operation's response it also decides if the learning should continue or not. If it continues, the state machine jumps to the `reset` state and executes another iteration of the learning loop. Otherwise the learning is done and the state machine finishes with success. 95 | 96 | #### Running the Plug in learning App 97 | If you successfully installed the `ride_learning_tutorial` bundle you should see the `Plug In Learning` App (most probably grey shaded) in the app area of Desk. To 98 | 99 | 1. Run the `learning` service:
100 | Open a terminal and go to the root of the repository and run 101 | 102 | ```sh 103 | ./build/4_Learning/services/learning_service 11511 104 | ``` 105 | 106 | with the following arguments: 107 | 108 | * ``: is the IP address of the robot (`robot.franka.de` if you are connected to the *Arm* base or your preconfigured robot ip otherwise). 109 | * ``: is the network interface used by your computer to reach the robot. You can check your network interfaces with the `ip a` command. If you are using an ethernet adapter, it will be named `enpXXXX`. Wireless adapters are denoted as `wlpXXX`. Ask your system administrator if you can't figure out your network interface. 110 | 111 | After running the service, it should appear listed when you enter the following command 112 | 113 | ```sh 114 | ride cli services 115 | ``` 116 | 117 | In addition, the `Plug In Learning` App should now be colored in Desk. 118 | 119 | 2. Run the `Plug In Learning` App in Desk: 120 | 121 | 1. Create new Task 122 | 2. *Program the task:* Drag the `Plug In Learning` App into the Timeline of the Task 123 | 3. *Parameterize the task:* Click on the `Plug In Learning` App and follow the instruction to teach the robot. 124 | 4. Learn the task: 125 | 126 | 1. Go to the *Mode* tab and move the slider to *Learning*. Then set the 127 | Hyperparameter of the learning algorithm.
128 | **Note:** The Hyperparameters are already configured. The only thing you should change is the number of iterations. For a short demo, 10 iterations are a good start. 129 | 2. Activate the external activation device and click on the run button. 130 | 5. Run the learned task:
131 | 1. Go to the *Mode* tab and move the slider to *Execution*. 132 | 2. Activate the external activation device and click on the run button. 133 | 134 | ## Plug in *FCI* learning 135 | 136 | In this example we will learn the parameters of a custom FCI controller. It is the same code used in the previous subsection, but instead of exploring the `plug_in_wrapper` state machine of the `ride_tutorial` bundle, the `plug_in_learning_fci` explores the `plug_in_fci` state machine of the `fci_tutorial` bundle. The implementation of this example is collected in the `fci_learning_tutorial` bundle which is located in the `statemachines/source` folder. 137 | ![](fig/fci_learning_structure_bundles.png "Interplay of components and bundles of the FCI learnig example, with two external services: the FCI service and the learning service.") 138 | 139 | #### Running the Plug in *FCI* learning App 140 | If you successfully installed the `fci_learning_tutorial` bundle you should see the `Plug In FCI Learning` App (most probably grey shaded) in the app area of Desk.
141 | 142 | 1. Run the `learning` service as explained in the [*Running the Plug in learning App* subsection](#Running-the-Plug-in-learning-App). 143 | 2. Run the `fci` service:
144 | From your repository root, run: 145 | 146 | ```sh 147 | ./build/3_FCI/ride_integration/services/fci_service 11511 148 | ``` 149 | 150 | Now the services are available and you should be able to see them when you enter the following command 151 | 152 | ```sh 153 | ride cli services 154 | ``` 155 | 156 | In addition to that, the `Plug In FCI Learning` App should now be colored in Desk. 157 | 158 | 3. Run the `Plug In FCI Learning` App in Desk. It operates the same way as the `Plug In Learning` App described in the [*Running the Plug in learning App* subsection](#Running-the-Plug-in-learning-App) 159 | -------------------------------------------------------------------------------- /4_Learning/statemachines/bundles/ride_learning_tutorial/sources/plug_in_learning_ride.lf: -------------------------------------------------------------------------------- 1 | plug_in_learning_ride { 2 | port success child("set_result").port("learning_done") 3 | port error child("init").port("error") or 4 | child("reset").port("error") or 5 | child("get_params").port("error") or 6 | child("set_result").port("error") 7 | 8 | clientData{ 9 | description: @{ 10 | This statemachine learns the parameters of the plug_in app. 11 | The optimizer is implemented in the racecom service 'learning' 12 | and includes operations like init, get_params and set_result. 13 | The statemachine calls the corresponding operations 14 | to init the optimizer, get next parameters to explore and 15 | set the result of the exploration. 16 | }@; 17 | } 18 | 19 | parameterType { 20 | -- safety parameters 21 | { 22 | [7]float lower_torque_thresholds_nominal; 23 | [7]float upper_torque_thresholds_nominal; 24 | [7]float lower_torque_thresholds_acceleration; 25 | [7]float upper_torque_thresholds_acceleration; 26 | [6]float lower_force_thresholds_nominal; 27 | [6]float upper_force_thresholds_nominal; 28 | [6]float lower_force_thresholds_acceleration; 29 | [6]float upper_force_thresholds_acceleration; 30 | } safety; 31 | -- socket pose for plug in 32 | { 33 | [16]float pose; 34 | []float joint_angles; 35 | } socket_pose; 36 | -- end effector pose when plug is inserted 37 | { 38 | [16]float pose; 39 | []float joint_angles; 40 | } hole_pose; 41 | -- start/reset pose 42 | { 43 | [16]float pose; 44 | []float joint_angles; 45 | } home_pose; 46 | -- hyperparameters for optimizer 47 | { 48 | int max_iteration; 49 | int param_count; 50 | []float lower_bounds; 51 | []float upper_bounds; 52 | } hyperparameters; 53 | -- tolerance between the end effector pose 54 | -- and hole pose to check if plug is successfully inserted [m] 55 | [3]float tolerance; 56 | -- maximum insertion time [s] 57 | float duration; 58 | } 59 | 60 | resultType { 61 | string error_cause; 62 | } 63 | 64 | -- Initialize optimizer, e.g.set hyperparameter 65 | --> init { 66 | port success service("learning").operation("init").status == "success" -> reset 67 | port error service("learning").operation("init").status == "error" 68 | 69 | parameterType { 70 | { 71 | int max_iteration; 72 | int param_count; 73 | []float lower_bounds; 74 | []float upper_bounds; 75 | } hyperparameters; 76 | } 77 | 78 | entry @{ 79 | service("learning").operation("init").call(parameter.hyperparameters) 80 | }@ 81 | 82 | } where { 83 | hyperparameters: parameter.hyperparameters; 84 | } 85 | 86 | -- reset errors and move robot to start pose 87 | reset { 88 | port success child("home").port("success") -> get_params 89 | port error child("reset_errors").port("error") or 90 | child("home").port("error") 91 | 92 | parameterType { 93 | { 94 | [16]float pose; 95 | []float joint_angles; 96 | } home_pose; 97 | -- Safety parameters 98 | { 99 | [7]float lower_torque_thresholds_nominal; 100 | [7]float upper_torque_thresholds_nominal; 101 | [7]float lower_torque_thresholds_acceleration; 102 | [7]float upper_torque_thresholds_acceleration; 103 | [6]float lower_force_thresholds_nominal; 104 | [6]float upper_force_thresholds_nominal; 105 | [6]float lower_force_thresholds_acceleration; 106 | [6]float upper_force_thresholds_acceleration; 107 | } safety; 108 | } 109 | 110 | resultType { 111 | string error_cause; 112 | } 113 | 114 | --> reset_errors <- reset_errors_library { 115 | port success -> home 116 | } 117 | 118 | home <- move_via_with_move_configuration { 119 | } where { 120 | poses: [ 121 | { 122 | pose: { pose: parameter.home_pose.pose; }; 123 | point2point: true; -- Robot needs to reach this point exactly 124 | cartesian_velocity_factor: 0.1; 125 | cartesian_acceleration_factor: 0.1; 126 | cartesian_deceleration_factor: 0.1; 127 | -- q3 is the robot's third joint 128 | q3: parameter.home_pose.joint_angles[3]; 129 | } 130 | ]; 131 | controller_mode: 2; 132 | -- high stiffness for high precision 133 | cartesian_impedance: { 134 | K_x: [2000, 2000, 3000, 150, 150, 150]; 135 | }; 136 | joint_impedance: { 137 | K_theta: [5000, 5000, 4500, 4000, 3000, 2500, 2000]; 138 | }; 139 | -- collision thresholds 140 | safety: parameter.safety; 141 | } 142 | 143 | action child("reset_errors").port("error") @{ 144 | setResult({error_cause = child("reset_errors").result.error_cause;}) 145 | }@ 146 | 147 | action child("home").port("error") @{ 148 | setResult({error_cause = child("home").result.error_cause;}) 149 | }@ 150 | 151 | } where { 152 | home_pose: parameter.home_pose; 153 | safety: parameter.safety; 154 | } 155 | 156 | get_params { 157 | port success service("learning").operation("getParams").status == "success" -> explore 158 | port error service("learning").operation("getParams").status == "error" 159 | 160 | resultType { 161 | { 162 | float amplitude_factor_translation; 163 | float amplitude_factor_rotation; 164 | float impedance_factor; 165 | float F_z; 166 | } plug_in_params; 167 | } 168 | 169 | entry @{ 170 | service("learning").operation("getParams").call({}) 171 | }@ 172 | 173 | exit @{ 174 | local res = service("learning").operation("getParams").result 175 | local plug_in_params = { 176 | amplitude_factor_translation = res.parameters[1], 177 | amplitude_factor_rotation = res.parameters[2], 178 | impedance_factor = res.parameters[3], 179 | F_z = res.parameters[4] 180 | } 181 | setResult({plug_in_params = plug_in_params;}) 182 | print("Next Parameters: ") 183 | print(result) 184 | }@ 185 | } 186 | 187 | -- explore suggested parameters by running plug_in statemachine 188 | explore { 189 | port done child("plug_in").port("success") or child("plug_in").port("error") -> set_result 190 | 191 | parameterType { 192 | -- Safety parameters 193 | { 194 | [7]float lower_torque_thresholds_nominal; 195 | [7]float upper_torque_thresholds_nominal; 196 | [7]float lower_torque_thresholds_acceleration; 197 | [7]float upper_torque_thresholds_acceleration; 198 | [6]float lower_force_thresholds_nominal; 199 | [6]float upper_force_thresholds_nominal; 200 | [6]float lower_force_thresholds_acceleration; 201 | [6]float upper_force_thresholds_acceleration; 202 | } safety; 203 | { 204 | [16]float pose; 205 | float q3; 206 | } socket_pose; 207 | [16]float hole_pose; 208 | { 209 | -- parameter for wiggle motion 210 | float amplitude_factor_translation; 211 | float amplitude_factor_rotation; 212 | -- stiffness of robot during insertion 213 | float impedance_factor; 214 | -- insertion force in z direction of robot's end-effector frame 215 | float F_z; 216 | } plug_in_params; 217 | -- maximum insertion time 218 | float duration; 219 | -- tolerance between the end effector pose 220 | -- and hole pose to check if plug inserted 221 | [3]float tolerance; 222 | } 223 | 224 | resultType { 225 | float cost; 226 | bool is_inserted; 227 | } 228 | 229 | -- Execute plug_in statemachine and observe the robot_state to compute the cost function 230 | --> barrier plug_in_and_observe { 231 | -> plug_in 232 | -> observe 233 | } 234 | 235 | -- Run the plug_in statemachine 236 | plug_in <- plug_in_wrapper { 237 | } where parameter 238 | 239 | -- observe the robot state and compute parts of the cost function 240 | observe { 241 | 242 | resultType { 243 | float cost; 244 | } 245 | 246 | entry @{ 247 | setResult(nil) 248 | }@ 249 | 250 | action service("robot").event("sensor_data") ~= nil @{ 251 | local F = service("robot").event("sensor_data").O_F_ext_hat_K 252 | local F_norm = 0 253 | -- compute norm 254 | for i = 1,6,1 do 255 | F_norm = F_norm + F[i]*F[i] 256 | end 257 | -- add to history 258 | local res = {} 259 | if result ~= nil then 260 | res.cost = result.cost + F_norm 261 | else 262 | res.cost = F_norm 263 | end 264 | setResult(res) 265 | }@ 266 | } 267 | 268 | exit @{ 269 | -- set final cost function and flag if plug was successfully inserted 270 | setResult({cost = child("observe").result.cost; 271 | is_inserted = child("plug_in").port("success");}) 272 | }@ 273 | 274 | } where { 275 | socket_pose: { 276 | pose: parameter.socket_pose.pose; 277 | q3: parameter.socket_pose.joint_angles[3]; 278 | }; 279 | hole_pose: parameter.hole_pose.pose; 280 | plug_in_params: child("get_params").result.plug_in_params; 281 | duration: parameter.duration; 282 | tolerance: parameter.tolerance; 283 | safety: parameter.safety; 284 | } 285 | 286 | -- Return the cost and success indicator to optimizer 287 | -- Depending on the service`s response the program decides if 288 | -- the robot should to continue learning or not 289 | set_result { 290 | port learning_done service("learning").operation("setResult").status == "success" and 291 | service("learning").operation("setResult").result 292 | port continue service("learning").operation("setResult").status == "success" and 293 | not service("learning").operation("setResult").result -> reset 294 | port error service("learning").operation("setResult").status == "error" 295 | 296 | parameterType { 297 | float cost; 298 | bool is_inserted; 299 | } 300 | 301 | entry @{ 302 | print("Result of exploration: ") 303 | print(parameter) 304 | service("learning").operation("setResult").call({cost = parameter.cost; 305 | success = parameter.is_inserted;}) 306 | }@ 307 | } where child("explore").result 308 | 309 | action child("init").port("error") @{ 310 | setResult({error_cause = "Error in init state";}) 311 | }@ 312 | 313 | action child("reset").port("error") @{ 314 | setResult({error_cause = child("reset").result.error_cause;}) 315 | }@ 316 | 317 | action child("get_params").port("error") @{ 318 | setResult({error_cause = "Error in get_params state";}) 319 | }@ 320 | 321 | action child("set_result").port("error") @{ 322 | setResult({error_cause = "Error in set_result state";}) 323 | }@ 324 | 325 | } where { 326 | hyperparameters: { 327 | max_iteration: 2; 328 | param_count: 4; 329 | lower_bounds: [0.0, 0.0, 1.0, 0.5]; 330 | upper_bounds: [3.0, 3.0, 10.0, 25.0]; 331 | }; 332 | } 333 | -------------------------------------------------------------------------------- /2_RIDE/statemachines/bundles/ride_tutorial/sources/plug_in_wrapper.lf: -------------------------------------------------------------------------------- 1 | plug_in_wrapper { 2 | port success child("plug_in").port("success") 3 | port error child("plug_in").port("error") 4 | 5 | clientData { 6 | description: @{ 7 | This statemachine is a high-level wrapper for plug_in. 8 | It calculates parameters required for plug_in using high-level parameters. 9 | After the conversion the plug_in statemachine is executed. 10 | }@; 11 | } 12 | 13 | parameterType { 14 | -- Safety parameters 15 | { 16 | [7]float lower_torque_thresholds_nominal; 17 | [7]float upper_torque_thresholds_nominal; 18 | [7]float lower_torque_thresholds_acceleration; 19 | [7]float upper_torque_thresholds_acceleration; 20 | [6]float lower_force_thresholds_nominal; 21 | [6]float upper_force_thresholds_nominal; 22 | [6]float lower_force_thresholds_acceleration; 23 | [6]float upper_force_thresholds_acceleration; 24 | } safety; 25 | -- socket pose consisting of pose and q3 (angle of joint 3) 26 | { 27 | [16]float pose; 28 | float q3; 29 | } socket_pose; 30 | -- end effector pose when plug is inserted 31 | [16]float hole_pose; 32 | { 33 | -- parameter for wiggle motion 34 | float amplitude_factor_translation; 35 | float amplitude_factor_rotation; 36 | -- stiffness of robot during insertion 37 | float impedance_factor; 38 | -- insertion force in z direction of robot's end-effector frame [N] 39 | float F_z; 40 | } plug_in_params; 41 | -- tolerance between the end effector pose 42 | -- and hole pose to check if plug is successfully inserted [m] 43 | [3]float tolerance; 44 | -- maximum insertion time [s] 45 | float duration; 46 | } 47 | 48 | resultType { 49 | string error_cause; 50 | } 51 | 52 | entry @{ 53 | print("Plug in parameters:") 54 | print(parameter.plug_in_params) 55 | }@ 56 | 57 | -- Now, the parameters required for plug in are calculated 58 | --> calculate_parameters { 59 | port done result~=nil -> plug_in 60 | 61 | parameterType { 62 | { 63 | [7]float lower_torque_thresholds_nominal; 64 | [7]float upper_torque_thresholds_nominal; 65 | [7]float lower_torque_thresholds_acceleration; 66 | [7]float upper_torque_thresholds_acceleration; 67 | [6]float lower_force_thresholds_nominal; 68 | [6]float upper_force_thresholds_nominal; 69 | [6]float lower_force_thresholds_acceleration; 70 | [6]float upper_force_thresholds_acceleration; 71 | } safety; 72 | { 73 | [16]float pose; 74 | float q3; 75 | } socket_pose; 76 | [16]float hole_pose; 77 | { 78 | float amplitude_factor_translation; 79 | float amplitude_factor_rotation; 80 | float impedance_factor; 81 | float F_z; 82 | } plug_in_params; 83 | float duration; 84 | [3]float tolerance; 85 | } 86 | 87 | resultType { 88 | -- motion for moving to socket pose 89 | { 90 | []{ 91 | {[16]float pose;} pose; 92 | bool point2point; 93 | float cartesian_velocity_factor; 94 | float cartesian_acceleration_factor; 95 | float cartesian_deceleration_factor; 96 | float q3; 97 | } poses; 98 | { 99 | [6]int K_x; 100 | } cartesian_impedance; 101 | { 102 | [7]int K_theta; 103 | } joint_impedance; 104 | { 105 | [7]float lower_torque_thresholds_nominal; 106 | [7]float upper_torque_thresholds_nominal; 107 | [7]float lower_torque_thresholds_acceleration; 108 | [7]float upper_torque_thresholds_acceleration; 109 | [6]float lower_force_thresholds_nominal; 110 | [6]float upper_force_thresholds_nominal; 111 | [6]float lower_force_thresholds_acceleration; 112 | [6]float upper_force_thresholds_acceleration; 113 | } safety; 114 | int controller_mode; 115 | } socket_motion; 116 | -- wiggle motion for insertion 117 | { 118 | [6]{ 119 | int type; 120 | float a_0; 121 | [5]float a_n; 122 | [5]float b_n; 123 | float T_fourier; 124 | } profiles; 125 | float duration; 126 | int frame_selection; 127 | float safety_distance_x; 128 | float safety_distance_y; 129 | float safety_distance_z; 130 | float safety_distance_theta; 131 | } wiggle_motion; 132 | -- force profile for insertion 133 | { 134 | [6]float F_d; 135 | float duration; 136 | float safety_distance_x; 137 | float safety_distance_y; 138 | float safety_distance_z; 139 | float safety_distance_theta; 140 | float force_tolerance; 141 | } force; 142 | -- move configuration during the actual insertion: high collision thresholds, 143 | -- low stiffness (depends on high-level parameter impedance_factor) 144 | { 145 | -- cartesian impedance during insertion 146 | { 147 | [6]int K_x; 148 | } cartesian_impedance; 149 | -- joint impedance during insertion, affecting nullspace if a cartesian controller is used 150 | { 151 | [7]int K_theta; 152 | } joint_impedance; 153 | { 154 | [7]float lower_torque_thresholds_nominal; 155 | [7]float upper_torque_thresholds_nominal; 156 | [7]float lower_torque_thresholds_acceleration; 157 | [7]float upper_torque_thresholds_acceleration; 158 | [6]float lower_force_thresholds_nominal; 159 | [6]float upper_force_thresholds_nominal; 160 | [6]float lower_force_thresholds_acceleration; 161 | [6]float upper_force_thresholds_acceleration; 162 | } safety; 163 | } insertion_move_configuration; 164 | -- pose of the hole and tolerance to check if plug is inserted 165 | [16]float hole_pose; 166 | [3]float tolerance; 167 | } 168 | 169 | entry @{ 170 | -- define movement to socket pose, accepted by move_via statemachine 171 | local socket_motion = { 172 | poses = { 173 | { 174 | pose = { pose = parameter.socket_pose.pose; }, 175 | point2point = true, -- Robot needs to reach this point exactly 176 | cartesian_velocity_factor = 0.1, 177 | cartesian_acceleration_factor = 0.1, 178 | cartesian_deceleration_factor = 0.1, 179 | -- q3 is the robot's third joint 180 | q3 = parameter.socket_pose.q3, 181 | } 182 | }, 183 | controller_mode = 2, 184 | -- high stiffness for high precision 185 | cartesian_impedance = { 186 | K_x = {2500, 2500, 1500, 150, 150, 150} 187 | }, 188 | joint_impedance = { 189 | K_theta = {4000, 4000, 4000, 3500, 3500, 3000, 3000} 190 | }, 191 | -- standard collision thresholds (from idle_statemachine) 192 | safety = parameter.safety; 193 | } 194 | 195 | -- define desired wiggle motion as fourier profile, accepted by move_velocity statemachine 196 | local wiggle_motion = { 197 | -- compute fourier coefficients 198 | profiles = { 199 | {type = 0, a_0 = 0., a_n = {0.01*parameter.plug_in_params.amplitude_factor_translation/4, 0., 0., 0., 0.}, 200 | b_n = {0., 0., 0., 0., 0.}, T_fourier = 4.}, 201 | {type = 0, a_0 = 0., a_n = {0., 0., 0., 0.03*parameter.plug_in_params.amplitude_factor_translation/2, 0.}, 202 | b_n = {0., 0., 0., 0., 0.}, T_fourier = 2.}, 203 | {type = 0, a_0 = 0., a_n = {0., 0., 0., 0., 0.}, 204 | b_n = {0., 0., 0., 0., 0.}, T_fourier = 1}, 205 | {type = 0, a_0 = 0., a_n = {0.1*parameter.plug_in_params.amplitude_factor_rotation/4, 0., 0., 0. , 0.}, 206 | b_n = {0., 0., 0., 0., 0.}, T_fourier = 4.}, 207 | {type = 0, a_0 = 0., a_n = {0., 0., 0.3*parameter.plug_in_params.amplitude_factor_rotation/2, 0., 0.}, 208 | b_n = {0. , 0., 0., 0., 0.}, T_fourier = 2.}, 209 | {type = 0, a_0 = 0., a_n = {0., 0., 0.3*parameter.plug_in_params.amplitude_factor_rotation, 0., 0.}, 210 | b_n = {0., 0., 0., 0., 0.}, T_fourier = 2.} 211 | }, 212 | duration = parameter.duration, 213 | frame_selection = 1, 214 | safety_distance_x = 0.05, 215 | safety_distance_y = 0.05, 216 | safety_distance_z = 0.015, 217 | safety_distance_theta = 0.3 218 | } 219 | 220 | -- define desired force 221 | local force = { 222 | F_d ={0., 0., parameter.plug_in_params.F_z, 0., 0., 0.}; -- desired force 223 | force_tolerance = 0.1; 224 | safety_distance_x = 0.05, 225 | safety_distance_y = 0.05, 226 | safety_distance_z = 0.015, 227 | safety_distance_theta = 0.3, 228 | duration = parameter.duration 229 | } 230 | 231 | -- compute desired cartesian and joint impedance for insertion 232 | local K_x = {math.floor(50*parameter.plug_in_params.impedance_factor), math.floor(50*parameter.plug_in_params.impedance_factor), 233 | math.floor(50*parameter.plug_in_params.impedance_factor), math.floor(5*parameter.plug_in_params.impedance_factor), 234 | math.floor(5*parameter.plug_in_params.impedance_factor), math.floor(5*parameter.plug_in_params.impedance_factor)} 235 | local K_theta = {math.floor(50*parameter.plug_in_params.impedance_factor), math.floor(50*parameter.plug_in_params.impedance_factor), 236 | math.floor(50*parameter.plug_in_params.impedance_factor), math.floor(30*parameter.plug_in_params.impedance_factor), 237 | math.floor(30*parameter.plug_in_params.impedance_factor), math.floor(30*parameter.plug_in_params.impedance_factor), 238 | math.floor(30*parameter.plug_in_params.impedance_factor)} 239 | -- define desired move configuration for insertion: high thresholds, lower stiffness 240 | local insertion_move_configuration = { 241 | cartesian_impedance = {K_x = K_x}, 242 | joint_impedance = {K_theta = K_theta}, 243 | safety = parameter.safety; 244 | } 245 | 246 | -- set result 247 | local result = {} 248 | result.socket_motion = socket_motion 249 | result.wiggle_motion = wiggle_motion 250 | result.force = force 251 | result.insertion_move_configuration = insertion_move_configuration 252 | result.hole_pose = parameter.hole_pose 253 | result.tolerance = parameter.tolerance 254 | setResult(result) 255 | }@ 256 | } where parameter 257 | 258 | -- Execute plug_in statemachine to insert plug into the hole 259 | plug_in <- plug_in { 260 | } where child("calculate_parameters").result 261 | 262 | -- Check for triggered ports and set result accordingly. 263 | action child("plug_in").port("error") @{ 264 | setResult(child("plug_in").result) 265 | }@ 266 | 267 | } where { 268 | safety: nil; 269 | socket_pose: nil; 270 | hole_pose: nil; 271 | plug_in_params: 272 | { 273 | amplitude_factor_translation: 1.0; 274 | amplitude_factor_rotation: 1.0; 275 | impedance_factor: 7.0; 276 | F_z: 4.0; 277 | }; 278 | duration: 5.0; 279 | tolerance: [0.015, 0.015, 0.00025]; 280 | } 281 | --------------------------------------------------------------------------------