Merge branch 'kroshu:master' into master

CalvaryDesign · Jan 9, 2024 · f4c08bc · f4c08bc
2 parents 6905dc4 + 6a650ba
commit f4c08bc
Show file tree

Hide file tree

Showing 127 changed files with 3,672 additions and 1,031 deletions.
diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md
@@ -0,0 +1,12 @@
+### Before submitting this PR into master please make sure:
+If you added a new robot model:
+- [ ] you extended the table of verified data in `README.md` with the new model
+- [ ] you extended the CMakeLists.txt of the appropriate moveit configuration package with the new model
+- [ ] you added a `test_<robot_model>.launch.py` and after launching the robot was visible in `rviz`
+- [ ] you added a `<robot_model>_joint_limits.yaml` file in the `config` directory (to provide moveit support)
+
+If you modified an already existing robot model:
+- [ ] you checked and optionally updated the table of verified data in `README.md` with the changes
+- [ ] you have run the `test_<robot_model>.launch.py` and the robot was visible in `rviz`
+
+### Short description of the change
diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml
@@ -0,0 +1,23 @@
+# This is a format job. Pre-commit has a first-party GitHub action, so we use
+# that: https://github.com/pre-commit/action
+
+name: Format
+
+on:
+  workflow_dispatch:
+  pull_request:
+
+jobs:
+  pre-commit:
+    name: Format
+    runs-on: ubuntu-latest
+    steps:
+    - uses: actions/checkout@v4
+    - uses: actions/[email protected]
+      with:
+        python-version: '3.10'
+    - name: Install system hooks
+      run: sudo apt install -qq cppcheck
+    - uses: pre-commit/[email protected]
+      with:
+        extra_args: --all-files --hook-stage manual
diff --git a/.gitignore b/.gitignore
@@ -0,0 +1 @@
+__pycache__/
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -0,0 +1,136 @@
+
+# To use:
+#
+#     pre-commit run -a
+#
+# Or:
+#
+#     pre-commit install  # (runs every time you commit in git)
+#
+# To update this file:
+#
+#     pre-commit autoupdate
+#
+# See https://github.com/pre-commit/pre-commit
+
+repos:
+  # Standard hooks
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.4.0
+    hooks:
+      - id: check-added-large-files
+      - id: check-ast
+      - id: check-case-conflict
+      - id: check-docstring-first
+      - id: check-merge-conflict
+      - id: check-symlinks
+      - id: check-xml
+      - id: check-yaml
+      - id: debug-statements
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+        exclude_types: [rst]
+      - id: fix-byte-order-marker
+
+
+  # Python hooks
+  - repo: https://github.com/asottile/pyupgrade
+    rev: v3.4.0
+    hooks:
+    -   id: pyupgrade
+        args: [--py36-plus]
+
+  # PyDocStyle
+  - repo: https://github.com/PyCQA/pydocstyle
+    rev: 6.3.0
+    hooks:
+    - id: pydocstyle
+      args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
+
+  - repo: https://github.com/psf/black
+    rev: 23.3.0
+    hooks:
+      - id: black
+        args: ["--line-length=99"]
+
+  - repo: https://github.com/pycqa/flake8
+    rev: 6.0.0
+    hooks:
+    - id: flake8
+      args: ["--extend-ignore=E501"]
+
+  # CPP hooks
+  - repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v15.0.6
+    hooks:
+      - id: clang-format
+
+
+  - repo: local
+    hooks:
+      - id: ament_cppcheck
+        name: ament_cppcheck
+        description: Static code analysis of C/C++ files.
+        stages: [commit]
+        entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=TRUE ament_cppcheck
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+
+  # Maybe use https://github.com/cpplint/cpplint instead
+  - repo: local
+    hooks:
+      - id: ament_cpplint
+        name: ament_cpplint
+        description: Static code analysis of C/C++ files.
+        stages: [commit]
+        entry: ament_cpplint
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+        args: ["--linelength=100", "--filter=-whitespace/newline"]
+
+  # Cmake hooks
+  - repo: local
+    hooks:
+      - id: ament_lint_cmake
+        name: ament_lint_cmake
+        description: Check format of CMakeLists.txt files.
+        stages: [commit]
+        entry: ament_lint_cmake
+        language: system
+        files: CMakeLists\.txt$
+
+  # Copyright
+  - repo: local
+    hooks:
+      - id: ament_copyright
+        name: ament_copyright
+        description: Check if copyright notice is available in all files.
+        stages: [commit]
+        entry: ament_copyright
+        language: system
+
+  # Docs - RestructuredText hooks
+  - repo: https://github.com/PyCQA/doc8
+    rev: v1.1.1
+    hooks:
+      - id: doc8
+        args: ['--max-line-length=100', '--ignore=D001']
+        exclude: CHANGELOG\.rst$
+
+  - repo: https://github.com/pre-commit/pygrep-hooks
+    rev: v1.10.0
+    hooks:
+      - id: rst-backticks
+        exclude: CHANGELOG\.rst$
+      - id: rst-directive-colons
+      - id: rst-inline-touching-normal
+
+  # Spellcheck in comments and docs
+  # skipping of *.svg files is not working...
+  - repo: https://github.com/codespell-project/codespell
+    rev: v2.2.4
+    hooks:
+      - id: codespell
+        args: ['--write-changes', '-L', 'linz']
+        exclude: CHANGELOG\.rst|\.(svg|pyc)$
diff --git a/README.md b/README.md
@@ -8,27 +8,107 @@ Github CI
 
 ## What is included?
 
