├── .gitignore ├── .gitmodules ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── add_license.py ├── iiwa_control ├── CMakeLists.txt ├── cmake │ └── FindRobotControllers.cmake ├── config │ └── iiwa_control.yaml ├── include │ └── iiwa_control │ │ └── custom_effort_controller.hpp ├── launch │ └── iiwa_control.launch ├── package.xml ├── plugins │ └── custom_controller.xml └── src │ └── custom_effort_controller.cpp ├── iiwa_description ├── CMakeLists.txt ├── README.md ├── config │ └── config.rviz ├── launch │ ├── display_iiwa.launch │ ├── iiwa14_upload.launch │ ├── iiwa7_upload.launch │ └── iiwa_setup.launch ├── meshes │ ├── iiwa14 │ │ ├── collision │ │ │ ├── link_0.stl │ │ │ ├── link_0_s.stl │ │ │ ├── link_1.stl │ │ │ ├── link_1_s.stl │ │ │ ├── link_2.stl │ │ │ ├── link_2_s.stl │ │ │ ├── link_3.stl │ │ │ ├── link_3_s.stl │ │ │ ├── link_4.stl │ │ │ ├── link_4_s.stl │ │ │ ├── link_5.stl │ │ │ ├── link_5_s.stl │ │ │ ├── link_6.stl │ │ │ ├── link_6_s.stl │ │ │ ├── link_7-MF-Touch-pneumatisch.stl │ │ │ ├── link_7-MF-Touch-pneumatisch_s.stl │ │ │ ├── link_7.stl │ │ │ └── link_7_s.stl │ │ └── visual │ │ │ ├── link_0.stl │ │ │ ├── link_1.stl │ │ │ ├── link_2.stl │ │ │ ├── link_3.stl │ │ │ ├── link_4.stl │ │ │ ├── link_5.stl │ │ │ ├── link_6.stl │ │ │ ├── link_7-MF-Touch-pneumatisch.stl │ │ │ └── link_7.stl │ └── iiwa7 │ │ ├── collision │ │ ├── link_0.stl │ │ ├── link_1.stl │ │ ├── link_2.stl │ │ ├── link_3.stl │ │ ├── link_4.stl │ │ ├── link_5.stl │ │ ├── link_6.stl │ │ └── link_7.stl │ │ └── visual │ │ ├── link_0.stl │ │ ├── link_1.stl │ │ ├── link_2.stl │ │ ├── link_3.stl │ │ ├── link_4.stl │ │ ├── link_5.stl │ │ ├── link_6.stl │ │ └── link_7.stl ├── package.xml └── urdf │ ├── iiwa.gazebo.xacro │ ├── iiwa.transmission.xacro │ ├── iiwa14.urdf.xacro │ ├── iiwa14.xacro │ ├── iiwa7.urdf.xacro │ ├── iiwa7.xacro │ ├── materials.xacro │ └── utilities.xacro ├── iiwa_driver ├── CMakeLists.txt ├── cmake │ └── FindFRI.cmake ├── config │ └── iiwa.yaml ├── include │ └── iiwa_driver │ │ └── iiwa.h ├── java │ ├── FRIOverlay.java │ └── FRIOverlayGripper.java ├── launch │ └── iiwa_bringup.launch ├── msg │ ├── AdditionalOutputs.msg │ ├── ConnectionQuality.msg │ └── FRIState.msg ├── package.xml └── src │ ├── iiwa.cpp │ └── iiwa_driver.cpp ├── iiwa_gazebo ├── CMakeLists.txt ├── config │ └── iiwa_control.yaml ├── iiwa_robot_hw_sim_plugins.xml ├── include │ └── iiwa_gazebo │ │ └── gravity_compensation_hw_sim.h ├── launch │ └── iiwa_gazebo.launch ├── package.xml ├── src │ └── gravity_compensation_hw_sim.cpp └── world │ └── iiwa.world ├── iiwa_moveit ├── .gitignore ├── .setup_assistant ├── CMakeLists.txt ├── README.md ├── config │ ├── EffortJointInterface_controllers.yaml │ ├── PositionJointInterface_controllers.yaml │ ├── VelocityJointInterface_controllers.yaml │ ├── fake_controllers.yaml │ ├── iiwa14.srdf │ ├── iiwa7.srdf │ ├── joint_limits.yaml │ ├── kinematics.yaml │ └── ompl_planning.yaml ├── launch │ ├── default_warehouse_db.launch │ ├── demo.launch │ ├── fake_moveit_controller_manager.launch.xml │ ├── iiwa_moveit_controller_manager.launch.xml │ ├── iiwa_moveit_sensor_manager.launch.xml │ ├── joystick_control.launch │ ├── move_group.launch │ ├── moveit.rviz │ ├── moveit_planning_execution.launch │ ├── moveit_rviz.launch │ ├── ompl_planning_pipeline.launch.xml │ ├── planning_context.launch │ ├── planning_pipeline.launch.xml │ ├── run_benchmark_ompl.launch │ ├── sensor_manager.launch.xml │ ├── setup_assistant.launch │ ├── trajectory_execution.launch.xml │ ├── warehouse.launch │ └── warehouse_settings.launch.xml └── package.xml ├── iiwa_ros ├── CMakeLists.txt └── package.xml └── iiwa_tools ├── CMakeLists.txt ├── config └── iiwa_service.yaml ├── include └── iiwa_tools │ ├── cvxgen │ ├── description.cvxgen │ ├── format_cvxgen.py │ └── iiwa_ik_cvxgen.hpp │ ├── iiwa_service.h │ └── iiwa_tools.h ├── launch └── iiwa_service.launch ├── package.xml ├── src ├── iiwa_service.cpp ├── iiwa_service_node.cpp └── iiwa_tools.cpp └── srv ├── GetFK.srv ├── GetGravity.srv ├── GetIK.srv ├── GetJacobian.srv ├── GetJacobians.srv └── GetMassMatrix.srv /.gitignore: -------------------------------------------------------------------------------- 1 | iiwa_driver-build 2 | CMakeLists.txt.user 3 | *.pyc 4 | .vscode 5 | .DS_Store -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/.gitmodules -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Bug reports, feature requests or code contributions are always very welcome. 2 | 3 | To make things easier, here are a few tips: 4 | 5 | Reporting bugs, requesting features 6 | ----------------------------------- 7 | 8 | - Best way to report bugs and request new features is to use GitHub 9 | [Issues](https://github.com/epfl-lasa/iiwa_ros/issues), but you can contact the maintainers also any other way — see the [README](README.md) for details. 10 | 11 | Code contribution 12 | ----------------- 13 | 14 | - Best way to contribute is using GitHub [Pull Requests](https://github.com/epfl-lasa/iiwa_ros/pulls) 15 | — fork the repository and make a pull request from a feature branch. You can also send patches via e-mail or contact the maintainers in any other way — see the [README](README.md) for details. 16 | - Follow the project coding guidelines. In case of doubt, please contact the maintainers. 17 | - All your code will be released under the project license (see the [LICENSE](LICENSE) file for details), so make sure you and your collaborators (or employers) have no problems with it. -------------------------------------------------------------------------------- /add_license.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # encoding: utf-8 3 | #| 4 | #| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 5 | #| Authors: Konstantinos Chatzilygeroudis (maintainer) 6 | #| Matthias Mayr 7 | #| Bernardo Fichera 8 | #| email: costashatz@gmail.com 9 | #| matthias.mayr@cs.lth.se 10 | #| bernardo.fichera@epfl.ch 11 | #| Other contributors: 12 | #| Yoan Mollard (yoan@aubrune.eu) 13 | #| Walid Amanhoud (walid.amanhoud@epfl.ch) 14 | #| website: lasa.epfl.ch 15 | #| 16 | #| This file is part of iiwa_ros. 17 | #| 18 | #| iiwa_ros is free software: you can redistribute it and/or modify 19 | #| it under the terms of the GNU General Public License as published by 20 | #| the Free Software Foundation, either version 3 of the License, or 21 | #| (at your option) any later version. 22 | #| 23 | #| iiwa_ros is distributed in the hope that it will be useful, 24 | #| but WITHOUT ANY WARRANTY; without even the implied warranty of 25 | #| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 26 | #| GNU General Public License for more details. 27 | #| 28 | import fnmatch,re 29 | import os, shutil, sys 30 | 31 | license = '''Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 32 | Authors: Konstantinos Chatzilygeroudis (maintainer) 33 | Matthias Mayr 34 | Bernardo Fichera 35 | email: costashatz@gmail.com 36 | matthias.mayr@cs.lth.se 37 | bernardo.fichera@epfl.ch 38 | Other contributors: 39 | Yoan Mollard (yoan@aubrune.eu) 40 | Walid Amanhoud (walid.amanhoud@epfl.ch) 41 | website: lasa.epfl.ch 42 | 43 | This file is part of iiwa_ros. 44 | 45 | iiwa_ros is free software: you can redistribute it and/or modify 46 | it under the terms of the GNU General Public License as published by 47 | the Free Software Foundation, either version 3 of the License, or 48 | (at your option) any later version. 49 | 50 | iiwa_ros is distributed in the hope that it will be useful, 51 | but WITHOUT ANY WARRANTY; without even the implied warranty of 52 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 53 | GNU General Public License for more details. 54 | ''' 55 | 56 | def make_dirlist(folder, extensions): 57 | matches = [] 58 | for root, dirnames, filenames in os.walk(folder): 59 | for ext in extensions: 60 | for filename in fnmatch.filter(filenames, '*' + ext): 61 | matches.append(os.path.join(root, filename)) 62 | return matches 63 | 64 | def insert_header(fname, prefix, postfix, license, kept_header = []): 65 | input = open(fname, 'r') 66 | ofname = '/tmp/' + fname.split('/')[-1] 67 | output = open(ofname, 'w') 68 | for line in kept_header: 69 | output.write(line + '\n') 70 | output.write(prefix + '\n') 71 | has_postfix = len(postfix)>0 72 | my_prefix = prefix 73 | if has_postfix: 74 | my_prefix = '' 75 | for line in license.split('\n'): 76 | if len(line)>0: 77 | output.write(my_prefix + ' ' + line + '\n') 78 | else: 79 | output.write(my_prefix + '\n') 80 | if has_postfix: 81 | output.write(postfix + '\n') 82 | in_header = False 83 | for line in input: 84 | header = len(list(filter(lambda x: x == line[0:len(x)], kept_header))) != 0 85 | check_prefix = (line[0:len(prefix)] == prefix) 86 | check_postfix = (has_postfix and (line[0:len(postfix)] == postfix)) 87 | if check_prefix and has_postfix: 88 | in_header = True 89 | if check_postfix: 90 | in_header = False 91 | if (not in_header) and (not check_prefix) and (not header) and (not check_postfix): 92 | output.write(line) 93 | output.close() 94 | shutil.move(ofname, fname) 95 | 96 | def insert(directory): 97 | # cpp 98 | cpp = make_dirlist(directory, ['.hpp', '.cpp', '.h', '.c', '.cc']) 99 | for i in cpp: 100 | insert_header(i, '//|', '', license) 101 | # py 102 | py = make_dirlist(directory, ['.py']) 103 | for i in py: 104 | insert_header(i, '#|', '', license, ['#!/usr/bin/env python', '# encoding: utf-8']) 105 | # CMake 106 | cmake = make_dirlist(directory, ['CMakeLists.txt']) 107 | for i in cmake: 108 | # metapackages should not have any comments 109 | if i.endswith('iiwa_ros/CMakeLists.txt'): 110 | continue 111 | insert_header(i, '#|', '', license) 112 | # # XML/URDF 113 | xml_urdf = make_dirlist(directory, ['.xml', '.urdf', '.xacro', '.launch']) 114 | for i in xml_urdf: 115 | header = [''] 116 | insert_header(i, '', license, header) 117 | 118 | if __name__ == '__main__': 119 | insert('.') -------------------------------------------------------------------------------- /iiwa_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #| 2 | #| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 3 | #| Authors: Konstantinos Chatzilygeroudis (maintainer) 4 | #| Matthias Mayr 5 | #| Bernardo Fichera 6 | #| email: costashatz@gmail.com 7 | #| matthias.mayr@cs.lth.se 8 | #| bernardo.fichera@epfl.ch 9 | #| Other contributors: 10 | #| Yoan Mollard (yoan@aubrune.eu) 11 | #| Walid Amanhoud (walid.amanhoud@epfl.ch) 12 | #| website: lasa.epfl.ch 13 | #| 14 | #| This file is part of iiwa_ros. 15 | #| 16 | #| iiwa_ros is free software: you can redistribute it and/or modify 17 | #| it under the terms of the GNU General Public License as published by 18 | #| the Free Software Foundation, either version 3 of the License, or 19 | #| (at your option) any later version. 20 | #| 21 | #| iiwa_ros is distributed in the hope that it will be useful, 22 | #| but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | #| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | #| GNU General Public License for more details. 25 | #| 26 | cmake_minimum_required(VERSION 3.1.0) 27 | project(iiwa_control) 28 | 29 | cmake_policy(SET CMP0057 NEW) 30 | 31 | # When enabling SIMD, both the dependencies (e.g. RBDyn) and subsequent code need to be compiled with SIMD 32 | option(ENABLE_SIMD "Build with all SIMD instructions on the current local machine" OFF) 33 | 34 | # Tell CMake where to find "Find.cmake" 35 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") 36 | 37 | ## Find catkin 38 | find_package(catkin REQUIRED COMPONENTS 39 | roscpp 40 | std_msgs 41 | hardware_interface 42 | controller_manager 43 | controller_interface 44 | urdf 45 | iiwa_tools) 46 | 47 | ## Find control_stack 48 | find_package(RobotControllers REQUIRED COMPONENTS 49 | RobotControllers 50 | ) 51 | 52 | find_package(PkgConfig) 53 | ## Find Eigen 54 | pkg_search_module(Eigen3 REQUIRED eigen3) 55 | 56 | # Find Corrade 57 | find_package(Corrade REQUIRED Utility PluginManager) 58 | 59 | ##Needed for ros packages 60 | catkin_package( 61 | CATKIN_DEPENDS roscpp std_msgs hardware_interface controller_interface controller_manager urdf iiwa_tools 62 | DEPENDS RobotControllers 63 | LIBRARIES CustomEffortController 64 | ) 65 | 66 | set(iiwa_control_SOURCES 67 | src/custom_effort_controller.cpp 68 | ) 69 | 70 | add_library(CustomEffortController ${iiwa_control_SOURCES}) 71 | 72 | if(ENABLE_SIMD) 73 | if(CMAKE_COMPILER_IS_GNUCXX) 74 | execute_process( 75 | COMMAND ${CMAKE_CXX_COMPILER} -dumpfullversion -dumpversion OUTPUT_VARIABLE GCC_VERSION) 76 | set(CXX_COMPILER_VERSION ${GCC_VERSION}) 77 | target_compile_options(CustomEffortController PUBLIC -march=native) 78 | if(GCC_VERSION VERSION_GREATER 7.0) 79 | target_compile_options(CustomEffortController PUBLIC -faligned-new) 80 | endif() 81 | elseif("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") 82 | target_compile_options(CustomEffortController PUBLIC -march=native -faligned-new) 83 | endif() 84 | endif() 85 | 86 | target_include_directories(CustomEffortController PUBLIC 87 | include 88 | ${catkin_INCLUDE_DIRS} 89 | ${RobotControllers_INCLUDE_DIRS} 90 | ${Eigen3_INCLUDE_DIRS} 91 | $) 92 | 93 | target_link_libraries(CustomEffortController PUBLIC 94 | ${catkin_LIBRARIES} 95 | ${RobotControllers_LIBRARIES} 96 | ${Eigen3_LIBRARIES} 97 | Corrade::PluginManager) 98 | 99 | add_dependencies(CustomEffortController ${iiwa_tools_EXPORTED_TARGETS}) 100 | 101 | # Require C++11 102 | set_property(TARGET CustomEffortController PROPERTY CXX_STANDARD 11) 103 | set_property(TARGET CustomEffortController PROPERTY CXX_STANDARD_REQUIRED ON) 104 | 105 | install(TARGETS CustomEffortController 106 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 107 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 108 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 109 | ) 110 | 111 | install(DIRECTORY launch 112 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 113 | 114 | install(DIRECTORY config 115 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 116 | 117 | install(FILES plugins/custom_controller.xml 118 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -------------------------------------------------------------------------------- /iiwa_control/cmake/FindRobotControllers.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find RobotControllers 2 | # Once executed this script will define the following: 3 | # RobotControllers_FOUND - RobotControllers was succesfully found 4 | # RobotControllers_INCLUDE_DIRS - RobotControllers's include directories 5 | # RobotControllers_LIBRARIES - RobotControllers's libraries 6 | # RobotControllers_DIR - Directory where RobotControllers was found 7 | #------------------------------------------------------------------------------ 8 | # Users can set RobotControllers_DIR to force CMake to look in a particular location, 9 | # setting the RobotControllers_DIR environment variable will have a similar effect. 10 | 11 | cmake_minimum_required(VERSION 2.8.3) 12 | 13 | #These are RobotControllers's known components (ie. libraries) 14 | set(RobotControllers_COMPONENTS RobotControllers) 15 | 16 | 17 | #Set INCLUDE hints 18 | set(RobotControllers_INCLUDE_HINTS 19 | "${RobotControllers_DIR}/include" 20 | "$ENV{RobotControllers_DIR}/include" ) 21 | 22 | # Set LIBRARY hints 23 | set(RobotControllers_LIBRARY_HINTS 24 | "${RobotControllers_DIR}/lib" 25 | "$ENV{RobotControllers_DIR}/lib" ) 26 | 27 | # Find include directories 28 | find_path(RobotControllers_INCLUDE_DIR robot_controllers/AbstractController.hpp HINTS ${RobotControllers_INCLUDE_HINTS} ) 29 | 30 | # Verify we know about all the components requested 31 | # and remove those we don't know about 32 | set(RobotControllers_FILTERED_COMPONENTS ${RobotControllers_FIND_COMPONENTS}) 33 | 34 | if ( RobotControllers_FIND_COMPONENTS ) 35 | foreach(comp ${RobotControllers_FIND_COMPONENTS}) 36 | list(FIND RobotControllers_COMPONENTS ${comp} ${comp}_KNOWN) 37 | if (${comp}_KNOWN EQUAL -1) 38 | list(REMOVE_ITEM RobotControllers_FILTERED_COMPONENTS ${comp}) 39 | message(STATUS "Unknown RobotControllers component ${comp}") 40 | endif() 41 | endforeach() 42 | endif() 43 | 44 | list(LENGTH RobotControllers_FILTERED_COMPONENTS RobotControllers_NUMBER_OF_COMPONENTS) 45 | set(RobotControllers_FOUND_COMPONENTS TRUE) 46 | 47 | # Look for components (ie. libraries) 48 | if( ${RobotControllers_NUMBER_OF_COMPONENTS} ) 49 | foreach(comp ${RobotControllers_FILTERED_COMPONENTS}) 50 | #Look for the actual library here 51 | find_library(${comp}_LIBRARY NAMES ${comp} HINTS ${RobotControllers_LIBRARY_HINTS}) 52 | if ( ${${comp}_LIBRARY} STREQUAL ${comp}_LIBRARY-NOTFOUND) 53 | message(STATUS "Could not find RobotControllers's ${comp}") 54 | set(RobotControllers_FOUND_COMPONENTS FALSE) 55 | else() 56 | #If everything went well append this component to list of libraries 57 | list(APPEND RobotControllers_LIBRARY ${${comp}_LIBRARY}) 58 | endif() 59 | endforeach() 60 | else() 61 | message(STATUS "No RobotControllers components specified") 62 | endif() 63 | 64 | 65 | # Handle the QUIET and REQUIRED arguments 66 | include(FindPackageHandleStandardArgs) 67 | find_package_handle_standard_args( 68 | RobotControllers #Package name 69 | DEFAULT_MSG 70 | # Variables required to evaluate as TRUE 71 | RobotControllers_LIBRARY 72 | RobotControllers_INCLUDE_DIR 73 | RobotControllers_FOUND_COMPONENTS) 74 | 75 | # Copy the values of the advanced variables to the user-facing ones 76 | set(RobotControllers_LIBRARIES ${RobotControllers_LIBRARY} ) 77 | set(RobotControllers_INCLUDE_DIRS ${RobotControllers_INCLUDE_DIR} ) 78 | set(RobotControllers_FOUND ${RobotControllers_FOUND}) 79 | 80 | # If RobotControllers was found, update RobotControllers_DIR to show where it was found 81 | if ( RobotControllers_FOUND ) 82 | get_filename_component(RobotControllers_NEW_DIR "${RobotControllers_INCLUDE_DIRS}/../" ABSOLUTE) 83 | endif() 84 | set(RobotControllers_DIR ${RobotControllers_NEW_DIR} CACHE FILEPATH "RobotControllers root directory" FORCE) 85 | 86 | #Hide these variables 87 | mark_as_advanced(RobotControllers_INCLUDE_DIR RobotControllers_LIBRARY RobotControllers_FOUND) -------------------------------------------------------------------------------- /iiwa_control/config/iiwa_control.yaml: -------------------------------------------------------------------------------- 1 | iiwa: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 500 # in Hz 6 | 7 | # Default Controllers ------------------------------------------------ 8 | TorqueController: 9 | type: effort_controllers/JointGroupEffortController 10 | joints: 11 | - iiwa_joint_1 12 | - iiwa_joint_2 13 | - iiwa_joint_3 14 | - iiwa_joint_4 15 | - iiwa_joint_5 16 | - iiwa_joint_6 17 | - iiwa_joint_7 18 | 19 | PositionTorqueController: 20 | type: effort_controllers/JointGroupPositionController 21 | joints: 22 | - iiwa_joint_1 23 | - iiwa_joint_2 24 | - iiwa_joint_3 25 | - iiwa_joint_4 26 | - iiwa_joint_5 27 | - iiwa_joint_6 28 | - iiwa_joint_7 29 | iiwa_joint_1: 30 | pid: {p: 800.0, i: 100, d: 80.0, i_clamp_min: -10000, i_clamp_max: 10000} 31 | iiwa_joint_2: 32 | pid: {p: 800.0, i: 1000, d: 100.0, i_clamp_min: -10000, i_clamp_max: 10000} 33 | iiwa_joint_3: 34 | pid: {p: 800.0, i: 10, d: 80.0, i_clamp_min: -10000, i_clamp_max: 10000} 35 | iiwa_joint_4: 36 | pid: {p: 800.0, i: 10, d: 80.0, i_clamp_min: -10000, i_clamp_max: 10000} 37 | iiwa_joint_5: 38 | pid: {p: 300.0, i: 5, d: 10.0, i_clamp_min: -10000, i_clamp_max: 10000} 39 | iiwa_joint_6: 40 | pid: {p: 50.0, i: 1, d: 1.0, i_clamp_min: -10000, i_clamp_max: 10000} 41 | iiwa_joint_7: 42 | pid: {p: 10.0, i: 0.1, d: 1.0, i_clamp_min: -10000, i_clamp_max: 10000} 43 | 44 | PositionController: 45 | type: position_controllers/JointGroupPositionController 46 | joints: 47 | - iiwa_joint_1 48 | - iiwa_joint_2 49 | - iiwa_joint_3 50 | - iiwa_joint_4 51 | - iiwa_joint_5 52 | - iiwa_joint_6 53 | - iiwa_joint_7 54 | 55 | # Custom Controllers ------------------------------------------------ 56 | # The space variable can be either set to "task" or "joint" 57 | CustomControllers: 58 | type: iiwa_control/CustomEffortController 59 | joints: 60 | - iiwa_joint_1 61 | - iiwa_joint_2 62 | - iiwa_joint_3 63 | - iiwa_joint_4 64 | - iiwa_joint_5 65 | - iiwa_joint_6 66 | - iiwa_joint_7 67 | params: 68 | space: task 69 | # end_effector is only needed in task space 70 | end_effector: iiwa_link_ee 71 | null_space: 72 | joints: [0.044752691045324394, 0.6951627023357917, -0.01416978801753847, -1.0922311725109015, -0.0050429618456282, 1.1717338014778385, -0.01502630060305613] 73 | Kp: 20. 74 | Kd: 0.1 75 | max_torque: 10. 76 | controllers: 77 | LinearDS: {type: "LinearDSController", params: [1., 1., 1.]} 78 | # ForceDS: {type: "ForceModDSController", params: [200]} 79 | PassiveDS: {type: "PassiveDSController", params: [200, 200]} 80 | PD: {type: "PidController", params: [20., 2.], input: "Orientation", output: "Torque"} 81 | structure: 82 | # AddCtrl: 83 | # - LinearDS 84 | # - ForceDS 85 | CascadeCtrl: 86 | - LinearDS 87 | - PassiveDS 88 | 89 | PositionTrajectoryController: 90 | type: position_controllers/JointTrajectoryController 91 | joints: 92 | - iiwa_joint_1 93 | - iiwa_joint_2 94 | - iiwa_joint_3 95 | - iiwa_joint_4 96 | - iiwa_joint_5 97 | - iiwa_joint_6 98 | - iiwa_joint_7 99 | 100 | constraints: 101 | goal_time: 0.5 # Override default 102 | 103 | state_publish_rate: 25 # Override default 104 | action_monitor_rate: 30 # Override default 105 | stop_trajectory_duration: 0 # Override default 106 | 107 | -------------------------------------------------------------------------------- /iiwa_control/include/iiwa_control/custom_effort_controller.hpp: -------------------------------------------------------------------------------- 1 | //| 2 | //| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 3 | //| Authors: Konstantinos Chatzilygeroudis (maintainer) 4 | //| Matthias Mayr 5 | //| Bernardo Fichera 6 | //| email: costashatz@gmail.com 7 | //| matthias.mayr@cs.lth.se 8 | //| bernardo.fichera@epfl.ch 9 | //| Other contributors: 10 | //| Yoan Mollard (yoan@aubrune.eu) 11 | //| Walid Amanhoud (walid.amanhoud@epfl.ch) 12 | //| website: lasa.epfl.ch 13 | //| 14 | //| This file is part of iiwa_ros. 15 | //| 16 | //| iiwa_ros is free software: you can redistribute it and/or modify 17 | //| it under the terms of the GNU General Public License as published by 18 | //| the Free Software Foundation, either version 3 of the License, or 19 | //| (at your option) any later version. 20 | //| 21 | //| iiwa_ros is distributed in the hope that it will be useful, 22 | //| but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | //| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | //| GNU General Public License for more details. 25 | //| 26 | #ifndef IIWA_CONTROL_CUSTOM_EFFORT_CONTROLLER_H 27 | #define IIWA_CONTROL_CUSTOM_EFFORT_CONTROLLER_H 28 | 29 | // ROS headers 30 | #include 31 | // ros control 32 | #include 33 | #include 34 | 35 | // realtime tools 36 | #include 37 | #include 38 | 39 | // msgs 40 | #include 41 | 42 | // URDF 43 | #include 44 | 45 | // Iiwa tools 46 | #include 47 | 48 | // RobotControllers 49 | #include 50 | 51 | namespace iiwa_control { 52 | class CustomEffortController : public controller_interface::Controller { 53 | public: 54 | using ControllerPtr = Corrade::Containers::Pointer; 55 | 56 | CustomEffortController(); 57 | ~CustomEffortController(); 58 | 59 | bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle& n); 60 | 61 | void update(const ros::Time& /*time*/, const ros::Duration& /*period*/); 62 | 63 | std::vector joints_; 64 | 65 | realtime_tools::RealtimeBuffer> commands_buffer_; 66 | 67 | unsigned int n_joints_; 68 | 69 | std::vector joint_names_; 70 | 71 | protected: 72 | ros::Subscriber sub_command_; 73 | 74 | // Controller 75 | ControllerPtr controller_; 76 | // Plugin controller manager 77 | Corrade::PluginManager::Manager manager_; 78 | 79 | // Controller's settings 80 | unsigned int space_dim_; 81 | unsigned int cmd_dim_; 82 | bool has_orientation_, null_space_control_; 83 | std::string operation_space_, gravity_comp_; 84 | 85 | // Iiwa tools 86 | iiwa_tools::IiwaTools tools_; 87 | 88 | // URDF 89 | std::vector joint_urdfs_; 90 | 91 | // Null-space control 92 | Eigen::VectorXd null_space_joint_config_; 93 | double null_space_Kp_, null_space_Kd_, null_space_max_torque_; 94 | 95 | // Command callback 96 | void commandCB(const std_msgs::Float64MultiArrayConstPtr& msg); 97 | 98 | // Enforce effort limits 99 | void enforceJointLimits(double& command, unsigned int index); 100 | }; 101 | } // namespace iiwa_control 102 | 103 | #endif -------------------------------------------------------------------------------- /iiwa_control/launch/iiwa_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 38 | 39 | 40 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /iiwa_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | iiwa_control 30 | 0.0.1 31 | Control for KUKA's IIWA robot 32 | 33 | Konstantinos Chatzilygeroudis 34 | 35 | GPL-3.0-or-later 36 | 37 | https://github.com/epfl-lasa/iiwa_ros 38 | https://github.com/epfl-lasa/iiwa_ros/issues 39 | 40 | Konstantinos Chatzilygeroudis 41 | 42 | catkin 43 | 44 | roscpp 45 | std_msgs 46 | controller_manager 47 | controller_interface 48 | hardware_interface 49 | urdf 50 | iiwa_tools 51 | 52 | robot_state_publisher 53 | roscpp 54 | std_msgs 55 | controller_manager 56 | controller_interface 57 | hardware_interface 58 | urdf 59 | iiwa_tools 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /iiwa_control/plugins/custom_controller.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 33 | CustomEffortController 34 | 35 | -------------------------------------------------------------------------------- /iiwa_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #| 2 | #| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 3 | #| Authors: Konstantinos Chatzilygeroudis (maintainer) 4 | #| Matthias Mayr 5 | #| Bernardo Fichera 6 | #| email: costashatz@gmail.com 7 | #| matthias.mayr@cs.lth.se 8 | #| bernardo.fichera@epfl.ch 9 | #| Other contributors: 10 | #| Yoan Mollard (yoan@aubrune.eu) 11 | #| Walid Amanhoud (walid.amanhoud@epfl.ch) 12 | #| website: lasa.epfl.ch 13 | #| 14 | #| This file is part of iiwa_ros. 15 | #| 16 | #| iiwa_ros is free software: you can redistribute it and/or modify 17 | #| it under the terms of the GNU General Public License as published by 18 | #| the Free Software Foundation, either version 3 of the License, or 19 | #| (at your option) any later version. 20 | #| 21 | #| iiwa_ros is distributed in the hope that it will be useful, 22 | #| but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | #| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | #| GNU General Public License for more details. 25 | #| 26 | cmake_minimum_required(VERSION 3.1.0) 27 | project(iiwa_description) 28 | 29 | find_package(catkin REQUIRED) 30 | 31 | catkin_package( 32 | # INCLUDE_DIRS include 33 | # LIBRARIES iiwa_description 34 | # CATKIN_DEPENDS other_catkin_pkg 35 | # DEPENDS system_lib 36 | ) 37 | 38 | install(DIRECTORY launch/ 39 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 40 | 41 | install(DIRECTORY config/ 42 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) 43 | 44 | install(DIRECTORY meshes/ 45 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes) 46 | 47 | install(DIRECTORY urdf/ 48 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf) -------------------------------------------------------------------------------- /iiwa_description/README.md: -------------------------------------------------------------------------------- 1 | # iiwa_description 2 | 3 | Urdf description for the Kuka iiwa 7 R800 and iiwa 14 R820. 4 | Extracted and adapted from https://github.com/IFL-CAMP/iiwa_stack 5 | -------------------------------------------------------------------------------- /iiwa_description/config/config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 705 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.029999999329447746 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 1 54 | Class: rviz/RobotModel 55 | Collision Enabled: false 56 | Enabled: true 57 | Links: 58 | All Links Enabled: true 59 | Expand Joint Details: false 60 | Expand Link Details: false 61 | Expand Tree: false 62 | Link Tree Style: Links in Alphabetic Order 63 | iiwa_link_0: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | Value: true 68 | iiwa_link_1: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | iiwa_link_2: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | iiwa_link_3: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | iiwa_link_4: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | iiwa_link_5: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | iiwa_link_6: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | iiwa_link_7: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | iiwa_link_ee: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | world: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | Name: RobotModel 112 | Robot Description: robot_description 113 | TF Prefix: "" 114 | Update Interval: 0 115 | Value: true 116 | Visual Enabled: true 117 | Enabled: true 118 | Global Options: 119 | Background Color: 48; 48; 48 120 | Default Light: true 121 | Fixed Frame: world 122 | Frame Rate: 30 123 | Name: root 124 | Tools: 125 | - Class: rviz/Interact 126 | Hide Inactive Objects: true 127 | - Class: rviz/MoveCamera 128 | - Class: rviz/Select 129 | - Class: rviz/FocusCamera 130 | - Class: rviz/Measure 131 | - Class: rviz/SetInitialPose 132 | Topic: /initialpose 133 | - Class: rviz/SetGoal 134 | Topic: /move_base_simple/goal 135 | - Class: rviz/PublishPoint 136 | Single click: true 137 | Topic: /clicked_point 138 | Value: true 139 | Views: 140 | Current: 141 | Class: rviz/Orbit 142 | Distance: 3.969125270843506 143 | Enable Stereo Rendering: 144 | Stereo Eye Separation: 0.05999999865889549 145 | Stereo Focal Distance: 1 146 | Swap Stereo Eyes: false 147 | Value: false 148 | Focal Point: 149 | X: 0 150 | Y: 0 151 | Z: 0 152 | Focal Shape Fixed Size: true 153 | Focal Shape Size: 0.05000000074505806 154 | Invert Z Axis: false 155 | Name: Current View 156 | Near Clip Distance: 0.009999999776482582 157 | Pitch: 0.5303982496261597 158 | Target Frame: 159 | Value: Orbit (rviz) 160 | Yaw: 0.43039852380752563 161 | Saved: ~ 162 | Window Geometry: 163 | Displays: 164 | collapsed: false 165 | Height: 1009 166 | Hide Left Dock: false 167 | Hide Right Dock: false 168 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000034ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000034f000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000034ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f0000034f000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000027400fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000034f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 169 | Selection: 170 | collapsed: false 171 | Time: 172 | collapsed: false 173 | Tool Properties: 174 | collapsed: false 175 | Views: 176 | collapsed: false 177 | Width: 1920 178 | X: 0 179 | Y: 29 180 | -------------------------------------------------------------------------------- /iiwa_description/launch/display_iiwa.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | --> 46 | -------------------------------------------------------------------------------- /iiwa_description/launch/iiwa14_upload.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 38 | 39 | -------------------------------------------------------------------------------- /iiwa_description/launch/iiwa7_upload.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 38 | 39 | -------------------------------------------------------------------------------- /iiwa_description/launch/iiwa_setup.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_0.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_0_s.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_0_s.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_1.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_1_s.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_1_s.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_2.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_2_s.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_2_s.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_3.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_3_s.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_3_s.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_4.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_4_s.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_4_s.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_5.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_5_s.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_5_s.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_6.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_6_s.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_6_s.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_7-MF-Touch-pneumatisch.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_7-MF-Touch-pneumatisch.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_7-MF-Touch-pneumatisch_s.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_7-MF-Touch-pneumatisch_s.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/collision/link_7.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/collision/link_7_s.stl: -------------------------------------------------------------------------------- 1 | solid STL generated by MeshLab 2 | facet normal 2.695690e-001 -7.001720e-002 9.604323e-001 3 | outer loop 4 | vertex -1.730778e-002 -2.206785e-002 -1.054347e-002 5 | vertex -5.317893e-003 2.829032e-002 -1.023753e-002 6 | vertex -5.404312e-002 -2.073785e-002 -1.358332e-004 7 | endloop 8 | endfacet 9 | facet normal -1.623354e-001 1.984250e-001 9.665789e-001 10 | outer loop 11 | vertex 6.087431e-003 -5.373264e-002 -1.139504e-004 12 | vertex 5.474702e-002 -1.076422e-003 -2.751230e-003 13 | vertex -1.730778e-002 -2.206785e-002 -1.054347e-002 14 | endloop 15 | endfacet 16 | facet normal 6.707342e-001 -6.511044e-001 -3.552165e-001 17 | outer loop 18 | vertex -1.004377e-002 4.221739e-002 4.447833e-002 19 | vertex -4.737297e-002 1.033968e-002 3.242299e-002 20 | vertex -3.744183e-002 3.819902e-002 1.097853e-004 21 | endloop 22 | endfacet 23 | facet normal 1.359367e-001 -4.700230e-001 8.721237e-001 24 | outer loop 25 | vertex -3.744183e-002 3.819902e-002 1.097853e-004 26 | vertex -5.317893e-003 2.829032e-002 -1.023753e-002 27 | vertex 1.260781e-002 5.276289e-002 1.576811e-004 28 | endloop 29 | endfacet 30 | facet normal 2.821116e-001 -8.344801e-002 9.557455e-001 31 | outer loop 32 | vertex -5.404312e-002 -2.073785e-002 -1.358332e-004 33 | vertex -5.317893e-003 2.829032e-002 -1.023753e-002 34 | vertex -3.744183e-002 3.819902e-002 1.097853e-004 35 | endloop 36 | endfacet 37 | facet normal 3.651352e-001 -7.671183e-002 -9.277885e-001 38 | outer loop 39 | vertex -1.004377e-002 4.221739e-002 4.447833e-002 40 | vertex -2.770314e-002 -3.295686e-002 4.374400e-002 41 | vertex -4.737297e-002 1.033968e-002 3.242299e-002 42 | endloop 43 | endfacet 44 | facet normal -7.866660e-001 -6.169518e-001 2.295663e-002 45 | outer loop 46 | vertex 5.474702e-002 -1.076422e-003 -2.751230e-003 47 | vertex 3.925912e-002 2.024418e-002 3.950143e-002 48 | vertex 1.260781e-002 5.276289e-002 1.576811e-004 49 | endloop 50 | endfacet 51 | facet normal -2.311901e-001 -2.319983e-001 9.448429e-001 52 | outer loop 53 | vertex 5.474702e-002 -1.076422e-003 -2.751230e-003 54 | vertex 1.260781e-002 5.276289e-002 1.576811e-004 55 | vertex -5.317893e-003 2.829032e-002 -1.023753e-002 56 | endloop 57 | endfacet 58 | facet normal 2.784565e-001 -9.566528e-001 -8.530794e-002 59 | outer loop 60 | vertex -1.004377e-002 4.221739e-002 4.447833e-002 61 | vertex -3.744183e-002 3.819902e-002 1.097853e-004 62 | vertex 1.260781e-002 5.276289e-002 1.576811e-004 63 | endloop 64 | endfacet 65 | facet normal -9.197367e-001 8.936638e-002 -3.822275e-001 66 | outer loop 67 | vertex 3.264198e-002 -2.758173e-002 4.424206e-002 68 | vertex 3.925912e-002 2.024418e-002 3.950143e-002 69 | vertex 5.474702e-002 -1.076422e-003 -2.751230e-003 70 | endloop 71 | endfacet 72 | facet normal 9.606407e-001 -2.708496e-001 6.172596e-002 73 | outer loop 74 | vertex -5.404312e-002 -2.073785e-002 -1.358332e-004 75 | vertex -3.744183e-002 3.819902e-002 1.097853e-004 76 | vertex -4.737297e-002 1.033968e-002 3.242299e-002 77 | endloop 78 | endfacet 79 | facet normal -7.329314e-001 6.792216e-001 3.833532e-002 80 | outer loop 81 | vertex 6.087431e-003 -5.373264e-002 -1.139504e-004 82 | vertex 3.264198e-002 -2.758173e-002 4.424206e-002 83 | vertex 5.474702e-002 -1.076422e-003 -2.751230e-003 84 | endloop 85 | endfacet 86 | facet normal -7.429042e-002 8.778751e-001 -4.730922e-001 87 | outer loop 88 | vertex 3.264198e-002 -2.758173e-002 4.424206e-002 89 | vertex 6.087431e-003 -5.373264e-002 -1.139504e-004 90 | vertex -2.770314e-002 -3.295686e-002 4.374400e-002 91 | endloop 92 | endfacet 93 | facet normal -4.062134e-001 -8.202236e-001 -4.027704e-001 94 | outer loop 95 | vertex 3.925912e-002 2.024418e-002 3.950143e-002 96 | vertex -1.004377e-002 4.221739e-002 4.447833e-002 97 | vertex 1.260781e-002 5.276289e-002 1.576811e-004 98 | endloop 99 | endfacet 100 | facet normal 8.562536e-001 2.745988e-001 -4.375217e-001 101 | outer loop 102 | vertex -4.737297e-002 1.033968e-002 3.242299e-002 103 | vertex -2.770314e-002 -3.295686e-002 4.374400e-002 104 | vertex -5.404312e-002 -2.073785e-002 -1.358332e-004 105 | endloop 106 | endfacet 107 | facet normal 4.805901e-001 8.758103e-001 -4.460366e-002 108 | outer loop 109 | vertex -5.404312e-002 -2.073785e-002 -1.358332e-004 110 | vertex -2.770314e-002 -3.295686e-002 4.374400e-002 111 | vertex 6.087431e-003 -5.373264e-002 -1.139504e-004 112 | endloop 113 | endfacet 114 | facet normal 2.565282e-001 4.680645e-001 8.456411e-001 115 | outer loop 116 | vertex 6.087431e-003 -5.373264e-002 -1.139504e-004 117 | vertex -1.730778e-002 -2.206785e-002 -1.054347e-002 118 | vertex -5.404312e-002 -2.073785e-002 -1.358332e-004 119 | endloop 120 | endfacet 121 | facet normal -1.135372e-001 2.099758e-002 9.933118e-001 122 | outer loop 123 | vertex 5.474702e-002 -1.076422e-003 -2.751230e-003 124 | vertex -5.317893e-003 2.829032e-002 -1.023753e-002 125 | vertex -1.730778e-002 -2.206785e-002 -1.054347e-002 126 | endloop 127 | endfacet 128 | facet normal 7.540769e-003 7.996351e-003 -9.999396e-001 129 | outer loop 130 | vertex 3.264198e-002 -2.758173e-002 4.424206e-002 131 | vertex -2.770314e-002 -3.295686e-002 4.374400e-002 132 | vertex -1.004377e-002 4.221739e-002 4.447833e-002 133 | endloop 134 | endfacet 135 | facet normal -1.350081e-001 -7.922112e-002 -9.876724e-001 136 | outer loop 137 | vertex -1.004377e-002 4.221739e-002 4.447833e-002 138 | vertex 3.925912e-002 2.024418e-002 3.950143e-002 139 | vertex 3.264198e-002 -2.758173e-002 4.424206e-002 140 | endloop 141 | endfacet 142 | endsolid vcg 143 | -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/visual/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/visual/link_0.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/visual/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/visual/link_1.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/visual/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/visual/link_2.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/visual/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/visual/link_3.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/visual/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/visual/link_4.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/visual/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/visual/link_5.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/visual/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/visual/link_6.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/visual/link_7-MF-Touch-pneumatisch.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/visual/link_7-MF-Touch-pneumatisch.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa14/visual/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa14/visual/link_7.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/collision/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/collision/link_0.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/collision/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/collision/link_1.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/collision/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/collision/link_2.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/collision/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/collision/link_3.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/collision/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/collision/link_4.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/collision/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/collision/link_5.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/collision/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/collision/link_6.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/collision/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/collision/link_7.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/visual/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/visual/link_0.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/visual/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/visual/link_1.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/visual/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/visual/link_2.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/visual/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/visual/link_3.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/visual/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/visual/link_4.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/visual/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/visual/link_5.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/visual/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/visual/link_6.stl -------------------------------------------------------------------------------- /iiwa_description/meshes/iiwa7/visual/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/epfl-lasa/iiwa_ros/9d2b99f1c62e1dd57c9c351958206a7ee5066785/iiwa_description/meshes/iiwa7/visual/link_7.stl -------------------------------------------------------------------------------- /iiwa_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | iiwa_description 30 | 1.4.0 31 | This package contains the URDF of the KUKA LBR IIWA robot 32 | Salvo Virga 33 | Salvo Virga 34 | 35 | GPL-3.0-or-later 36 | 37 | catkin 38 | 39 | 40 | -------------------------------------------------------------------------------- /iiwa_description/urdf/iiwa.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | /${robot_name} 37 | iiwa_gazebo/GravityCompensationHWSim 38 | 39 | 40 | 41 | 42 | 43 | Gazebo/Grey 44 | 0.2 45 | 0.2 46 | 47 | 48 | 49 | 50 | Gazebo/Orange 51 | 0.2 52 | 0.2 53 | 54 | 55 | 56 | 57 | Gazebo/Orange 58 | 0.2 59 | 0.2 60 | 61 | 62 | 63 | 64 | Gazebo/Orange 65 | 0.2 66 | 0.2 67 | 68 | 69 | 70 | 71 | 72 | Gazebo/Orange 73 | 0.2 74 | 0.2 75 | 76 | 77 | 78 | 79 | Gazebo/Orange 80 | 0.2 81 | 0.2 82 | 83 | 84 | 85 | 86 | Gazebo/Orange 87 | 0.2 88 | 0.2 89 | 90 | 91 | 92 | 93 | Gazebo/Grey 94 | 0.2 95 | 0.2 96 | 97 | 98 | 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /iiwa_description/urdf/iiwa.transmission.xacro: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | /${robot_name} 32 | transmission_interface/SimpleTransmission 33 | 34 | hardware_interface/${hardware_interface} 35 | 36 | 37 | hardware_interface/${hardware_interface} 38 | 1 39 | 40 | 41 | 42 | 43 | /${robot_name} 44 | transmission_interface/SimpleTransmission 45 | 46 | hardware_interface/${hardware_interface} 47 | 48 | 49 | hardware_interface/${hardware_interface} 50 | 1 51 | 52 | 53 | 54 | 55 | /${robot_name} 56 | transmission_interface/SimpleTransmission 57 | 58 | hardware_interface/${hardware_interface} 59 | 60 | 61 | hardware_interface/${hardware_interface} 62 | 1 63 | 64 | 65 | 66 | 67 | /${robot_name} 68 | transmission_interface/SimpleTransmission 69 | 70 | hardware_interface/${hardware_interface} 71 | 72 | 73 | hardware_interface/${hardware_interface} 74 | 1 75 | 76 | 77 | 78 | 79 | /${robot_name} 80 | transmission_interface/SimpleTransmission 81 | 82 | hardware_interface/${hardware_interface} 83 | 84 | 85 | hardware_interface/${hardware_interface} 86 | 1 87 | 88 | 89 | 90 | 91 | /${robot_name} 92 | transmission_interface/SimpleTransmission 93 | 94 | hardware_interface/${hardware_interface} 95 | 96 | 97 | hardware_interface/${hardware_interface} 98 | 1 99 | 100 | 101 | 102 | 103 | /${robot_name} 104 | transmission_interface/SimpleTransmission 105 | 106 | hardware_interface/${hardware_interface} 107 | 108 | 109 | hardware_interface/${hardware_interface} 110 | 1 111 | 112 | 113 | 114 | 115 | -------------------------------------------------------------------------------- /iiwa_description/urdf/iiwa14.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /iiwa_description/urdf/iiwa7.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /iiwa_description/urdf/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /iiwa_description/urdf/utilities.xacro: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 40 | 41 | 42 | 43 | 44 | 48 | 49 | 50 | 51 | 52 | 53 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /iiwa_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #| 2 | #| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 3 | #| Authors: Konstantinos Chatzilygeroudis (maintainer) 4 | #| Matthias Mayr 5 | #| Bernardo Fichera 6 | #| email: costashatz@gmail.com 7 | #| matthias.mayr@cs.lth.se 8 | #| bernardo.fichera@epfl.ch 9 | #| Other contributors: 10 | #| Yoan Mollard (yoan@aubrune.eu) 11 | #| Walid Amanhoud (walid.amanhoud@epfl.ch) 12 | #| website: lasa.epfl.ch 13 | #| 14 | #| This file is part of iiwa_ros. 15 | #| 16 | #| iiwa_ros is free software: you can redistribute it and/or modify 17 | #| it under the terms of the GNU General Public License as published by 18 | #| the Free Software Foundation, either version 3 of the License, or 19 | #| (at your option) any later version. 20 | #| 21 | #| iiwa_ros is distributed in the hope that it will be useful, 22 | #| but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | #| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | #| GNU General Public License for more details. 25 | #| 26 | cmake_minimum_required(VERSION 3.1.0) 27 | project(iiwa_driver) 28 | 29 | # Tell CMake where to find "FindFRI.cmake" 30 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") 31 | 32 | # Find catkin macros and libraries 33 | find_package(catkin REQUIRED COMPONENTS 34 | roscpp 35 | message_generation 36 | rospy 37 | std_msgs 38 | geometry_msgs 39 | tf 40 | hardware_interface 41 | controller_manager 42 | sensor_msgs 43 | urdf 44 | realtime_tools 45 | ) 46 | 47 | find_package(FRI REQUIRED COMPONENTS 48 | kuka_fri 49 | ) 50 | 51 | # Message files 52 | add_message_files( 53 | FILES 54 | AdditionalOutputs.msg 55 | ConnectionQuality.msg 56 | FRIState.msg 57 | ) 58 | 59 | generate_messages( 60 | DEPENDENCIES 61 | std_msgs 62 | ) 63 | 64 | # Needed for ros packages 65 | catkin_package(CATKIN_DEPENDS roscpp message_runtime geometry_msgs tf std_msgs sensor_msgs hardware_interface controller_manager urdf realtime_tools) 66 | 67 | # Sets build type to "Release" in case no build type has not been set before 68 | if(NOT CMAKE_BUILD_TYPE) 69 | set(CMAKE_BUILD_TYPE Release) 70 | endif(NOT CMAKE_BUILD_TYPE) 71 | 72 | add_executable(iiwa_driver src/iiwa.cpp src/iiwa_driver.cpp) 73 | 74 | # Require C++11 75 | set_property(TARGET iiwa_driver PROPERTY CXX_STANDARD 11) 76 | set_property(TARGET iiwa_driver PROPERTY CXX_STANDARD_REQUIRED ON) 77 | 78 | target_include_directories(iiwa_driver PUBLIC include ${catkin_INCLUDE_DIRS} ${FRI_INCLUDE_DIRS}) 79 | target_link_libraries(iiwa_driver ${catkin_LIBRARIES} ${FRI_LIBRARIES}) 80 | add_dependencies(iiwa_driver ${catkin_EXPORTED_TARGETS}) 81 | 82 | add_dependencies(iiwa_driver iiwa_driver_generate_messages_cpp) 83 | 84 | 85 | install(TARGETS iiwa_driver 86 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 87 | ) 88 | 89 | install(DIRECTORY launch 90 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 91 | 92 | install(DIRECTORY config 93 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -------------------------------------------------------------------------------- /iiwa_driver/cmake/FindFRI.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find FRI 2 | # Once executed this script will define the following: 3 | # FRI_FOUND - FRI was succesfully found 4 | # FRI_INCLUDE_DIRS - FRI's include directories 5 | # FRI_LIBRARIES - FRI's libraries 6 | # FRI_DIR - Directory where FRI was found 7 | #------------------------------------------------------------------------------ 8 | # Users can set FRI_DIR to force CMake to look in a particular location, 9 | # setting the FRI_DIR environment variable will have a similar effect. 10 | 11 | cmake_minimum_required(VERSION 2.8.3) 12 | 13 | #These are FRI's known components (ie. libraries) 14 | set(FRI_COMPONENTS kuka_fri) 15 | 16 | 17 | #Set INCLUDE hints 18 | set(FRI_INCLUDE_HINTS 19 | "${FRI_DIR}/include" 20 | "$ENV{FRI_DIR}/include" ) 21 | 22 | # Set LIBRARY hints 23 | set(FRI_LIBRARY_HINTS 24 | "${FRI_DIR}/lib" 25 | "$ENV{FRI_DIR}/lib" ) 26 | 27 | # Find include directories 28 | find_path(FRI_INCLUDE_DIR kuka/fri/LBRClient.h HINTS ${FRI_INCLUDE_HINTS} ) 29 | 30 | # Verify we know about all the components requested 31 | # and remove those we don't know about 32 | set(FRI_FILTERED_COMPONENTS ${FRI_FIND_COMPONENTS}) 33 | 34 | if ( FRI_FIND_COMPONENTS ) 35 | foreach(comp ${FRI_FIND_COMPONENTS}) 36 | list(FIND FRI_COMPONENTS ${comp} ${comp}_KNOWN) 37 | if (${comp}_KNOWN EQUAL -1) 38 | list(REMOVE_ITEM FRI_FILTERED_COMPONENTS ${comp}) 39 | message(STATUS "Unknown FRI component ${comp}") 40 | endif() 41 | endforeach() 42 | endif() 43 | 44 | list(LENGTH FRI_FILTERED_COMPONENTS FRI_NUMBER_OF_COMPONENTS) 45 | set(FRI_FOUND_COMPONENTS TRUE) 46 | 47 | # Look for components (ie. libraries) 48 | if( ${FRI_NUMBER_OF_COMPONENTS} ) 49 | foreach(comp ${FRI_FILTERED_COMPONENTS}) 50 | #Look for the actual library here 51 | find_library(${comp}_LIBRARY NAMES ${comp} HINTS ${FRI_LIBRARY_HINTS}) 52 | if ( ${${comp}_LIBRARY} STREQUAL ${comp}_LIBRARY-NOTFOUND) 53 | message(STATUS "Could not find FRI's ${comp}") 54 | set(FRI_FOUND_COMPONENTS FALSE) 55 | else() 56 | #If everything went well append this component to list of libraries 57 | list(APPEND FRI_LIBRARY ${${comp}_LIBRARY}) 58 | endif() 59 | endforeach() 60 | else() 61 | message(STATUS "No FRI components specified") 62 | endif() 63 | 64 | 65 | # Handle the QUIET and REQUIRED arguments 66 | include(FindPackageHandleStandardArgs) 67 | find_package_handle_standard_args( 68 | FRI #Package name 69 | DEFAULT_MSG 70 | # Variables required to evaluate as TRUE 71 | FRI_LIBRARY 72 | FRI_INCLUDE_DIR 73 | FRI_FOUND_COMPONENTS) 74 | 75 | # Copy the values of the advanced variables to the user-facing ones 76 | set(FRI_LIBRARIES ${FRI_LIBRARY} ) 77 | set(FRI_INCLUDE_DIRS ${FRI_INCLUDE_DIR} ) 78 | set(FRI_FOUND ${FRI_FOUND}) 79 | 80 | # If FRI was found, update FRI_DIR to show where it was found 81 | if ( FRI_FOUND ) 82 | get_filename_component(FRI_NEW_DIR "${FRI_INCLUDE_DIRS}/../" ABSOLUTE) 83 | endif() 84 | set(FRI_DIR ${FRI_NEW_DIR} CACHE FILEPATH "FRI root directory" FORCE) 85 | 86 | #Hide these variables 87 | mark_as_advanced(FRI_INCLUDE_DIR FRI_LIBRARY FRI_FOUND) -------------------------------------------------------------------------------- /iiwa_driver/config/iiwa.yaml: -------------------------------------------------------------------------------- 1 | fri: 2 | port: 30200 3 | robot_ip: 192.170.10.2 4 | robot_description: /robot_description 5 | 6 | hardware_interface: 7 | control_freq: 200 # in Hz 8 | joints: 9 | - iiwa_joint_1 10 | - iiwa_joint_2 11 | - iiwa_joint_3 12 | - iiwa_joint_4 13 | - iiwa_joint_5 14 | - iiwa_joint_6 15 | - iiwa_joint_7 16 | 17 | publish: 18 | additional_info: true 19 | commanded_torques: true -------------------------------------------------------------------------------- /iiwa_driver/include/iiwa_driver/iiwa.h: -------------------------------------------------------------------------------- 1 | //| 2 | //| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 3 | //| Authors: Konstantinos Chatzilygeroudis (maintainer) 4 | //| Matthias Mayr 5 | //| Bernardo Fichera 6 | //| email: costashatz@gmail.com 7 | //| matthias.mayr@cs.lth.se 8 | //| bernardo.fichera@epfl.ch 9 | //| Other contributors: 10 | //| Yoan Mollard (yoan@aubrune.eu) 11 | //| Walid Amanhoud (walid.amanhoud@epfl.ch) 12 | //| website: lasa.epfl.ch 13 | //| 14 | //| This file is part of iiwa_ros. 15 | //| 16 | //| iiwa_ros is free software: you can redistribute it and/or modify 17 | //| it under the terms of the GNU General Public License as published by 18 | //| the Free Software Foundation, either version 3 of the License, or 19 | //| (at your option) any later version. 20 | //| 21 | //| iiwa_ros is distributed in the hope that it will be useful, 22 | //| but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | //| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | //| GNU General Public License for more details. 25 | //| 26 | #ifndef IIWA_DRIVER_IIWA_H 27 | #define IIWA_DRIVER_IIWA_H 28 | 29 | // ROS Headers 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | // FRI Headers 50 | #include 51 | #include 52 | #include 53 | 54 | namespace controller_manager { 55 | class ControllerManager; 56 | } 57 | 58 | namespace kuka { 59 | namespace fri { 60 | class ClientData; 61 | 62 | class DummyState : public LBRState { 63 | public: 64 | FRIMonitoringMessage* message() { return _message; } 65 | void set_message(FRIMonitoringMessage* msg) { _message = msg; } 66 | int monitoring_message_id() { return LBRMONITORMESSAGEID; } 67 | }; 68 | 69 | class DummyCommand : public LBRCommand { 70 | public: 71 | FRICommandMessage* message() { return _message; } 72 | void set_message(FRICommandMessage* msg) { _message = msg; } 73 | int command_message_id() { return LBRCOMMANDMESSAGEID; } 74 | }; 75 | } // namespace fri 76 | } // namespace kuka 77 | 78 | namespace iiwa_ros { 79 | class Iiwa : public hardware_interface::RobotHW { 80 | public: 81 | Iiwa(ros::NodeHandle& nh); 82 | ~Iiwa(); 83 | 84 | void init(ros::NodeHandle& nh); 85 | void run(); 86 | bool initialized(); 87 | 88 | protected: 89 | void _init(); 90 | void _ctrl_loop(); 91 | void _load_params(); 92 | void _read(const ros::Duration& ctrl_duration); 93 | void _write(const ros::Duration& ctrl_duration); 94 | bool _init_fri(); 95 | bool _connect_fri(); 96 | void _disconnect_fri(); 97 | bool _read_fri(kuka::fri::ESessionState& current_state); 98 | bool _write_fri(); 99 | void _publish(); 100 | void _on_fri_state_change(kuka::fri::ESessionState old_state, kuka::fri::ESessionState current_state) {} 101 | 102 | // External torque and commanding status publishers 103 | realtime_tools::RealtimePublisher _additional_pub; 104 | realtime_tools::RealtimePublisher _fri_state_pub; 105 | realtime_tools::RealtimePublisher _commanding_status_pub; 106 | 107 | // Interfaces 108 | hardware_interface::JointStateInterface _joint_state_interface; 109 | hardware_interface::PositionJointInterface _position_joint_interface; 110 | hardware_interface::VelocityJointInterface _velocity_joint_interface; 111 | hardware_interface::EffortJointInterface _effort_joint_interface; 112 | 113 | joint_limits_interface::EffortJointSaturationInterface _effort_joint_saturation_interface; 114 | joint_limits_interface::EffortJointSoftLimitsInterface _effort_joint_limits_interface; 115 | joint_limits_interface::PositionJointSaturationInterface _position_joint_saturation_interface; 116 | joint_limits_interface::PositionJointSoftLimitsInterface _position_joint_limits_interface; 117 | joint_limits_interface::VelocityJointSaturationInterface _velocity_joint_saturation_interface; 118 | joint_limits_interface::VelocityJointSoftLimitsInterface _velocity_joint_limits_interface; 119 | 120 | // Shared memory 121 | size_t _num_joints; 122 | int _joint_mode; // position, velocity, or effort 123 | std::vector _joint_names; 124 | std::vector _joint_types; 125 | std::vector _joint_position, _joint_position_prev; 126 | std::vector _joint_velocity; 127 | std::vector _joint_effort; 128 | std::vector _joint_position_command; 129 | std::vector _joint_velocity_command; 130 | std::vector _joint_effort_command; 131 | 132 | // Controller manager 133 | std::shared_ptr _controller_manager; 134 | 135 | // FRI Connection 136 | kuka::fri::UdpConnection _fri_connection; 137 | kuka::fri::ClientData* _fri_message_data; 138 | kuka::fri::DummyState _robot_state; //!< wrapper class for the FRI monitoring message 139 | kuka::fri::DummyCommand _robot_command; //!< wrapper class for the FRI command message 140 | int _message_size; 141 | bool _idle, _commanding; 142 | 143 | int _port; 144 | std::string _remote_host; 145 | 146 | // ROS communication/timing related 147 | ros::NodeHandle _nh; 148 | std::string _robot_description; 149 | ros::Duration _control_period; 150 | double _control_freq; 151 | bool _initialized; 152 | bool _publish_additional_info{false}; 153 | bool _publish_commanding_status{false}; 154 | }; 155 | } // namespace iiwa_ros 156 | 157 | #endif 158 | -------------------------------------------------------------------------------- /iiwa_driver/java/FRIOverlay.java: -------------------------------------------------------------------------------- 1 | package application; 2 | 3 | import java.util.concurrent.TimeUnit; 4 | import java.util.concurrent.TimeoutException; 5 | 6 | import com.kuka.connectivity.fastRobotInterface.ClientCommandMode; 7 | import com.kuka.connectivity.fastRobotInterface.FRIConfiguration; 8 | import com.kuka.connectivity.fastRobotInterface.FRIJointOverlay; 9 | import com.kuka.connectivity.fastRobotInterface.FRISession; 10 | import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; 11 | import com.kuka.roboticsAPI.controllerModel.Controller; 12 | import com.kuka.roboticsAPI.deviceModel.LBR; 13 | import com.kuka.roboticsAPI.motionModel.PositionHold; 14 | import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; 15 | import com.kuka.roboticsAPI.uiModel.ApplicationDialogType; 16 | import com.kuka.roboticsAPI.executionModel.CommandInvalidException; 17 | 18 | 19 | /** 20 | * Moves the LBR in a start position, creates an FRI-Session and executes a 21 | * PositionHold motion with FRI overlay. During this motion joint angles and 22 | * joint torques can be additionally commanded via FRI. 23 | */ 24 | public class FRIOverlay extends RoboticsAPIApplication 25 | { 26 | private Controller _lbrController; 27 | private LBR _lbr; 28 | private String _clientName; 29 | 30 | @Override 31 | public void initialize() 32 | { 33 | _lbrController = (Controller) getContext().getControllers().toArray()[0]; 34 | _lbr = (LBR) _lbrController.getDevices().toArray()[0]; 35 | // ********************************************************************** 36 | // *** change next line to the FRIClient's IP address *** 37 | // ********************************************************************** 38 | _clientName = "192.170.10.1"; 39 | } 40 | 41 | @Override 42 | public void run() 43 | { 44 | // configure and start FRI session 45 | FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(_lbr, _clientName); 46 | // for torque mode, there has to be a command value at least all 5ms 47 | friConfiguration.setSendPeriodMilliSec(5); 48 | friConfiguration.setReceiveMultiplier(1); 49 | 50 | getLogger().info("Creating FRI connection to " + friConfiguration.getHostName()); 51 | getLogger().info("SendPeriod: " + friConfiguration.getSendPeriodMilliSec() + "ms |" 52 | + " ReceiveMultiplier: " + friConfiguration.getReceiveMultiplier()); 53 | 54 | FRISession friSession = new FRISession(friConfiguration); 55 | 56 | // wait until FRI session is ready to switch to command mode 57 | try 58 | { 59 | friSession.await(10, TimeUnit.SECONDS); 60 | } 61 | catch (final TimeoutException e) 62 | { 63 | getLogger().error(e.getLocalizedMessage()); 64 | friSession.close(); 65 | return; 66 | } 67 | 68 | getLogger().info("FRI connection established."); 69 | 70 | int modeChoice = getApplicationUI().displayModalDialog(ApplicationDialogType.QUESTION, "Choose control mode", "Torque", "Position", "Wrench"); 71 | 72 | ClientCommandMode mode = ClientCommandMode.TORQUE; 73 | if (modeChoice == 0) { 74 | getLogger().info("Torque control mode chosen"); 75 | mode = ClientCommandMode.TORQUE; 76 | } 77 | else if (modeChoice == 1) { 78 | getLogger().info("Position control mode chosen"); 79 | mode = ClientCommandMode.POSITION; 80 | } 81 | else if (modeChoice == 2) { 82 | getLogger().warn("Wrench control mode not supported yet. Using position control mode instead"); 83 | mode = ClientCommandMode.POSITION; 84 | } 85 | else { 86 | getLogger().warn("Invalid choice: using position control mode"); 87 | mode = ClientCommandMode.POSITION; 88 | } 89 | FRIJointOverlay jointOverlay = new FRIJointOverlay(friSession, mode); 90 | 91 | int choice = getApplicationUI().displayModalDialog(ApplicationDialogType.QUESTION, "Choose stiffness for actuators", "0", "20", "50", "150", "300", "500"); 92 | 93 | double stiffness = 0.; 94 | if (choice == 0) { 95 | getLogger().info("Stiffness of '0' chosen"); 96 | stiffness = 0.; 97 | } 98 | else if (choice == 1) { 99 | getLogger().info("Stiffness of '20' chosen"); 100 | stiffness = 20.; 101 | } 102 | else if (choice == 2) { 103 | getLogger().info("Stiffness of '50' chosen"); 104 | stiffness = 50.; 105 | } 106 | else if (choice == 3) { 107 | getLogger().info("Stiffness of '150' chosen"); 108 | stiffness = 150.; 109 | } 110 | else if (choice == 4) { 111 | getLogger().info("Stiffness of '300' chosen"); 112 | stiffness = 300.; 113 | } 114 | else if (choice == 5) { 115 | getLogger().info("Stiffness of '500' chosen"); 116 | stiffness = 500.; 117 | } 118 | else { 119 | getLogger().warn("Invalid choice: setting stiffness to '20'"); 120 | stiffness = 20.; 121 | } 122 | 123 | // start PositionHold with overlay 124 | JointImpedanceControlMode ctrMode = new JointImpedanceControlMode(stiffness, stiffness, stiffness, stiffness, stiffness, stiffness, stiffness); 125 | if (mode == ClientCommandMode.TORQUE) 126 | ctrMode.setDampingForAllJoints(0.); 127 | 128 | try { 129 | PositionHold posHold = new PositionHold(ctrMode, -1, TimeUnit.SECONDS); 130 | getLogger().info("Robot is ready for ROS control."); 131 | _lbr.move(posHold.addMotionOverlay(jointOverlay)); 132 | } 133 | catch(final CommandInvalidException e) { 134 | getLogger().error("ROS has been disconnected."); 135 | } 136 | 137 | // done 138 | friSession.close(); 139 | } 140 | 141 | /** 142 | * main. 143 | * 144 | * @param args 145 | * args 146 | */ 147 | public static void main(final String[] args) 148 | { 149 | final FRIOverlay app = new FRIOverlay(); 150 | app.runApplication(); 151 | } 152 | 153 | } 154 | -------------------------------------------------------------------------------- /iiwa_driver/java/FRIOverlayGripper.java: -------------------------------------------------------------------------------- 1 | package application; 2 | 3 | import java.util.concurrent.TimeUnit; 4 | import java.util.concurrent.TimeoutException; 5 | 6 | import com.kuka.connectivity.fastRobotInterface.ClientCommandMode; 7 | import com.kuka.connectivity.fastRobotInterface.FRIConfiguration; 8 | import com.kuka.connectivity.fastRobotInterface.FRIJointOverlay; 9 | import com.kuka.connectivity.fastRobotInterface.FRISession; 10 | import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; 11 | import com.kuka.roboticsAPI.controllerModel.Controller; 12 | import com.kuka.roboticsAPI.deviceModel.LBR; 13 | import com.kuka.roboticsAPI.geometricModel.Tool; 14 | import com.kuka.roboticsAPI.motionModel.PositionHold; 15 | import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode; 16 | import com.kuka.roboticsAPI.uiModel.ApplicationDialogType; 17 | 18 | /** 19 | * Moves the LBR in a start position, creates an FRI-Session and executes a 20 | * PositionHold motion with FRI overlay. During this motion joint angles and 21 | * joint torques can be additionally commanded via FRI. 22 | */ 23 | public class FRIOverlayGripper extends RoboticsAPIApplication 24 | { 25 | private Controller _lbrController; 26 | private LBR _lbr; 27 | private String _clientName; 28 | private Tool _toolAttached; 29 | 30 | @Override 31 | public void initialize() 32 | { 33 | _lbrController = (Controller) getContext().getControllers().toArray()[0]; 34 | _lbr = (LBR) _lbrController.getDevices().toArray()[0]; 35 | // ********************************************************************** 36 | // *** change next line to the FRIClient's IP address *** 37 | // ********************************************************************** 38 | _clientName = "192.170.10.1"; 39 | 40 | // attach a gripper 41 | _toolAttached = getApplicationData().createFromTemplate("MyVirtualGripper"); 42 | _toolAttached.attachTo(_lbr.getFlange()); 43 | } 44 | 45 | @Override 46 | public void run() 47 | { 48 | // configure and start FRI session 49 | FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(_lbr, _clientName); 50 | // for torque mode, there has to be a command value at least all 5ms 51 | friConfiguration.setSendPeriodMilliSec(5); 52 | friConfiguration.setReceiveMultiplier(1); 53 | 54 | getLogger().info("Creating FRI connection to " + friConfiguration.getHostName()); 55 | getLogger().info("SendPeriod: " + friConfiguration.getSendPeriodMilliSec() + "ms |" 56 | + " ReceiveMultiplier: " + friConfiguration.getReceiveMultiplier()); 57 | 58 | FRISession friSession = new FRISession(friConfiguration); 59 | 60 | // wait until FRI session is ready to switch to command mode 61 | try 62 | { 63 | friSession.await(10, TimeUnit.SECONDS); 64 | } 65 | catch (final TimeoutException e) 66 | { 67 | getLogger().error(e.getLocalizedMessage()); 68 | friSession.close(); 69 | return; 70 | } 71 | 72 | getLogger().info("FRI connection established."); 73 | 74 | int modeChoice = getApplicationUI().displayModalDialog(ApplicationDialogType.QUESTION, "Choose control mode", "Torque", "Position", "Wrench"); 75 | 76 | ClientCommandMode mode = ClientCommandMode.TORQUE; 77 | if (modeChoice == 0) { 78 | getLogger().info("Torque control mode chosen"); 79 | mode = ClientCommandMode.TORQUE; 80 | } 81 | else if (modeChoice == 1) { 82 | getLogger().info("Position control mode chosen"); 83 | mode = ClientCommandMode.POSITION; 84 | } 85 | else if (modeChoice == 2) { 86 | getLogger().warn("Wrench control mode not supported yet. Using position control mode instead"); 87 | mode = ClientCommandMode.POSITION; 88 | } 89 | else { 90 | getLogger().warn("Invalid choice: using position control mode"); 91 | mode = ClientCommandMode.POSITION; 92 | } 93 | FRIJointOverlay jointOverlay = new FRIJointOverlay(friSession, mode); 94 | 95 | int choice = getApplicationUI().displayModalDialog(ApplicationDialogType.QUESTION, "Choose stiffness for actuators", "0", "20", "50", "150", "300", "500"); 96 | 97 | double stiffness = 0.; 98 | if (choice == 0) { 99 | getLogger().info("Stiffness of '0' chosen"); 100 | stiffness = 0.; 101 | } 102 | else if (choice == 1) { 103 | getLogger().info("Stiffness of '20' chosen"); 104 | stiffness = 20.; 105 | } 106 | else if (choice == 2) { 107 | getLogger().info("Stiffness of '50' chosen"); 108 | stiffness = 50.; 109 | } 110 | else if (choice == 3) { 111 | getLogger().info("Stiffness of '150' chosen"); 112 | stiffness = 150.; 113 | } 114 | else if (choice == 4) { 115 | getLogger().info("Stiffness of '300' chosen"); 116 | stiffness = 300.; 117 | } 118 | else if (choice == 5) { 119 | getLogger().info("Stiffness of '500' chosen"); 120 | stiffness = 500.; 121 | } 122 | else { 123 | getLogger().warn("Invalid choice: setting stiffness to '20'"); 124 | stiffness = 20.; 125 | } 126 | 127 | // start PositionHold with overlay 128 | JointImpedanceControlMode ctrMode = new JointImpedanceControlMode(stiffness, stiffness, stiffness, stiffness, stiffness, stiffness, stiffness); 129 | if (mode == ClientCommandMode.TORQUE) 130 | ctrMode.setDampingForAllJoints(0.); 131 | PositionHold posHold = new PositionHold(ctrMode, -1, TimeUnit.SECONDS); 132 | 133 | _toolAttached.move(posHold.addMotionOverlay(jointOverlay)); 134 | 135 | // done 136 | friSession.close(); 137 | } 138 | 139 | /** 140 | * main. 141 | * 142 | * @param args 143 | * args 144 | */ 145 | public static void main(final String[] args) 146 | { 147 | final FRIOverlayGripper app = new FRIOverlayGripper(); 148 | app.runApplication(); 149 | } 150 | 151 | } 152 | -------------------------------------------------------------------------------- /iiwa_driver/launch/iiwa_bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /iiwa_driver/msg/AdditionalOutputs.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | std_msgs/Float64MultiArray external_torques 3 | std_msgs/Float64MultiArray commanded_torques 4 | std_msgs/Float64MultiArray commanded_positions -------------------------------------------------------------------------------- /iiwa_driver/msg/ConnectionQuality.msg: -------------------------------------------------------------------------------- 1 | int8 connection_quality 2 | int8 POOR = 0 # poor connection quality 3 | int8 FAIR = 1 # connection quality insufficient for command mode 4 | int8 GOOD = 2 # connection quality sufficient for command mode 5 | int8 EXCELLENT = 3 # excellent connection quality -------------------------------------------------------------------------------- /iiwa_driver/msg/FRIState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | ConnectionQuality connection_quality -------------------------------------------------------------------------------- /iiwa_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | iiwa_driver 30 | 0.0.1 31 | Package containing the hardware interface to connect to KUKA's IIWA robot (via the FRI library). 32 | 33 | Konstantinos Chatzilygeroudis 34 | 35 | 36 | GPL-3.0-or-later 37 | 38 | https://github.com/epfl-lasa/iiwa_ros 39 | https://github.com/epfl-lasa/iiwa_ros/issues 40 | 41 | Konstantinos Chatzilygeroudis 42 | 43 | 44 | catkin 45 | 46 | roscpp 47 | message_generation 48 | rospy 49 | std_msgs 50 | controller_manager 51 | hardware_interface 52 | geometry_msgs 53 | tf 54 | sensor_msgs 55 | urdf 56 | realtime_tools 57 | 58 | roscpp 59 | message_runtime 60 | rospy 61 | std_msgs 62 | geometry_msgs 63 | controller_manager 64 | hardware_interface 65 | tf 66 | sensor_msgs 67 | urdf 68 | realtime_tools 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /iiwa_driver/src/iiwa_driver.cpp: -------------------------------------------------------------------------------- 1 | //| 2 | //| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 3 | //| Authors: Konstantinos Chatzilygeroudis (maintainer) 4 | //| Matthias Mayr 5 | //| Bernardo Fichera 6 | //| email: costashatz@gmail.com 7 | //| matthias.mayr@cs.lth.se 8 | //| bernardo.fichera@epfl.ch 9 | //| Other contributors: 10 | //| Yoan Mollard (yoan@aubrune.eu) 11 | //| Walid Amanhoud (walid.amanhoud@epfl.ch) 12 | //| website: lasa.epfl.ch 13 | //| 14 | //| This file is part of iiwa_ros. 15 | //| 16 | //| iiwa_ros is free software: you can redistribute it and/or modify 17 | //| it under the terms of the GNU General Public License as published by 18 | //| the Free Software Foundation, either version 3 of the License, or 19 | //| (at your option) any later version. 20 | //| 21 | //| iiwa_ros is distributed in the hope that it will be useful, 22 | //| but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | //| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | //| GNU General Public License for more details. 25 | //| 26 | #include 27 | 28 | int main(int argc, char** argv) 29 | { 30 | ros::init(argc, argv, "iiwa_hardware_interface"); 31 | 32 | ros::NodeHandle nh; 33 | ros::AsyncSpinner spinner(1); 34 | spinner.start(); 35 | 36 | iiwa_ros::Iiwa iiwa(nh); 37 | 38 | iiwa.run(); 39 | 40 | spinner.stop(); 41 | 42 | return 0; 43 | } -------------------------------------------------------------------------------- /iiwa_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #| 2 | #| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 3 | #| Authors: Konstantinos Chatzilygeroudis (maintainer) 4 | #| Matthias Mayr 5 | #| Bernardo Fichera 6 | #| email: costashatz@gmail.com 7 | #| matthias.mayr@cs.lth.se 8 | #| bernardo.fichera@epfl.ch 9 | #| Other contributors: 10 | #| Yoan Mollard (yoan@aubrune.eu) 11 | #| Walid Amanhoud (walid.amanhoud@epfl.ch) 12 | #| website: lasa.epfl.ch 13 | #| 14 | #| This file is part of iiwa_ros. 15 | #| 16 | #| iiwa_ros is free software: you can redistribute it and/or modify 17 | #| it under the terms of the GNU General Public License as published by 18 | #| the Free Software Foundation, either version 3 of the License, or 19 | #| (at your option) any later version. 20 | #| 21 | #| iiwa_ros is distributed in the hope that it will be useful, 22 | #| but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | #| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | #| GNU General Public License for more details. 25 | #| 26 | cmake_minimum_required(VERSION 3.1.0) 27 | project(iiwa_gazebo) 28 | 29 | # Find catkin components 30 | find_package(catkin REQUIRED COMPONENTS 31 | roscpp 32 | gazebo_ros 33 | gazebo_ros_control 34 | iiwa_tools 35 | ) 36 | 37 | # Depend on system install of Gazebo 38 | find_package(gazebo REQUIRED) 39 | # Gazebo cxx flags should have all the required C++ flags 40 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 41 | 42 | catkin_package( 43 | # INCLUDE_DIRS include 44 | # LIBRARIES iiwa_gazebo 45 | CATKIN_DEPENDS roscpp gazebo_ros gazebo_ros_control iiwa_tools 46 | ) 47 | 48 | add_library(iiwa_gazebo_gravity_compensation_hw_sim src/gravity_compensation_hw_sim.cpp) 49 | 50 | # Require C++11 51 | set_property(TARGET iiwa_gazebo_gravity_compensation_hw_sim PROPERTY CXX_STANDARD 11) 52 | set_property(TARGET iiwa_gazebo_gravity_compensation_hw_sim PROPERTY CXX_STANDARD_REQUIRED ON) 53 | 54 | target_include_directories(iiwa_gazebo_gravity_compensation_hw_sim PUBLIC include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS}) 55 | target_link_libraries(iiwa_gazebo_gravity_compensation_hw_sim PUBLIC ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 56 | 57 | add_dependencies(iiwa_gazebo_gravity_compensation_hw_sim ${iiwa_tools_EXPORTED_TARGETS}) 58 | 59 | # Install 60 | install(TARGETS iiwa_gazebo_gravity_compensation_hw_sim 61 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 63 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 64 | ) 65 | 66 | install(DIRECTORY include/${PROJECT_NAME} 67 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 68 | ) 69 | 70 | install(DIRECTORY launch 71 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 72 | 73 | install(FILES iiwa_robot_hw_sim_plugins.xml 74 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 75 | ) -------------------------------------------------------------------------------- /iiwa_gazebo/config/iiwa_control.yaml: -------------------------------------------------------------------------------- 1 | gazebo_ros_control/pid_gains: 2 | iiwa_joint_1: {p: 700, i: 0.01, d: 100} 3 | iiwa_joint_2: {p: 10000, i: 100, d: 100} 4 | iiwa_joint_3: {p: 4500, i: 35, d: 1} 5 | iiwa_joint_4: {p: 5500, i: 60, d: 2} 6 | iiwa_joint_5: {p: 1000, i: 30, d: 0.01} 7 | iiwa_joint_6: {p: 500, i: 0.1, d: 0.01} 8 | iiwa_joint_7: {p: 100, i: 0.1, d: 0.01} -------------------------------------------------------------------------------- /iiwa_gazebo/iiwa_robot_hw_sim_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 33 | 34 | A robot simulation interface which inherits the default gazebo interface and includes a gravity compensation term. 35 | 36 | 37 | -------------------------------------------------------------------------------- /iiwa_gazebo/include/iiwa_gazebo/gravity_compensation_hw_sim.h: -------------------------------------------------------------------------------- 1 | //| 2 | //| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 3 | //| Authors: Konstantinos Chatzilygeroudis (maintainer) 4 | //| Matthias Mayr 5 | //| Bernardo Fichera 6 | //| email: costashatz@gmail.com 7 | //| matthias.mayr@cs.lth.se 8 | //| bernardo.fichera@epfl.ch 9 | //| Other contributors: 10 | //| Yoan Mollard (yoan@aubrune.eu) 11 | //| Walid Amanhoud (walid.amanhoud@epfl.ch) 12 | //| website: lasa.epfl.ch 13 | //| 14 | //| This file is part of iiwa_ros. 15 | //| 16 | //| iiwa_ros is free software: you can redistribute it and/or modify 17 | //| it under the terms of the GNU General Public License as published by 18 | //| the Free Software Foundation, either version 3 of the License, or 19 | //| (at your option) any later version. 20 | //| 21 | //| iiwa_ros is distributed in the hope that it will be useful, 22 | //| but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | //| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | //| GNU General Public License for more details. 25 | //| 26 | #include 27 | 28 | // Iiwa service headers 29 | #include 30 | 31 | // std headers 32 | #include 33 | 34 | namespace iiwa_gazebo { 35 | class GravityCompensationHWSim : public gazebo_ros_control::DefaultRobotHWSim { 36 | public: 37 | virtual bool initSim( 38 | const std::string& robot_namespace, 39 | ros::NodeHandle model_nh, 40 | gazebo::physics::ModelPtr parent_model, 41 | const urdf::Model* const urdf_model, 42 | std::vector transmissions) override; 43 | 44 | virtual void readSim(ros::Time time, ros::Duration period) override; 45 | virtual void writeSim(ros::Time time, ros::Duration period) override; 46 | 47 | protected: 48 | // ROS related 49 | ros::ServiceClient _iiwa_gravity_client; 50 | iiwa_tools::GetGravity _gravity_srv; 51 | }; 52 | 53 | } // namespace iiwa_gazebo -------------------------------------------------------------------------------- /iiwa_gazebo/launch/iiwa_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /iiwa_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | iiwa_gazebo 30 | 0.0.1 31 | This package allows to load a KUKA LBR IIWA robot onto Gazebo 32 | Konstantinos Chatzilygeroudis 33 | 34 | GPL-3.0-or-later 35 | 36 | https://github.com/epfl-lasa/iiwa_ros 37 | https://github.com/epfl-lasa/iiwa_ros/issues 38 | 39 | Konstantinos Chatzilygeroudis 40 | 41 | catkin 42 | 43 | gazebo_ros 44 | gazebo_ros_control 45 | roscpp 46 | iiwa_tools 47 | 48 | iiwa_description 49 | gazebo_ros 50 | gazebo_ros_control 51 | roscpp 52 | iiwa_tools 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /iiwa_gazebo/world/iiwa.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | model://ground_plane 10 | 11 | 12 | 13 | 14 | model://sun 15 | 16 | 17 | 18 | 19 | 20 | 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 21 | orbit 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /iiwa_moveit/.gitignore: -------------------------------------------------------------------------------- 1 | default_warehouse_mongo_db/ 2 | -------------------------------------------------------------------------------- /iiwa_moveit/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: iiwa_description 4 | relative_path: urdf/iiwa14.urdf 5 | SRDF: 6 | relative_path: config/iiwa14.srdf 7 | CONFIG: 8 | generated_timestamp: 1445789930 -------------------------------------------------------------------------------- /iiwa_moveit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #| 2 | #| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 3 | #| Authors: Konstantinos Chatzilygeroudis (maintainer) 4 | #| Matthias Mayr 5 | #| Bernardo Fichera 6 | #| email: costashatz@gmail.com 7 | #| matthias.mayr@cs.lth.se 8 | #| bernardo.fichera@epfl.ch 9 | #| Other contributors: 10 | #| Yoan Mollard (yoan@aubrune.eu) 11 | #| Walid Amanhoud (walid.amanhoud@epfl.ch) 12 | #| website: lasa.epfl.ch 13 | #| 14 | #| This file is part of iiwa_ros. 15 | #| 16 | #| iiwa_ros is free software: you can redistribute it and/or modify 17 | #| it under the terms of the GNU General Public License as published by 18 | #| the Free Software Foundation, either version 3 of the License, or 19 | #| (at your option) any later version. 20 | #| 21 | #| iiwa_ros is distributed in the hope that it will be useful, 22 | #| but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | #| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | #| GNU General Public License for more details. 25 | #| 26 | cmake_minimum_required(VERSION 3.5) 27 | project(iiwa_moveit) 28 | 29 | find_package(catkin REQUIRED) 30 | 31 | catkin_package() 32 | 33 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 34 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 35 | -------------------------------------------------------------------------------- /iiwa_moveit/README.md: -------------------------------------------------------------------------------- 1 | # IIWA MoveIt package 2 | 3 | Starting the MoveIt package requires that you have first setup your robot and environment as specified in the main [README](/README.md). 4 | 5 | In simulation this is launched in two shells as follows: 6 | ```sh 7 | roslaunch iiwa_gazebo iiwa_gazebo.launch controller:=PositionTrajectoryController 8 | 9 | roslaunch iiwa_moveit demo.launch 10 | ``` 11 | 12 | Alternatively it can then be started on a real robot by activating the `driver:=true` option: 13 | ```sh 14 | roslaunch iiwa_moveit demo.launch driver:=true 15 | ``` 16 | Your robot will release brakes at startup of MoveIt. 17 | -------------------------------------------------------------------------------- /iiwa_moveit/config/EffortJointInterface_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: EffortJointInterface_trajectory_controller 3 | action_ns: follow_joint_trajectory 4 | type: FollowJointTrajectory 5 | default: true 6 | joints: 7 | - iiwa_joint_1 8 | - iiwa_joint_2 9 | - iiwa_joint_3 10 | - iiwa_joint_4 11 | - iiwa_joint_5 12 | - iiwa_joint_6 13 | - iiwa_joint_7 14 | -------------------------------------------------------------------------------- /iiwa_moveit/config/PositionJointInterface_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: PositionTrajectoryController 3 | action_ns: follow_joint_trajectory 4 | type: FollowJointTrajectory 5 | default: true 6 | joints: 7 | - iiwa_joint_1 8 | - iiwa_joint_2 9 | - iiwa_joint_3 10 | - iiwa_joint_4 11 | - iiwa_joint_5 12 | - iiwa_joint_6 13 | - iiwa_joint_7 14 | -------------------------------------------------------------------------------- /iiwa_moveit/config/VelocityJointInterface_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: VelocityJointInterface_trajectory_controller 3 | action_ns: follow_joint_trajectory 4 | type: FollowJointTrajectory 5 | default: true 6 | joints: 7 | - iiwa_joint_1 8 | - iiwa_joint_2 9 | - iiwa_joint_3 10 | - iiwa_joint_4 11 | - iiwa_joint_5 12 | - iiwa_joint_6 13 | - iiwa_joint_7 14 | -------------------------------------------------------------------------------- /iiwa_moveit/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_manipulator_controller 3 | joints: 4 | - iiwa_joint_1 5 | - iiwa_joint_2 6 | - iiwa_joint_3 7 | - iiwa_joint_4 8 | - iiwa_joint_5 9 | - iiwa_joint_6 10 | - iiwa_joint_7 -------------------------------------------------------------------------------- /iiwa_moveit/config/iiwa14.srdf: -------------------------------------------------------------------------------- 1 | 2 | 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 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /iiwa_moveit/config/iiwa7.srdf: -------------------------------------------------------------------------------- 1 | 2 | 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 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /iiwa_moveit/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 3 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 4 | joint_limits: 5 | iiwa_joint_1: 6 | has_velocity_limits: true 7 | max_velocity: 10 8 | has_acceleration_limits: false 9 | max_acceleration: 0 10 | iiwa_joint_2: 11 | has_velocity_limits: true 12 | max_velocity: 10 13 | has_acceleration_limits: false 14 | max_acceleration: 0 15 | iiwa_joint_3: 16 | has_velocity_limits: true 17 | max_velocity: 10 18 | has_acceleration_limits: false 19 | max_acceleration: 0 20 | iiwa_joint_4: 21 | has_velocity_limits: true 22 | max_velocity: 10 23 | has_acceleration_limits: false 24 | max_acceleration: 0 25 | iiwa_joint_5: 26 | has_velocity_limits: true 27 | max_velocity: 10 28 | has_acceleration_limits: false 29 | max_acceleration: 0 30 | iiwa_joint_6: 31 | has_velocity_limits: true 32 | max_velocity: 10 33 | has_acceleration_limits: false 34 | max_acceleration: 0 35 | iiwa_joint_7: 36 | has_velocity_limits: true 37 | max_velocity: 10 38 | has_acceleration_limits: false 39 | max_acceleration: 0 -------------------------------------------------------------------------------- /iiwa_moveit/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | manipulator: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.005 5 | -------------------------------------------------------------------------------- /iiwa_moveit/config/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planner_configs: 2 | SBLkConfigDefault: 3 | type: geometric::SBL 4 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 5 | ESTkConfigDefault: 6 | type: geometric::EST 7 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 8 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 9 | LBKPIECEkConfigDefault: 10 | type: geometric::LBKPIECE 11 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 12 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 13 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 14 | BKPIECEkConfigDefault: 15 | type: geometric::BKPIECE 16 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 17 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 18 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 19 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 20 | KPIECEkConfigDefault: 21 | type: geometric::KPIECE 22 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 23 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 24 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 25 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 26 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 27 | RRTkConfigDefault: 28 | type: geometric::RRT 29 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 30 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 31 | RRTConnectkConfigDefault: 32 | type: geometric::RRTConnect 33 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 34 | RRTstarkConfigDefault: 35 | type: geometric::RRTstar 36 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 37 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 38 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 39 | TRRTkConfigDefault: 40 | type: geometric::TRRT 41 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 42 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 43 | max_states_failed: 10 # when to start increasing temp. default: 10 44 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 45 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 46 | init_temperature: 10e-6 # initial temperature. default: 10e-6 47 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 48 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 49 | k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() 50 | PRMkConfigDefault: 51 | type: geometric::PRM 52 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 53 | PRMstarkConfigDefault: 54 | type: geometric::PRMstar 55 | manipulator: 56 | planner_configs: 57 | - SBLkConfigDefault 58 | - ESTkConfigDefault 59 | - LBKPIECEkConfigDefault 60 | - BKPIECEkConfigDefault 61 | - KPIECEkConfigDefault 62 | - RRTkConfigDefault 63 | - RRTConnectkConfigDefault 64 | - RRTstarkConfigDefault 65 | - TRRTkConfigDefault 66 | - PRMkConfigDefault 67 | - PRMstarkConfigDefault -------------------------------------------------------------------------------- /iiwa_moveit/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/iiwa_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/iiwa_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/move_group.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/moveit_planning_execution.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 34 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/planning_context.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/run_benchmark_ompl.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/trajectory_execution.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /iiwa_moveit/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /iiwa_moveit/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | iiwa_moveit 30 | 1.4.0 31 | 32 | A package with all the configuration and launch files for using the KUKA LBR IIWA with the MoveIt Motion Planning Framework 33 | 34 | Yoan Mollard 35 | Konstantinos Chatzilygeroudis 36 | 37 | GPL-3.0-or-later 38 | 39 | catkin 40 | 41 | moveit_ros_move_group 42 | moveit_setup_assistant 43 | moveit_planners_ompl 44 | moveit_fake_controller_manager 45 | moveit_simple_controller_manager 46 | joint_state_controller 47 | joint_trajectory_controller 48 | position_controllers 49 | velocity_controllers 50 | effort_controllers 51 | gazebo_ros_control 52 | moveit_ros_planning_interface 53 | moveit_ros_warehouse 54 | moveit_ros_visualization 55 | joint_state_publisher 56 | robot_state_publisher 57 | xacro 58 | iiwa_description 59 | 60 | 61 | -------------------------------------------------------------------------------- /iiwa_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(iiwa_ros) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() -------------------------------------------------------------------------------- /iiwa_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | iiwa_ros 30 | 0.0.1 31 | iiwa_ros metapackage contains packages to integrate into KUKA's IIWA robots. 32 | 33 | Konstantinos Chatzilygeroudis 34 | 35 | 36 | GPL-3.0-or-later 37 | 38 | https://github.com/epfl-lasa/iiwa_ros 39 | https://github.com/epfl-lasa/iiwa_ros/issues 40 | 41 | Konstantinos Chatzilygeroudis 42 | 43 | catkin 44 | 45 | iiwa_description 46 | iiwa_driver 47 | iiwa_control 48 | iiwa_gazebo 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /iiwa_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #| 2 | #| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 3 | #| Authors: Konstantinos Chatzilygeroudis (maintainer) 4 | #| Matthias Mayr 5 | #| Bernardo Fichera 6 | #| email: costashatz@gmail.com 7 | #| matthias.mayr@cs.lth.se 8 | #| bernardo.fichera@epfl.ch 9 | #| Other contributors: 10 | #| Yoan Mollard (yoan@aubrune.eu) 11 | #| Walid Amanhoud (walid.amanhoud@epfl.ch) 12 | #| website: lasa.epfl.ch 13 | #| 14 | #| This file is part of iiwa_ros. 15 | #| 16 | #| iiwa_ros is free software: you can redistribute it and/or modify 17 | #| it under the terms of the GNU General Public License as published by 18 | #| the Free Software Foundation, either version 3 of the License, or 19 | #| (at your option) any later version. 20 | #| 21 | #| iiwa_ros is distributed in the hope that it will be useful, 22 | #| but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | #| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | #| GNU General Public License for more details. 25 | #| 26 | cmake_minimum_required(VERSION 3.1.0) 27 | project(iiwa_tools) 28 | 29 | # When enabling SIMD, both the dependencies (e.g. RBDyn) and subsequent code need to be compiled with SIMD 30 | option(ENABLE_SIMD "Build with all SIMD instructions on the current local machine" OFF) 31 | 32 | find_package(PkgConfig) 33 | 34 | find_package(mc_rbdyn_urdf REQUIRED) 35 | 36 | # Find catkin components 37 | find_package(catkin REQUIRED COMPONENTS 38 | roscpp 39 | message_generation 40 | std_msgs 41 | sensor_msgs 42 | geometry_msgs 43 | ) 44 | 45 | add_service_files( 46 | FILES 47 | GetFK.srv 48 | GetIK.srv 49 | GetJacobian.srv 50 | GetJacobians.srv 51 | GetGravity.srv 52 | GetMassMatrix.srv 53 | ) 54 | 55 | generate_messages( 56 | DEPENDENCIES 57 | std_msgs 58 | sensor_msgs 59 | geometry_msgs 60 | ) 61 | 62 | catkin_package( 63 | INCLUDE_DIRS include 64 | CATKIN_DEPENDS roscpp message_runtime std_msgs sensor_msgs geometry_msgs 65 | LIBRARIES iiwa_tools 66 | ) 67 | 68 | add_library(iiwa_tools SHARED src/iiwa_tools.cpp) 69 | target_compile_options(iiwa_tools PUBLIC -std=c++11) 70 | target_include_directories(iiwa_tools PUBLIC include ${catkin_INCLUDE_DIRS}) 71 | target_link_libraries(iiwa_tools PUBLIC ${catkin_LIBRARIES} mc_rbdyn_urdf::mc_rbdyn_urdf) 72 | 73 | add_executable(iiwa_service src/iiwa_service.cpp src/iiwa_service_node.cpp) 74 | 75 | if(ENABLE_SIMD) 76 | if(CMAKE_COMPILER_IS_GNUCXX) 77 | execute_process( 78 | COMMAND ${CMAKE_CXX_COMPILER} -dumpfullversion -dumpversion OUTPUT_VARIABLE GCC_VERSION) 79 | set(CXX_COMPILER_VERSION ${GCC_VERSION}) 80 | target_compile_options(iiwa_service PUBLIC -march=native) 81 | target_compile_options(iiwa_tools PUBLIC -march=native) 82 | if(GCC_VERSION VERSION_GREATER 7.0) 83 | target_compile_options(iiwa_service PUBLIC -faligned-new) 84 | target_compile_options(iiwa_tools PUBLIC -faligned-new) 85 | endif() 86 | elseif("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") 87 | target_compile_options(iiwa_service PUBLIC -march=native -faligned-new) 88 | target_compile_options(iiwa_tools PUBLIC -march=native -faligned-new) 89 | endif() 90 | endif() 91 | 92 | target_compile_options(iiwa_service PUBLIC -std=c++11) 93 | target_include_directories(iiwa_service PUBLIC include ${catkin_INCLUDE_DIRS}) 94 | target_link_libraries(iiwa_service PUBLIC ${catkin_LIBRARIES} iiwa_tools) 95 | 96 | add_dependencies(iiwa_service iiwa_tools_generate_messages_cpp) 97 | 98 | # Install 99 | install(TARGETS iiwa_service iiwa_tools 100 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 101 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 102 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 103 | ) 104 | 105 | install(DIRECTORY include/${PROJECT_NAME} 106 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 107 | ) 108 | 109 | install(DIRECTORY launch 110 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 111 | 112 | install(DIRECTORY config 113 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 114 | -------------------------------------------------------------------------------- /iiwa_tools/config/iiwa_service.yaml: -------------------------------------------------------------------------------- 1 | service: 2 | robot_description: /robot_description 3 | end_effector: iiwa_link_ee 4 | fk_service_name: iiwa_fk_server 5 | ik_service_name: iiwa_ik_server 6 | jacobian_service_name: iiwa_jacobian_server 7 | jacobian_deriv_service_name: iiwa_jacobian_deriv_server 8 | jacobians_service_name: iiwa_jacobians_server 9 | gravity_service_name: iiwa_gravity_server -------------------------------------------------------------------------------- /iiwa_tools/include/iiwa_tools/cvxgen/description.cvxgen: -------------------------------------------------------------------------------- 1 | # Produced by CVXGEN, 2019-02-27 17:57:27 -0500. 2 | # CVXGEN is Copyright (C) 2006-2017 Jacob Mattingley, jem@cvxgen.com. 3 | # The code in this file is Copyright (C) 2006-2017 Jacob Mattingley. 4 | # CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | # applications without prior written permission from Jacob Mattingley. 6 | 7 | # Filename: description.cvxgen. 8 | # Description: A description of the CVXGEN problem. 9 | 10 | dimensions 11 | N = 7 12 | M = 6 13 | end 14 | 15 | parameters 16 | damping (N) nonnegative 17 | slack (M) nonnegative 18 | J[i] (N),i=1..M # Jacobian 19 | dx (M) # error in end-effector 20 | qlow (N) # lower limits 21 | qup (N) # upper limits 22 | qref (N) # actual angles 23 | end 24 | 25 | variables 26 | dq (N) # step for joint angles 27 | delta (M) # slack variable 28 | end 29 | 30 | minimize 31 | damping'*square(qref+dq) + slack'*square(delta) 32 | subject to 33 | sum[i=1..N](J[j][i]*dq[i]) == dx[j] + delta[j], j=1..M 34 | qlow[i] <= qref[i]+dq[i] <= qup[i], i=1..N 35 | end 36 | -------------------------------------------------------------------------------- /iiwa_tools/include/iiwa_tools/cvxgen/format_cvxgen.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # encoding: utf-8 3 | #| 4 | #| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 5 | #| Authors: Konstantinos Chatzilygeroudis (maintainer) 6 | #| Matthias Mayr 7 | #| Bernardo Fichera 8 | #| email: costashatz@gmail.com 9 | #| matthias.mayr@cs.lth.se 10 | #| bernardo.fichera@epfl.ch 11 | #| Other contributors: 12 | #| Yoan Mollard (yoan@aubrune.eu) 13 | #| Walid Amanhoud (walid.amanhoud@epfl.ch) 14 | #| website: lasa.epfl.ch 15 | #| 16 | #| This file is part of iiwa_ros. 17 | #| 18 | #| iiwa_ros is free software: you can redistribute it and/or modify 19 | #| it under the terms of the GNU General Public License as published by 20 | #| the Free Software Foundation, either version 3 of the License, or 21 | #| (at your option) any later version. 22 | #| 23 | #| iiwa_ros is distributed in the hope that it will be useful, 24 | #| but WITHOUT ANY WARRANTY; without even the implied warranty of 25 | #| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 26 | #| GNU General Public License for more details. 27 | #| 28 | import sys 29 | import subprocess 30 | 31 | def read_file(fname): 32 | with open(fname) as f: 33 | return f.readlines() 34 | return [] 35 | 36 | 37 | def remove_initial_comments(data): 38 | i = 0 39 | for k in range(len(data)): 40 | if data[k][0] == '#': 41 | i = k 42 | break 43 | return data[i:] 44 | 45 | def remove_ifdefs(data, label): 46 | to_del = [] 47 | 48 | depth = 0 49 | for k in range(len(data)): 50 | line = data[k] 51 | if line.startswith("#ifndef " + label) or line.startswith("#ifdef " + label): 52 | depth = depth + 1 53 | if depth > 0: 54 | to_del.append(k) 55 | if line.startswith("#endif"): 56 | depth = depth - 1 57 | 58 | cdata = data 59 | for i in range(len(to_del)): 60 | del cdata[to_del[i]-i] 61 | return cdata 62 | 63 | def struct_names(): 64 | return ['Params', 'Vars', 'Workspace', 'Settings'] 65 | 66 | def function_names(): 67 | return ['ldl_solve', 'ldl_factor', 'check_factorization', 'matrix_multiply', 'check_residual', 'fill_KKT', 'multbymA', 'multbymAT', 'multbymG', 'multbymGT', 'multbyP', 'fillq', 'fillh', 'fillb', 'pre_ops', 'eval_gap', 'set_defaults', 'setup_pointers', 'setup_indexed_params', 'setup_indexing', 'set_start', 'eval_objv', 'fillrhs_aff', 'fillrhs_cc', 'refine', 'calc_ineq_resid_squared', 'calc_eq_resid_squared', 'better_start', 'fillrhs_start', 'solve', 'printmatrix', 'unif', 'ran1', 'randn_internal', 'randn', 'reset_rand'] 68 | # return ['ldl_solve', 'ldl_factor', 'check_factorization', 'matrix_multiply', 'check_residual', 'fill_KKT', 'multbymA', 'multbymAT', 'multbymG', 'multbymGT', 'multbyP', 'fillq', 'fillh', 'fillb', 'pre_ops', 'eval_gap', 'set_defaults', 'setup_pointers', 'setup_indexed_params', 'setup_indexing', 'set_start', 'eval_objv', 'fillrhs_aff', 'fillrhs_cc', 'refine', 'calc_ineq_resid_squared', 'calc_eq_resid_squared', 'better_start', 'fillrhs_start', 'solve', 'tic', 'toc', 'tocq', 'printmatrix', 'unif', 'ran1', 'randn_internal', 'randn', 'reset_rand'] 69 | 70 | def get_function(data, fname): 71 | func_name = fname + "(" 72 | ret_data = [] 73 | depth = 0 74 | 75 | for l in data: 76 | line = l.strip() 77 | if line.startswith('void ' + func_name) or line.startswith('long ' + func_name) or line.startswith('double ' + func_name) or line.startswith('int ' + func_name) or line.startswith('float ' + func_name): 78 | depth = depth + 1 79 | if depth > 0: 80 | ret_data.append(l) 81 | if line.endswith('{') and (func_name not in line): 82 | depth = depth + 1 83 | if line.startswith('}') or line.endswith('}'): 84 | depth = depth - 1 85 | 86 | return ret_data 87 | 88 | def get_struct(data, sname): 89 | struct_name = 'typedef struct ' + sname + "_t" 90 | ret_data = [] 91 | depth = 0 92 | 93 | for line in data: 94 | if depth > 0: 95 | if line.startswith('}'): 96 | depth = depth - 1 97 | else: 98 | ret_data.append(line) 99 | 100 | if line.startswith(struct_name): 101 | depth = depth + 1 102 | 103 | return ret_data 104 | 105 | if __name__== "__main__": 106 | arg_num = len(sys.argv[1:]) 107 | 108 | prefix = 'cvxgen' 109 | directory = "./" 110 | if arg_num >=1: 111 | directory = sys.argv[1] + "/" 112 | if arg_num >=2: 113 | prefix = sys.argv[2] 114 | output = prefix + ".hpp" 115 | if arg_num >=3: 116 | output = sys.argv[3] 117 | 118 | filenames = ['solver.h', 'solver.c', 'ldl.c', 'util.c', 'matrix_support.c'] 119 | filenames = [directory + f for f in filenames] 120 | 121 | sources = [] 122 | for f in filenames: 123 | sources.append(remove_initial_comments(read_file(f))) 124 | 125 | structs = [] 126 | for sname in struct_names(): 127 | structs.append(get_struct(sources[0], sname)) 128 | 129 | functions = [] 130 | 131 | for func_name in function_names(): 132 | for source in sources[1:]: 133 | d = get_function(source, func_name) 134 | if len(d)>0: 135 | functions.append(d) 136 | break 137 | 138 | tab = " " 139 | out_file = "#include \n"; 140 | out_file += "#include \n"; 141 | out_file += "#include \n\n"; 142 | 143 | out_file += "#define IA 16807\n"; 144 | out_file += "#define IM 2147483647\n" 145 | out_file += "#define AM (1.0/IM)\n" 146 | out_file += "#define IQ 127773\n" 147 | out_file += "#define IR 2836\n" 148 | out_file += "#define NTAB 32\n" 149 | out_file += "#define NDIV (1+(IM-1)/NTAB)\n" 150 | out_file += "#define EPS 1.2e-7\n" 151 | out_file += "#define RNMX (1.0-EPS)\n\n" 152 | 153 | out_file += "namespace " + prefix + " {\n\n" 154 | 155 | for i in range(len(structs)): 156 | name = struct_names()[i] 157 | out_file += tab + "struct " + name + " {\n" 158 | for l in structs[i]: 159 | out_file += tab + l 160 | out_file += tab + "};\n\n" 161 | 162 | out_file += tab + "class Solver {\n" + tab + "public:\n"; 163 | 164 | # out_file += "\n" + tab + "protected:\n"; 165 | 166 | for i in range(len(structs)): 167 | name = struct_names()[i] 168 | lower = name.lower() 169 | if name == 'Workspace': 170 | lower = 'work' 171 | out_file += tab + tab + name + " " + lower + ";\n" 172 | 173 | out_file += "\n" + tab + tab + "long global_seed = 1;\n"; 174 | # out_file += tab + tab + "clock_t tic_timestart;\n\n" 175 | out_file += "\n" 176 | 177 | for f in functions: 178 | for l in f: 179 | out_file += tab + tab + l 180 | out_file += "\n" 181 | 182 | out_file += tab + "};\n}" 183 | 184 | text_file = open(directory + output, "w") 185 | text_file.write(out_file) 186 | text_file.close() 187 | 188 | subprocess.call("clang-format -i " + directory + output, shell=True) 189 | 190 | # for arg in sys.argv[1:]: 191 | # print arg -------------------------------------------------------------------------------- /iiwa_tools/include/iiwa_tools/iiwa_service.h: -------------------------------------------------------------------------------- 1 | //| 2 | //| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 3 | //| Authors: Konstantinos Chatzilygeroudis (maintainer) 4 | //| Matthias Mayr 5 | //| Bernardo Fichera 6 | //| email: costashatz@gmail.com 7 | //| matthias.mayr@cs.lth.se 8 | //| bernardo.fichera@epfl.ch 9 | //| Other contributors: 10 | //| Yoan Mollard (yoan@aubrune.eu) 11 | //| Walid Amanhoud (walid.amanhoud@epfl.ch) 12 | //| website: lasa.epfl.ch 13 | //| 14 | //| This file is part of iiwa_ros. 15 | //| 16 | //| iiwa_ros is free software: you can redistribute it and/or modify 17 | //| it under the terms of the GNU General Public License as published by 18 | //| the Free Software Foundation, either version 3 of the License, or 19 | //| (at your option) any later version. 20 | //| 21 | //| iiwa_ros is distributed in the hope that it will be useful, 22 | //| but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | //| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | //| GNU General Public License for more details. 25 | //| 26 | #ifndef IIWA_TOOLS_IIWA_SERVICE_H 27 | #define IIWA_TOOLS_IIWA_SERVICE_H 28 | 29 | // std headers 30 | #include 31 | 32 | // ROS headers 33 | #include 34 | #include 35 | 36 | // IIWA Tools 37 | #include 38 | 39 | // Iiwa IK server headers 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | namespace iiwa_tools { 48 | class IiwaService { 49 | public: 50 | IiwaService(ros::NodeHandle nh); 51 | 52 | void init(); 53 | 54 | bool perform_fk(iiwa_tools::GetFK::Request& request, 55 | iiwa_tools::GetFK::Response& response); 56 | 57 | bool perform_ik(iiwa_tools::GetIK::Request& request, 58 | iiwa_tools::GetIK::Response& response); 59 | 60 | bool get_jacobian(iiwa_tools::GetJacobian::Request& request, 61 | iiwa_tools::GetJacobian::Response& response); 62 | 63 | bool get_jacobian_deriv(iiwa_tools::GetJacobian::Request& request, 64 | iiwa_tools::GetJacobian::Response& response); 65 | 66 | bool get_jacobians(iiwa_tools::GetJacobians::Request& request, 67 | iiwa_tools::GetJacobians::Response& response); 68 | 69 | bool get_gravity(iiwa_tools::GetGravity::Request& request, 70 | iiwa_tools::GetGravity::Response& response); 71 | 72 | bool get_mass_matrix(iiwa_tools::GetMassMatrix::Request& request, 73 | iiwa_tools::GetMassMatrix::Response& response); 74 | 75 | protected: 76 | void _load_params(); 77 | 78 | // ROS related 79 | ros::NodeHandle _nh; 80 | std::string _robot_description, _fk_service_name, _ik_service_name, _jacobian_service_name, _jacobian_deriv_service_name, _jacobians_service_name, _gravity_service_name, _mass_service_name; 81 | ros::ServiceServer _fk_server, _ik_server, _jacobian_server, _jacobian_deriv_server, _jacobians_server, _gravity_server, _mass_server; 82 | 83 | // Robot 84 | unsigned int _n_joints; 85 | std::string _end_effector; 86 | 87 | // IIWA Tools 88 | IiwaTools _tools; 89 | }; // class IiwaService 90 | } // namespace iiwa_tools 91 | 92 | #endif // IIWA_SERVICE_IIWA_SERVICE_H 93 | -------------------------------------------------------------------------------- /iiwa_tools/include/iiwa_tools/iiwa_tools.h: -------------------------------------------------------------------------------- 1 | //| 2 | //| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 3 | //| Authors: Konstantinos Chatzilygeroudis (maintainer) 4 | //| Matthias Mayr 5 | //| Bernardo Fichera 6 | //| email: costashatz@gmail.com 7 | //| matthias.mayr@cs.lth.se 8 | //| bernardo.fichera@epfl.ch 9 | //| Other contributors: 10 | //| Yoan Mollard (yoan@aubrune.eu) 11 | //| Walid Amanhoud (walid.amanhoud@epfl.ch) 12 | //| website: lasa.epfl.ch 13 | //| 14 | //| This file is part of iiwa_ros. 15 | //| 16 | //| iiwa_ros is free software: you can redistribute it and/or modify 17 | //| it under the terms of the GNU General Public License as published by 18 | //| the Free Software Foundation, either version 3 of the License, or 19 | //| (at your option) any later version. 20 | //| 21 | //| iiwa_ros is distributed in the hope that it will be useful, 22 | //| but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | //| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | //| GNU General Public License for more details. 25 | //| 26 | #ifndef IIWA_TOOLS_IIWA_TOOLS_H 27 | #define IIWA_TOOLS_IIWA_TOOLS_H 28 | 29 | // std headers 30 | #include 31 | #include 32 | 33 | // RBDyn headers 34 | #include 35 | #include 36 | 37 | namespace iiwa_tools { 38 | struct RobotState { 39 | Eigen::VectorXd position, 40 | velocity, 41 | torque; 42 | }; 43 | 44 | struct EefState { 45 | Eigen::Vector3d translation; 46 | Eigen::Quaterniond orientation; 47 | }; 48 | 49 | class IiwaTools { 50 | public: 51 | IiwaTools() {} 52 | ~IiwaTools() {} 53 | 54 | void init_rbdyn(const std::string& urdf_string, const std::string& end_effector); 55 | 56 | std::vector get_indices() { return _rbd_indices; } 57 | 58 | EefState perform_fk(const RobotState& robot_state); 59 | Eigen::VectorXd perform_ik(const EefState& ee_state, const RobotState& seed_state = RobotState()); 60 | Eigen::MatrixXd jacobian(const RobotState& robot_state); 61 | Eigen::MatrixXd jacobian_deriv(const RobotState& robot_state); 62 | std::pair jacobians(const RobotState& robot_state); 63 | Eigen::VectorXd gravity(const std::vector& gravity, const RobotState& robot_state); 64 | Eigen::MatrixXd mass_matrix(const RobotState& robot_state); 65 | 66 | protected: 67 | size_t _rbd_index(const std::string& body_name) const; 68 | void _update_urdf_state(mc_rbdyn_urdf::URDFParserResult& rbdyn_urdf, const RobotState& robot_state); 69 | 70 | // RBDyn related 71 | mc_rbdyn_urdf::URDFParserResult _rbdyn_urdf; 72 | std::unique_ptr _ik; 73 | 74 | // Helper variables 75 | std::vector _rbd_indices; 76 | size_t _ef_index; 77 | 78 | }; // class IiwaTools 79 | } // namespace iiwa_tools 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /iiwa_tools/launch/iiwa_service.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 36 | 37 | -------------------------------------------------------------------------------- /iiwa_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | iiwa_tools 30 | 0.0.1 31 | This package creates a few tools as a library and as services (e.g., IK, Jacobian etc.) for the IIWA robot 32 | Konstantinos Chatzilygeroudis 33 | 34 | GPL-3.0-or-later 35 | 36 | https://github.com/epfl-lasa/iiwa_ros 37 | https://github.com/epfl-lasa/iiwa_ros/issues 38 | 39 | Konstantinos Chatzilygeroudis 40 | 41 | catkin 42 | 43 | roscpp 44 | message_generation 45 | std_msgs 46 | sensor_msgs 47 | geometry_msgs 48 | 49 | iiwa_description 50 | roscpp 51 | message_runtime 52 | std_msgs 53 | sensor_msgs 54 | geometry_msgs 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /iiwa_tools/src/iiwa_service_node.cpp: -------------------------------------------------------------------------------- 1 | //| 2 | //| Copyright (C) 2019-2022 Learning Algorithms and Systems Laboratory, EPFL, Switzerland 3 | //| Authors: Konstantinos Chatzilygeroudis (maintainer) 4 | //| Matthias Mayr 5 | //| Bernardo Fichera 6 | //| email: costashatz@gmail.com 7 | //| matthias.mayr@cs.lth.se 8 | //| bernardo.fichera@epfl.ch 9 | //| Other contributors: 10 | //| Yoan Mollard (yoan@aubrune.eu) 11 | //| Walid Amanhoud (walid.amanhoud@epfl.ch) 12 | //| website: lasa.epfl.ch 13 | //| 14 | //| This file is part of iiwa_ros. 15 | //| 16 | //| iiwa_ros is free software: you can redistribute it and/or modify 17 | //| it under the terms of the GNU General Public License as published by 18 | //| the Free Software Foundation, either version 3 of the License, or 19 | //| (at your option) any later version. 20 | //| 21 | //| iiwa_ros is distributed in the hope that it will be useful, 22 | //| but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | //| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | //| GNU General Public License for more details. 25 | //| 26 | #include 27 | 28 | // Iiwa IK server headers 29 | #include 30 | 31 | int main(int argc, char** argv) 32 | { 33 | ros::init(argc, argv, "iiwa_service"); 34 | ros::NodeHandle nh; 35 | 36 | iiwa_tools::IiwaService tools_server(nh); 37 | 38 | ros::spin(); 39 | 40 | return 0; 41 | } -------------------------------------------------------------------------------- /iiwa_tools/srv/GetFK.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Float64MultiArray joints 2 | --- 3 | geometry_msgs/Pose[] poses -------------------------------------------------------------------------------- /iiwa_tools/srv/GetGravity.srv: -------------------------------------------------------------------------------- 1 | float64[] joint_angles 2 | float64[] joint_velocities 3 | float64[] joint_torques 4 | float64[] gravity 5 | --- 6 | float64[] compensation_torques -------------------------------------------------------------------------------- /iiwa_tools/srv/GetIK.srv: -------------------------------------------------------------------------------- 1 | # end-effector poses to request IK for 2 | geometry_msgs/Pose[] poses 3 | 4 | # (optional) initial joint position seeds for IK solver 5 | # one for each pose 6 | std_msgs/Float64MultiArray seed_angles 7 | 8 | # (optional) optimization parameters 9 | float32 tolerance 10 | int32 max_iterations 11 | float64[] slack 12 | float64[] damping 13 | --- 14 | # joints[i] == joint angle solution for each pose_state[i] 15 | std_msgs/Float64MultiArray joints 16 | float64[] accepted_tolerance 17 | bool[] is_valid -------------------------------------------------------------------------------- /iiwa_tools/srv/GetJacobian.srv: -------------------------------------------------------------------------------- 1 | float64[] joint_angles 2 | float64[] joint_velocities 3 | --- 4 | std_msgs/Float64MultiArray jacobian -------------------------------------------------------------------------------- /iiwa_tools/srv/GetJacobians.srv: -------------------------------------------------------------------------------- 1 | float64[] joint_angles 2 | float64[] joint_velocities 3 | --- 4 | std_msgs/Float64MultiArray jacobian 5 | std_msgs/Float64MultiArray jacobian_deriv -------------------------------------------------------------------------------- /iiwa_tools/srv/GetMassMatrix.srv: -------------------------------------------------------------------------------- 1 | float64[] joint_angles 2 | --- 3 | std_msgs/Float64MultiArray mass_matrix --------------------------------------------------------------------------------