├── lbr_description ├── test │ └── __init__.py ├── lbr_description.dsv ├── meshes │ ├── 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 │ ├── med14 │ │ └── 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 │ ├── med7 │ │ └── 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 │ └── iiwa14 │ │ └── 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 ├── ros2_control │ ├── initial_joint_positions.yaml │ ├── gazebo_controllers.yaml │ ├── mock_controllers.yaml │ └── lbr_system_config.yaml ├── README.md ├── CMakeLists.txt ├── package.xml ├── urdf │ ├── iiwa14 │ │ ├── joint_limits.yaml │ │ └── iiwa14.xacro │ ├── iiwa7 │ │ ├── joint_limits.yaml │ │ └── iiwa7.xacro │ ├── med14 │ │ ├── joint_limits.yaml │ │ └── med14.xacro │ └── med7 │ │ ├── joint_limits.yaml │ │ └── med7.xacro └── gazebo │ └── lbr_gazebo.xacro ├── lbr_bringup ├── lbr_bringup │ ├── __init__.py │ ├── gazebo.py │ └── rviz.py ├── CMakeLists.txt ├── launch │ ├── rviz.launch.py │ └── moveit_servo.launch.py └── package.xml ├── lbr_demos ├── lbr_moveit │ ├── lbr_moveit │ │ └── __init__.py │ ├── resource │ │ └── lbr_moveit │ ├── scripts │ │ ├── __init__.py │ │ └── forward_keyboard.py │ ├── setup.cfg │ ├── doc │ │ └── img │ │ │ └── iiwa7_moveit_rviz.png │ ├── package.xml │ ├── test │ │ ├── test_pep257.py │ │ ├── test_flake8.py │ │ └── test_copyright.py │ ├── setup.py │ ├── config │ │ └── forward_keyboard.yaml │ └── launch │ │ └── keyboard_driver.launch.py ├── lbr_demos_py │ ├── lbr_demos_py │ │ └── __init__.py │ ├── resource │ │ └── lbr_demos_py │ ├── setup.cfg │ ├── test │ │ ├── test_pep257.py │ │ ├── test_flake8.py │ │ └── test_copyright.py │ ├── package.xml │ └── setup.py ├── lbr_demos_advanced_py │ ├── resource │ │ └── lbr_demos_advanced_py │ ├── lbr_demos_advanced_py │ │ ├── __init__.py │ │ ├── admittance_controller.py │ │ └── lbr_base_position_command_node.py │ ├── setup.cfg │ ├── config │ │ ├── admittance_control.yaml │ │ └── admittance_rcm_control.yaml │ ├── test │ │ ├── test_pep257.py │ │ ├── test_flake8.py │ │ └── test_copyright.py │ ├── package.xml │ └── setup.py ├── doc │ ├── img │ │ ├── applications_brake_test.png │ │ ├── applications_lbr_server.png │ │ ├── applications_joint_sine_overlay.png │ │ ├── applications_torque_sine_overlay.png │ │ └── applications_wrench_sine_overlay.png │ └── lbr_demos.rst ├── lbr_moveit_cpp │ ├── package.xml │ ├── CMakeLists.txt │ ├── launch │ │ └── hello_moveit.launch.py │ └── src │ │ └── hello_moveit.cpp ├── lbr_demos_advanced_cpp │ ├── package.xml │ ├── src │ │ └── pose_planning_node.cpp │ └── CMakeLists.txt └── lbr_demos_cpp │ ├── package.xml │ ├── CMakeLists.txt │ └── src │ ├── torque_sine_overlay.cpp │ └── joint_sine_overlay.cpp ├── .gitignore ├── lbr_fri_ros2_stack ├── doc │ ├── lbr_fri_ros2_stack.rst │ └── img │ │ ├── rviz │ │ ├── iiwa14_r820.png │ │ ├── iiwa7_r800.png │ │ ├── med14_r820.png │ │ └── med7_r800.png │ │ ├── foxglove │ │ ├── med7_r800.png │ │ ├── iiwa14_r820.png │ │ ├── iiwa7_r800.png │ │ └── med14_r820.png │ │ ├── controller │ │ ├── raw │ │ │ ├── home.png │ │ │ ├── safety.png │ │ │ ├── activation.png │ │ │ ├── applications.png │ │ │ ├── lbr_server_ip_address.png │ │ │ ├── lbr_server_control_mode.png │ │ │ ├── lbr_server_send_period.png │ │ │ ├── lbr_server_wait_connection.png │ │ │ └── lbr_server_client_command_mode.png │ │ └── highlighted │ │ │ ├── activation_safety.png │ │ │ ├── activation_activate.png │ │ │ └── activation_activation.png │ │ └── computer │ │ ├── 00_link_path_refresh.png │ │ ├── 00_station_setup_topology.png │ │ ├── 00_sunrise_create_project.png │ │ ├── 01_station_setup_software.png │ │ ├── 03_station_setup_installation.png │ │ ├── 00_lbr_fri_ros2_create_package.png │ │ ├── 02_station_setup_configuration.png │ │ ├── 01_lbr_fri_ros2_create_package_name.png │ │ ├── 01_sunrise_create_project_default_ip.png │ │ ├── 04_station_setup_installation_install.png │ │ ├── 05_station_setup_installation_reboot.png │ │ ├── 02_sunrise_create_project_project_name.png │ │ ├── 03_sunrise_create_project_robot_select.png │ │ ├── 04_sunrise_create_project_media_flange.png │ │ ├── 05_sunrise_create_project_unselect_wizard.png │ │ └── 06_station_setup_installation_synchronize.png ├── CMakeLists.txt ├── repos-fri-1.11.yaml ├── repos-fri-1.14.yaml ├── repos-fri-1.15.yaml ├── repos-fri-1.16.yaml ├── repos-fri-2.5.yaml ├── repos-fri-2.7.yaml ├── repos-fri-2.6.yaml └── package.xml ├── docker ├── container_new_console.sh ├── container_start.sh ├── container_build.sh ├── Dockerfile └── doc │ └── docker.rst ├── lbr_moveit_config ├── doc │ └── img │ │ ├── 04_robot_poses.png │ │ ├── 10_perception.png │ │ ├── 00_start_screen.png │ │ ├── 01_self_collision.png │ │ ├── 02_virtual_joints.png │ │ ├── 05_end_effectors.png │ │ ├── 06_passive_joints.png │ │ ├── 07_ros2_control.png │ │ ├── 11_launch_files.png │ │ ├── 03_planning_groups.png │ │ ├── 04_robot_poses_zero.png │ │ ├── 08_ros2_controllers.png │ │ ├── 09_moveit_controllers.png │ │ ├── 12_author_information.png │ │ ├── 13_configuration_files.png │ │ ├── 03_define_planning_groups.png │ │ ├── 04_robot_poses_zero_transport.png │ │ ├── 04_define_robot_poses_transport.png │ │ └── 03_define_planning_groups_kinematic_chain.png ├── iiwa7_moveit_config │ ├── config │ │ ├── pilz_cartesian_limits.yaml │ │ ├── kinematics.yaml │ │ ├── moveit_controllers.yaml │ │ └── joint_limits.yaml │ ├── launch │ │ ├── moveit_rviz.launch.py │ │ ├── setup_assistant.launch.py │ │ └── move_group.launch.py │ ├── CMakeLists.txt │ ├── .setup_assistant │ └── package.xml ├── med14_moveit_config │ ├── config │ │ ├── pilz_cartesian_limits.yaml │ │ ├── kinematics.yaml │ │ ├── moveit_controllers.yaml │ │ └── joint_limits.yaml │ ├── launch │ │ ├── moveit_rviz.launch.py │ │ ├── setup_assistant.launch.py │ │ └── move_group.launch.py │ ├── CMakeLists.txt │ ├── .setup_assistant │ └── package.xml ├── med7_moveit_config │ ├── config │ │ ├── pilz_cartesian_limits.yaml │ │ ├── kinematics.yaml │ │ ├── moveit_controllers.yaml │ │ └── joint_limits.yaml │ ├── launch │ │ ├── moveit_rviz.launch.py │ │ ├── setup_assistant.launch.py │ │ └── move_group.launch.py │ ├── CMakeLists.txt │ ├── .setup_assistant │ └── package.xml └── iiwa14_moveit_config │ ├── config │ ├── pilz_cartesian_limits.yaml │ ├── kinematics.yaml │ ├── moveit_controllers.yaml │ └── joint_limits.yaml │ ├── launch │ ├── moveit_rviz.launch.py │ ├── setup_assistant.launch.py │ └── move_group.launch.py │ ├── CMakeLists.txt │ ├── .setup_assistant │ └── package.xml ├── .github ├── workflows │ ├── black.yml │ ├── build-ubuntu-24.04-fri-2.5.yml │ ├── build-ubuntu-24.04-fri-2.7.yml │ ├── build-ubuntu-24.04-fri-1.11.yml │ ├── build-ubuntu-24.04-fri-1.14.yml │ ├── build-ubuntu-24.04-fri-1.15.yml │ ├── build-ubuntu-24.04-fri-1.16.yml │ ├── build.yml │ └── backport.yaml └── CONTRIBUTING.md ├── lbr_ros2_control ├── plugin_description_files │ ├── system_interface.xml │ └── controllers.xml ├── package.xml └── include │ └── lbr_ros2_control │ ├── system_interface_type_values.hpp │ └── controllers │ ├── lbr_joint_position_command_controller.hpp │ ├── lbr_state_broadcaster.hpp │ └── lbr_torque_command_controller.hpp ├── lbr_fri_ros2 ├── include │ └── lbr_fri_ros2 │ │ ├── math.hpp │ │ ├── worker.hpp │ │ ├── interfaces │ │ ├── torque_command.hpp │ │ ├── wrench_command.hpp │ │ ├── position_command.hpp │ │ ├── state.hpp │ │ └── base_command.hpp │ │ ├── pinv.hpp │ │ ├── guards │ │ ├── state_guard.hpp │ │ └── command_guard.hpp │ │ ├── utils.hpp │ │ ├── app.hpp │ │ ├── kinematics.hpp │ │ ├── types.hpp │ │ ├── async_client.hpp │ │ ├── formatting.hpp │ │ └── wrench_estimator.hpp ├── src │ ├── math.cpp │ ├── interfaces │ │ └── base_command.cpp │ ├── filters.cpp │ ├── worker.cpp │ ├── guards │ │ └── state_guard.cpp │ ├── wrench_estimator.cpp │ └── kinematics.cpp ├── doc │ └── lbr_fri_ros2.rst └── package.xml └── CITATION.cff /lbr_description/test/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /lbr_bringup/lbr_bringup/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit/lbr_moveit/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit/resource/lbr_moveit: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit/scripts/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_py/lbr_demos_py/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_py/resource/lbr_demos_py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | **/__pycache__ 3 | COLCON_IGNORE 4 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_py/resource/lbr_demos_advanced_py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /lbr_description/lbr_description.dsv: -------------------------------------------------------------------------------- 1 | prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/lbr_fri_ros2_stack.rst: -------------------------------------------------------------------------------- 1 | .. include:: ../../README.md 2 | :parser: myst_parser.sphinx_ 3 | -------------------------------------------------------------------------------- /docker/container_new_console.sh: -------------------------------------------------------------------------------- 1 | # dont forget to chmod +x container_new_console.sh 2 | docker exec -it lbr_stack_container bash 3 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/lbr_moveit 3 | [install] 4 | install_scripts=$base/lib/lbr_moveit 5 | -------------------------------------------------------------------------------- /docker/container_start.sh: -------------------------------------------------------------------------------- 1 | # dont forget to chmod +x container_new_console.sh 2 | xhost + 3 | 4 | docker start lbr_stack_container -i 5 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_py/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/lbr_demos_py 3 | [install] 4 | install_scripts=$base/lib/lbr_demos_py 5 | -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/04_robot_poses.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/04_robot_poses.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/10_perception.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/10_perception.png -------------------------------------------------------------------------------- /lbr_demos/doc/img/applications_brake_test.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_demos/doc/img/applications_brake_test.png -------------------------------------------------------------------------------- /lbr_demos/doc/img/applications_lbr_server.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_demos/doc/img/applications_lbr_server.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/rviz/iiwa14_r820.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/rviz/iiwa14_r820.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/rviz/iiwa7_r800.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/rviz/iiwa7_r800.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/rviz/med14_r820.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/rviz/med14_r820.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/rviz/med7_r800.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/rviz/med7_r800.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/00_start_screen.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/00_start_screen.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/01_self_collision.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/01_self_collision.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/02_virtual_joints.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/02_virtual_joints.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/05_end_effectors.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/05_end_effectors.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/06_passive_joints.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/06_passive_joints.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/07_ros2_control.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/07_ros2_control.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/11_launch_files.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/11_launch_files.png -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa7/collision/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa7/collision/link_0.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa7/collision/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa7/collision/link_1.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa7/collision/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa7/collision/link_2.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa7/collision/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa7/collision/link_3.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa7/collision/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa7/collision/link_4.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa7/collision/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa7/collision/link_5.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa7/collision/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa7/collision/link_6.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa7/collision/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa7/collision/link_7.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med14/collision/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med14/collision/link_0.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med14/collision/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med14/collision/link_1.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med14/collision/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med14/collision/link_2.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med14/collision/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med14/collision/link_3.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med14/collision/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med14/collision/link_4.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med14/collision/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med14/collision/link_5.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med14/collision/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med14/collision/link_6.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med14/collision/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med14/collision/link_7.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med7/collision/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med7/collision/link_0.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med7/collision/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med7/collision/link_1.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med7/collision/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med7/collision/link_2.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med7/collision/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med7/collision/link_3.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med7/collision/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med7/collision/link_4.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med7/collision/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med7/collision/link_5.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med7/collision/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med7/collision/link_6.stl -------------------------------------------------------------------------------- /lbr_description/meshes/med7/collision/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/med7/collision/link_7.stl -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/foxglove/med7_r800.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/foxglove/med7_r800.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/03_planning_groups.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/03_planning_groups.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/04_robot_poses_zero.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/04_robot_poses_zero.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/08_ros2_controllers.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/08_ros2_controllers.png -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_py/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/lbr_demos_advanced_py 3 | [install] 4 | install_scripts=$base/lib/lbr_demos_advanced_py 5 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit/doc/img/iiwa7_moveit_rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_demos/lbr_moveit/doc/img/iiwa7_moveit_rviz.png -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa14/collision/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa14/collision/link_0.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa14/collision/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa14/collision/link_1.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa14/collision/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa14/collision/link_2.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa14/collision/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa14/collision/link_3.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa14/collision/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa14/collision/link_4.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa14/collision/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa14/collision/link_5.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa14/collision/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa14/collision/link_6.stl -------------------------------------------------------------------------------- /lbr_description/meshes/iiwa14/collision/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_description/meshes/iiwa14/collision/link_7.stl -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/controller/raw/home.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/controller/raw/home.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/controller/raw/safety.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/controller/raw/safety.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/foxglove/iiwa14_r820.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/foxglove/iiwa14_r820.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/foxglove/iiwa7_r800.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/foxglove/iiwa7_r800.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/foxglove/med14_r820.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/foxglove/med14_r820.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/09_moveit_controllers.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/09_moveit_controllers.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/12_author_information.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/12_author_information.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/13_configuration_files.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/13_configuration_files.png -------------------------------------------------------------------------------- /lbr_demos/doc/img/applications_joint_sine_overlay.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_demos/doc/img/applications_joint_sine_overlay.png -------------------------------------------------------------------------------- /lbr_demos/doc/img/applications_torque_sine_overlay.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_demos/doc/img/applications_torque_sine_overlay.png -------------------------------------------------------------------------------- /lbr_demos/doc/img/applications_wrench_sine_overlay.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_demos/doc/img/applications_wrench_sine_overlay.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(lbr_fri_ros2_stack) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | ament_package() 6 | -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/controller/raw/activation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/controller/raw/activation.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/03_define_planning_groups.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/03_define_planning_groups.png -------------------------------------------------------------------------------- /lbr_description/ros2_control/initial_joint_positions.yaml: -------------------------------------------------------------------------------- 1 | # initial joint positions [degrees] 2 | A1: 0.0 3 | A2: 25.0 4 | A3: 0.0 5 | A4: 90.0 6 | A5: 0.0 7 | A6: 0.0 8 | A7: 0.0 9 | -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/controller/raw/applications.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/controller/raw/applications.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/04_robot_poses_zero_transport.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/04_robot_poses_zero_transport.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/00_link_path_refresh.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/00_link_path_refresh.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/04_define_robot_poses_transport.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/04_define_robot_poses_transport.png -------------------------------------------------------------------------------- /lbr_description/README.md: -------------------------------------------------------------------------------- 1 | # LBR Description 2 | Launch and example with 3 | 4 | ```shell 5 | ros2 launch lbr_description view_robot.launch.py description_file:=urdf/med7/med7.xacro 6 | ``` 7 | -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/00_station_setup_topology.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/00_station_setup_topology.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/00_sunrise_create_project.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/00_sunrise_create_project.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/01_station_setup_software.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/01_station_setup_software.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_ip_address.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_ip_address.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/03_station_setup_installation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/03_station_setup_installation.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_control_mode.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_control_mode.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_send_period.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_send_period.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/00_lbr_fri_ros2_create_package.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/00_lbr_fri_ros2_create_package.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/02_station_setup_configuration.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/02_station_setup_configuration.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/controller/highlighted/activation_safety.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/controller/highlighted/activation_safety.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_wait_connection.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_wait_connection.png -------------------------------------------------------------------------------- /lbr_moveit_config/doc/img/03_define_planning_groups_kinematic_chain.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_moveit_config/doc/img/03_define_planning_groups_kinematic_chain.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/controller/highlighted/activation_activate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/controller/highlighted/activation_activate.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/01_lbr_fri_ros2_create_package_name.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/01_lbr_fri_ros2_create_package_name.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/01_sunrise_create_project_default_ip.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/01_sunrise_create_project_default_ip.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/04_station_setup_installation_install.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/04_station_setup_installation_install.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/05_station_setup_installation_reboot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/05_station_setup_installation_reboot.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/controller/highlighted/activation_activation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/controller/highlighted/activation_activation.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_client_command_mode.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_client_command_mode.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/02_sunrise_create_project_project_name.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/02_sunrise_create_project_project_name.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/03_sunrise_create_project_robot_select.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/03_sunrise_create_project_robot_select.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/04_sunrise_create_project_media_flange.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/04_sunrise_create_project_media_flange.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/05_sunrise_create_project_unselect_wizard.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/05_sunrise_create_project_unselect_wizard.png -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/doc/img/computer/06_station_setup_installation_synchronize.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/HEAD/lbr_fri_ros2_stack/doc/img/computer/06_station_setup_installation_synchronize.png -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa7_moveit_config/config/pilz_cartesian_limits.yaml: -------------------------------------------------------------------------------- 1 | # Limits for the Pilz planner 2 | cartesian_limits: 3 | max_trans_vel: 1.0 4 | max_trans_acc: 2.25 5 | max_trans_dec: -5.0 6 | max_rot_vel: 1.57 7 | -------------------------------------------------------------------------------- /lbr_moveit_config/med14_moveit_config/config/pilz_cartesian_limits.yaml: -------------------------------------------------------------------------------- 1 | # Limits for the Pilz planner 2 | cartesian_limits: 3 | max_trans_vel: 1.0 4 | max_trans_acc: 2.25 5 | max_trans_dec: -5.0 6 | max_rot_vel: 1.57 7 | -------------------------------------------------------------------------------- /lbr_moveit_config/med7_moveit_config/config/pilz_cartesian_limits.yaml: -------------------------------------------------------------------------------- 1 | # Limits for the Pilz planner 2 | cartesian_limits: 3 | max_trans_vel: 1.0 4 | max_trans_acc: 2.25 5 | max_trans_dec: -5.0 6 | max_rot_vel: 1.57 7 | -------------------------------------------------------------------------------- /.github/workflows/black.yml: -------------------------------------------------------------------------------- 1 | name: Lint 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | lint: 7 | runs-on: ubuntu-latest 8 | steps: 9 | - uses: actions/checkout@v3 10 | - uses: psf/black@stable 11 | -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa14_moveit_config/config/pilz_cartesian_limits.yaml: -------------------------------------------------------------------------------- 1 | # Limits for the Pilz planner 2 | cartesian_limits: 3 | max_trans_vel: 1.0 4 | max_trans_acc: 2.25 5 | max_trans_dec: -5.0 6 | max_rot_vel: 1.57 7 | -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa14_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.0050000000000000001 4 | kinematics_solver_timeout: 0.0050000000000000001 -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa7_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.0050000000000000001 4 | kinematics_solver_timeout: 0.0050000000000000001 -------------------------------------------------------------------------------- /lbr_moveit_config/med14_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.0050000000000000001 4 | kinematics_solver_timeout: 0.0050000000000000001 -------------------------------------------------------------------------------- /lbr_moveit_config/med7_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.0050000000000000001 4 | kinematics_solver_timeout: 0.0050000000000000001 -------------------------------------------------------------------------------- /lbr_ros2_control/plugin_description_files/system_interface.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml: -------------------------------------------------------------------------------- 1 | /**/admittance_control: 2 | ros__parameters: 3 | base_link: "lbr_link_0" 4 | end_effector_link: "lbr_link_ee" 5 | f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] 6 | dq_gains: [1., 1., 1., 1., 1., 1., 1.] 7 | dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] 8 | exp_smooth: 0.95 9 | -------------------------------------------------------------------------------- /.github/workflows/build-ubuntu-24.04-fri-2.5.yml: -------------------------------------------------------------------------------- 1 | name: ubuntu-24.04-fri-2.5 2 | on: 3 | pull_request: 4 | branches: 5 | - jazzy 6 | workflow_dispatch: 7 | schedule: 8 | - cron: "0 0 * * 0" 9 | 10 | jobs: 11 | build: 12 | uses: ./.github/workflows/build.yml 13 | with: 14 | os: ubuntu-24.04 15 | fri_version: 2.5 16 | -------------------------------------------------------------------------------- /.github/workflows/build-ubuntu-24.04-fri-2.7.yml: -------------------------------------------------------------------------------- 1 | name: ubuntu-24.04-fri-2.7 2 | on: 3 | pull_request: 4 | branches: 5 | - jazzy 6 | workflow_dispatch: 7 | schedule: 8 | - cron: "0 0 * * 0" 9 | 10 | jobs: 11 | build: 12 | uses: ./.github/workflows/build.yml 13 | with: 14 | os: ubuntu-24.04 15 | fri_version: 2.7 16 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml: -------------------------------------------------------------------------------- 1 | /**/admittance_rcm_control: 2 | ros__parameters: 3 | base_link: "lbr_link_0" 4 | end_effector_link: "lbr_link_ee" 5 | f_ext_th: [4.0, 4.0, 4.0, 1.0, 1.0, 1.0] 6 | dq_gains: [1., 1., 1., 1., 1., 1., 1.] 7 | dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] 8 | exp_smooth: 0.95 9 | -------------------------------------------------------------------------------- /.github/workflows/build-ubuntu-24.04-fri-1.11.yml: -------------------------------------------------------------------------------- 1 | name: ubuntu-24.04-fri-1.11 2 | on: 3 | pull_request: 4 | branches: 5 | - jazzy 6 | workflow_dispatch: 7 | schedule: 8 | - cron: "0 0 * * 0" 9 | 10 | jobs: 11 | build: 12 | uses: ./.github/workflows/build.yml 13 | with: 14 | os: ubuntu-24.04 15 | fri_version: 1.11 16 | -------------------------------------------------------------------------------- /.github/workflows/build-ubuntu-24.04-fri-1.14.yml: -------------------------------------------------------------------------------- 1 | name: ubuntu-24.04-fri-1.14 2 | on: 3 | pull_request: 4 | branches: 5 | - jazzy 6 | workflow_dispatch: 7 | schedule: 8 | - cron: "0 0 * * 0" 9 | 10 | jobs: 11 | build: 12 | uses: ./.github/workflows/build.yml 13 | with: 14 | os: ubuntu-24.04 15 | fri_version: 1.14 16 | -------------------------------------------------------------------------------- /.github/workflows/build-ubuntu-24.04-fri-1.15.yml: -------------------------------------------------------------------------------- 1 | name: ubuntu-24.04-fri-1.15 2 | on: 3 | pull_request: 4 | branches: 5 | - jazzy 6 | workflow_dispatch: 7 | schedule: 8 | - cron: "0 0 * * 0" 9 | 10 | jobs: 11 | build: 12 | uses: ./.github/workflows/build.yml 13 | with: 14 | os: ubuntu-24.04 15 | fri_version: 1.15 16 | -------------------------------------------------------------------------------- /.github/workflows/build-ubuntu-24.04-fri-1.16.yml: -------------------------------------------------------------------------------- 1 | name: ubuntu-24.04-fri-1.16 2 | on: 3 | pull_request: 4 | branches: 5 | - jazzy 6 | workflow_dispatch: 7 | schedule: 8 | - cron: "0 0 * * 0" 9 | 10 | jobs: 11 | build: 12 | uses: ./.github/workflows/build.yml 13 | with: 14 | os: ubuntu-24.04 15 | fri_version: 1.16 16 | -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/repos-fri-1.11.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | fri: 3 | type: git 4 | url: https://github.com/lbr-stack/fri 5 | version: fri-1.11 6 | lbr_fri_idl: 7 | type: git 8 | url: https://github.com/lbr-stack/lbr_fri_idl 9 | version: fri-1 10 | lbr_fri_ros2_stack: 11 | type: git 12 | url: https://github.com/lbr-stack/lbr_fri_ros2_stack 13 | version: jazzy 14 | -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/repos-fri-1.14.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | fri: 3 | type: git 4 | url: https://github.com/lbr-stack/fri 5 | version: fri-1.14 6 | lbr_fri_idl: 7 | type: git 8 | url: https://github.com/lbr-stack/lbr_fri_idl 9 | version: fri-1 10 | lbr_fri_ros2_stack: 11 | type: git 12 | url: https://github.com/lbr-stack/lbr_fri_ros2_stack 13 | version: jazzy 14 | -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/repos-fri-1.15.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | fri: 3 | type: git 4 | url: https://github.com/lbr-stack/fri 5 | version: fri-1.15 6 | lbr_fri_idl: 7 | type: git 8 | url: https://github.com/lbr-stack/lbr_fri_idl 9 | version: fri-1 10 | lbr_fri_ros2_stack: 11 | type: git 12 | url: https://github.com/lbr-stack/lbr_fri_ros2_stack 13 | version: jazzy 14 | -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/repos-fri-1.16.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | fri: 3 | type: git 4 | url: https://github.com/lbr-stack/fri 5 | version: fri-1.16 6 | lbr_fri_idl: 7 | type: git 8 | url: https://github.com/lbr-stack/lbr_fri_idl 9 | version: fri-1 10 | lbr_fri_ros2_stack: 11 | type: git 12 | url: https://github.com/lbr-stack/lbr_fri_ros2_stack 13 | version: jazzy 14 | -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/repos-fri-2.5.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | fri: 3 | type: git 4 | url: https://github.com/lbr-stack/fri 5 | version: fri-2.5 6 | lbr_fri_idl: 7 | type: git 8 | url: https://github.com/lbr-stack/lbr_fri_idl 9 | version: fri-2 10 | lbr_fri_ros2_stack: 11 | type: git 12 | url: https://github.com/lbr-stack/lbr_fri_ros2_stack 13 | version: jazzy 14 | -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/repos-fri-2.7.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | fri: 3 | type: git 4 | url: https://github.com/lbr-stack/fri 5 | version: fri-2.7 6 | lbr_fri_idl: 7 | type: git 8 | url: https://github.com/lbr-stack/lbr_fri_idl 9 | version: fri-2 10 | lbr_fri_ros2_stack: 11 | type: git 12 | url: https://github.com/lbr-stack/lbr_fri_ros2_stack 13 | version: jazzy 14 | -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa7_moveit_config/launch/moveit_rviz.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_moveit_rviz_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder( 7 | "iiwa7", package_name="iiwa7_moveit_config" 8 | ).to_moveit_configs() 9 | return generate_moveit_rviz_launch(moveit_config) 10 | -------------------------------------------------------------------------------- /lbr_moveit_config/med14_moveit_config/launch/moveit_rviz.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_moveit_rviz_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder( 7 | "med14", package_name="med14_moveit_config" 8 | ).to_moveit_configs() 9 | return generate_moveit_rviz_launch(moveit_config) 10 | -------------------------------------------------------------------------------- /lbr_moveit_config/med7_moveit_config/launch/moveit_rviz.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_moveit_rviz_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder( 7 | "med7", package_name="med7_moveit_config" 8 | ).to_moveit_configs() 9 | return generate_moveit_rviz_launch(moveit_config) 10 | -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa14_moveit_config/launch/moveit_rviz.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_moveit_rviz_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder( 7 | "iiwa14", package_name="iiwa14_moveit_config" 8 | ).to_moveit_configs() 9 | return generate_moveit_rviz_launch(moveit_config) 10 | -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa7_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(iiwa7_moveit_config) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | ament_package() 7 | 8 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} 9 | PATTERN "setup_assistant.launch" EXCLUDE) 10 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) 11 | install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) 12 | -------------------------------------------------------------------------------- /lbr_moveit_config/med14_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(med14_moveit_config) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | ament_package() 7 | 8 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} 9 | PATTERN "setup_assistant.launch" EXCLUDE) 10 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) 11 | install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) 12 | -------------------------------------------------------------------------------- /lbr_moveit_config/med7_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(med7_moveit_config) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | ament_package() 7 | 8 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} 9 | PATTERN "setup_assistant.launch" EXCLUDE) 10 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) 11 | install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) 12 | -------------------------------------------------------------------------------- /lbr_moveit_config/med7_moveit_config/launch/setup_assistant.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_setup_assistant_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder( 7 | "med7", package_name="med7_moveit_config" 8 | ).to_moveit_configs() 9 | return generate_setup_assistant_launch(moveit_config) 10 | -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa14_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(iiwa14_moveit_config) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | ament_package() 7 | 8 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} 9 | PATTERN "setup_assistant.launch" EXCLUDE) 10 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) 11 | install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) 12 | -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa14_moveit_config/launch/setup_assistant.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_setup_assistant_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder( 7 | "iiwa14", package_name="iiwa14_moveit_config" 8 | ).to_moveit_configs() 9 | return generate_setup_assistant_launch(moveit_config) 10 | -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa7_moveit_config/launch/setup_assistant.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_setup_assistant_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder( 7 | "iiwa7", package_name="iiwa7_moveit_config" 8 | ).to_moveit_configs() 9 | return generate_setup_assistant_launch(moveit_config) 10 | -------------------------------------------------------------------------------- /lbr_moveit_config/med14_moveit_config/launch/setup_assistant.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_setup_assistant_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder( 7 | "med14", package_name="med14_moveit_config" 8 | ).to_moveit_configs() 9 | return generate_setup_assistant_launch(moveit_config) 10 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit/scripts/forward_keyboard.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from lbr_moveit.forward_keyboard_node import ForwardKeyboardNode 3 | from lbr_moveit.keyboard_listener import KeyboardListener 4 | 5 | 6 | def main(): 7 | rclpy.init() 8 | forward_keyboard_node = ForwardKeyboardNode() 9 | try: 10 | with KeyboardListener(forward_keyboard_node): 11 | rclpy.spin(forward_keyboard_node) 12 | except KeyboardInterrupt: 13 | pass 14 | -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/repos-fri-2.6.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | fri: 3 | type: git 4 | url: https://github.com/lbr-stack/fri 5 | version: fri-2.5 # please refer to https://github.com/lbr-stack/fri/issues/33. KUKA released Sunrise 2.6 but kept FRI 2.5 6 | lbr_fri_idl: 7 | type: git 8 | url: https://github.com/lbr-stack/lbr_fri_idl 9 | version: fri-2 10 | lbr_fri_ros2_stack: 11 | type: git 12 | url: https://github.com/lbr-stack/lbr_fri_ros2_stack 13 | version: jazzy 14 | -------------------------------------------------------------------------------- /docker/container_build.sh: -------------------------------------------------------------------------------- 1 | # dont forget to chmod +x container_build.sh 2 | xhost + 3 | 4 | docker rm lbr_stack_container 5 | 6 | docker build -t lbr_stack_container . 7 | 8 | docker run -it \ 9 | --network host \ 10 | --ipc host \ 11 | --volume ./src:/home/ros2_ws/src \ 12 | --volume /tmp/.X11-unix:/tmp/.X11-unix \ 13 | --volume /dev/shm:/dev/shm \ 14 | --volume /dev:/dev --privileged \ 15 | --env DISPLAY \ 16 | --env QT_X11_NO_MITSHM=1 \ 17 | --name lbr_stack_container \ 18 | lbr_stack_container 19 | -------------------------------------------------------------------------------- /lbr_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(lbr_bringup) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_python REQUIRED) 11 | 12 | install( 13 | DIRECTORY config launch 14 | DESTINATION share/${PROJECT_NAME} 15 | ) 16 | 17 | # Launch mixins 18 | ament_python_install_package(lbr_bringup) 19 | 20 | ament_package() 21 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/math.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__MATH_HPP_ 2 | #define LBR_FRI_ROS2__MATH_HPP_ 3 | 4 | #include 5 | 6 | #include "lbr_fri_ros2/types.hpp" 7 | 8 | namespace lbr_fri_ros2 { 9 | bool norm_in_bounds(const double &x0, const double &x1, const double &x2, const double &max); 10 | bool norm_in_bounds(const std::array &vec, const double &max); 11 | bool all_jnts_in_bounds(const_jnt_array_t_ref vals, const_jnt_array_t_ref lower, 12 | const_jnt_array_t_ref upper); 13 | } // namespace lbr_fri_ros2 14 | #endif // LBR_FRI_ROS2__MATH_HPP_ 15 | -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | # MoveIt uses this configuration for controller management 2 | 3 | moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager 4 | 5 | moveit_simple_controller_manager: 6 | controller_names: 7 | - joint_trajectory_controller 8 | 9 | joint_trajectory_controller: 10 | type: FollowJointTrajectory 11 | joints: 12 | - lbr_A1 13 | - lbr_A2 14 | - lbr_A3 15 | - lbr_A4 16 | - lbr_A5 17 | - lbr_A6 18 | - lbr_A7 19 | action_ns: follow_joint_trajectory 20 | default: true -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | # MoveIt uses this configuration for controller management 2 | 3 | moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager 4 | 5 | moveit_simple_controller_manager: 6 | controller_names: 7 | - joint_trajectory_controller 8 | 9 | joint_trajectory_controller: 10 | type: FollowJointTrajectory 11 | joints: 12 | - lbr_A1 13 | - lbr_A2 14 | - lbr_A3 15 | - lbr_A4 16 | - lbr_A5 17 | - lbr_A6 18 | - lbr_A7 19 | action_ns: follow_joint_trajectory 20 | default: true -------------------------------------------------------------------------------- /lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | # MoveIt uses this configuration for controller management 2 | 3 | moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager 4 | 5 | moveit_simple_controller_manager: 6 | controller_names: 7 | - joint_trajectory_controller 8 | 9 | joint_trajectory_controller: 10 | type: FollowJointTrajectory 11 | joints: 12 | - lbr_A1 13 | - lbr_A2 14 | - lbr_A3 15 | - lbr_A4 16 | - lbr_A5 17 | - lbr_A6 18 | - lbr_A7 19 | action_ns: follow_joint_trajectory 20 | default: true -------------------------------------------------------------------------------- /lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | # MoveIt uses this configuration for controller management 2 | 3 | moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager 4 | 5 | moveit_simple_controller_manager: 6 | controller_names: 7 | - joint_trajectory_controller 8 | 9 | joint_trajectory_controller: 10 | type: FollowJointTrajectory 11 | joints: 12 | - lbr_A1 13 | - lbr_A2 14 | - lbr_A3 15 | - lbr_A4 16 | - lbr_A5 17 | - lbr_A6 18 | - lbr_A7 19 | action_ns: follow_joint_trajectory 20 | default: true -------------------------------------------------------------------------------- /lbr_moveit_config/med7_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | urdf: 3 | package: lbr_description 4 | relative_path: urdf/med7/med7.xacro 5 | srdf: 6 | relative_path: config/med7.srdf 7 | package_settings: 8 | author_name: mhubii 9 | author_email: m.huber_1994@hotmail.de 10 | generated_timestamp: 1729340711 11 | control_xacro: 12 | command: 13 | - position 14 | state: 15 | - position 16 | - velocity 17 | modified_urdf: 18 | xacros: 19 | - control_xacro 20 | control_xacro: 21 | command: 22 | - position 23 | state: 24 | - position 25 | - velocity -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa7_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | urdf: 3 | package: lbr_description 4 | relative_path: urdf/iiwa7/iiwa7.xacro 5 | srdf: 6 | relative_path: config/iiwa7.srdf 7 | package_settings: 8 | author_name: mhubii 9 | author_email: m.huber_1994@hotmail.de 10 | generated_timestamp: 1729340852 11 | control_xacro: 12 | command: 13 | - position 14 | state: 15 | - position 16 | - velocity 17 | modified_urdf: 18 | xacros: 19 | - control_xacro 20 | control_xacro: 21 | command: 22 | - position 23 | state: 24 | - position 25 | - velocity -------------------------------------------------------------------------------- /lbr_moveit_config/med14_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | urdf: 3 | package: lbr_description 4 | relative_path: urdf/med14/med14.xacro 5 | srdf: 6 | relative_path: config/med14.srdf 7 | package_settings: 8 | author_name: mhubii 9 | author_email: m.huber_1994@hotmail.de 10 | generated_timestamp: 1729341043 11 | control_xacro: 12 | command: 13 | - position 14 | state: 15 | - position 16 | - velocity 17 | modified_urdf: 18 | xacros: 19 | - control_xacro 20 | control_xacro: 21 | command: 22 | - position 23 | state: 24 | - position 25 | - velocity -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa14_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | urdf: 3 | package: lbr_description 4 | relative_path: urdf/iiwa14/iiwa14.xacro 5 | srdf: 6 | relative_path: config/iiwa14.srdf 7 | package_settings: 8 | author_name: mhubii 9 | author_email: m.huber_1994@hotmail.de 10 | generated_timestamp: 1729340960 11 | control_xacro: 12 | command: 13 | - position 14 | state: 15 | - position 16 | - velocity 17 | modified_urdf: 18 | xacros: 19 | - control_xacro 20 | control_xacro: 21 | command: 22 | - position 23 | state: 24 | - position 25 | - velocity -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lbr_moveit 5 | 2.4.3 6 | MoveIt demos for the LBRs 7 | mhubii 8 | Apache-2.0 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /lbr_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(lbr_description) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_pytest REQUIRED) 11 | 12 | # install 13 | install( 14 | DIRECTORY gazebo meshes ros2_control urdf 15 | DESTINATION share/${PROJECT_NAME} 16 | ) 17 | 18 | ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}.dsv") 19 | 20 | if(BUILD_TESTING) 21 | ament_add_pytest_test(test_urdf ${CMAKE_CURRENT_SOURCE_DIR}/test/test_urdf.py) 22 | endif() 23 | 24 | ament_package() 25 | -------------------------------------------------------------------------------- /lbr_moveit_config/med7_moveit_config/launch/move_group.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python import get_package_share_directory 4 | from moveit_configs_utils import MoveItConfigsBuilder 5 | from moveit_configs_utils.launches import generate_move_group_launch 6 | 7 | 8 | def generate_launch_description(): 9 | moveit_config = ( 10 | MoveItConfigsBuilder("med7", package_name="med7_moveit_config") 11 | .robot_description( 12 | os.path.join( 13 | get_package_share_directory("lbr_description"), 14 | "urdf/med7/med7.xacro", 15 | ) 16 | ) 17 | .to_moveit_configs() 18 | ) 19 | return generate_move_group_launch(moveit_config) 20 | -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa7_moveit_config/launch/move_group.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python import get_package_share_directory 4 | from moveit_configs_utils import MoveItConfigsBuilder 5 | from moveit_configs_utils.launches import generate_move_group_launch 6 | 7 | 8 | def generate_launch_description(): 9 | moveit_config = ( 10 | MoveItConfigsBuilder("iiwa7", package_name="iiwa7_moveit_config") 11 | .robot_description( 12 | os.path.join( 13 | get_package_share_directory("lbr_description"), 14 | "urdf/iiwa7/iiwa7.xacro", 15 | ) 16 | ) 17 | .to_moveit_configs() 18 | ) 19 | return generate_move_group_launch(moveit_config) 20 | -------------------------------------------------------------------------------- /lbr_moveit_config/med14_moveit_config/launch/move_group.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python import get_package_share_directory 4 | from moveit_configs_utils import MoveItConfigsBuilder 5 | from moveit_configs_utils.launches import generate_move_group_launch 6 | 7 | 8 | def generate_launch_description(): 9 | moveit_config = ( 10 | MoveItConfigsBuilder("med14", package_name="med14_moveit_config") 11 | .robot_description( 12 | os.path.join( 13 | get_package_share_directory("lbr_description"), 14 | "urdf/med14/med14.xacro", 15 | ) 16 | ) 17 | .to_moveit_configs() 18 | ) 19 | return generate_move_group_launch(moveit_config) 20 | -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa14_moveit_config/launch/move_group.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python import get_package_share_directory 4 | from moveit_configs_utils import MoveItConfigsBuilder 5 | from moveit_configs_utils.launches import generate_move_group_launch 6 | 7 | 8 | def generate_launch_description(): 9 | moveit_config = ( 10 | MoveItConfigsBuilder("iiwa14", package_name="iiwa14_moveit_config") 11 | .robot_description( 12 | os.path.join( 13 | get_package_share_directory("lbr_description"), 14 | "urdf/iiwa14/iiwa14.xacro", 15 | ) 16 | ) 17 | .to_moveit_configs() 18 | ) 19 | return generate_move_group_launch(moveit_config) 20 | -------------------------------------------------------------------------------- /lbr_fri_ros2/src/math.cpp: -------------------------------------------------------------------------------- 1 | #include "lbr_fri_ros2/math.hpp" 2 | 3 | namespace lbr_fri_ros2 { 4 | bool norm_in_bounds(const double &x0, const double &x1, const double &x2, const double &max) { 5 | double norm_sq = x0 * x0 + x1 * x1 + x2 * x2; 6 | return norm_sq <= max * max; 7 | } 8 | bool norm_in_bounds(const std::array &vec, const double &max) { 9 | return norm_in_bounds(vec[0], vec[1], vec[2], max); 10 | } 11 | bool all_jnts_in_bounds(const_jnt_array_t_ref vals, const_jnt_array_t_ref lower, 12 | const_jnt_array_t_ref upper) { 13 | for (std::size_t i = 0; i < vals.size(); ++i) { 14 | if (vals[i] < lower[i] || upper[i] < vals[i]) { 15 | return false; 16 | } 17 | } 18 | return true; 19 | } 20 | } // namespace lbr_fri_ros2 21 | -------------------------------------------------------------------------------- /lbr_fri_ros2_stack/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lbr_fri_ros2_stack 5 | 2.4.3 6 | ROS 2 stack for KUKA LBRs. 7 | mhubii 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | lbr_fri_idl 13 | lbr_fri_ros2 14 | lbr_ros2_control 15 | 16 | lbr_bringup 17 | lbr_description 18 | 19 | 20 | ament_cmake 21 | 22 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit_cpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lbr_moveit_cpp 5 | 2.4.3 6 | Demo for using MoveIt C++ API. 7 | mhubii 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | geometry_msgs 13 | moveit_ros_planning_interface 14 | rclcpp 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | 20 | ament_cmake 21 | 22 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__WORKER_HPP_ 2 | #define LBR_FRI_ROS2__WORKER_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "rclcpp/logger.hpp" 9 | #include "rclcpp/logging.hpp" 10 | #include "realtime_tools/realtime_helpers.hpp" 11 | 12 | #include "lbr_fri_ros2/formatting.hpp" 13 | 14 | namespace lbr_fri_ros2 { 15 | class Worker { 16 | public: 17 | Worker(); 18 | ~Worker(); 19 | 20 | virtual void run_async(int rt_prio = 80); 21 | void request_stop(); 22 | inline virtual std::string LOGGER_NAME() const = 0; 23 | 24 | protected: 25 | virtual void perform_work_() = 0; 26 | 27 | std::atomic_bool should_stop_, running_; 28 | std::thread run_thread_; 29 | }; 30 | } // namespace lbr_fri_ros2 31 | #endif // LBR_FRI_ROS2__WORKER_HPP_ 32 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:jazzy-ros-base 2 | 3 | # change default shell to bash 4 | SHELL ["/bin/bash", "-c"] 5 | 6 | # upgrade packages 7 | RUN apt-get update 8 | RUN apt-get upgrade -y 9 | 10 | # create workspace 11 | RUN mkdir -p home/ros2_ws/src 12 | WORKDIR /home/ros2_ws/ 13 | 14 | COPY ./src /home/ros2_ws/src 15 | RUN rosdep install -i --from-paths src --rosdistro ${ROS_DISTRO} -y 16 | 17 | # "--symlink-install" allows the code in the locally mounted volume ./src to be adjusted without rebuilding 18 | RUN source /opt/ros/${ROS_DISTRO}/setup.bash && \ 19 | colcon build --symlink-install 20 | 21 | # source the workspace 22 | RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc 23 | RUN source /home/ros2_ws/install/local_setup.bash && echo "source /home/ros2_ws/install/local_setup.bash" >> ~/.bashrc 24 | CMD ["/bin/bash"] 25 | -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | on: 4 | workflow_call: # https://github.com/orgs/community/discussions/52616#discussioncomment-9364532 5 | inputs: 6 | os: 7 | required: true 8 | type: string 9 | fri_version: 10 | required: true 11 | type: string 12 | 13 | # ros 2 ci: https://github.com/marketplace/actions/ros-2-ci-action 14 | # doc: https://ubuntu.com/blog/ros-2-ci-with-github-actions 15 | jobs: 16 | build: 17 | runs-on: ${{ inputs.os }} 18 | steps: 19 | - uses: ros-tooling/setup-ros@v0.7 20 | - uses: ros-tooling/action-ros-ci@v0.4 21 | with: 22 | package-name: lbr_fri_ros2_stack 23 | target-ros2-distro: jazzy 24 | vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/jazzy/lbr_fri_ros2_stack/repos-fri-${{ inputs.fri_version }}.yaml 25 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_py/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=[".", "test"]) 23 | assert rc == 0, "Found code style errors / warnings" 24 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=[".", "test"]) 23 | assert rc == 0, "Found code style errors / warnings" 24 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_py/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=[".", "test"]) 23 | assert rc == 0, "Found code style errors / warnings" 24 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit_cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(lbr_moveit_cpp) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | if(NOT CMAKE_BUILD_TYPE) 9 | set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE) 10 | endif() 11 | 12 | # find dependencies 13 | find_package(ament_cmake REQUIRED) 14 | find_package(geometry_msgs REQUIRED) 15 | find_package(moveit_ros_planning_interface REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | 18 | add_executable(hello_moveit src/hello_moveit.cpp) 19 | 20 | ament_target_dependencies(hello_moveit 21 | geometry_msgs 22 | moveit_ros_planning_interface 23 | rclcpp 24 | ) 25 | 26 | install(DIRECTORY launch 27 | DESTINATION share/${PROJECT_NAME} 28 | ) 29 | 30 | install(TARGETS hello_moveit 31 | DESTINATION lib/${PROJECT_NAME} 32 | ) 33 | 34 | ament_package() 35 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you enjoyed using this repository for your work, we would really appreciate ❤️ if you could cite it, as it helps us to continue offering support." 3 | authors: 4 | - family-names: Huber 5 | given-names: Martin 6 | orcid: https://orcid.org/0000-0003-4603-6773 7 | - family-names: Mower 8 | given-names: Christopher E. 9 | orcid: https://orcid.org/0000-0002-3929-9391 10 | - family-names: Ourselin 11 | given-names: Sebastien 12 | orcid: https://orcid.org/0000-0002-5694-5340 13 | - family-names: Vercauteren 14 | given-names: Tom 15 | orcid: https://orcid.org/0000-0003-1794-0456 16 | - family-names: Bergeles 17 | given-names: Christos 18 | orcid: https://orcid.org/0000-0002-9152-3194 19 | 20 | 21 | title: "LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots" 22 | version: 2.4.3 23 | doi: 10.21105/joss.06138 24 | date-released: 2025-12-09 25 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__INTERFACES__TORQUE_COMMAND_HPP_ 2 | #define LBR_FRI_ROS2__INTERFACES__TORQUE_COMMAND_HPP_ 3 | 4 | #include 5 | 6 | #include "lbr_fri_ros2/interfaces/base_command.hpp" 7 | 8 | namespace lbr_fri_ros2 { 9 | class TorqueCommandInterface : public BaseCommandInterface { 10 | protected: 11 | std::string LOGGER_NAME() const override { return "lbr_fri_ros2::TorqueCommandInterface"; } 12 | 13 | public: 14 | TorqueCommandInterface() = delete; 15 | TorqueCommandInterface(const double &joint_position_tau, 16 | const CommandGuardParameters &command_guard_parameters, 17 | const std::string &command_guard_variant = "default"); 18 | 19 | void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) override; 20 | }; 21 | } // namespace lbr_fri_ros2 22 | #endif // LBR_FRI_ROS2__INTERFACES__TORQUE_COMMAND_HPP_ 23 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__INTERFACES__WRENCH_COMMAND_HPP_ 2 | #define LBR_FRI_ROS2__INTERFACES__WRENCH_COMMAND_HPP_ 3 | 4 | #include 5 | 6 | #include "lbr_fri_ros2/interfaces/base_command.hpp" 7 | 8 | namespace lbr_fri_ros2 { 9 | class WrenchCommandInterface : public BaseCommandInterface { 10 | protected: 11 | std::string LOGGER_NAME() const override { return "lbr_fri_ros2::WrenchCommandInterface"; } 12 | 13 | public: 14 | WrenchCommandInterface() = delete; 15 | WrenchCommandInterface(const double &joint_position_tau, 16 | const CommandGuardParameters &command_guard_parameters, 17 | const std::string &command_guard_variant = "default"); 18 | 19 | void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) override; 20 | }; 21 | } // namespace lbr_fri_ros2 22 | #endif // LBR_FRI_ROS2__INTERFACES__WRENCH_COMMAND_HPP_ 23 | -------------------------------------------------------------------------------- /lbr_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lbr_description 5 | 2.4.3 6 | KUKA LBR description files 7 | mhubii 8 | Apache-2.0 9 | 10 | ament_cmake 11 | ament_cmake_pytest 12 | 13 | gz_ros2_control 14 | joint_state_publisher_gui 15 | robot_state_publisher 16 | rviz2 17 | tf2_ros 18 | xacro 19 | 20 | python3-pytest 21 | 22 | 23 | ament_cmake 24 | 25 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__INTERFACES__POSITION_COMMAND_HPP_ 2 | #define LBR_FRI_ROS2__INTERFACES__POSITION_COMMAND_HPP_ 3 | 4 | #include 5 | 6 | #include "lbr_fri_ros2/interfaces/base_command.hpp" 7 | 8 | namespace lbr_fri_ros2 { 9 | class PositionCommandInterface : public BaseCommandInterface { 10 | protected: 11 | std::string LOGGER_NAME() const override { return "lbr_fri_ros2::PositionCommandInterface"; } 12 | 13 | public: 14 | PositionCommandInterface() = delete; 15 | PositionCommandInterface(const double &joint_position_tau, 16 | const CommandGuardParameters &command_guard_parameters, 17 | const std::string &command_guard_variant = "default"); 18 | 19 | void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) override; 20 | }; 21 | } // namespace lbr_fri_ros2 22 | #endif // LBR_FRI_ROS2__INTERFACES__POSITION_COMMAND_HPP_ 23 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | 3 | package_name = "lbr_moveit" 4 | 5 | setup( 6 | name=package_name, 7 | version="2.4.3", 8 | packages=find_packages(exclude=["test"]), 9 | data_files=[ 10 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 11 | ("share/" + package_name, ["package.xml"]), 12 | ("share/" + package_name + "/config", ["config/forward_keyboard.yaml"]), 13 | ("share/" + package_name + "/launch", ["launch/keyboard_driver.launch.py"]), 14 | ], 15 | install_requires=["setuptools"], 16 | zip_safe=True, 17 | maintainer="mhubii", 18 | maintainer_email="m.huber_1994@hotmail.de", 19 | description="MoveIt demos for the LBRs.", 20 | license="Apache-2.0", 21 | tests_require=["pytest"], 22 | entry_points={ 23 | "console_scripts": [ 24 | "forward_keyboard = scripts.forward_keyboard:main", 25 | ], 26 | }, 27 | ) 28 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_py/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, "Found %d code style errors / warnings:\n" % len( 24 | errors 25 | ) + "\n".join(errors) 26 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, "Found %d code style errors / warnings:\n" % len( 24 | errors 25 | ) + "\n".join(errors) 26 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_py/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, "Found %d code style errors / warnings:\n" % len( 24 | errors 25 | ) + "\n".join(errors) 26 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_py/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lbr_demos_advanced_py 5 | 2.4.3 6 | Advanced Python demos for the lbr_ros2_control. 7 | mhubii 8 | cmower 9 | Apache-2.0 10 | 11 | lbr_description 12 | lbr_fri_idl 13 | lbr_fri_ros2 14 | rclpy 15 | rcl_interfaces 16 | 17 | ament_copyright 18 | ament_flake8 19 | ament_pep257 20 | python3-pytest 21 | 22 | 23 | ament_python 24 | 25 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_py/setup.py: -------------------------------------------------------------------------------- 1 | import glob 2 | 3 | from setuptools import setup 4 | 5 | package_name = "lbr_demos_advanced_py" 6 | 7 | setup( 8 | name=package_name, 9 | version="2.4.3", 10 | packages=[package_name], 11 | data_files=[ 12 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 13 | ("share/" + package_name, ["package.xml"]), 14 | ("share/" + package_name + "/config", glob.glob("config/*.yaml")), 15 | ], 16 | install_requires=["setuptools"], 17 | zip_safe=True, 18 | maintainer="mhubii", 19 | maintainer_email="m.huber_1994@hotmail.de", 20 | description="Advanced Python demos for the lbr_ros2_control.", 21 | license="Apache-2.0", 22 | tests_require=["pytest"], 23 | entry_points={ 24 | "console_scripts": [ 25 | "admittance_control = lbr_demos_advanced_py.admittance_control_node:main", 26 | "admittance_rcm_control = lbr_demos_advanced_py.admittance_rcm_control_node:main", 27 | ], 28 | }, 29 | ) 30 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_py/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lbr_demos_py 5 | 2.4.3 6 | Python demos for lbr_ros2_control. 7 | mhubii 8 | Apache-2.0 9 | 10 | control_msgs 11 | lbr_bringup 12 | lbr_description 13 | lbr_fri_idl 14 | lbr_ros2_control 15 | rclpy 16 | trajectory_msgs 17 | 18 | ament_copyright 19 | ament_flake8 20 | ament_pep257 21 | python3-pytest 22 | 23 | 24 | ament_python 25 | 26 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_py/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = "lbr_demos_py" 4 | 5 | setup( 6 | name=package_name, 7 | version="2.4.3", 8 | packages=[package_name], 9 | data_files=[ 10 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 11 | ("share/" + package_name, ["package.xml"]), 12 | ], 13 | install_requires=["setuptools"], 14 | zip_safe=True, 15 | maintainer="mhubii", 16 | maintainer_email="m.huber_1994@hotmail.de", 17 | description="Python demos for lbr_ros2_control.", 18 | license="Apache-2.0", 19 | tests_require=["pytest"], 20 | entry_points={ 21 | "console_scripts": [ 22 | "joint_sine_overlay = lbr_demos_py.joint_sine_overlay:main", 23 | "joint_trajectory_client = lbr_demos_py.joint_trajectory_client:main", 24 | "torque_sine_overlay = lbr_demos_py.torque_sine_overlay:main", 25 | "wrench_sine_overlay = lbr_demos_py.wrench_sine_overlay:main", 26 | ], 27 | }, 28 | ) 29 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_cpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lbr_demos_advanced_cpp 5 | 2.4.3 6 | Advanced C++ demos for the lbr_ros2_control. 7 | mhubii 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | lbr_description 13 | 14 | fri_client_sdk 15 | geometry_msgs 16 | kdl_parser 17 | lbr_fri_idl 18 | lbr_fri_ros2 19 | orocos_kdl_vendor 20 | rclcpp 21 | rclcpp_components 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | 26 | 27 | ament_cmake 28 | 29 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip( 21 | reason="No copyright header has been placed in the generated source file." 22 | ) 23 | @pytest.mark.copyright 24 | @pytest.mark.linter 25 | def test_copyright(): 26 | rc = main(argv=[".", "test"]) 27 | assert rc == 0, "Found errors" 28 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_py/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip( 21 | reason="No copyright header has been placed in the generated source file." 22 | ) 23 | @pytest.mark.copyright 24 | @pytest.mark.linter 25 | def test_copyright(): 26 | rc = main(argv=[".", "test"]) 27 | assert rc == 0, "Found errors" 28 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_py/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip( 21 | reason="No copyright header has been placed in the generated source file." 22 | ) 23 | @pytest.mark.copyright 24 | @pytest.mark.linter 25 | def test_copyright(): 26 | rc = main(argv=[".", "test"]) 27 | assert rc == 0, "Found errors" 28 | -------------------------------------------------------------------------------- /lbr_demos/doc/lbr_demos.rst: -------------------------------------------------------------------------------- 1 | lbr_demos 2 | ========= 3 | All demos build on :ref:`lbr_ros2_control`. Different ``ros2_controllers`` are loaded to command the robot. 4 | 5 | .. note:: 6 | Some demos require a real robot. Make sure you followed :doc:`Hardware Setup <../../lbr_fri_ros2_stack/doc/hardware_setup>` first. 7 | 8 | .. warning:: 9 | On the real robot, do always execute in ``T1`` mode first. 10 | 11 | Simple 12 | ------ 13 | .. toctree:: 14 | :titlesonly: 15 | 16 | lbr_demos_cpp <../lbr_demos_cpp/doc/lbr_demos_cpp.rst> 17 | lbr_demos_py <../lbr_demos_py/doc/lbr_demos_py.rst> 18 | 19 | Advanced 20 | -------- 21 | .. toctree:: 22 | :titlesonly: 23 | 24 | lbr_demos_advanced_cpp <../lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst> 25 | lbr_demos_advanced_py <../lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst> 26 | 27 | MoveIt 28 | ------- 29 | .. toctree:: 30 | :titlesonly: 31 | 32 | lbr_moveit <../lbr_moveit/doc/lbr_moveit.rst> 33 | lbr_moveit_cpp <../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst> 34 | 35 | Integration 36 | ----------- 37 | TODO system integration demos 38 | -------------------------------------------------------------------------------- /lbr_description/urdf/iiwa14/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | A1: 2 | lower: -169 # each lower / upper limit is 1 degree stricter to avoid hard limits 3 | upper: 169 4 | velocity: 85 5 | effort: 200 6 | safety_position_margin: 5 # inside the URDF, we compute k_position=velocity/safety_position_margin 7 | k_velocity: 0 8 | A2: 9 | lower: -119 10 | upper: 119 11 | velocity: 85 12 | effort: 200 13 | safety_position_margin: 5 14 | k_velocity: 0 15 | A3: 16 | lower: -169 17 | upper: 169 18 | velocity: 100 19 | effort: 200 20 | safety_position_margin: 5 21 | k_velocity: 0 22 | A4: 23 | lower: -119 24 | upper: 119 25 | velocity: 75 26 | effort: 200 27 | safety_position_margin: 5 28 | k_velocity: 0 29 | A5: 30 | lower: -169 31 | upper: 169 32 | velocity: 130 33 | effort: 200 34 | safety_position_margin: 5 35 | k_velocity: 0 36 | A6: 37 | lower: -119 38 | upper: 119 39 | velocity: 135 40 | effort: 200 41 | safety_position_margin: 5 42 | k_velocity: 0 43 | A7: 44 | lower: -174 45 | upper: 174 46 | velocity: 135 47 | effort: 200 48 | safety_position_margin: 5 49 | k_velocity: 0 50 | -------------------------------------------------------------------------------- /lbr_description/urdf/iiwa7/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | A1: 2 | lower: -169 # each lower / upper limit is 1 degree stricter to avoid hard limits 3 | upper: 169 4 | velocity: 98 5 | effort: 200 6 | safety_position_margin: 5 # inside the URDF, we compute k_position=velocity/safety_position_margin 7 | k_velocity: 0 8 | A2: 9 | lower: -119 10 | upper: 119 11 | velocity: 98 12 | effort: 200 13 | safety_position_margin: 5 14 | k_velocity: 0 15 | A3: 16 | lower: -169 17 | upper: 169 18 | velocity: 100 19 | effort: 200 20 | safety_position_margin: 5 21 | k_velocity: 0 22 | A4: 23 | lower: -119 24 | upper: 119 25 | velocity: 130 26 | effort: 200 27 | safety_position_margin: 5 28 | k_velocity: 0 29 | A5: 30 | lower: -169 31 | upper: 169 32 | velocity: 140 33 | effort: 200 34 | safety_position_margin: 5 35 | k_velocity: 0 36 | A6: 37 | lower: -119 38 | upper: 119 39 | velocity: 180 40 | effort: 200 41 | safety_position_margin: 5 42 | k_velocity: 0 43 | A7: 44 | lower: -174 45 | upper: 174 46 | velocity: 180 47 | effort: 200 48 | safety_position_margin: 5 49 | k_velocity: 0 50 | -------------------------------------------------------------------------------- /lbr_description/urdf/med14/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | A1: 2 | lower: -169 # each lower / upper limit is 1 degree stricter to avoid hard limits 3 | upper: 169 4 | velocity: 85 5 | effort: 200 6 | safety_position_margin: 5 # inside the URDF, we compute k_position=velocity/safety_position_margin 7 | k_velocity: 0 8 | A2: 9 | lower: -119 10 | upper: 119 11 | velocity: 85 12 | effort: 200 13 | safety_position_margin: 5 14 | k_velocity: 0 15 | A3: 16 | lower: -169 17 | upper: 169 18 | velocity: 100 19 | effort: 200 20 | safety_position_margin: 5 21 | k_velocity: 0 22 | A4: 23 | lower: -119 24 | upper: 119 25 | velocity: 75 26 | effort: 200 27 | safety_position_margin: 5 28 | k_velocity: 0 29 | A5: 30 | lower: -169 31 | upper: 169 32 | velocity: 130 33 | effort: 200 34 | safety_position_margin: 5 35 | k_velocity: 0 36 | A6: 37 | lower: -119 38 | upper: 119 39 | velocity: 135 40 | effort: 200 41 | safety_position_margin: 5 42 | k_velocity: 0 43 | A7: 44 | lower: -174 45 | upper: 174 46 | velocity: 135 47 | effort: 200 48 | safety_position_margin: 5 49 | k_velocity: 0 50 | -------------------------------------------------------------------------------- /lbr_description/urdf/med7/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | A1: 2 | lower: -169 # each lower / upper limit is 1 degree stricter to avoid hard limits 3 | upper: 169 4 | velocity: 98 5 | effort: 200 6 | safety_position_margin: 5 # inside the URDF, we compute k_position=velocity/safety_position_margin 7 | k_velocity: 0 8 | A2: 9 | lower: -119 10 | upper: 119 11 | velocity: 98 12 | effort: 200 13 | safety_position_margin: 5 14 | k_velocity: 0 15 | A3: 16 | lower: -169 17 | upper: 169 18 | velocity: 100 19 | effort: 200 20 | safety_position_margin: 5 21 | k_velocity: 0 22 | A4: 23 | lower: -119 24 | upper: 119 25 | velocity: 130 26 | effort: 200 27 | safety_position_margin: 5 28 | k_velocity: 0 29 | A5: 30 | lower: -169 31 | upper: 169 32 | velocity: 140 33 | effort: 200 34 | safety_position_margin: 5 35 | k_velocity: 0 36 | A6: 37 | lower: -119 38 | upper: 119 39 | velocity: 180 40 | effort: 200 41 | safety_position_margin: 5 42 | k_velocity: 0 43 | A7: 44 | lower: -174 45 | upper: 174 46 | velocity: 180 47 | effort: 200 48 | safety_position_margin: 5 49 | k_velocity: 0 50 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_cpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lbr_demos_cpp 5 | 2.4.3 6 | C++ demos for lbr_ros2_control. 7 | mhubii 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | control_msgs 13 | fri_client_sdk 14 | lbr_fri_idl 15 | lbr_fri_ros2 16 | rclcpp 17 | rclcpp_action 18 | sensor_msgs 19 | trajectory_msgs 20 | 21 | lbr_bringup 22 | lbr_description 23 | lbr_ros2_control 24 | 25 | ament_lint_auto 26 | ament_lint_common 27 | 28 | 29 | ament_cmake 30 | 31 | -------------------------------------------------------------------------------- /lbr_fri_ros2/doc/lbr_fri_ros2.rst: -------------------------------------------------------------------------------- 1 | lbr_fri_ros2 2 | ============ 3 | .. note:: 4 | 5 | Users will interface the hardware through :ref:`lbr_ros2_control`. Documentation for ``lbr_fri_ros2`` is **intended for developers**. 6 | 7 | .. toctree:: 8 | :caption: API Reference 9 | 10 | ../../../docs/doxygen/lbr_fri_ros2/html/annotated_classes 11 | 12 | Software Architecture 13 | --------------------- 14 | The ``lbr_fri_ros2`` package extends the FRI with: 15 | 16 | #. :lbr_fri_ros2:`AsyncClient ` for asynchronous communication to the hardware 17 | #. :lbr_fri_ros2:`App ` for running the :lbr_fri_ros2:`AsyncClient ` asynchronously 18 | 19 | The software architecture is shown :ref:`below ` (click to expand): 20 | 21 | .. _lbr_fri_ros2 software architecture figure: 22 | .. thumbnail:: img/lbr_fri_ros2_v2.0.0.svg 23 | :alt: lbr_fri_ros2 24 | 25 | The :ref:`lbr_ros2_control` package can be considered a **User** in the above figure. It builds on top of the ``lbr_fri_ros2`` package to provide a ROS 2 interface to the hardware. 26 | -------------------------------------------------------------------------------- /.github/CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | The [LBR-Stack](https://github.com/lbr-stack/) thrives to build a friendly community for `KUKA` users to foster knowledge exchange! :) 2 | 3 | We **encourage** everyone to open issues for questions / problems / features. For additional information, we would like to provide some links below. 4 | 5 | ### Knowledge Sources 6 | - [Open issues](https://github.com/lbr-stack/lbr_fri_ros2_stack/issues) 7 | - [Closed issues](https://github.com/lbr-stack/lbr_fri_ros2_stack/issues?q=is%3Aissue+is%3Aclosed) 8 | - [Read the Docs](https://lbr-stack.readthedocs.io/en/latest/) 9 | 10 | ## Contributing 11 | ### LBR FRI ROS 2 Stack 12 | Contributions are vital to this project. If you want to contribute a feature / fix proceed as follows 13 | 14 | 1. Open an issue: 15 | - Explain the issue and your solution 16 | - Request a new branch: `dev--`, e.g. `dev-rolling-my-new-demo` 17 | 2. Fork this repository 18 | 3. Create a [pull request](https://github.com/lbr-stack/lbr_fri_ros2_stack/pulls) against `dev--` 19 | 20 | ### New FRI version 21 | Refer [README](https://github.com/lbr-stack/fri#contributing) for instructions. 22 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit/config/forward_keyboard.yaml: -------------------------------------------------------------------------------- 1 | /**/forward_keyboard: 2 | ros__parameters: 3 | velocity_scales: 4 | translation: 5 | x: 1.0 6 | y: 1.0 7 | z: 1.0 8 | rotation: 9 | x: 1.0 10 | y: 1.0 11 | z: 1.0 12 | joints: 13 | - 1.0 14 | - 1.0 15 | - 1.0 16 | - 1.0 17 | - 1.0 18 | - 1.0 19 | - 1.0 20 | keyboard_layout: 21 | # uses pynput: https://pypi.org/project/pynput/ 22 | translation: 23 | x: 24 | increase: "w" 25 | decrease: "s" 26 | y: 27 | increase: "a" 28 | decrease: "d" 29 | z: 30 | increase: "Key.up" 31 | decrease: "Key.down" 32 | rotation: 33 | x: 34 | increase: "u" 35 | decrease: "j" 36 | y: 37 | increase: "h" 38 | decrease: "k" 39 | z: 40 | increase: "Key.left" 41 | decrease: "Key.right" 42 | joints: 43 | - "1" 44 | - "2" 45 | - "3" 46 | - "4" 47 | - "5" 48 | - "6" 49 | - "7" 50 | escape: "Key.esc" 51 | pause: "p" 52 | reverse_joints: "r" 53 | -------------------------------------------------------------------------------- /lbr_fri_ros2/src/interfaces/base_command.cpp: -------------------------------------------------------------------------------- 1 | #include "lbr_fri_ros2/interfaces/base_command.hpp" 2 | 3 | namespace lbr_fri_ros2 { 4 | 5 | BaseCommandInterface::BaseCommandInterface(const double &joint_position_tau, 6 | const CommandGuardParameters &command_guard_parameters, 7 | const std::string &command_guard_variant) 8 | : command_initialized_(false), joint_position_filter_(joint_position_tau) { 9 | command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); 10 | }; 11 | 12 | void BaseCommandInterface::init_command(const_idl_state_t_ref state) { 13 | neutralize_command_(state, command_target_); 14 | command_ = command_target_; 15 | command_initialized_ = true; 16 | } 17 | 18 | void BaseCommandInterface::log_info() const { 19 | command_guard_->log_info(); 20 | joint_position_filter_.log_info(); 21 | } 22 | 23 | void BaseCommandInterface::neutralize_command_(const_idl_state_t_ref state, 24 | idl_command_t_ref command) { 25 | command.joint_position = state.measured_joint_position; 26 | command.torque.fill(0.); 27 | command.wrench.fill(0.); 28 | } 29 | } // namespace lbr_fri_ros2 30 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__PINV_HPP_ 2 | #define LBR_FRI_ROS2__PINV_HPP_ 3 | 4 | #include 5 | 6 | #include "eigen3/Eigen/Core" 7 | #include "eigen3/Eigen/SVD" 8 | 9 | namespace lbr_fri_ros2 { 10 | template 11 | Eigen::Matrix 12 | pinv(const MatT &mat, 13 | typename MatT::Scalar lambda = typename MatT::Scalar{2e-1}) // choose appropriately 14 | { 15 | typedef typename MatT::Scalar Scalar; 16 | auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); 17 | const auto &singularValues = svd.singularValues(); 18 | Eigen::Matrix dampedSingularValuesInv( 19 | mat.cols(), mat.rows()); 20 | dampedSingularValuesInv.setZero(); 21 | std::for_each(singularValues.data(), singularValues.data() + singularValues.size(), 22 | [&, i = 0](const Scalar &s) mutable { 23 | dampedSingularValuesInv(i, i) = s / (s * s + lambda * lambda); 24 | ++i; 25 | }); 26 | return svd.matrixV() * dampedSingularValuesInv * svd.matrixU().adjoint(); 27 | } 28 | } // namespace lbr_fri_ros2 29 | #endif // LBR_FRI_ROS2__PINV_HPP_ 30 | -------------------------------------------------------------------------------- /.github/workflows/backport.yaml: -------------------------------------------------------------------------------- 1 | name: Backport merged pull request 2 | on: 3 | issue_comment: 4 | types: [created] 5 | permissions: 6 | contents: write # so it can comment 7 | pull-requests: write # so it can create pull requests 8 | jobs: 9 | backport: 10 | name: Backport pull request 11 | runs-on: ubuntu-latest 12 | 13 | # Only run when pull request is merged 14 | # or when a comment starting with `/backport` is created by someone other than the 15 | # https://github.com/backport-action bot user (user id: 97796249). Note that if you use your 16 | # own PAT as `github_token`, that you should replace this id with yours. 17 | if: > 18 | ( 19 | github.event_name == 'pull_request_target' && 20 | github.event.pull_request.merged 21 | ) || ( 22 | github.event_name == 'issue_comment' && 23 | github.event.issue.pull_request && 24 | github.event.comment.user.id != 97796249 && 25 | startsWith(github.event.comment.body, '/backport') 26 | ) 27 | steps: 28 | - uses: actions/checkout@v4 29 | - name: Create backport pull requests 30 | uses: korthout/backport-action@v3 31 | with: 32 | label_pattern: "" 33 | target_branches: jazzy 34 | merge_commits: skip 35 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/guards/state_guard.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__STATE_GUARD_HPP_ 2 | #define LBR_FRI_ROS2__STATE_GUARD_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include "rclcpp/logger.hpp" 8 | #include "rclcpp/logging.hpp" 9 | 10 | #include "lbr_fri_idl/msg/lbr_state.hpp" 11 | #include "lbr_fri_ros2/formatting.hpp" 12 | #include "lbr_fri_ros2/types.hpp" 13 | 14 | namespace lbr_fri_ros2 { 15 | struct StateGuardParameters { 16 | jnt_name_array_t joint_names; /**< Joint names.*/ 17 | jnt_array_t max_external_torque{2., 2., 2., 2., 2., 2., 2.}; /**< Maximum external torque [Nm].*/ 18 | bool external_torque_safety_check{true}; /**< External torque safety check. */ 19 | }; 20 | 21 | class StateGuard { 22 | protected: 23 | static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::StateGuard"; 24 | 25 | public: 26 | StateGuard() = default; 27 | StateGuard(const StateGuardParameters &state_guard_parameters); 28 | virtual bool is_valid_state(const_idl_state_t_ref lbr_state); 29 | 30 | void log_info() const; 31 | 32 | protected: 33 | virtual bool state_in_external_torque_limits_(const_idl_state_t_ref lbr_state) const; 34 | 35 | StateGuardParameters parameters_; 36 | }; 37 | } // namespace lbr_fri_ros2 38 | #endif // LBR_FRI_ROS2__STATE_GUARD_HPP_ 39 | -------------------------------------------------------------------------------- /lbr_bringup/launch/rviz.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.substitutions import LaunchConfiguration, PathSubstitution 4 | from launch_ros.actions import Node 5 | from launch_ros.substitutions import FindPackageShare 6 | 7 | 8 | def generate_launch_description() -> LaunchDescription: 9 | return LaunchDescription( 10 | [ 11 | DeclareLaunchArgument( 12 | name="rviz_cfg", 13 | default_value="config/mock.rviz", 14 | description="The RViz configuration file relative to rviz_cfg_pkg.", 15 | ), 16 | DeclareLaunchArgument( 17 | name="rviz_cfg_pkg", 18 | default_value="lbr_bringup", 19 | description="The package containing the RViz configuration file.", 20 | ), 21 | Node( 22 | package="rviz2", 23 | executable="rviz2", 24 | name="rviz2", 25 | arguments=[ 26 | "-d", 27 | PathSubstitution( 28 | FindPackageShare(LaunchConfiguration("rviz_cfg_pkg")) 29 | ) 30 | / LaunchConfiguration("rviz_cfg"), 31 | ], 32 | ), 33 | ] 34 | ) 35 | -------------------------------------------------------------------------------- /lbr_fri_ros2/src/filters.cpp: -------------------------------------------------------------------------------- 1 | #include "lbr_fri_ros2/filters.hpp" 2 | 3 | namespace lbr_fri_ros2 { 4 | ExponentialFilter::ExponentialFilter() 5 | : tau_(std::numeric_limits::quiet_NaN()), 6 | sample_time_(std::numeric_limits::quiet_NaN()), 7 | alpha_(std::numeric_limits::quiet_NaN()) {} 8 | 9 | ExponentialFilter::ExponentialFilter(const double &tau) : tau_(tau) {} 10 | 11 | void ExponentialFilter::initialize(const double &sample_time) { 12 | if (std::isnan(tau_)) { 13 | throw std::runtime_error("Time constant must be set before initializing."); 14 | } 15 | return initialize(tau_, sample_time); 16 | } 17 | 18 | void ExponentialFilter::initialize(const double &tau, const double &sample_time) { 19 | if (tau <= 0.0) { 20 | throw std::runtime_error("Time constant must be positive and greater zero."); 21 | } 22 | if (sample_time < 0.0) { 23 | throw std::runtime_error("Sample time must be positive."); 24 | } 25 | double alpha = 1.0 - std::exp(-sample_time / tau); 26 | if (!validate_alpha_(alpha)) { 27 | throw std::runtime_error("Alpha is not within [0, 1]"); 28 | } 29 | tau_ = tau; 30 | sample_time_ = sample_time; 31 | alpha_ = alpha; 32 | } 33 | 34 | bool ExponentialFilter::validate_alpha_(const double &alpha) { return alpha <= 1. && alpha >= 0.; } 35 | } // namespace lbr_fri_ros2 36 | -------------------------------------------------------------------------------- /lbr_fri_ros2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lbr_fri_ros2 5 | 2.4.3 6 | The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS 7 | 2. Robot states can be extracted and commanded. 8 | mhubii 9 | Apache-2.0 10 | 11 | ament_cmake 12 | eigen3_cmake_module 13 | 14 | eigen 15 | 16 | fri_client_sdk 17 | geometry_msgs 18 | kdl_parser 19 | lbr_fri_idl 20 | orocos_kdl_vendor 21 | rclcpp 22 | realtime_tools 23 | urdf 24 | 25 | lbr_description 26 | 27 | eigen3_cmake_module 28 | eigen 29 | 30 | ament_lint_auto 31 | ament_lint_common 32 | 33 | 34 | ament_cmake 35 | 36 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__UTILS_HPP_ 2 | #define LBR_FRI_ROS2__UTILS_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include "rclcpp/rclcpp.hpp" 8 | 9 | namespace lbr_fri_ros2 { 10 | rclcpp::Parameter retrieve_paramter(const rclcpp::Node::SharedPtr node, 11 | const std::string &remote_node_name, 12 | const std::string ¶meter_name) { 13 | rclcpp::AsyncParametersClient parameter_client(node, remote_node_name); 14 | while (!parameter_client.wait_for_service(std::chrono::seconds(1))) { 15 | if (!rclcpp::ok()) { 16 | std::string err = "Interrupted while waiting for the service. Exiting."; 17 | RCLCPP_ERROR(node->get_logger(), err.c_str()); 18 | throw std::runtime_error(err); 19 | } 20 | RCLCPP_INFO(node->get_logger(), "Wating for '%s' service...", remote_node_name.c_str()); 21 | } 22 | auto future = parameter_client.get_parameters({parameter_name}); 23 | if (rclcpp::spin_until_future_complete(node->get_node_base_interface(), future) != 24 | rclcpp::FutureReturnCode::SUCCESS) { 25 | std::string err = "Failed to retrieve '%s'."; 26 | RCLCPP_ERROR(node->get_logger(), err.c_str()); 27 | throw std::runtime_error(err); 28 | } 29 | return future.get()[0]; 30 | }; 31 | } // namespace lbr_fri_ros2 32 | #endif // LBR_FRI_ROS2__UTILS_HPP_ 33 | -------------------------------------------------------------------------------- /lbr_ros2_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lbr_ros2_control 5 | 2.4.3 6 | ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI). 7 | mhubii 8 | Apache-2.0 9 | 10 | ament_cmake 11 | eigen3_cmake_module 12 | 13 | eigen 14 | 15 | controller_interface 16 | fri_client_sdk 17 | geometry_msgs 18 | hardware_interface 19 | kinematics_interface 20 | lbr_fri_idl 21 | lbr_fri_ros2 22 | pluginlib 23 | rclcpp 24 | realtime_tools 25 | ros2_control 26 | 27 | lbr_description 28 | ros2_controllers 29 | 30 | eigen3_cmake_module 31 | eigen 32 | 33 | 34 | ament_cmake 35 | 36 | -------------------------------------------------------------------------------- /lbr_description/ros2_control/gazebo_controllers.yaml: -------------------------------------------------------------------------------- 1 | # Use of /** so that the configurations hold for controller 2 | # managers regardless of their namespace. Usefull in multi-robot setups. 3 | /**/controller_manager: 4 | ros__parameters: 5 | update_rate: 100 6 | enforce_command_limits: true # can be removed from Kilted onward 7 | 8 | # ROS 2 control broadcasters 9 | joint_state_broadcaster: 10 | type: joint_state_broadcaster/JointStateBroadcaster 11 | 12 | # ROS 2 control controllers 13 | joint_trajectory_controller: 14 | type: joint_trajectory_controller/JointTrajectoryController 15 | 16 | forward_position_controller: 17 | type: position_controllers/JointGroupPositionController 18 | 19 | # ROS 2 control controllers 20 | /**/joint_trajectory_controller: 21 | ros__parameters: 22 | joints: 23 | - lbr_A1 24 | - lbr_A2 25 | - lbr_A3 26 | - lbr_A4 27 | - lbr_A5 28 | - lbr_A6 29 | - lbr_A7 30 | command_interfaces: 31 | - position 32 | state_interfaces: 33 | - position 34 | - velocity 35 | state_publish_rate: 50.0 36 | action_monitor_rate: 20.0 37 | 38 | /**/forward_position_controller: 39 | ros__parameters: 40 | joints: 41 | - lbr_A1 42 | - lbr_A2 43 | - lbr_A3 44 | - lbr_A4 45 | - lbr_A5 46 | - lbr_A6 47 | - lbr_A7 48 | interface_name: position 49 | -------------------------------------------------------------------------------- /lbr_description/ros2_control/mock_controllers.yaml: -------------------------------------------------------------------------------- 1 | # Use of /** so that the configurations hold for controller 2 | # managers regardless of their namespace. Usefull in multi-robot setups. 3 | /**/controller_manager: 4 | ros__parameters: 5 | update_rate: 100 6 | enforce_command_limits: true # can be removed from Kilted onward 7 | 8 | # ROS 2 control broadcasters 9 | joint_state_broadcaster: 10 | type: joint_state_broadcaster/JointStateBroadcaster 11 | 12 | # ROS 2 control controllers 13 | joint_trajectory_controller: 14 | type: joint_trajectory_controller/JointTrajectoryController 15 | 16 | forward_position_controller: 17 | type: position_controllers/JointGroupPositionController 18 | 19 | # ROS 2 control controllers 20 | /**/joint_trajectory_controller: 21 | ros__parameters: 22 | joints: 23 | - lbr_A1 24 | - lbr_A2 25 | - lbr_A3 26 | - lbr_A4 27 | - lbr_A5 28 | - lbr_A6 29 | - lbr_A7 30 | command_interfaces: 31 | - position 32 | state_interfaces: 33 | - position 34 | - velocity 35 | state_publish_rate: 50.0 36 | action_monitor_rate: 20.0 37 | 38 | /**/forward_position_controller: 39 | ros__parameters: 40 | joints: 41 | - lbr_A1 42 | - lbr_A2 43 | - lbr_A3 44 | - lbr_A4 45 | - lbr_A5 46 | - lbr_A6 47 | - lbr_A7 48 | interface_name: position 49 | -------------------------------------------------------------------------------- /lbr_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lbr_bringup 5 | 2.4.3 6 | LBR launch files. 7 | mhubii 8 | Apache-2.0 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | 13 | controller_manager 14 | gz_ros2_control 15 | joint_state_broadcaster 16 | joint_trajectory_controller 17 | lbr_description 18 | lbr_fri_ros2 19 | lbr_ros2_control 20 | moveit_planners_chomp 21 | moveit_planners_ompl 22 | moveit_planners_stomp 23 | moveit_ros_move_group 24 | moveit_servo 25 | rclpy 26 | robot_state_publisher 27 | ros_gz_sim 28 | ros_gz_bridge 29 | rviz2 30 | xacro 31 | 32 | 33 | ament_cmake 34 | 35 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit/launch/keyboard_driver.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.substitutions import LaunchConfiguration, PathSubstitution 4 | from launch_ros.actions import Node 5 | from launch_ros.substitutions import FindPackageShare 6 | from lbr_bringup.description import LBRDescriptionMixin 7 | 8 | 9 | def generate_launch_description() -> LaunchDescription: 10 | return LaunchDescription( 11 | [ 12 | DeclareLaunchArgument( 13 | name="keyboard_config_pkg", 14 | default_value="lbr_moveit", 15 | description="The package containing the keyboard configurations.", 16 | ), 17 | DeclareLaunchArgument( 18 | name="keyboard_config", 19 | default_value="config/forward_keyboard.yaml", 20 | description="Location of the keyboard configuration file relative to keyboard_config_pkg.", 21 | ), 22 | LBRDescriptionMixin.arg_robot_name(), 23 | Node( 24 | package="lbr_moveit", 25 | executable="forward_keyboard", 26 | output="screen", 27 | parameters=[ 28 | PathSubstitution( 29 | FindPackageShare(LaunchConfiguration("keyboard_config_pkg")) 30 | ) 31 | / LaunchConfiguration("keyboard_config"), 32 | ], 33 | namespace=LaunchConfiguration("robot_name"), 34 | ), 35 | ] 36 | ) 37 | -------------------------------------------------------------------------------- /lbr_description/urdf/med7/med7.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 17 | 20 | 21 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 39 | -------------------------------------------------------------------------------- /lbr_description/urdf/iiwa7/iiwa7.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 17 | 20 | 21 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 39 | -------------------------------------------------------------------------------- /lbr_description/urdf/med14/med14.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 17 | 20 | 21 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 39 | -------------------------------------------------------------------------------- /lbr_description/urdf/iiwa14/iiwa14.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 17 | 20 | 21 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 39 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/app.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__APP_HPP_ 2 | #define LBR_FRI_ROS2__APP_HPP_ 3 | 4 | #include 5 | #include 6 | 7 | #include "rclcpp/logger.hpp" 8 | #include "rclcpp/logging.hpp" 9 | #include "rclcpp/utilities.hpp" 10 | 11 | #include "friClientApplication.h" 12 | #include "friUdpConnection.h" 13 | 14 | #include "lbr_fri_ros2/async_client.hpp" 15 | #include "lbr_fri_ros2/formatting.hpp" 16 | #include "lbr_fri_ros2/worker.hpp" 17 | 18 | namespace lbr_fri_ros2 { 19 | class App : public Worker { 20 | /** 21 | * @brief This clas provides utilities to run the KUKA::FRI::ClientApplication asynchronously. 22 | * Note that the rate at which the application runs is determined by the robot. This is because 23 | * the run_thread_ uses blocking function calls from the FRI client SDK, i.e. 24 | * KUKA::FRI::ClientApplication::step() (this is by KUKA's design). 25 | * 26 | */ 27 | public: 28 | App(const std::shared_ptr async_client_ptr); 29 | ~App(); 30 | 31 | bool open_udp_socket(const int &port_id = 30200, const char *const remote_host = NULL); 32 | bool close_udp_socket(); 33 | void run_async(int rt_prio = 80) override; 34 | 35 | inline std::string LOGGER_NAME() const override { return "lbr_fri_ros2::App"; }; 36 | 37 | protected: 38 | void perform_work_() override; 39 | bool valid_port_(const int &port_id); 40 | 41 | std::shared_ptr async_client_ptr_; 42 | std::unique_ptr connection_ptr_; 43 | std::unique_ptr app_ptr_; 44 | }; 45 | } // namespace lbr_fri_ros2 46 | #endif // LBR_FRI_ROS2__APP_HPP_ 47 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py: -------------------------------------------------------------------------------- 1 | from typing import List 2 | 3 | from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity 4 | from launch.actions import OpaqueFunction 5 | from launch.substitutions import LaunchConfiguration 6 | from launch_ros.actions import Node 7 | from lbr_bringup.description import LBRDescriptionMixin 8 | from lbr_bringup.moveit import LBRMoveGroupMixin 9 | 10 | 11 | def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: 12 | ld = LaunchDescription() 13 | 14 | model = LaunchConfiguration("model").perform(context) 15 | mode = LaunchConfiguration("mode").perform(context) 16 | use_sim_time = False 17 | if mode == "gazebo": 18 | use_sim_time = True 19 | 20 | # generate moveit configs 21 | moveit_configs = LBRMoveGroupMixin.moveit_configs_builder( 22 | robot_name=model, 23 | package_name=f"{model}_moveit_config", 24 | ) 25 | 26 | # launch demo node 27 | ld.add_action( 28 | Node( 29 | package="lbr_moveit_cpp", 30 | executable="hello_moveit", 31 | parameters=[ 32 | moveit_configs.to_dict(), 33 | {"use_sim_time": use_sim_time}, 34 | LBRDescriptionMixin.param_robot_name(), 35 | ], 36 | ) 37 | ) 38 | return ld.entities 39 | 40 | 41 | def generate_launch_description() -> LaunchDescription: 42 | ld = LaunchDescription() 43 | 44 | ld.add_action(LBRDescriptionMixin.arg_model()) 45 | ld.add_action(LBRDescriptionMixin.arg_mode()) 46 | 47 | ld.add_action(OpaqueFunction(function=hidden_setup)) 48 | 49 | return ld 50 | -------------------------------------------------------------------------------- /docker/doc/docker.rst: -------------------------------------------------------------------------------- 1 | docker 2 | ====== 3 | To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions below. 4 | 5 | #. Install ``vcstool`` 6 | 7 | .. code-block:: bash 8 | 9 | pip3 install vcstool 10 | 11 | #. Create a workspace and clone 12 | 13 | .. code-block:: bash 14 | 15 | export FRI_CLIENT_VERSION=1.15 # replace by your FRI client version 16 | mkdir -p lbr-stack/src && cd lbr-stack 17 | vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/jazzy/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml 18 | 19 | #. Install `Docker `_:octicon:`link-external`. 20 | 21 | #. Copy the Dockerfile and the container scripts to the ``lbr-stack`` directory. Build and start the container 22 | 23 | .. code-block:: bash 24 | 25 | cp -r src/lbr_fri_ros2_stack/docker/* . 26 | sudo ./container_build.sh # this will run the container once finished 27 | 28 | #. Once inside the container, launch e.g. the mock setup via 29 | 30 | .. code-block:: bash 31 | 32 | ros2 launch lbr_bringup mock.launch.py model:=iiwa7 33 | 34 | .. hint:: 35 | 36 | List all arguments for the launch file via 37 | 38 | .. code-block:: bash 39 | 40 | ros2 launch lbr_bringup mock.launch.py 41 | 42 | .. hint:: 43 | 44 | Refer to :ref:`lbr_bringup` for more launch options. 45 | 46 | #. Connect another shell to the running container 47 | 48 | .. code-block:: bash 49 | 50 | ./container_new_console.sh 51 | 52 | #. Run e.g. MoveIt with RViz 53 | 54 | .. code-block:: bash 55 | 56 | ros2 launch lbr_bringup move_group.launch.py rviz:=true 57 | -------------------------------------------------------------------------------- /lbr_demos/lbr_moveit_cpp/src/hello_moveit.cpp: -------------------------------------------------------------------------------- 1 | #include "geometry_msgs/msg/pose.hpp" 2 | #include "moveit/move_group_interface/move_group_interface.hpp" 3 | #include "rclcpp/rclcpp.hpp" 4 | 5 | int main(int argc, char **argv) { 6 | rclcpp::init(argc, argv); 7 | 8 | // Configure node 9 | auto node_ptr = rclcpp::Node::make_shared("hello_moveit"); 10 | node_ptr->declare_parameter("robot_name", "lbr"); 11 | auto robot_name = node_ptr->get_parameter("robot_name").as_string(); 12 | 13 | // Create MoveGroupInterface (lives inside robot_name namespace) 14 | auto move_group_interface = moveit::planning_interface::MoveGroupInterface( 15 | node_ptr, moveit::planning_interface::MoveGroupInterface::Options("arm", "robot_description", 16 | robot_name)); 17 | 18 | // Limite the velocity for this demo 19 | move_group_interface.setMaxVelocityScalingFactor(0.02); 20 | 21 | // Set a target pose 22 | geometry_msgs::msg::Pose target_pose; 23 | target_pose.orientation.w = 1.0; 24 | target_pose.position.x = -0.4; 25 | target_pose.position.y = 0.0; 26 | target_pose.position.z = 0.9; 27 | move_group_interface.setPoseTarget(target_pose); 28 | 29 | // Create a plan to that target pose 30 | moveit::planning_interface::MoveGroupInterface::Plan plan; 31 | auto error_code = move_group_interface.plan(plan); 32 | 33 | if (error_code == moveit::core::MoveItErrorCode::SUCCESS) { 34 | // Execute the plan 35 | move_group_interface.execute(plan); 36 | } else { 37 | RCLCPP_ERROR(node_ptr->get_logger(), "Failed to plan to target pose"); 38 | } 39 | 40 | rclcpp::shutdown(); 41 | return 0; 42 | } 43 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__KINEMATICS_HPP_ 2 | #define LBR_FRI_ROS2__KINEMATICS_HPP_ 3 | 4 | #include 5 | 6 | #include "eigen3/Eigen/Core" 7 | #include "kdl/chain.hpp" 8 | #include "kdl/chainfksolverpos_recursive.hpp" 9 | #include "kdl/chainjnttojacsolver.hpp" 10 | #include "kdl/tree.hpp" 11 | #include "kdl_parser/kdl_parser.hpp" 12 | #include "rclcpp/logger.hpp" 13 | #include "rclcpp/logging.hpp" 14 | 15 | #include "friLBRState.h" 16 | 17 | #include "lbr_fri_idl/msg/lbr_state.hpp" 18 | #include "lbr_fri_ros2/types.hpp" 19 | 20 | namespace lbr_fri_ros2 { 21 | class Kinematics { 22 | protected: 23 | static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::Kinematics"; 24 | 25 | public: 26 | Kinematics(const std::string &robot_description, const std::string &chain_root = "lbr_link_0", 27 | const std::string &chain_tip = "lbr_link_ee"); 28 | 29 | // internally computes the jacobian and return a reference to it 30 | const KDL::Jacobian &compute_jacobian(const_jnt_array_t_ref q); 31 | const KDL::Jacobian &compute_jacobian(const std::vector &q); 32 | 33 | // forward kinematics 34 | const KDL::Frame &compute_fk(const_jnt_array_t_ref q); 35 | const KDL::Frame &compute_fk(const std::vector &q); 36 | 37 | protected: 38 | KDL::Tree tree_; 39 | KDL::Chain chain_; 40 | 41 | // solvers 42 | std::unique_ptr jacobian_solver_; 43 | std::unique_ptr fk_solver_; 44 | 45 | // robot state 46 | KDL::JntArray q_; 47 | 48 | // forward kinematics 49 | KDL::Frame chain_tip_frame_; 50 | 51 | // jacobian 52 | KDL::Jacobian jacobian_; 53 | }; 54 | } // namespace lbr_fri_ros2 55 | #endif // LBR_FRI_ROS2__KINEMATICS_HPP_ 56 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__INTERFACES__STATE_HPP_ 2 | #define LBR_FRI_ROS2__INTERFACES__STATE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "rclcpp/logger.hpp" 10 | #include "rclcpp/logging.hpp" 11 | 12 | #include "friClientVersion.h" 13 | 14 | #include "lbr_fri_idl/msg/lbr_state.hpp" 15 | #include "lbr_fri_ros2/filters.hpp" 16 | #include "lbr_fri_ros2/types.hpp" 17 | 18 | namespace lbr_fri_ros2 { 19 | struct StateInterfaceParameters { 20 | double external_torque_tau; /*seconds*/ 21 | double measured_torque_tau; /*seconds*/ 22 | }; 23 | 24 | class StateInterface { 25 | protected: 26 | static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::StateInterface"; 27 | 28 | public: 29 | StateInterface() = delete; 30 | StateInterface(const StateInterfaceParameters &state_interface_parameters = {0.04, 0.04}); 31 | 32 | idl_state_t get_state() const { 33 | std::lock_guard lock(state_mutex_); 34 | return state_; 35 | }; 36 | 37 | void set_state(const_fri_state_t_ref state); 38 | void set_state_open_loop(const_fri_state_t_ref state, const_jnt_array_t_ref joint_position); 39 | 40 | inline void uninitialize() { state_initialized_ = false; } 41 | inline bool is_initialized() const { return state_initialized_; }; 42 | 43 | void log_info() const; 44 | 45 | protected: 46 | void init_filters_(); 47 | 48 | mutable std::mutex state_mutex_; 49 | std::atomic_bool state_initialized_; 50 | idl_state_t state_; 51 | StateInterfaceParameters parameters_; 52 | ExponentialFilterArray external_torque_filter_, measured_torque_filter_; 53 | }; 54 | } // namespace lbr_fri_ros2 55 | #endif // LBR_FRI_ROS2__INTERFACES__STATE_HPP_ 56 | -------------------------------------------------------------------------------- /lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | 3 | # For beginners, we downscale velocity and acceleration limits. 4 | # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. 5 | default_velocity_scaling_factor: 0.1 6 | default_acceleration_scaling_factor: 0.1 7 | 8 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 9 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 10 | joint_limits: 11 | lbr_A1: 12 | has_velocity_limits: true 13 | max_velocity: 1.7104226669544429 14 | has_acceleration_limits: true 15 | max_acceleration: 10.0 16 | lbr_A2: 17 | has_velocity_limits: true 18 | max_velocity: 1.7104226669544429 19 | has_acceleration_limits: true 20 | max_acceleration: 10.0 21 | lbr_A3: 22 | has_velocity_limits: true 23 | max_velocity: 1.7453292519943295 24 | has_acceleration_limits: true 25 | max_acceleration: 10.0 26 | lbr_A4: 27 | has_velocity_limits: true 28 | max_velocity: 2.2689280275926285 29 | has_acceleration_limits: true 30 | max_acceleration: 10.0 31 | lbr_A5: 32 | has_velocity_limits: true 33 | max_velocity: 2.4434609527920612 34 | has_acceleration_limits: true 35 | max_acceleration: 10.0 36 | lbr_A6: 37 | has_velocity_limits: true 38 | max_velocity: 3.1415926535897931 39 | has_acceleration_limits: true 40 | max_acceleration: 10.0 41 | lbr_A7: 42 | has_velocity_limits: true 43 | max_velocity: 3.1415926535897931 44 | has_acceleration_limits: true 45 | max_acceleration: 10.0 -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa14_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | 3 | # For beginners, we downscale velocity and acceleration limits. 4 | # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. 5 | default_velocity_scaling_factor: 0.1 6 | default_acceleration_scaling_factor: 0.1 7 | 8 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 9 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 10 | joint_limits: 11 | lbr_A1: 12 | has_velocity_limits: true 13 | max_velocity: 1.4835298641951802 14 | has_acceleration_limits: true 15 | max_acceleration: 10.0 16 | lbr_A2: 17 | has_velocity_limits: true 18 | max_velocity: 1.4835298641951802 19 | has_acceleration_limits: true 20 | max_acceleration: 10.0 21 | lbr_A3: 22 | has_velocity_limits: true 23 | max_velocity: 1.7453292519943295 24 | has_acceleration_limits: true 25 | max_acceleration: 10.0 26 | lbr_A4: 27 | has_velocity_limits: true 28 | max_velocity: 1.3089969389957472 29 | has_acceleration_limits: true 30 | max_acceleration: 10.0 31 | lbr_A5: 32 | has_velocity_limits: true 33 | max_velocity: 2.2689280275926285 34 | has_acceleration_limits: true 35 | max_acceleration: 10.0 36 | lbr_A6: 37 | has_velocity_limits: true 38 | max_velocity: 2.3561944901923448 39 | has_acceleration_limits: true 40 | max_acceleration: 10.0 41 | lbr_A7: 42 | has_velocity_limits: true 43 | max_velocity: 2.3561944901923448 44 | has_acceleration_limits: true 45 | max_acceleration: 10.0 -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa7_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | 3 | # For beginners, we downscale velocity and acceleration limits. 4 | # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. 5 | default_velocity_scaling_factor: 0.1 6 | default_acceleration_scaling_factor: 0.1 7 | 8 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 9 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 10 | joint_limits: 11 | lbr_A1: 12 | has_velocity_limits: true 13 | max_velocity: 1.7104226669544429 14 | has_acceleration_limits: true 15 | max_acceleration: 10.0 16 | lbr_A2: 17 | has_velocity_limits: true 18 | max_velocity: 1.7104226669544429 19 | has_acceleration_limits: true 20 | max_acceleration: 10.0 21 | lbr_A3: 22 | has_velocity_limits: true 23 | max_velocity: 1.7453292519943295 24 | has_acceleration_limits: true 25 | max_acceleration: 10.0 26 | lbr_A4: 27 | has_velocity_limits: true 28 | max_velocity: 2.2689280275926285 29 | has_acceleration_limits: true 30 | max_acceleration: 10.0 31 | lbr_A5: 32 | has_velocity_limits: true 33 | max_velocity: 2.4434609527920612 34 | has_acceleration_limits: true 35 | max_acceleration: 10.0 36 | lbr_A6: 37 | has_velocity_limits: true 38 | max_velocity: 3.1415926535897931 39 | has_acceleration_limits: true 40 | max_acceleration: 10.0 41 | lbr_A7: 42 | has_velocity_limits: true 43 | max_velocity: 3.1415926535897931 44 | has_acceleration_limits: true 45 | max_acceleration: 10.0 -------------------------------------------------------------------------------- /lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | 3 | # For beginners, we downscale velocity and acceleration limits. 4 | # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. 5 | default_velocity_scaling_factor: 0.1 6 | default_acceleration_scaling_factor: 0.1 7 | 8 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 9 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 10 | joint_limits: 11 | lbr_A1: 12 | has_velocity_limits: true 13 | max_velocity: 1.4835298641951802 14 | has_acceleration_limits: true 15 | max_acceleration: 10.0 16 | lbr_A2: 17 | has_velocity_limits: true 18 | max_velocity: 1.4835298641951802 19 | has_acceleration_limits: true 20 | max_acceleration: 10.0 21 | lbr_A3: 22 | has_velocity_limits: true 23 | max_velocity: 1.7453292519943295 24 | has_acceleration_limits: true 25 | max_acceleration: 10.0 26 | lbr_A4: 27 | has_velocity_limits: true 28 | max_velocity: 1.3089969389957472 29 | has_acceleration_limits: true 30 | max_acceleration: 10.0 31 | lbr_A5: 32 | has_velocity_limits: true 33 | max_velocity: 2.2689280275926285 34 | has_acceleration_limits: true 35 | max_acceleration: 10.0 36 | lbr_A6: 37 | has_velocity_limits: true 38 | max_velocity: 2.3561944901923448 39 | has_acceleration_limits: true 40 | max_acceleration: 10.0 41 | lbr_A7: 42 | has_velocity_limits: true 43 | max_velocity: 2.3561944901923448 44 | has_acceleration_limits: true 45 | max_acceleration: 10.0 -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_cpp/src/pose_planning_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "geometry_msgs/msg/pose.hpp" 4 | #include "rclcpp/rclcpp.hpp" 5 | 6 | namespace lbr_demos { 7 | class PosePlanningNode : public rclcpp::Node { 8 | protected: 9 | rclcpp::Publisher::SharedPtr pose_pub_; 10 | rclcpp::Subscription::SharedPtr pose_sub_; 11 | 12 | geometry_msgs::msg::Pose initial_pose_; // robot starts from this pose 13 | bool is_init_; 14 | 15 | double amplitude_; // rad 16 | double frequency_; // Hz 17 | double sampling_time_; // sampling time for sending position command 18 | double phase_; // initial phase 19 | 20 | public: 21 | PosePlanningNode(const rclcpp::NodeOptions &options) : Node("pose_planning", options) { 22 | is_init_ = false; 23 | amplitude_ = 0.05; 24 | frequency_ = 0.5; 25 | sampling_time_ = 0.01; 26 | phase_ = 0.0; 27 | 28 | pose_pub_ = this->create_publisher("command/pose", 1); 29 | pose_sub_ = this->create_subscription( 30 | "state/pose", 1, std::bind(&PosePlanningNode::on_pose_, this, std::placeholders::_1)); 31 | } 32 | 33 | protected: 34 | void on_pose_(const geometry_msgs::msg::Pose &msg) { 35 | if (!is_init_) { 36 | initial_pose_ = msg; 37 | is_init_ = true; 38 | } else { 39 | geometry_msgs::msg::Pose cartesian_pose_command = initial_pose_; 40 | 41 | phase_ = phase_ + 2 * M_PI * frequency_ * sampling_time_; 42 | cartesian_pose_command.position.z += amplitude_ * sin(phase_); 43 | 44 | pose_pub_->publish(cartesian_pose_command); 45 | } 46 | } 47 | }; 48 | } // namespace lbr_demos 49 | 50 | #include "rclcpp_components/register_node_macro.hpp" 51 | RCLCPP_COMPONENTS_REGISTER_NODE(lbr_demos::PosePlanningNode) 52 | -------------------------------------------------------------------------------- /lbr_fri_ros2/src/worker.cpp: -------------------------------------------------------------------------------- 1 | #include "lbr_fri_ros2/worker.hpp" 2 | 3 | namespace lbr_fri_ros2 { 4 | Worker::Worker() : should_stop_(true), running_(false) {} 5 | 6 | Worker::~Worker() { 7 | this->request_stop(); 8 | if (run_thread_.joinable()) { 9 | run_thread_.join(); 10 | } 11 | } 12 | 13 | void Worker::run_async(int rt_prio) { 14 | if (running_) { 15 | RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME()), 16 | ColorScheme::WARNING << "Worker already running" << ColorScheme::ENDC); 17 | return; 18 | } 19 | run_thread_ = std::thread([this, rt_prio]() { 20 | if (!realtime_tools::configure_sched_fifo(rt_prio)) { 21 | RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME()), 22 | ColorScheme::WARNING 23 | << "Failed to set FIFO realtime scheduling policy. Refer to " 24 | "[https://control.ros.org/master/doc/ros2_control/" 25 | "controller_manager/doc/userdoc.html]." 26 | << ColorScheme::ENDC); 27 | } else { 28 | RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), 29 | ColorScheme::OKGREEN 30 | << "Realtime scheduling policy set to FIFO with priority '" << rt_prio 31 | << "'" << ColorScheme::ENDC); 32 | } 33 | 34 | RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Starting run thread"); 35 | should_stop_ = false; 36 | 37 | // perform work in child-class 38 | this->perform_work_(); 39 | // perform work end 40 | 41 | running_ = false; 42 | RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Exiting run thread"); 43 | }); 44 | } 45 | 46 | void Worker::request_stop() { 47 | if (!running_) { 48 | return; 49 | } 50 | RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Requesting run thread stop"); 51 | should_stop_ = true; 52 | } 53 | } // namespace lbr_fri_ros2 54 | -------------------------------------------------------------------------------- /lbr_fri_ros2/src/guards/state_guard.cpp: -------------------------------------------------------------------------------- 1 | #include "lbr_fri_ros2/guards/state_guard.hpp" 2 | 3 | namespace lbr_fri_ros2 { 4 | StateGuard::StateGuard(const StateGuardParameters &state_guard_parameters) 5 | : parameters_(state_guard_parameters) {}; 6 | 7 | bool StateGuard::is_valid_state(const_idl_state_t_ref lbr_state) { 8 | if (!state_in_external_torque_limits_(lbr_state)) { 9 | return false; 10 | } 11 | return true; 12 | } 13 | 14 | void StateGuard::log_info() const { 15 | RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); 16 | RCLCPP_INFO_STREAM( 17 | rclcpp::get_logger(LOGGER_NAME), 18 | "* External torque safety check (only on activation of compliant control modes): '" 19 | << (parameters_.external_torque_safety_check ? "true" : "false") << "'"); 20 | for (std::size_t i = 0; i < parameters_.joint_names.size(); ++i) { 21 | RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Joint %s limits: External torque: %.1f Nm", 22 | parameters_.joint_names[i].c_str(), parameters_.max_external_torque[i]); 23 | } 24 | } 25 | 26 | bool StateGuard::state_in_external_torque_limits_(const_idl_state_t_ref lbr_state) const { 27 | for (std::size_t i = 0; i < lbr_state.external_torque.size(); ++i) { 28 | if (std::abs(lbr_state.external_torque[i]) > parameters_.max_external_torque[i]) { 29 | RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), 30 | ColorScheme::ERROR << "External torque not in limits for joint " 31 | << parameters_.joint_names[i].c_str() << ". Measured: " 32 | << std::abs(lbr_state.external_torque[i]) 33 | << " Nm, limit: " << parameters_.max_external_torque[i] 34 | << " Nm" << ColorScheme::ENDC); 35 | return false; 36 | } 37 | } 38 | return true; 39 | } 40 | } // namespace lbr_fri_ros2 41 | -------------------------------------------------------------------------------- /lbr_bringup/lbr_bringup/gazebo.py: -------------------------------------------------------------------------------- 1 | from typing import List, Optional, Union 2 | 3 | from launch.actions import IncludeLaunchDescription 4 | from launch.substitutions import LaunchConfiguration, PathSubstitution 5 | from launch_ros.actions import Node 6 | from launch_ros.substitutions import FindPackageShare 7 | 8 | 9 | class GazeboMixin: 10 | @staticmethod 11 | def include_gazebo(**kwargs) -> IncludeLaunchDescription: 12 | return IncludeLaunchDescription( 13 | PathSubstitution( 14 | FindPackageShare("ros_gz_sim"), 15 | ) 16 | / "launch" 17 | / "gz_sim.launch.py", 18 | launch_arguments={"gz_args": "-r empty.sdf"}.items(), 19 | **kwargs, 20 | ) 21 | 22 | @staticmethod 23 | def node_create( 24 | robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( 25 | "robot_name", default="lbr" 26 | ), 27 | tf: List[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 28 | **kwargs, 29 | ) -> Node: 30 | label = ["-x", "-y", "-z", "-R", "-P", "-Y"] 31 | tf = [str(x) for x in tf] 32 | return Node( 33 | package="ros_gz_sim", 34 | executable="create", 35 | arguments=[ 36 | "-topic", 37 | "robot_description", 38 | "-name", 39 | robot_name, 40 | "-allow_renaming", 41 | ] 42 | + [item for pair in zip(label, tf) for item in pair], 43 | output="screen", 44 | namespace=robot_name, 45 | **kwargs, 46 | ) 47 | 48 | @staticmethod 49 | def node_clock_bridge(**kwargs) -> Node: 50 | return Node( 51 | package="ros_gz_bridge", 52 | executable="parameter_bridge", 53 | arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], 54 | output="screen", 55 | **kwargs, 56 | ) 57 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__INTERACES__COMMAND_HPP_ 2 | #define LBR_FRI_ROS2__INTERACES__COMMAND_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "rclcpp/logger.hpp" 11 | #include "rclcpp/logging.hpp" 12 | 13 | #include "friClientVersion.h" 14 | 15 | #include "lbr_fri_idl/msg/lbr_command.hpp" 16 | #include "lbr_fri_ros2/filters.hpp" 17 | #include "lbr_fri_ros2/formatting.hpp" 18 | #include "lbr_fri_ros2/guards/command_guard.hpp" 19 | #include "lbr_fri_ros2/types.hpp" 20 | 21 | namespace lbr_fri_ros2 { 22 | class BaseCommandInterface { 23 | protected: 24 | virtual std::string LOGGER_NAME() const = 0; 25 | 26 | public: 27 | BaseCommandInterface() = delete; 28 | BaseCommandInterface(const double &joint_position_tau, 29 | const CommandGuardParameters &command_guard_parameters, 30 | const std::string &command_guard_variant = "default"); 31 | 32 | virtual void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) = 0; 33 | void buffer_command_target(const_idl_command_t_ref command) { 34 | std::lock_guard lock(command_mutex_); 35 | command_target_ = command; 36 | } 37 | void init_command(const_idl_state_t_ref state); 38 | 39 | idl_command_t get_command() const { 40 | std::lock_guard lock(command_mutex_); 41 | return command_; 42 | } 43 | 44 | void log_info() const; 45 | 46 | protected: 47 | void neutralize_command_(const_idl_state_t_ref state, idl_command_t_ref command); 48 | 49 | protected: 50 | mutable std::mutex command_mutex_; 51 | bool command_initialized_; 52 | std::unique_ptr command_guard_; 53 | ExponentialFilterArray joint_position_filter_; 54 | idl_command_t command_, command_target_; 55 | }; 56 | } // namespace lbr_fri_ros2 57 | #endif // LBR_FRI_ROS2__INTERACES__COMMAND_HPP_ 58 | -------------------------------------------------------------------------------- /lbr_bringup/lbr_bringup/rviz.py: -------------------------------------------------------------------------------- 1 | from typing import Optional, Union 2 | 3 | from launch.actions import DeclareLaunchArgument 4 | from launch.substitutions import LaunchConfiguration, PathSubstitution 5 | from launch_ros.actions import Node 6 | from launch_ros.substitutions import FindPackageShare 7 | 8 | 9 | class RVizMixin: 10 | @staticmethod 11 | def arg_rviz(default_value: str = "false") -> DeclareLaunchArgument: 12 | return DeclareLaunchArgument( 13 | name="rviz", 14 | default_value=default_value, 15 | description="Whether to launch RViz.", 16 | ) 17 | 18 | @staticmethod 19 | def arg_rviz_cfg_pkg( 20 | default_value: str = "lbr_bringup", 21 | ) -> DeclareLaunchArgument: 22 | return DeclareLaunchArgument( 23 | name="rviz_cfg_pkg", 24 | default_value=default_value, 25 | description="The package containing the RViz configuration file.", 26 | ) 27 | 28 | @staticmethod 29 | def arg_rviz_cfg( 30 | default_value: str = "config/mock.rviz", 31 | ) -> DeclareLaunchArgument: 32 | return DeclareLaunchArgument( 33 | name="rviz_cfg", 34 | default_value=default_value, 35 | description="The RViz configuration file relative to rviz_cfg_pkg.", 36 | ) 37 | 38 | @staticmethod 39 | def node_rviz( 40 | rviz_cfg_pkg: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( 41 | "rviz_cfg_pkg", default="lbr_bringup" 42 | ), 43 | rviz_cfg: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( 44 | "rviz_cfg", default="config/mock.rviz" 45 | ), 46 | **kwargs, 47 | ) -> Node: 48 | return Node( 49 | package="rviz2", 50 | executable="rviz2", 51 | name="rviz2", 52 | arguments=[ 53 | "-d", 54 | PathSubstitution(FindPackageShare(rviz_cfg_pkg)) / rviz_cfg, 55 | ], 56 | **kwargs, 57 | ) 58 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(lbr_demos_advanced_cpp) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | if(NOT CMAKE_BUILD_TYPE) 9 | set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE) 10 | endif() 11 | 12 | # find dependencies 13 | find_package(ament_cmake REQUIRED) 14 | find_package(FRIClient REQUIRED) 15 | find_package(geometry_msgs REQUIRED) 16 | find_package(kdl_parser REQUIRED) 17 | find_package(lbr_fri_idl REQUIRED) 18 | find_package(lbr_fri_ros2 REQUIRED) 19 | find_package(orocos_kdl_vendor REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(rclcpp_components REQUIRED) 22 | 23 | # pose planning node 24 | add_library(pose_planning_component 25 | SHARED 26 | src/pose_planning_node.cpp 27 | ) 28 | 29 | target_include_directories(pose_planning_component 30 | PRIVATE src 31 | ) 32 | 33 | ament_target_dependencies(pose_planning_component 34 | geometry_msgs 35 | rclcpp 36 | rclcpp_components 37 | ) 38 | 39 | rclcpp_components_register_node(pose_planning_component 40 | PLUGIN lbr_demos::PosePlanningNode 41 | EXECUTABLE pose_planning 42 | ) 43 | 44 | # pose contol node 45 | add_library(pose_control_component 46 | SHARED 47 | src/pose_control_node.cpp 48 | ) 49 | 50 | target_include_directories(pose_control_component 51 | PRIVATE src 52 | ) 53 | 54 | ament_target_dependencies(pose_control_component 55 | geometry_msgs 56 | kdl_parser 57 | lbr_fri_idl 58 | orocos_kdl_vendor 59 | rclcpp 60 | rclcpp_components 61 | ) 62 | 63 | target_link_libraries(pose_control_component 64 | FRIClient::FRIClient 65 | ) 66 | 67 | rclcpp_components_register_node(pose_control_component 68 | PLUGIN lbr_demos::PoseControlNode 69 | EXECUTABLE pose_control 70 | ) 71 | 72 | # install 73 | install( 74 | TARGETS pose_planning_component pose_control_component 75 | LIBRARY DESTINATION lib 76 | RUNTIME DESTINATION lib/${PROJECT_NAME} 77 | ) 78 | 79 | ament_package() 80 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/types.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__TYPES_HPP_ 2 | #define LBR_FRI_ROS2__TYPES_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "friLBRClient.h" 9 | 10 | #include "lbr_fri_idl/msg/lbr_command.hpp" 11 | #include "lbr_fri_idl/msg/lbr_state.hpp" 12 | 13 | namespace lbr_fri_ros2 { 14 | // joint DoF alias 15 | constexpr std::uint8_t N_JNTS = KUKA::FRI::LBRState::NUMBER_OF_JOINTS; 16 | 17 | // joint positions, velocities, accelerations, torques etc. 18 | using jnt_array_t = std::array; 19 | using jnt_array_t_ref = jnt_array_t &; 20 | using const_jnt_array_t_ref = const jnt_array_t &; 21 | 22 | // joint names 23 | using jnt_name_array_t = std::array; 24 | using jnt_name_array_t_ref = jnt_name_array_t &; 25 | using const_jnt_name_array_t_ref = const jnt_name_array_t &; 26 | 27 | // Cartesian DoF 28 | constexpr std::uint8_t CARTESIAN_DOF = 6; 29 | 30 | // Cartesian positions, velocities, accelerations, wrenches etc. 31 | using cart_array_t = std::array; 32 | using cart_array_t_ref = cart_array_t &; 33 | using const_cart_array_t_ref = const cart_array_t &; 34 | 35 | // Cartesian names 36 | using cart_name_array_t = std::array; 37 | using cart_name_array_t_ref = cart_name_array_t &; 38 | using const_cart_name_array_t_ref = const cart_name_array_t &; 39 | 40 | // FRI types 41 | using fri_command_t = KUKA::FRI::LBRCommand; 42 | using fri_command_t_ref = fri_command_t &; 43 | using const_fri_command_t_ref = const fri_command_t &; 44 | using fri_state_t = KUKA::FRI::LBRState; 45 | using fri_state_t_ref = fri_state_t &; 46 | using const_fri_state_t_ref = const fri_state_t &; 47 | 48 | // ROS IDL types 49 | using idl_command_t = lbr_fri_idl::msg::LBRCommand; 50 | using idl_command_t_ref = idl_command_t &; 51 | using const_idl_command_t_ref = const idl_command_t &; 52 | using idl_state_t = lbr_fri_idl::msg::LBRState; 53 | using idl_state_t_ref = idl_state_t &; 54 | using const_idl_state_t_ref = const idl_state_t &; 55 | } // namespace lbr_fri_ros2 56 | #endif // LBR_FRI_ROS2__TYPES_HPP_ 57 | -------------------------------------------------------------------------------- /lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_ROS2_CONTROL__SYSTEM_INTERFACE_TYPE_VALUES_HPP_ 2 | #define LBR_ROS2_CONTROL__SYSTEM_INTERFACE_TYPE_VALUES_HPP_ 3 | 4 | // see 5 | // https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp 6 | namespace lbr_ros2_control { 7 | // additional LBR state interfaces, reference KUKA::FRI::LBRState 8 | constexpr char HW_IF_SAMPLE_TIME[] = "sample_time"; 9 | constexpr char HW_IF_SESSION_STATE[] = "session_state"; 10 | constexpr char HW_IF_CONNECTION_QUALITY[] = "connection_quality"; 11 | constexpr char HW_IF_SAFETY_STATE[] = "safety_state"; 12 | constexpr char HW_IF_OPERATION_MODE[] = "operation_mode"; 13 | constexpr char HW_IF_DRIVE_STATE[] = "drive_state"; 14 | constexpr char HW_IF_CLIENT_COMMAND_MODE[] = "client_command_mode"; 15 | constexpr char HW_IF_OVERLAY_TYPE[] = "overlay_type"; 16 | constexpr char HW_IF_CONTROL_MODE[] = "control_mode"; 17 | 18 | constexpr char HW_IF_TIME_STAMP_SEC[] = "time_stamp_sec"; 19 | constexpr char HW_IF_TIME_STAMP_NANO_SEC[] = "time_stamp_nano_sec"; 20 | 21 | constexpr char HW_IF_COMMANDED_JOINT_POSITION[] = "commanded_joint_position"; 22 | constexpr char HW_IF_COMMANDED_TORQUE[] = "commanded_torque"; 23 | 24 | constexpr char HW_IF_EXTERNAL_TORQUE[] = "external_torque"; 25 | 26 | constexpr char HW_IF_IPO_JOINT_POSITION[] = "ipo_joint_position"; 27 | constexpr char HW_IF_TRACKING_PERFORMANCE[] = "tracking_performance"; 28 | 29 | // additional force-torque command and state interfaces 30 | constexpr char HW_IF_FORCE_X[] = "force.x"; 31 | constexpr char HW_IF_FORCE_Y[] = "force.y"; 32 | constexpr char HW_IF_FORCE_Z[] = "force.z"; 33 | constexpr char HW_IF_TORQUE_X[] = "torque.x"; 34 | constexpr char HW_IF_TORQUE_Y[] = "torque.y"; 35 | constexpr char HW_IF_TORQUE_Z[] = "torque.z"; 36 | 37 | // additional LBR command interfaces, reference KUKA::FRI::LBRCommand 38 | constexpr char HW_IF_WRENCH_PREFIX[] = "wrench"; 39 | constexpr char HW_IF_AUXILIARY_PREFIX[] = "auxiliary_sensor"; 40 | } // namespace lbr_ros2_control 41 | #endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_TYPE_VALUES_HPP_ 42 | -------------------------------------------------------------------------------- /lbr_moveit_config/med7_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | med7_moveit_config 5 | 0.3.0 6 | 7 | An automatically generated package with all the configuration and launch files for using the med7 with the MoveIt Motion Planning Framework 8 | 9 | mhubii 10 | 11 | BSD-3-Clause 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/moveit/moveit2/issues 15 | https://github.com/moveit/moveit2 16 | 17 | mhubii 18 | 19 | ament_cmake 20 | 21 | moveit_ros_move_group 22 | moveit_kinematics 23 | moveit_planners 24 | moveit_simple_controller_manager 25 | joint_state_publisher 26 | joint_state_publisher_gui 27 | tf2_ros 28 | xacro 29 | 31 | 32 | 33 | lbr_description 34 | moveit_configs_utils 35 | moveit_ros_move_group 36 | moveit_ros_visualization 37 | moveit_setup_assistant 38 | rviz2 39 | rviz_common 40 | rviz_default_plugins 41 | xacro 42 | 43 | 44 | 45 | ament_cmake 46 | 47 | 48 | -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa14_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | iiwa14_moveit_config 5 | 0.3.0 6 | 7 | An automatically generated package with all the configuration and launch files for using the iiwa14 with the MoveIt Motion Planning Framework 8 | 9 | mhubii 10 | 11 | BSD-3-Clause 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/moveit/moveit2/issues 15 | https://github.com/moveit/moveit2 16 | 17 | mhubii 18 | 19 | ament_cmake 20 | 21 | moveit_ros_move_group 22 | moveit_kinematics 23 | moveit_planners 24 | moveit_simple_controller_manager 25 | joint_state_publisher 26 | joint_state_publisher_gui 27 | tf2_ros 28 | xacro 29 | 31 | 32 | 33 | lbr_description 34 | moveit_configs_utils 35 | moveit_ros_move_group 36 | moveit_ros_visualization 37 | moveit_setup_assistant 38 | rviz2 39 | rviz_common 40 | rviz_default_plugins 41 | xacro 42 | 43 | 44 | 45 | ament_cmake 46 | 47 | 48 | -------------------------------------------------------------------------------- /lbr_moveit_config/iiwa7_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | iiwa7_moveit_config 5 | 0.3.0 6 | 7 | An automatically generated package with all the configuration and launch files for using the iiwa7 with the MoveIt Motion Planning Framework 8 | 9 | mhubii 10 | 11 | BSD-3-Clause 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/moveit/moveit2/issues 15 | https://github.com/moveit/moveit2 16 | 17 | mhubii 18 | 19 | ament_cmake 20 | 21 | moveit_ros_move_group 22 | moveit_kinematics 23 | moveit_planners 24 | moveit_simple_controller_manager 25 | joint_state_publisher 26 | joint_state_publisher_gui 27 | tf2_ros 28 | xacro 29 | 31 | 32 | 33 | lbr_description 34 | moveit_configs_utils 35 | moveit_ros_move_group 36 | moveit_ros_visualization 37 | moveit_setup_assistant 38 | rviz2 39 | rviz_common 40 | rviz_default_plugins 41 | xacro 42 | 43 | 44 | 45 | ament_cmake 46 | 47 | 48 | -------------------------------------------------------------------------------- /lbr_moveit_config/med14_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | med14_moveit_config 5 | 0.3.0 6 | 7 | An automatically generated package with all the configuration and launch files for using the med14 with the MoveIt Motion Planning Framework 8 | 9 | mhubii 10 | 11 | BSD-3-Clause 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/moveit/moveit2/issues 15 | https://github.com/moveit/moveit2 16 | 17 | mhubii 18 | 19 | ament_cmake 20 | 21 | moveit_ros_move_group 22 | moveit_kinematics 23 | moveit_planners 24 | moveit_simple_controller_manager 25 | joint_state_publisher 26 | joint_state_publisher_gui 27 | tf2_ros 28 | xacro 29 | 31 | 32 | 33 | lbr_description 34 | moveit_configs_utils 35 | moveit_ros_move_group 36 | moveit_ros_visualization 37 | moveit_setup_assistant 38 | rviz2 39 | rviz_common 40 | rviz_default_plugins 41 | xacro 42 | 43 | 44 | 45 | ament_cmake 46 | 47 | 48 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(lbr_demos_cpp) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | if(NOT CMAKE_BUILD_TYPE) 9 | set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE) 10 | endif() 11 | 12 | # find dependencies 13 | find_package(ament_cmake REQUIRED) 14 | find_package(control_msgs REQUIRED) 15 | find_package(FRIClient REQUIRED) 16 | find_package(lbr_fri_idl REQUIRED) 17 | find_package(lbr_fri_ros2 REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(rclcpp_action REQUIRED) 20 | find_package(sensor_msgs REQUIRED) 21 | find_package(trajectory_msgs REQUIRED) 22 | 23 | # joint sine overlay 24 | add_executable(joint_sine_overlay 25 | src/joint_sine_overlay.cpp 26 | ) 27 | 28 | ament_target_dependencies(joint_sine_overlay 29 | lbr_fri_idl 30 | lbr_fri_ros2 31 | rclcpp 32 | ) 33 | 34 | target_link_libraries(joint_sine_overlay 35 | FRIClient::FRIClient 36 | ) 37 | 38 | # joint trajectory client 39 | add_executable(joint_trajectory_client 40 | src/joint_trajectory_client.cpp 41 | ) 42 | 43 | ament_target_dependencies(joint_trajectory_client 44 | control_msgs 45 | rclcpp 46 | rclcpp_action 47 | sensor_msgs 48 | trajectory_msgs 49 | ) 50 | 51 | target_link_libraries(joint_trajectory_client 52 | FRIClient::FRIClient 53 | ) 54 | 55 | # torque sine overlay 56 | add_executable(torque_sine_overlay 57 | src/torque_sine_overlay.cpp 58 | ) 59 | 60 | ament_target_dependencies(torque_sine_overlay 61 | lbr_fri_idl 62 | lbr_fri_ros2 63 | rclcpp 64 | ) 65 | 66 | target_link_libraries(torque_sine_overlay 67 | FRIClient::FRIClient 68 | ) 69 | 70 | # wrench sine overlay 71 | add_executable(wrench_sine_overlay 72 | src/wrench_sine_overlay.cpp 73 | ) 74 | 75 | ament_target_dependencies(wrench_sine_overlay 76 | lbr_fri_idl 77 | lbr_fri_ros2 78 | rclcpp 79 | ) 80 | 81 | target_link_libraries(wrench_sine_overlay 82 | FRIClient::FRIClient 83 | ) 84 | 85 | install(TARGETS 86 | joint_sine_overlay 87 | joint_trajectory_client 88 | torque_sine_overlay 89 | wrench_sine_overlay 90 | DESTINATION lib/${PROJECT_NAME} 91 | ) 92 | 93 | ament_package() 94 | -------------------------------------------------------------------------------- /lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_ROS2_CONTROL__LBR_JOINT_POSITION_COMMAND_CONTROLLER_HPP_ 2 | #define LBR_ROS2_CONTROL__LBR_JOINT_POSITION_COMMAND_CONTROLLER_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "controller_interface/controller_interface.hpp" 11 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 12 | #include "rclcpp/rclcpp.hpp" 13 | #include "realtime_tools/realtime_buffer.hpp" 14 | 15 | #include "friLBRState.h" 16 | 17 | #include "lbr_fri_idl/msg/lbr_joint_position_command.hpp" 18 | #include "lbr_fri_ros2/formatting.hpp" 19 | #include "lbr_fri_ros2/types.hpp" 20 | 21 | namespace lbr_ros2_control { 22 | class LBRJointPositionCommandController : public controller_interface::ControllerInterface { 23 | public: 24 | LBRJointPositionCommandController(); 25 | 26 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 27 | 28 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 29 | 30 | controller_interface::CallbackReturn on_init() override; 31 | 32 | controller_interface::return_type update(const rclcpp::Time &time, 33 | const rclcpp::Duration &period) override; 34 | 35 | controller_interface::CallbackReturn 36 | on_configure(const rclcpp_lifecycle::State &previous_state) override; 37 | 38 | controller_interface::CallbackReturn 39 | on_activate(const rclcpp_lifecycle::State &previous_state) override; 40 | 41 | controller_interface::CallbackReturn 42 | on_deactivate(const rclcpp_lifecycle::State &previous_state) override; 43 | 44 | protected: 45 | void configure_joint_names_(); 46 | 47 | lbr_fri_ros2::jnt_name_array_t joint_names_; 48 | 49 | realtime_tools::RealtimeBuffer 50 | rt_lbr_joint_position_command_ptr_; 51 | rclcpp::Subscription::SharedPtr 52 | lbr_joint_position_command_subscription_ptr_; 53 | }; 54 | } // namespace lbr_ros2_control 55 | #endif // LBR_ROS2_CONTROL__LBR_JOINT_POSITION_COMMAND_CONTROLLER_HPP_ 56 | -------------------------------------------------------------------------------- /lbr_description/ros2_control/lbr_system_config.yaml: -------------------------------------------------------------------------------- 1 | # these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface 2 | fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro 3 | major_version: 1 4 | minor_version: 15 5 | client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] 6 | port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups 7 | remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection 8 | rt_prio: 80 # real-time priority for the control loop 9 | # exponential moving average time constant for joint position commands [s]: 10 | # set tau > robot sample time for more smoothing on the commands 11 | # set tau << robot sample time for no smoothing on the commands 12 | joint_position_tau: 0.04 13 | command_guard: 14 | # if requested position / velocities beyond limits, CommandGuard will be triggered and the command will be overriden with the current state instead 15 | # available: [default, safe_stop] 16 | variant: default 17 | state_guard: 18 | # enable / disable the load data safety check in compliant control modes [CARTESIAN_IMPEDANCE, IMPEDANCE] 19 | # in POSITION control mode, safety check is not performed 20 | external_torque_safety_check: true 21 | external_torque_limit: 2.0 # maximum allowed external joint torque on activation of compliant control modes [Nm] 22 | # exponential moving average time constant for external joint torque measurements [s]: 23 | # set tau > robot sample time for more smoothing on the external torque measurements 24 | # set tau << robot sample time for no smoothing on the external torque measurements 25 | external_torque_tau: 0.01 26 | # exponential moving average time constant for joint torque measurements [s]: 27 | # set tau > robot sample time for more smoothing on the raw torque measurements 28 | # set tau << robot sample time for no smoothing on the raw torque measurements 29 | measured_torque_tau: 0.01 30 | # for position control, LBR works best in open_loop control mode 31 | # note that open_loop is overridden to false if LBR is in impedance or Cartesian impedance control mode 32 | open_loop: true 33 | -------------------------------------------------------------------------------- /lbr_description/gazebo/lbr_gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | $(find lbr_description)/ros2_control/gazebo_controllers.yaml 10 | 11 | /${robot_name} 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 0.2 20 | 0.2 21 | 22 | 23 | 24 | 25 | 26 | true 27 | true 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /lbr_fri_ros2/src/wrench_estimator.cpp: -------------------------------------------------------------------------------- 1 | #include "lbr_fri_ros2/wrench_estimator.hpp" 2 | 3 | namespace lbr_fri_ros2 { 4 | WrenchEstimator::WrenchEstimator(const std::string &robot_description, 5 | const WrenchEstimatorParameters ¶meters) { 6 | if (!parameters.valid()) { 7 | throw std::invalid_argument("Invalid wrench estimator parameters."); 8 | } 9 | f_ext_th_[0] = parameters.force_x_th; 10 | f_ext_th_[1] = parameters.force_y_th; 11 | f_ext_th_[2] = parameters.force_z_th; 12 | f_ext_th_[3] = parameters.torque_x_th; 13 | f_ext_th_[4] = parameters.torque_y_th; 14 | f_ext_th_[5] = parameters.torque_z_th; 15 | damping_ = parameters.damping; 16 | kinematics_ptr_ = 17 | std::make_unique(robot_description, parameters.chain_root, parameters.chain_tip); 18 | reset(); 19 | } 20 | 21 | void WrenchEstimator::compute() { 22 | auto jacobian = kinematics_ptr_->compute_jacobian(q_); 23 | jacobian_inv_ = pinv(jacobian.data, damping_); 24 | f_ext_raw_ = jacobian_inv_.transpose() * tau_ext_; 25 | int i = -1; 26 | f_ext_ = f_ext_raw_.unaryExpr([&](double v) { 27 | ++i; 28 | if (std::abs(v) < f_ext_th_[i]) { 29 | return 0.; 30 | } else { 31 | return std::copysign(1., v) * (std::abs(v) - f_ext_th_[i]); 32 | } 33 | }); 34 | 35 | // rotate into chain tip frame 36 | auto chain_tip_frame = kinematics_ptr_->compute_fk(q_); 37 | f_ext_tf_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3); 38 | f_ext_tf_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3); 39 | } 40 | 41 | void WrenchEstimator::reset() { 42 | std::fill(q_.begin(), q_.end(), 0.0); 43 | tau_ext_.setZero(); 44 | f_ext_raw_.setZero(); 45 | f_ext_.setZero(); 46 | f_ext_tf_.setZero(); 47 | jacobian_inv_.setZero(); 48 | } 49 | 50 | void WrenchEstimator::log_info() const { 51 | RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); 52 | RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Damping: %.3f", damping_); 53 | RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Force thresholds: [%.3f, %.3f, %.3f]", 54 | f_ext_th_[0], f_ext_th_[1], f_ext_th_[2]); 55 | RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Torque thresholds: [%.3f, %.3f, %.3f]", 56 | f_ext_th_[3], f_ext_th_[4], f_ext_th_[5]); 57 | } 58 | } // namespace lbr_fri_ros2 59 | -------------------------------------------------------------------------------- /lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_ROS2_CONTROL__LBR_STATE_BROADCASTER_HPP_ 2 | #define LBR_ROS2_CONTROL__LBR_STATE_BROADCASTER_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "controller_interface/controller_interface.hpp" 13 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 14 | #include "rclcpp/rclcpp.hpp" 15 | #include "realtime_tools/realtime_publisher.hpp" 16 | 17 | #include "friClientIf.h" 18 | #include "friClientVersion.h" 19 | #include "friLBRState.h" 20 | 21 | #include "lbr_fri_idl/msg/lbr_state.hpp" 22 | #include "lbr_fri_ros2/formatting.hpp" 23 | #include "lbr_fri_ros2/types.hpp" 24 | #include "lbr_ros2_control/system_interface_type_values.hpp" 25 | 26 | namespace lbr_ros2_control { 27 | class LBRStateBroadcaster : public controller_interface::ControllerInterface { 28 | public: 29 | LBRStateBroadcaster() = default; 30 | 31 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 32 | 33 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 34 | 35 | controller_interface::CallbackReturn on_init() override; 36 | 37 | controller_interface::return_type update(const rclcpp::Time &time, 38 | const rclcpp::Duration &period) override; 39 | 40 | controller_interface::CallbackReturn 41 | on_configure(const rclcpp_lifecycle::State &previous_state) override; 42 | 43 | controller_interface::CallbackReturn 44 | on_activate(const rclcpp_lifecycle::State &previous_state) override; 45 | 46 | controller_interface::CallbackReturn 47 | on_deactivate(const rclcpp_lifecycle::State &previous_state) override; 48 | 49 | protected: 50 | void init_state_interface_map_(); 51 | void init_state_msg_(); 52 | void configure_joint_names_(); 53 | 54 | lbr_fri_ros2::jnt_name_array_t joint_names_; 55 | std::unordered_map> state_interface_map_; 56 | 57 | lbr_fri_idl::msg::LBRState lbr_state_; 58 | rclcpp::Publisher::SharedPtr state_publisher_ptr_; 59 | std::shared_ptr> 60 | rt_state_publisher_ptr_; 61 | }; 62 | } // namespace lbr_ros2_control 63 | #endif // LBR_ROS2_CONTROL__LBR_STATE_BROADCASTER_HPP_ 64 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__ASYNC_CLIENT_HPP_ 2 | #define LBR_FRI_ROS2__ASYNC_CLIENT_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "rclcpp/logger.hpp" 9 | #include "rclcpp/logging.hpp" 10 | 11 | #include "friClientVersion.h" 12 | #include "friLBRClient.h" 13 | 14 | #include "lbr_fri_ros2/formatting.hpp" 15 | #include "lbr_fri_ros2/guards/command_guard.hpp" 16 | #include "lbr_fri_ros2/guards/state_guard.hpp" 17 | #include "lbr_fri_ros2/interfaces/base_command.hpp" 18 | #include "lbr_fri_ros2/interfaces/position_command.hpp" 19 | #include "lbr_fri_ros2/interfaces/state.hpp" 20 | #include "lbr_fri_ros2/interfaces/torque_command.hpp" 21 | #include "lbr_fri_ros2/interfaces/wrench_command.hpp" 22 | 23 | namespace lbr_fri_ros2 { 24 | class AsyncClient : public KUKA::FRI::LBRClient { 25 | protected: 26 | static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::AsyncClient"; 27 | 28 | public: 29 | AsyncClient() = delete; 30 | AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mode, 31 | const double &joint_position_tau, 32 | const CommandGuardParameters &command_guard_parameters, 33 | const std::string &command_guard_variant, 34 | const StateGuardParameters &state_guard_parameters, 35 | const StateInterfaceParameters &state_interface_parameters = {0.04, 0.04}, 36 | const bool &open_loop = true); 37 | 38 | inline std::shared_ptr get_command_interface() { 39 | return command_interface_ptr_; 40 | } 41 | inline std::shared_ptr get_state_interface() { return state_interface_ptr_; } 42 | 43 | void onStateChange(KUKA::FRI::ESessionState old_state, 44 | KUKA::FRI::ESessionState new_state) override; 45 | void monitor() override; 46 | void waitForCommand() override; 47 | void command() override; 48 | 49 | protected: 50 | void on_enter_commanding_active_(); 51 | 52 | protected: 53 | std::shared_ptr command_interface_ptr_; 54 | std::shared_ptr state_interface_ptr_; 55 | 56 | // currently, only check external torque limits on enter commanding active with fixed limit, see 57 | // https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/271#issuecomment-2780642918 58 | StateGuard on_enter_commanding_active_state_guard_; 59 | 60 | bool open_loop_; 61 | }; 62 | } // namespace lbr_fri_ros2 63 | #endif // LBR_FRI_ROS2__ASYNC_CLIENT_HPP_ 64 | -------------------------------------------------------------------------------- /lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_ROS2_CONTROL__LBR_TORQUE_COMMAND_CONTROLLER_HPP_ 2 | #define LBR_ROS2_CONTROL__LBR_TORQUE_COMMAND_CONTROLLER_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "controller_interface/controller_interface.hpp" 12 | #include "hardware_interface/loaned_command_interface.hpp" 13 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 14 | #include "rclcpp/rclcpp.hpp" 15 | #include "realtime_tools/realtime_buffer.hpp" 16 | 17 | #include "friLBRState.h" 18 | 19 | #include "lbr_fri_idl/msg/lbr_torque_command.hpp" 20 | #include "lbr_fri_ros2/formatting.hpp" 21 | #include "lbr_fri_ros2/types.hpp" 22 | 23 | namespace lbr_ros2_control { 24 | class LBRTorqueCommandController : public controller_interface::ControllerInterface { 25 | public: 26 | LBRTorqueCommandController(); 27 | 28 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 29 | 30 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 31 | 32 | controller_interface::CallbackReturn on_init() override; 33 | 34 | controller_interface::return_type update(const rclcpp::Time &time, 35 | const rclcpp::Duration &period) override; 36 | 37 | controller_interface::CallbackReturn 38 | on_configure(const rclcpp_lifecycle::State &previous_state) override; 39 | 40 | controller_interface::CallbackReturn 41 | on_activate(const rclcpp_lifecycle::State &previous_state) override; 42 | 43 | controller_interface::CallbackReturn 44 | on_deactivate(const rclcpp_lifecycle::State &previous_state) override; 45 | 46 | protected: 47 | bool assign_command_interfaces_(); 48 | void release_command_interfaces_(); 49 | void configure_joint_names_(); 50 | 51 | lbr_fri_ros2::jnt_name_array_t joint_names_; 52 | 53 | std::vector> 54 | joint_position_command_interfaces_, torque_command_interfaces_; 55 | 56 | realtime_tools::RealtimeBuffer 57 | rt_lbr_torque_command_ptr_; 58 | rclcpp::Subscription::SharedPtr 59 | lbr_torque_command_subscription_ptr_; 60 | }; 61 | } // namespace lbr_ros2_control 62 | #endif // LBR_ROS2_CONTROL__LBR_TORQUE_COMMAND_CONTROLLER_HPP_ 63 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import optas 3 | 4 | from lbr_fri_idl.msg import LBRJointPositionCommand, LBRState 5 | 6 | 7 | class AdmittanceController(object): 8 | def __init__( 9 | self, 10 | robot_description: str, 11 | base_link: str = "lbr_link_0", 12 | end_effector_link: str = "lbr_link_ee", 13 | f_ext_th: np.ndarray = np.array([2.0, 2.0, 2.0, 0.5, 0.5, 0.5]), 14 | dq_gains: np.ndarray = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), 15 | dx_gains: np.ndarray = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]), 16 | ) -> None: 17 | self._lbr_joint_position_command = LBRJointPositionCommand() 18 | 19 | self._robot = optas.RobotModel(urdf_string=robot_description) 20 | 21 | self._jacobian_func = self._robot.get_link_geometric_jacobian_function( 22 | link=end_effector_link, base_link=base_link, numpy_output=True 23 | ) 24 | 25 | self._dof = self._robot.ndof 26 | self._jacobian = np.zeros((6, self._dof)) 27 | self._jacobian_inv = np.zeros((self._dof, 6)) 28 | self._q = np.zeros(self._dof) 29 | self._dq = np.zeros(self._dof) 30 | self._tau_ext = np.zeros(6) 31 | self._dq_gains = np.diag(dq_gains) 32 | self._dx_gains = np.diag(dx_gains) 33 | self._f_ext = np.zeros(6) 34 | self._f_ext_th = f_ext_th 35 | self._alpha = 0.95 36 | 37 | def __call__(self, lbr_state: LBRState, dt: float) -> LBRJointPositionCommand: 38 | self._q = np.array(lbr_state.measured_joint_position.tolist()) 39 | self._tau_ext = np.array(lbr_state.external_torque.tolist()) 40 | 41 | self._jacobian = self._jacobian_func(self._q) 42 | self._jacobian_inv = np.linalg.pinv(self._jacobian, rcond=0.1) 43 | self._f_ext = self._jacobian_inv.T @ self._tau_ext 44 | 45 | dx = np.where( 46 | abs(self._f_ext) > self._f_ext_th, 47 | self._dx_gains @ np.sign(self._f_ext) * (abs(self._f_ext) - self._f_ext_th), 48 | 0.0, 49 | ) 50 | 51 | # additional smoothing required in python 52 | self._dq = ( 53 | self._alpha * self._dq 54 | + (1 - self._alpha) * self._dq_gains @ self._jacobian_inv @ dx 55 | ) 56 | 57 | self._lbr_joint_position_command.joint_position = ( 58 | np.array(lbr_state.measured_joint_position.tolist()) + dt * self._dq 59 | ).data 60 | 61 | return self._lbr_joint_position_command 62 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/lbr_base_position_command_node.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | import rclpy.parameter 3 | from rcl_interfaces.msg import ParameterValue 4 | from rcl_interfaces.srv import GetParameters 5 | from rclpy.node import Node 6 | 7 | from lbr_fri_idl.msg import LBRJointPositionCommand, LBRState 8 | 9 | 10 | class LBRBasePositionCommandNode(Node): 11 | r"""Utility class for creating a base node for sending position commands to the KUKA LBRs. 12 | Retrieves update rate and robot description from the parameter servers. 13 | """ 14 | 15 | _update_rate: int 16 | _dt: float 17 | _robot_description: str 18 | 19 | def __init__(self, node_name: str) -> None: 20 | super().__init__(node_name) 21 | 22 | # retrieve parameters 23 | self._robot_description = self._retrieve_parameter( 24 | "robot_state_publisher/get_parameters", "robot_description" 25 | ).string_value 26 | self._update_rate = self._retrieve_parameter( 27 | "controller_manager/get_parameters", "update_rate" 28 | ).integer_value 29 | self._dt = 1.0 / float(self._update_rate) 30 | 31 | # publishers and subscribers 32 | self._lbr_state_sub = self.create_subscription( 33 | LBRState, "lbr_state", self._on_lbr_state, 1 34 | ) 35 | self._lbr_joint_position_command_pub = self.create_publisher( 36 | LBRJointPositionCommand, 37 | "command/lbr_joint_position_command", 38 | 1, 39 | ) 40 | 41 | def _retrieve_parameter(self, service: str, parameter_name: str) -> ParameterValue: 42 | parameter_client = self.create_client(GetParameters, service) 43 | while not parameter_client.wait_for_service(timeout_sec=1.0): 44 | if not rclpy.ok(): 45 | self.get_logger().error( 46 | "Interrupted while waiting for the service. Exiting." 47 | ) 48 | return None 49 | self.get_logger().info(f"Waiting for '{service}' service...") 50 | request = GetParameters.Request(names=[parameter_name]) 51 | future = parameter_client.call_async(request=request) 52 | rclpy.spin_until_future_complete(self, future) 53 | if future.result() is None: 54 | self.get_logger().error(f"Failed to retrieve '{parameter_name}'.") 55 | return None 56 | self.get_logger().info(f"Received '{parameter_name}' from '{service}'.") 57 | return future.result().values[0] 58 | 59 | def _on_lbr_state(self, lbr_state: LBRState) -> None: 60 | raise NotImplementedError 61 | -------------------------------------------------------------------------------- /lbr_fri_ros2/src/kinematics.cpp: -------------------------------------------------------------------------------- 1 | #include "lbr_fri_ros2/kinematics.hpp" 2 | 3 | namespace lbr_fri_ros2 { 4 | Kinematics::Kinematics(const std::string &robot_description, const std::string &chain_root, 5 | const std::string &chain_tip) { 6 | if (!kdl_parser::treeFromString(robot_description, tree_)) { 7 | std::string err = "Failed to construct kdl tree from robot description."; 8 | RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); 9 | throw std::runtime_error(err); 10 | } 11 | if (!tree_.getChain(chain_root, chain_tip, chain_)) { 12 | std::string err = "Failed to construct kdl chain from robot description."; 13 | RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); 14 | throw std::runtime_error(err); 15 | } 16 | if (chain_.getNrOfJoints() != N_JNTS) { 17 | std::string err = "Invalid number of joints in chain."; 18 | RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); 19 | throw std::runtime_error(err); 20 | } 21 | jacobian_solver_ = std::make_unique(chain_); 22 | fk_solver_ = std::make_unique(chain_); 23 | jacobian_.resize(N_JNTS); 24 | q_.resize(N_JNTS); 25 | q_.data.setZero(); 26 | } 27 | 28 | const KDL::Jacobian &Kinematics::compute_jacobian(const_jnt_array_t_ref q) { 29 | q_.data = Eigen::Map>(q.data()); 30 | jacobian_solver_->JntToJac(q_, jacobian_); 31 | return jacobian_; 32 | } 33 | 34 | const KDL::Jacobian &Kinematics::compute_jacobian(const std::vector &q) { 35 | if (q.size() != N_JNTS) { 36 | std::string err = "Invalid number of joint positions."; 37 | RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); 38 | throw std::runtime_error(err); 39 | } 40 | q_.data = Eigen::Map>(q.data()); 41 | jacobian_solver_->JntToJac(q_, jacobian_); 42 | return jacobian_; 43 | } 44 | 45 | const KDL::Frame &Kinematics::compute_fk(const_jnt_array_t_ref q) { 46 | q_.data = Eigen::Map>(q.data()); 47 | fk_solver_->JntToCart(q_, chain_tip_frame_); 48 | return chain_tip_frame_; 49 | } 50 | 51 | const KDL::Frame &Kinematics::compute_fk(const std::vector &q) { 52 | if (q.size() != N_JNTS) { 53 | std::string err = "Invalid number of joint positions."; 54 | RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); 55 | throw std::runtime_error(err); 56 | } 57 | q_.data = Eigen::Map>(q.data()); 58 | fk_solver_->JntToCart(q_, chain_tip_frame_); 59 | return chain_tip_frame_; 60 | } 61 | } // namespace lbr_fri_ros2 62 | -------------------------------------------------------------------------------- /lbr_bringup/launch/moveit_servo.launch.py: -------------------------------------------------------------------------------- 1 | from typing import List 2 | 3 | from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity 4 | from launch.actions import OpaqueFunction, RegisterEventHandler 5 | from launch.conditions import IfCondition 6 | from launch.event_handlers import OnProcessStart 7 | from launch.substitutions import LaunchConfiguration, PathSubstitution 8 | from launch_ros.substitutions import FindPackageShare 9 | from lbr_bringup.description import LBRDescriptionMixin 10 | from lbr_bringup.moveit import LBRMoveGroupMixin, LBRMoveItServoMixin 11 | 12 | 13 | def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: 14 | ld = LaunchDescription() 15 | 16 | moveit_servo_config = ( 17 | PathSubstitution(FindPackageShare("lbr_bringup")) / "config/moveit_servo.yaml" 18 | ) 19 | model = LaunchConfiguration("model").perform(context) 20 | moveit_configs = LBRMoveGroupMixin.moveit_configs_builder( 21 | robot_name=model, 22 | package_name=f"{model}_moveit_config", 23 | ).to_moveit_configs() 24 | 25 | mode = LaunchConfiguration("mode").perform(context) 26 | use_sim_time = False 27 | if mode == "gazebo": 28 | use_sim_time = True 29 | 30 | # moveit servo node 31 | servo_node = LBRMoveItServoMixin.node_moveit_servo( 32 | parameters=[ 33 | moveit_configs.robot_description_kinematics, 34 | moveit_configs.robot_description_semantic, 35 | {"use_sim_time": use_sim_time}, 36 | moveit_servo_config, 37 | { 38 | "moveit_servo.use_gazebo": mode 39 | == "gazebo", # we configure this parameter dynamically 40 | }, 41 | ], 42 | ) 43 | ld.add_action(servo_node) 44 | 45 | # call start servo after servo node start 46 | ld.add_action( 47 | RegisterEventHandler( 48 | OnProcessStart( 49 | target_action=servo_node, 50 | on_start=[ 51 | LBRMoveItServoMixin.call_start_servo_service( 52 | condition=IfCondition( 53 | LaunchConfiguration("default_enable_servo") 54 | ) 55 | ) 56 | ], 57 | ), 58 | ) 59 | ) 60 | 61 | return ld.entities 62 | 63 | 64 | def generate_launch_description() -> LaunchDescription: 65 | ld = LaunchDescription() 66 | 67 | ld.add_action(LBRDescriptionMixin.arg_mode()) 68 | ld.add_action(LBRDescriptionMixin.arg_model()) 69 | ld.add_action(LBRMoveItServoMixin.arg_default_enable_servo()) 70 | 71 | ld.add_action(OpaqueFunction(function=hidden_setup)) 72 | return ld 73 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "rclcpp/rclcpp.hpp" 7 | 8 | // include fri for session state 9 | #include "friClientIf.h" 10 | 11 | // include lbr_fri_idl 12 | #include "lbr_fri_idl/msg/lbr_state.hpp" 13 | #include "lbr_fri_idl/msg/lbr_torque_command.hpp" 14 | #include "lbr_fri_ros2/utils.hpp" 15 | 16 | class TorqueSineOverlay { 17 | static constexpr double amplitude_ = 15.; // Nm 18 | static constexpr double frequency_ = 0.25; // Hz 19 | 20 | public: 21 | TorqueSineOverlay(const rclcpp::Node::SharedPtr node) : node_(node), phase_(0.) { 22 | // create publisher to command/lbr_torque_command 23 | lbr_torque_command_pub_ = node_->create_publisher( 24 | "command/lbr_torque_command", 1); 25 | 26 | // create subscription to state 27 | lbr_state_sub_ = node_->create_subscription( 28 | "lbr_state", 1, std::bind(&TorqueSineOverlay::on_lbr_state_, this, std::placeholders::_1)); 29 | 30 | // get control rate from controller_manager 31 | auto update_rate = 32 | lbr_fri_ros2::retrieve_paramter(node_, "controller_manager", "update_rate").as_int(); 33 | dt_ = 1.0 / static_cast(update_rate); 34 | }; 35 | 36 | protected: 37 | void on_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) { 38 | if (!lbr_state_init_) { 39 | lbr_state_ = *lbr_state; 40 | lbr_state_init_ = true; 41 | } 42 | lbr_torque_command_.joint_position = lbr_state_.measured_joint_position; 43 | 44 | if (lbr_state->session_state == KUKA::FRI::COMMANDING_ACTIVE) { 45 | // overlay torque sine wave on 4th joint 46 | lbr_torque_command_.torque[3] = amplitude_ * sin(phase_); 47 | phase_ += 2 * M_PI * frequency_ * dt_; 48 | 49 | lbr_torque_command_pub_->publish(lbr_torque_command_); 50 | } else { 51 | // reset phase 52 | phase_ = 0.; 53 | } 54 | }; 55 | 56 | protected: 57 | rclcpp::Node::SharedPtr node_; 58 | double dt_; 59 | double phase_; 60 | rclcpp::Publisher::SharedPtr lbr_torque_command_pub_; 61 | rclcpp::Subscription::SharedPtr lbr_state_sub_; 62 | bool lbr_state_init_ = false; 63 | lbr_fri_idl::msg::LBRState lbr_state_; 64 | lbr_fri_idl::msg::LBRTorqueCommand lbr_torque_command_; 65 | }; 66 | 67 | int main(int argc, char **argv) { 68 | rclcpp::init(argc, argv); 69 | auto node = std::make_shared("torque_sine_overlay"); 70 | TorqueSineOverlay torque_sine_overlay(node); 71 | rclcpp::spin(node); 72 | rclcpp::shutdown(); 73 | return 0; 74 | }; 75 | -------------------------------------------------------------------------------- /lbr_demos/lbr_demos_cpp/src/joint_sine_overlay.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "rclcpp/rclcpp.hpp" 7 | 8 | // include fri for session state 9 | #include "friClientIf.h" 10 | 11 | // include lbr_fri_idl 12 | #include "lbr_fri_idl/msg/lbr_joint_position_command.hpp" 13 | #include "lbr_fri_idl/msg/lbr_state.hpp" 14 | #include "lbr_fri_ros2/utils.hpp" 15 | 16 | class JointSineOverlay { 17 | static constexpr double amplitude_ = 0.04; // rad 18 | static constexpr double frequency_ = 0.25; // Hz 19 | 20 | public: 21 | JointSineOverlay(const rclcpp::Node::SharedPtr node) : node_(node), phase_(0.) { 22 | // create publisher to command/lbr_joint_position_command 23 | lbr_joint_position_command_pub_ = 24 | node_->create_publisher( 25 | "command/lbr_joint_position_command", 1); 26 | 27 | // create subscription to state 28 | lbr_state_sub_ = node_->create_subscription( 29 | "lbr_state", 1, std::bind(&JointSineOverlay::on_lbr_state_, this, std::placeholders::_1)); 30 | 31 | // get control rate from controller_manager 32 | auto update_rate = 33 | lbr_fri_ros2::retrieve_paramter(node_, "controller_manager", "update_rate").as_int(); 34 | dt_ = 1.0 / static_cast(update_rate); 35 | }; 36 | 37 | protected: 38 | void on_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) { 39 | if (!lbr_state_init_) { 40 | lbr_state_ = *lbr_state; 41 | lbr_state_init_ = true; 42 | } 43 | lbr_joint_position_command_.joint_position = lbr_state_.measured_joint_position; 44 | 45 | if (lbr_state->session_state == KUKA::FRI::COMMANDING_ACTIVE) { 46 | // overlay sine wave on 4th joint 47 | lbr_joint_position_command_.joint_position[3] += amplitude_ * sin(phase_); 48 | phase_ += 2 * M_PI * frequency_ * dt_; 49 | 50 | lbr_joint_position_command_pub_->publish(lbr_joint_position_command_); 51 | } else { 52 | // reset phase 53 | phase_ = 0.; 54 | } 55 | }; 56 | 57 | protected: 58 | rclcpp::Node::SharedPtr node_; 59 | double dt_; 60 | double phase_; 61 | rclcpp::Publisher::SharedPtr 62 | lbr_joint_position_command_pub_; 63 | rclcpp::Subscription::SharedPtr lbr_state_sub_; 64 | bool lbr_state_init_ = false; 65 | lbr_fri_idl::msg::LBRState lbr_state_; 66 | lbr_fri_idl::msg::LBRJointPositionCommand lbr_joint_position_command_; 67 | }; 68 | 69 | int main(int argc, char **argv) { 70 | rclcpp::init(argc, argv); 71 | auto node = std::make_shared("joint_sine_overlay"); 72 | JointSineOverlay joint_sine_overlay(node); 73 | rclcpp::spin(node); 74 | rclcpp::shutdown(); 75 | return 0; 76 | }; 77 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/guards/command_guard.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__COMMAND_GUARD_HPP_ 2 | #define LBR_FRI_ROS2__COMMAND_GUARD_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "rclcpp/logger.hpp" 11 | #include "rclcpp/logging.hpp" 12 | 13 | #include "friClientVersion.h" 14 | #include "friLBRClient.h" 15 | #include "friLBRState.h" 16 | 17 | #include "lbr_fri_idl/msg/lbr_command.hpp" 18 | #include "lbr_fri_idl/msg/lbr_state.hpp" 19 | #include "lbr_fri_ros2/formatting.hpp" 20 | #include "lbr_fri_ros2/types.hpp" 21 | 22 | namespace lbr_fri_ros2 { 23 | struct CommandGuardParameters { 24 | jnt_name_array_t joint_names; /**< Joint names.*/ 25 | jnt_array_t min_positions{0., 0., 0., 0., 0., 0., 0.}; /**< Minimum joint positions [rad].*/ 26 | jnt_array_t max_positions{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint positions [rad].*/ 27 | jnt_array_t max_velocities{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint velocities [rad/s].*/ 28 | jnt_array_t max_torques{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint torque [Nm].*/ 29 | }; 30 | 31 | class CommandGuard { 32 | protected: 33 | static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::CommandGuard"; 34 | 35 | public: 36 | CommandGuard() = default; 37 | CommandGuard(const CommandGuardParameters &command_guard_parameters); 38 | virtual bool is_valid_command(const_idl_command_t_ref lbr_command, 39 | const_idl_state_t_ref lbr_state); 40 | 41 | void log_info() const; 42 | 43 | protected: 44 | virtual bool command_in_position_limits_(const_idl_command_t_ref lbr_command, 45 | const_idl_state_t_ref /*lbr_state*/) const; 46 | virtual bool command_in_velocity_limits_(const_idl_state_t_ref lbr_state); 47 | virtual bool command_in_torque_limits_(const_idl_command_t_ref lbr_command, 48 | const_idl_state_t_ref lbr_state) const; 49 | 50 | CommandGuardParameters parameters_; 51 | bool prev_measured_joint_position_init_; 52 | jnt_array_t prev_measured_joint_position_; 53 | }; 54 | 55 | class SafeStopCommandGuard : public CommandGuard { 56 | public: 57 | SafeStopCommandGuard(const CommandGuardParameters &command_guard_parameters) 58 | : CommandGuard(command_guard_parameters) {}; 59 | 60 | protected: 61 | virtual bool command_in_position_limits_(const_idl_command_t_ref lbr_command, 62 | const_idl_state_t_ref lbr_state) const override; 63 | }; 64 | 65 | std::unique_ptr 66 | command_guard_factory(const CommandGuardParameters &command_guard_parameters, 67 | const std::string &variant = "default"); 68 | } // namespace lbr_fri_ros2 69 | #endif // LBR_FRI_ROS2__COMMAND_GUARD_HPP_ 70 | -------------------------------------------------------------------------------- /lbr_ros2_control/plugin_description_files/controllers.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | A simple admittance controller. 8 | 9 | 10 | 11 | 14 | Estimates external wrenches using the external joint torques and exposes them 15 | as interface. 16 | 17 | 18 | 19 | 22 | Forward command controller for LBRJointPositionCommand message, see 23 | lbr_fri_idl/msg/LBRJointPositionCommand.msg. 24 | 25 | 26 | 27 | 29 | Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg. 30 | 31 | 32 | 33 | 36 | Forward command controller for LBRTorqueCommand message, see 37 | lbr_fri_idl/msg/LBRTorqueCommand.msg. 38 | 39 | 40 | 41 | 44 | Forward command controller for LBRWrenchCommand message, see 45 | lbr_fri_idl/msg/LBRWrenchCommand.msg. 46 | 47 | 48 | 49 | 52 | A simple twist controller. 53 | 54 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__FORMATTING_HPP_ 2 | #define LBR_FRI_ROS2__FORMATTING_HPP_ 3 | 4 | #include 5 | 6 | #include "friClientVersion.h" 7 | #include "friLBRClient.h" 8 | 9 | namespace lbr_fri_ros2 { 10 | struct ColorScheme { 11 | // refer https://stackoverflow.com/a/287944 12 | static constexpr char HEADER[] = "\033[95m"; 13 | static constexpr char OKBLUE[] = "\033[94m"; 14 | static constexpr char OKCYAN[] = "\033[96m"; 15 | static constexpr char OKGREEN[] = "\033[92m"; 16 | static constexpr char WARNING[] = "\033[93m"; 17 | static constexpr char ERROR[] = "\033[91m"; 18 | static constexpr char ENDC[] = "\033[0m"; 19 | static constexpr char BOLD[] = "\033[1m"; 20 | static constexpr char UNDERLINE[] = "\033[4m"; 21 | }; 22 | 23 | struct EnumMaps { 24 | static std::string session_state_map(const int &session_state) { 25 | switch (session_state) { 26 | case KUKA::FRI::ESessionState::IDLE: 27 | return "IDLE"; 28 | case KUKA::FRI::ESessionState::MONITORING_WAIT: 29 | return "MONITORING_WAIT"; 30 | case KUKA::FRI::ESessionState::MONITORING_READY: 31 | return "MONITORING_READY"; 32 | case KUKA::FRI::ESessionState::COMMANDING_WAIT: 33 | return "COMMANDING_WAIT"; 34 | case KUKA::FRI::ESessionState::COMMANDING_ACTIVE: 35 | return "COMMANDING_ACTIVE"; 36 | default: 37 | return "UNKNOWN"; 38 | } 39 | }; 40 | 41 | static std::string control_mode_map(const int &control_mode) { 42 | switch (control_mode) { 43 | case KUKA::FRI::EControlMode::CART_IMP_CONTROL_MODE: 44 | return "CART_IMP_CONTROL_MODE"; 45 | case KUKA::FRI::EControlMode::JOINT_IMP_CONTROL_MODE: 46 | return "JOINT_IMP_CONTROL_MODE"; 47 | case KUKA::FRI::EControlMode::NO_CONTROL: 48 | return "NO_CONTROL"; 49 | case KUKA::FRI::EControlMode::POSITION_CONTROL_MODE: 50 | return "POSITION_CONTROL_MODE"; 51 | default: 52 | return "UNKNOWN"; 53 | } 54 | }; 55 | 56 | static std::string client_command_mode_map(const int &client_command_mode) { 57 | switch (client_command_mode) { 58 | case KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE: 59 | return "NO_COMMAND_MODE"; 60 | #if FRI_CLIENT_VERSION_MAJOR == 1 61 | case KUKA::FRI::EClientCommandMode::POSITION: 62 | return "POSITION"; 63 | #endif 64 | #if FRI_CLIENT_VERSION_MAJOR >= 2 65 | case KUKA::FRI::EClientCommandMode::JOINT_POSITION: 66 | return "JOINT_POSITION"; 67 | case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE: 68 | return "CARTESIAN_POSE"; 69 | #endif 70 | case KUKA::FRI::EClientCommandMode::TORQUE: 71 | return "TORQUE"; 72 | case KUKA::FRI::EClientCommandMode::WRENCH: 73 | return "WRENCH"; 74 | default: 75 | return "UNKNOWN"; 76 | } 77 | }; 78 | }; 79 | } // namespace lbr_fri_ros2 80 | #endif // LBR_FRI_ROS2__ENUM_MAPS_HPP 81 | -------------------------------------------------------------------------------- /lbr_fri_ros2/include/lbr_fri_ros2/wrench_estimator.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LBR_FRI_ROS2__WRENCH_ESTIMATOR_HPP_ 2 | #define LBR_FRI_ROS2__WRENCH_ESTIMATOR_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "eigen3/Eigen/Core" 11 | #include "rclcpp/logger.hpp" 12 | #include "rclcpp/logging.hpp" 13 | 14 | #include "lbr_fri_ros2/kinematics.hpp" 15 | #include "lbr_fri_ros2/pinv.hpp" 16 | #include "lbr_fri_ros2/types.hpp" 17 | 18 | namespace lbr_fri_ros2 { 19 | struct WrenchEstimatorParameters { 20 | std::string chain_root{"lbr_link_0"}; 21 | std::string chain_tip{"lbr_link_ee"}; 22 | double damping{0.2}; 23 | double force_x_th{2.0}; 24 | double force_y_th{2.0}; 25 | double force_z_th{2.0}; 26 | double torque_x_th{0.5}; 27 | double torque_y_th{0.5}; 28 | double torque_z_th{0.5}; 29 | 30 | bool valid() const { 31 | return !(chain_root.empty() || chain_tip.empty() || damping < 0.0 || force_x_th < 0.0 || 32 | force_y_th < 0.0 || force_z_th < 0.0 || torque_x_th < 0.0 || torque_y_th < 0.0 || 33 | torque_z_th < 0.0); 34 | } 35 | }; 36 | 37 | class WrenchEstimator { 38 | /** 39 | * @brief A class to estimate wrenches from external joint torque readings. Note that only 40 | * forces beyond a specified threshold are returned. The specified threshold is removed from the 41 | * estimated force-torque. 42 | * 43 | */ 44 | protected: 45 | static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::WrenchEstimator"; 46 | 47 | public: 48 | WrenchEstimator() = delete; 49 | WrenchEstimator(const std::string &robot_description, 50 | const WrenchEstimatorParameters ¶meters = WrenchEstimatorParameters()); 51 | void compute(); 52 | void reset(); 53 | 54 | inline void get_f_ext(cart_array_t_ref f_ext) const { 55 | Eigen::Map>(f_ext.data()) = f_ext_; 56 | } 57 | inline void get_f_ext_tf(cart_array_t_ref f_ext) const { 58 | Eigen::Map>(f_ext.data()) = f_ext_tf_; 59 | } 60 | inline void set_tau_ext(const_jnt_array_t_ref tau_ext) { 61 | tau_ext_ = Eigen::Map>(tau_ext.data()); 62 | } 63 | inline void set_q(const_jnt_array_t_ref q) { q_ = q; } 64 | 65 | void log_info() const; 66 | 67 | protected: 68 | // force threshold 69 | cart_array_t f_ext_th_; 70 | 71 | // damping for pseudo-inverse of Jacobian 72 | double damping_; 73 | 74 | // joint positions and external joint torques 75 | jnt_array_t q_; 76 | 77 | // kinematics 78 | std::unique_ptr kinematics_ptr_; 79 | 80 | // force estimation 81 | Eigen::Matrix jacobian_inv_; 82 | Eigen::Matrix tau_ext_; 83 | Eigen::Matrix f_ext_raw_, f_ext_, f_ext_tf_; 84 | }; 85 | } // namespace lbr_fri_ros2 86 | #endif // LBR_FRI_ROS2__WRENCH_ESTIMATOR_HPP_ 87 | --------------------------------------------------------------------------------