-- **kuka_resources** contains general, common files. It is copied from [kuka_experimental](https://github.com/ros-industrial/kuka_experimental) and is ported from ROS to ROS2.
-- **kuka_cybertech_support** contains urdf, config and mesh files for KUKA cybertech robots.
-- **kuka_kr_moveit_config** contains configuration files for KUKA KR robots necessary for planning with moveit.
-- **kuka_lbr_iisy_support** contains urdf, config and mesh files for KUKA iisy robots.
-- **kuka_lbr_iisy_moveit_config** contains configuration files for KUKA iisy robots necessary for planning with moveit.
-- **kuka_agilus_support** contains urdf, config and mesh files for KUKA Agilus robots, it is copied from [kuka_experimental](https://github.com/ros-industrial/kuka_experimental) and ported to ROS2.
-- **kuka_lbr_iiwa_support** contains urdf, config and mesh files for KUKA LBR iiwa 7 robots
-- **kuka_rsi_simulator** implements a basic RSI simulator. It is also copied from [kuka_experimental](https://github.com/ros-industrial/kuka_experimental) and is ported to ROS2. It implements a UDP socket, and receives the commands from an RSI interface and sends back the actual position of the robot (as it is a simulation, it is a simple closed loop, it sends what it received).
+- `kuka_resources` contains general, common files. It is copied from [kuka_experimental](https://github.com/ros-industrial/kuka_experimental) and is ported from ROS to ROS2.
+- `kuka_cybertech_support` contains urdf, config and mesh files for KUKA cybertech robots.
+- `kuka_kr_moveit_config` contains configuration files for KUKA KR robots necessary for planning with MoveIt.
+- `kuka_lbr_iisy_support` contains urdf, config and mesh files for KUKA iisy robots.
+- `kuka_lbr_iisy_moveit_config` contains configuration files for KUKA LBR iisy robots necessary for planning with MoveIt.
+- `kuka_agilus_support` contains urdf, config and mesh files for KUKA Agilus robots, it is copied from [kuka_experimental](https://github.com/ros-industrial/kuka_experimental) and ported to ROS2.
+- `kuka_lbr_iiwa_support` contains urdf, config and mesh files for KUKA LBR iiwa robots
+- `kuka_lbr_iiwa_moveit_config` contains configuration files for KUKA LBR iiwa robots necessary for planning with MoveIt.
 
-## Running the RSI simulator
+## Structure of the support packages
 
+All support packages consist of 4 folders:
+- `config`: contains joint limits, necessary for time parametrization of trajectories
+- `launch`: contains launch files to be able to visualize the robot models
+- `meshes`: contains collision and visual meshes for the robots
+- `urdf`: contains the xacro files describing the robots, including `ros2_control` integration (with fake hardware argument)
+
+### Xacro files
+Each robot has two specific xacro files: a macro (`{robot_name}_macro.xacro`) and another file instantiating this macro (`{robot_name}.urdf.xacro`). Additionally there is a xacro providing `ros2_control` integration, including the name and type of the hardware interface, hardware parameters and the supported state and command interfaces.
+Additionally a transmission xacro is provided for gazebo support, but the `mechanicalReduction` parameters contained within are not valid, only placeholders.
+
+The macro files contain the links and joints of the main serial chain, including transformations, rotation axes, inertial properties, joint position, velocity and effort limits and the location of the mesh files.
+
+The macro file follows the ROS-Industrial conventions:
+ - link names are `link_{i}`
+ - joint names are `joint_{i}`
+ - all link and joint names have a `prefix` argument
+ - includes `base` frame: equivalent to the base frame defined by the industrial controller ($ROBROOT)
+ - includes `flange` frame: attachment point for EEF models
+ - includes `tool0` frame: all-zeros tool frame, identical to the tool frame defined by the industrial controller ($TOOL)
+
+All robots in the xacros are named according to the following pattern:
+
+`{kr/lbr_iisy/lbr_iiwa}{payload}_r{reach}_{version}`,
+
+where `version` is omitted, if the official product name does not contain it. (e.g. KR 120 R3100-2 is named `kr120_r3100_2` and LBR iisy 3 R760 is `lbr_iisy3_r760`)
+
+The MoveIt configuration packages also contain xacros, that describe the semantic information of the robots: planning groups, default states and link-pairs, for which collision checking should not be done. The default planning group (from `base_link` to `tool0`) is named `manipulator` for all robot arms. An end effector, named `end_effector` is also defined for all robots, which enables visualising end effector paths in rviz.
+
+To visualise the robot models, the launch files in the `launch` directory of the support packages can be used. These also start a `joint_state_publisher_gui` to enable visualisation of the robot meshes and frames with different joint configurations. However they have only visualisation purposes and cannot connect to real or fake hardware.
+
+### Frame conventions
+
+The frames of the main serial chain in the xacros (`base_link` to `link_6` or `link_7`) follow the Denavit–Hartenberg conventions of Khalil-Dombre.
+The other frames, which are added to conform to ROS-Industrial follow the conventions defined there: `base` and `tool0` are defined to be identical to the frames on the controller, while `flange` follows [REP-103](https://www.ros.org/reps/rep-0103.html#coordinate-frame-conventions), meaning that in default position x+ points forwards and z+ upwards.
+
+
+### Joint limit configurations
+
+The support packages contain a joint limits file for every supported robot model, necessary time parametrization of MoveIt-planned paths. They contain the velocity limits also available in the URDF model and additional acceleration limits. Acceleration limits can never be global, these values are calculated from the worst-case ramp-up time to reach maximum velocity. The easiest way to modify the allowed velocities and accelerations is to change the velocity and acceleration scaling factors also available in the same configuration files. (The scaling factor can never be smaller than 1.)
+
+
+### Extending the models
+
+In real applications, it's likely that the description will be more complex, involving multiple objects next to the robot and optionally end effectors. It is recommended to create a new, dedicated ROS2 package specifically for managing this extended description by copying and extending the base robot model.
+
+Example of attaching an end effector (with link name `eef_base_link`) to the `flange` frame, which could be defined in a different xacro file:
+```xml
+<joint name="${prefix}flange-${prefix}eef" type="fixed">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <parent link="${prefix}flange" />
+ <child link="${prefix}eef_base_link" />
+</joint>
 ```
-ros2 launch kuka_rsi_simulator kuka_rsi_simulator_launch.py
-```
 
-## Starting the move group server for KR robots
+## What data is verified?
+
+Some of the data in the xacros might not be valid or missing, the following table shows what can be considered valid.
+
+|Robot name | Robot family | Transformations | Joint position limits | Joint velocity limits | Joint effort limits | Inertial values | Simplified collision meshes|
+|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
+|lbr_iisy3_r760| - | ✓ | ✓ | ✓ | ✓ | | ✓ |
+|lbr_iisy11_r1300| - | ✓ | ✓ | ✓ | ✓ | | |
+|lbr_iisy15_r930| - | ✓ | ✓ | ✓ | ✓ | | |
+|lbr_iiwa14_r820| - | ✓ | ✓ | ✓ | | | |
+|kr6_r700_sixx| agilus | ✓ | ✓ | ✓ | | | ✓ |
+|kr6_r900_sixx| agilus | ✓ | ✓ | ✓ | | | ✓ |
+|kr10_r1100_2| agilus | ✓ | ✓ | ✓ | ✓ | | |
+|kr16_r2010_2| cybertech | ✓ | ✓ | ✓ | ✓ | | |
+|kr210_r2700_2| quantec | ✓ | ✓ | ✓ | ✓ | | |
+|kr210_r3100_2| quantec | ✓ | ✓ | ✓ | ✓ | | |
+
+## Starting the move group server with fake hardware
 
-The following launch file will start the driver with fake hardware, matching robot_model and robot_family arguments can be added to the command e.g. (robot_model:=kr16_r2010_2 robot_family:=cybertech):
+To start rviz with the motion planning plugin using fake hardware, the following launch files can be used:
 
+#### KR robots (KSS):
 ```
 ros2 launch kuka_kr_moveit_config moveit_planning_fake_hardware.launch.py
 ```
+Matching `robot_model` and `robot_family` arguments can be added after the command (e.g. `robot_model:=kr16_r2010_2 robot_family:=cybertech`). The default robot model is `kr6_r700_sixx`
+
+#### LBR iiwa robots (Sunrise):
+```
+ros2 launch kuka_lbr_iisy_moveit_config moveit_planning_fake_hardware.launch.py
+```
+
+#### LBR iisy robots (iiQKA):
+```
+ros2 launch kuka_lbr_iiwa_moveit_config moveit_planning_fake_hardware.launch.py
+```
+A `robot_model` argument can be added after the command (e.g. `robot_model:=lbr_iisy11_r1300`). The default robot model is `lbr_iisy3_r760`
 
-The robot_manager lifecycle node must be configured and activated to be able to see the robot in rviz, afterwards the server will be able to accept planning requests. (An example how to create such a request can be found in the (kuka_drivers/examples)/iiqka_moveit_example package.)
+These launch files are not using the actual driver implementation, they only start `rviz` the `move_group` server and a `ros2_control_node` with fake hardware and two controllers `joint_state_broadcaster` and `joint_trajectory_controller` The server will be able to accept planning requests from the plugin or from code. (An example how to create such a request from C++ code can be found in the `iiqka_moveit_example` package in the `kuka_drivers` repository.) To support hardwares with less performance, the update rate of the control node was reduced to 50 Hz for all robots.
diff --git a/kuka_agilus_support/CMakeLists.txt b/kuka_agilus_support/CMakeLists.txt
@@ -9,4 +9,9 @@ find_package(xacro REQUIRED)
 install(DIRECTORY config launch meshes urdf
   DESTINATION share/${PROJECT_NAME})
 
+if(BUILD_TESTING)
+  find_package(launch_testing_ament_cmake)
+  add_launch_test(test/test_kr_agilus.py)
+endif()
+
 ament_package()
diff --git a/kuka_agilus_support/config/kr10_r1100_2_joint_limits.yaml b/kuka_agilus_support/config/kr10_r1100_2_joint_limits.yaml
@@ -0,0 +1,38 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration, max_jerk]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+
+# Easiest way to slow down the robot is to decrease the velocity and acceleration scaling factors
+default_velocity_scaling_factor: 1.0
+default_acceleration_scaling_factor: 1.0
+joint_limits:
+  joint_1:
+    has_velocity_limits: true
+    max_velocity: 5.23599
+    has_acceleration_limits: false
+    has_jerk: false
+  joint_2:
+    has_velocity_limits: true
+    max_velocity: 3.926991
+    has_acceleration_limits: false
+    has_jerk: false
+  joint_3:
+    has_velocity_limits: true
+    max_velocity: 5.75959
+    has_acceleration_limits: false
+    has_jerk: false
+  joint_4:
+    has_velocity_limits: true
+    max_velocity:  6.28319
+    has_acceleration_limits: false
+    has_jerk: false
+  joint_5:
+    has_velocity_limits: true
+    max_velocity:  6.28319
+    has_acceleration_limits: false
+    has_jerk: false
+  joint_6:
+    has_velocity_limits: true
+    max_velocity: 7.55728
+    has_acceleration_limits: false
+    has_jerk: false
diff --git a/kuka_agilus_support/config/kr6_r700_sixx_joint_limits.yaml b/kuka_agilus_support/config/kr6_r700_sixx_joint_limits.yaml
@@ -6,32 +6,32 @@
 default_velocity_scaling_factor: 1.0
 default_acceleration_scaling_factor: 1.0
 joint_limits:
-  joint_a1:
+  joint_1:
     has_velocity_limits: true
     max_velocity: 6.283185
     has_acceleration_limits: false
     has_jerk: false
-  joint_a2:
+  joint_2:
     has_velocity_limits: true
     max_velocity: 5.23599
     has_acceleration_limits: false
     has_jerk: false
-  joint_a3:
+  joint_3:
     has_velocity_limits: true
     max_velocity: 6.283185
     has_acceleration_limits: false
     has_jerk: false
-  joint_a4:
+  joint_4:
     has_velocity_limits: true
     max_velocity: 6.64970
     has_acceleration_limits: false
     has_jerk: false
-  joint_a5:
+  joint_5:
     has_velocity_limits: true
     max_velocity: 6.77188
     has_acceleration_limits: false
     has_jerk: false
-  joint_a6:
+  joint_6:
     has_velocity_limits: true
     max_velocity: 10.73377
     has_acceleration_limits: false