diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index 6fccbd26..5ef0323a 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -1,6 +1,20 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package LBR FRI ROS 2 Stack
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Humble v2.1.0 (2024-09-10)
+--------------------------
+* De-couple launch files from ``lbr_bringup`` for easier customization (breaking change):
+
+ * Removed ``sim:=true / false`` argument from launch files in favor of dedicated launch files (since no feature parity between simulation and real robot)
+ * MoveIt and RViz need to be launched separately now
+ * User can now launch via:
+
+ * ``ros2 launch lbr_bringup mock.launch.py`` (new: mock system)
+ * ``ros2 launch lbr_bringup hardware.launch.py`` (real robot)
+ * ``ros2 launch lbr_bringup gazebo.launch.py`` (Gazebo simulation)
+* Added mock hardware to ``lbr_ros2_control`` (for simple ``ros2_control`` testing without the need for Gazebo, refer https://control.ros.org/humble/doc/ros2_control/hardware_interface/doc/mock_components_userdoc.html)
+* Updated documentation
+
Humble v2.0.0 (2024-07-08)
--------------------------
* Remove ``app.launch.py`` from demos in favor for ``ros2_control`` variant (breaking change)
@@ -47,7 +61,7 @@ Humble v1.4.2 (2023-12-29)
Humble v1.4.1 (2023-12-15)
--------------------------
* Removes the ``base_frame`` parameter from ``lbr_bringup``, ``lbr_description``, ``lbr_fri_ros2``, ``lbr_ros2_control``
-* Updates RViZ default config in ``lbr_moveit_config``
+* Updates RViz default config in ``lbr_moveit_config``
* Refers to https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/144
Humble v1.4.0 (2023-12-08)
diff --git a/README.md b/README.md
index ba546466..612c46f1 100644
--- a/README.md
+++ b/README.md
@@ -62,19 +62,25 @@ Full documentation available on [Read the Docs](https://lbr-stack.readthedocs.io
colcon build --symlink-install
```
-4. Launch the simulation via
+4. In terminal 1, launch a mock setup via
```shell
source install/setup.bash
- ros2 launch lbr_bringup bringup.launch.py \
- model:=iiwa7 # [iiwa7, iiwa14, med7, med14] \
- sim:=true # [true, false] \
- rviz:=true # [true, false] \
- moveit:=true # [true, false]
+ ros2 launch lbr_bringup mock.launch.py \
+ model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
```
> [!TIP]
-> List all arguments for the launch file via `ros2 launch lbr_bringup bringup.launch.py -s`
+> List all arguments for the launch file via `ros2 launch lbr_bringup mock.launch.py -s`
+
+5. In terminal 2, visualize the setup via
+
+ ```shell
+ source install/setup.bash
+ ros2 launch lbr_bringup rviz.launch.py \
+ rviz_config_pkg:=lbr_bringup \
+ rviz_config:=config/mock.rviz
+ ```
Now, run the [demos](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Hardware Setup](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_fri_ros2_stack/doc/hardware_setup.html).
diff --git a/docker/doc/docker.rst b/docker/doc/docker.rst
index 9f894113..53803077 100644
--- a/docker/doc/docker.rst
+++ b/docker/doc/docker.rst
@@ -16,19 +16,20 @@ To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions
mkdir -p lbr-stack/src && cd lbr-stack
vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml
+#. Install `Docker `_:octicon:`link-external`.
+
#. Copy the Dockerfile and the container scripts to the ``lbr-stack`` directory. Build and start the container
.. code-block:: bash
cp -r src/lbr_fri_ros2_stack/docker/* .
- chmod +x container_build.sh
- sudo ./container_build.sh
+ sudo ./container_build.sh # this will run the container once finished
-#. Inside the container, launch e.g. simulation via (might require re-launch after Gazebo launched first time)
+#. Once inside the container, launch e.g. the mock setup via
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py model:=iiwa7 sim:=true moveit:=true
+ ros2 launch lbr_bringup mock.launch.py model:=iiwa7
.. hint::
@@ -36,4 +37,20 @@ To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py -s
+ ros2 launch lbr_bringup mock.launch.py
+
+ .. hint::
+
+ Refer to :ref:`lbr_bringup` for more launch options.
+
+#. Connect another shell to the running container
+
+ .. code-block:: bash
+
+ ./container_new_console.sh
+
+#. Run e.g. MoveIt with RViz
+
+ .. code-block:: bash
+
+ ros2 launch lbr_bringup move_group.launch.py rviz:=true
diff --git a/lbr_bringup/CMakeLists.txt b/lbr_bringup/CMakeLists.txt
index cd7e1288..54f14a29 100644
--- a/lbr_bringup/CMakeLists.txt
+++ b/lbr_bringup/CMakeLists.txt
@@ -15,6 +15,6 @@ install(
)
# Launch mixins
-ament_python_install_package(launch_mixins)
+ament_python_install_package(lbr_bringup)
ament_package()
diff --git a/lbr_description/config/config.rviz b/lbr_bringup/config/gazebo.rviz
similarity index 77%
rename from lbr_description/config/config.rviz
rename to lbr_bringup/config/gazebo.rviz
index f0e17afd..f4486f40 100644
--- a/lbr_description/config/config.rviz
+++ b/lbr_bringup/config/gazebo.rviz
@@ -1,14 +1,15 @@
Panels:
- Class: rviz_common/Displays
- Help Height: 78
+ Help Height: 87
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1
- Splitter Ratio: 0.5
- Tree Height: 787
+ - /RobotModel1/Description Topic1
+ Splitter Ratio: 0.40760868787765503
+ Tree Height: 657
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@@ -22,6 +23,11 @@ Panels:
- /Current View1
Name: Views
Splitter Ratio: 0.5
+ - Class: rviz_common/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
Visualization Manager:
Class: ""
Displays:
@@ -53,7 +59,7 @@ Visualization Manager:
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
- Value: /robot_description
+ Value: /lbr/robot_description
Enabled: true
Links:
All Links Enabled: true
@@ -105,25 +111,15 @@ Visualization Manager:
Alpha: 1
Show Axes: false
Show Trail: false
- storz_straight_exoscope_hsi_link_0:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- storz_straight_exoscope_hsi_link_cm:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- storz_straight_exoscope_hsi_link_cm_optical:
- Alpha: 1
- Show Axes: false
- Show Trail: false
world:
Alpha: 1
Show Axes: false
Show Trail: false
+ Mass Properties:
+ Inertia: false
+ Mass: false
Name: RobotModel
- TF Prefix: ""
+ TF Prefix: lbr
Update Interval: 0
Value: true
Visual Enabled: true
@@ -142,6 +138,9 @@ Visualization Manager:
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
@@ -170,39 +169,41 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
- Distance: 4.795630931854248
+ Distance: 3.38482666015625
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
- X: -0.19523456692695618
- Y: -0.17667146027088165
- Z: 0.2629773020744324
- Focal Shape Fixed Size: true
+ X: -0.10796727985143661
+ Y: -0.08450233191251755
+ Z: 0.1360965371131897
+ Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.785398006439209
+ Pitch: 0.7853981852531433
Target Frame:
- Value: Orbit (rviz_default_plugins)
- Yaw: 0.785398006439209
+ Value: Orbit (rviz)
+ Yaw: 0.7853981852531433
Saved: ~
Window Geometry:
Displays:
collapsed: false
- Height: 1016
+ Height: 1021
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd0000000400000000000001ba0000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004ab0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd00000004000000000000021200000337fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004e00000337000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f0000035b000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000004efc0100000002fb0000000800540069006d006501000000000000073a000002e701000003fb0000000800540069006d00650100000000000004500000000000000000000005270000033700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
+ Time:
+ collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
- Width: 1920
- X: 0
- Y: 27
+ Width: 1850
+ X: 1080
+ Y: 450
diff --git a/lbr_bringup/config/config.rviz b/lbr_bringup/config/hardware.rviz
similarity index 100%
rename from lbr_bringup/config/config.rviz
rename to lbr_bringup/config/hardware.rviz
diff --git a/lbr_bringup/config/mock.rviz b/lbr_bringup/config/mock.rviz
new file mode 100644
index 00000000..f4486f40
--- /dev/null
+++ b/lbr_bringup/config/mock.rviz
@@ -0,0 +1,209 @@
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 87
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /RobotModel1
+ - /RobotModel1/Description Topic1
+ Splitter Ratio: 0.40760868787765503
+ Tree Height: 657
+ - Class: rviz_common/Selection
+ Name: Selection
+ - Class: rviz_common/Tool Properties
+ Expanded:
+ - /2D Goal Pose1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz_common/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz_common/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz_default_plugins/Grid
+ Color: 34; 141; 255
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Alpha: 1
+ Class: rviz_default_plugins/RobotModel
+ Collision Enabled: false
+ Description File: ""
+ Description Source: Topic
+ Description Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /lbr/robot_description
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ link_0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_ee:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ world:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Mass Properties:
+ Inertia: false
+ Mass: false
+ Name: RobotModel
+ TF Prefix: lbr
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ Enabled: true
+ Global Options:
+ Background Color: 21; 21; 26
+ Fixed Frame: world
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/Interact
+ Hide Inactive Objects: true
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /initialpose
+ - Class: rviz_default_plugins/SetGoal
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /goal_pose
+ - Class: rviz_default_plugins/PublishPoint
+ Single click: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /clicked_point
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Distance: 3.38482666015625
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: -0.10796727985143661
+ Y: -0.08450233191251755
+ Z: 0.1360965371131897
+ Focal Shape Fixed Size: false
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.7853981852531433
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 0.7853981852531433
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1021
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000021200000337fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004e00000337000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f0000035b000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000004efc0100000002fb0000000800540069006d006501000000000000073a000002e701000003fb0000000800540069006d00650100000000000004500000000000000000000005270000033700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1850
+ X: 1080
+ Y: 450
diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst
index dba75dfd..da2c79d5 100644
--- a/lbr_bringup/doc/lbr_bringup.rst
+++ b/lbr_bringup/doc/lbr_bringup.rst
@@ -1,38 +1,169 @@
lbr_bringup
===========
-The ``lbr_bringup`` works for the simulation and the real robot. Run:
+The ``lbr_bringup`` package hosts some launch files, which can be included via standard procedures:
+
+.. code:: python
+
+ from launch import LaunchDescription
+ from launch.actions import IncludeLaunchDescription
+ from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+ def generate_launch_description() -> LaunchDescription:
+ ld = LaunchDescription()
+ ld.add_action(
+ IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ PathJoinSubstitution(
+ [
+ FindPackageShare("lbr_bringup"),
+ "launch",
+ "mock.launch.py",
+ ]
+ )
+ ),
+ )
+ )
+ return ld
+
+The launch files can also be run via the command line, as further described below.
+
+.. contents:: Table of Contents
+ :depth: 2
+ :local:
+ :backlinks: none
+
+Launch Files
+------------
+Mock Setup
+~~~~~~~~~~
+Useful for running a physics-free simulation of the system. This launch file will (see `mock.launch.py `_:octicon:`link-external`):
+
+#. Run the ``robot_state_publisher``
+#. Run the ``ros2_control_node`` with mock components as loaded from ``robot_description``
+#. Load ``ros2_controllers``
.. code:: bash
- ros2 launch lbr_bringup bringup.launch.py \
- model:=iiwa7 # [iiwa7, iiwa14, med7, med14] \
- sim:=true # [true, false] \
- rviz:=true # [true, false] \
- moveit:=true # [true, false]
+ ros2 launch lbr_bringup mock.launch.py \
+ model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
.. note::
- For a list of available parameters, call ``ros2 launch lbr_bringup bringup.launch.py -s``.
+ List all arguments for the launch file via ``ros2 launch lbr_bringup mock.launch.py -s``.
-When using the real robot, select ``sim:=false`` and
+Gazebo Simulation
+~~~~~~~~~~~~~~~~~
+Useful for running a physics simulation the the system. This launch file will will (see `gazebo.launch.py `_:octicon:`link-external`):
-.. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD``
+#. Start the ``robot_state_publisher``
+#. Start the ``Gazebo`` simulation
+#. Spawn the selected robot model (includes the ``ros2_control_node`` within the ``Gazebo`` plugin)
+#. Load ``ros2_controllers``
- .. thumbnail:: ../../lbr_demos/doc/img/applications_lbr_server.png
+.. code:: bash
-Select
+ ros2 launch lbr_bringup gazebo.launch.py \
+ model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
-- ``FRI send period``: ``10 ms``
-- ``IP address``: ``your configuration``
-- ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` (will put the robot into a compliant mode)
-- ``FRI client command mode``: ``POSITION``
+.. note::
+ List all arguments for the launch file via ``ros2 launch lbr_bringup gazebo.launch.py -s``.
-Users may also refer to :ref:`Software Architecture` for a better understanding of the underlying ``lbr_fri_ros2`` package.
+Hardware
+~~~~~~~~
+.. warning::
+ Do always execute in ``T1`` mode first.
.. note::
- For the real robot, make sure you have followed :doc:`Hardware Setup <../../lbr_fri_ros2_stack/doc/hardware_setup>` first.
+ Make sure you have followed :doc:`Hardware Setup <../../lbr_fri_ros2_stack/doc/hardware_setup>` first.
-.. warning::
- On the real robot, do always execute in ``T1`` mode first.
+#. Client side configurations:
+
+ .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD``
+
+ .. thumbnail:: ../../lbr_demos/doc/img/applications_lbr_server.png
+
+ Select
+
+ - ``FRI send period``: ``10 ms``
+ - ``IP address``: ``your configuration``
+ - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL``
+ - ``FRI client command mode``: ``POSITION``
+
+#. Launch file:
+
+ This launch file will (see `hardware.launch.py `_:octicon:`link-external`):
+
+ #. Run the ``robot_state_publisher``
+ #. Run the ``ros2_control_node`` with the ``lbr_fri_ros2::SystemInterface`` plugin from :doc:`lbr_ros2_control <../../lbr_ros2_control/doc/lbr_ros2_control>` as loaded from ``robot_description`` (which will attempt to establish a connection to the real robot).
+ #. Load ``ros2_controllers``
+
+ .. code:: bash
+
+ ros2 launch lbr_bringup hardware.launch.py \
+ model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
+
+ .. note::
+ List all arguments for the launch file via ``ros2 launch lbr_bringup hardware.launch.py -s``.
+
+RViz
+~~~~
+This launch file will spin up ``RViz`` for visualization. It will (see `rviz.launch.py `_:octicon:`link-external`):
+
+#. Read ``RViz`` configurations.
+#. Run ``RViz``.
+
+.. code:: bash
+
+ ros2 launch lbr_bringup rviz.launch.py \
+ rviz_config_pkg:=lbr_bringup \
+ rviz_config:=config/mock.rviz # [gazebo.rviz, hardware.rviz, mock.rviz]
+
+.. note::
+ List all arguments for the launch file via ``ros2 launch lbr_bringup rviz.launch.py -s``.
+
+.. note::
+ Requires the user to run `Mock Setup`_, `Gazebo Simulation`_ or `Hardware`_ first.
+
+MoveIt
+~~~~~~
+Please note that MoveIt configurations are specific and you as a user will need to create your own for your system (potentially containing multiple robots or an end-effector).
+
+.. code:: bash
+
+ ros2 launch lbr_bringup move_group.launch.py \
+ model:=iiwa7 \
+ mode:=mock \
+ rviz:=true
+
+.. note::
+ Requires the user to run `Mock Setup`_, `Gazebo Simulation`_ or `Hardware`_ first.
+
+.. note::
+ Runs ``RViz`` with specific MoveIt configurations.
+
+Mixins
+------
+The ``lbr_bringup`` package makes heavy use of mixins. Mixins are simply state-free classes with static methods. They are a convenient way of writing launch files.
+
+The below shows an example of the `rviz.launch.py `_:octicon:`link-external` file:
+
+.. code:: python
+
+ from launch import LaunchDescription
+ from lbr_bringup.rviz import RVizMixin
+
+
+ def generate_launch_description() -> LaunchDescription:
+ ld = LaunchDescription()
+
+ # launch arguments
+ ld.add_action(RVizMixin.arg_rviz_config())
+ ld.add_action(RVizMixin.arg_rviz_config_pkg())
+
+ # rviz
+ ld.add_action(RVizMixin.node_rviz())
+ return ld
+
+Which is quite compact and easy to read.
General Information on the FRI
------------------------------
diff --git a/lbr_bringup/launch/bringup.launch.py b/lbr_bringup/launch/bringup.launch.py
deleted file mode 100644
index b4fb2420..00000000
--- a/lbr_bringup/launch/bringup.launch.py
+++ /dev/null
@@ -1,45 +0,0 @@
-from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.conditions import IfCondition, UnlessCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_mixins.lbr_description import LBRDescriptionMixin
-from launch_ros.substitutions import FindPackageShare
-
-
-def generate_launch_description() -> LaunchDescription:
- ld = LaunchDescription()
-
- ld.add_action(LBRDescriptionMixin.arg_sim())
-
- ld.add_action(
- IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- PathJoinSubstitution(
- [
- FindPackageShare("lbr_bringup"),
- "launch",
- "sim.launch.py",
- ]
- )
- ),
- condition=IfCondition(LaunchConfiguration("sim")),
- )
- )
-
- ld.add_action(
- IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- PathJoinSubstitution(
- [
- FindPackageShare("lbr_bringup"),
- "launch",
- "real.launch.py",
- ]
- )
- ),
- condition=UnlessCondition(LaunchConfiguration("sim")),
- )
- )
-
- return ld
diff --git a/lbr_bringup/launch/gazebo.launch.py b/lbr_bringup/launch/gazebo.launch.py
new file mode 100644
index 00000000..0ff93e5b
--- /dev/null
+++ b/lbr_bringup/launch/gazebo.launch.py
@@ -0,0 +1,56 @@
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from lbr_bringup.description import LBRDescriptionMixin
+from lbr_bringup.gazebo import GazeboMixin
+from lbr_bringup.ros2_control import LBRROS2ControlMixin
+
+
+def generate_launch_description() -> LaunchDescription:
+ ld = LaunchDescription()
+
+ # launch arguments
+ ld.add_action(LBRDescriptionMixin.arg_model())
+ ld.add_action(LBRDescriptionMixin.arg_robot_name())
+ ld.add_action(
+ LBRROS2ControlMixin.arg_ctrl()
+ ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml
+
+ # static transform world -> robot_name/world
+ world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero
+ ld.add_action(
+ LBRDescriptionMixin.node_static_tf(
+ tf=world_robot_tf,
+ parent="world",
+ child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]),
+ )
+ )
+
+ # robot description
+ robot_description = LBRDescriptionMixin.param_robot_description(mode="gazebo")
+
+ # robot state publisher
+ robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
+ robot_description=robot_description, use_sim_time=True
+ )
+ ld.add_action(
+ robot_state_publisher
+ ) # Do not condition robot state publisher on joint state broadcaster as Gazebo uses robot state publisher to retrieve robot description
+
+ # Gazebo
+ ld.add_action(GazeboMixin.include_gazebo()) # Gazebo has its own controller manager
+ spawn_entity = GazeboMixin.node_spawn_entity(
+ tf=world_robot_tf
+ ) # spawns robot in Gazebo through robot_description topic of robot_state_publisher
+ ld.add_action(spawn_entity)
+
+ # controllers
+ joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
+ controller="joint_state_broadcaster"
+ )
+ ld.add_action(joint_state_broadcaster)
+ ld.add_action(
+ LBRROS2ControlMixin.node_controller_spawner(
+ controller=LaunchConfiguration("ctrl")
+ )
+ )
+ return ld
diff --git a/lbr_bringup/launch/hardware.launch.py b/lbr_bringup/launch/hardware.launch.py
new file mode 100644
index 00000000..5240557f
--- /dev/null
+++ b/lbr_bringup/launch/hardware.launch.py
@@ -0,0 +1,68 @@
+from launch import LaunchDescription
+from launch.actions import RegisterEventHandler
+from launch.event_handlers import OnProcessStart
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from lbr_bringup.description import LBRDescriptionMixin
+from lbr_bringup.ros2_control import LBRROS2ControlMixin
+
+
+def generate_launch_description() -> LaunchDescription:
+ ld = LaunchDescription()
+
+ # launch arguments
+ ld.add_action(LBRDescriptionMixin.arg_model())
+ ld.add_action(LBRDescriptionMixin.arg_robot_name())
+ ld.add_action(LBRDescriptionMixin.arg_port_id())
+ ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg())
+ ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg())
+ ld.add_action(LBRROS2ControlMixin.arg_ctrl())
+
+ # static transform world -> robot_name/world
+ ld.add_action(
+ LBRDescriptionMixin.node_static_tf(
+ tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ parent="world",
+ child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]),
+ )
+ )
+
+ # robot description
+ robot_description = LBRDescriptionMixin.param_robot_description(mode="hardware")
+
+ # robot state publisher
+ robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
+ robot_description=robot_description, use_sim_time=False
+ )
+ ld.add_action(robot_state_publisher)
+
+ # ros2 control node
+ ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False)
+ ld.add_action(ros2_control_node)
+
+ # joint state broad caster and controller on ros2 control node start
+ joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
+ controller="joint_state_broadcaster"
+ )
+ force_torque_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
+ controller="force_torque_broadcaster"
+ )
+ lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
+ controller="lbr_state_broadcaster"
+ )
+ controller = LBRROS2ControlMixin.node_controller_spawner(
+ controller=LaunchConfiguration("ctrl")
+ )
+
+ controller_event_handler = RegisterEventHandler(
+ OnProcessStart(
+ target_action=ros2_control_node,
+ on_start=[
+ joint_state_broadcaster,
+ force_torque_broadcaster,
+ lbr_state_broadcaster,
+ controller,
+ ],
+ )
+ )
+ ld.add_action(controller_event_handler)
+ return ld
diff --git a/lbr_bringup/launch/mock.launch.py b/lbr_bringup/launch/mock.launch.py
new file mode 100644
index 00000000..3443431f
--- /dev/null
+++ b/lbr_bringup/launch/mock.launch.py
@@ -0,0 +1,59 @@
+from launch import LaunchDescription
+from launch.actions import RegisterEventHandler
+from launch.event_handlers import OnProcessStart
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from lbr_bringup.description import LBRDescriptionMixin
+from lbr_bringup.ros2_control import LBRROS2ControlMixin
+
+
+def generate_launch_description() -> LaunchDescription:
+ ld = LaunchDescription()
+
+ # launch arguments
+ ld.add_action(LBRDescriptionMixin.arg_model())
+ ld.add_action(LBRDescriptionMixin.arg_robot_name())
+ ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg())
+ ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg())
+ ld.add_action(LBRROS2ControlMixin.arg_ctrl())
+
+ # static transform world -> robot_name/world
+ ld.add_action(
+ LBRDescriptionMixin.node_static_tf(
+ tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ parent="world",
+ child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]),
+ )
+ )
+
+ # robot description
+ robot_description = LBRDescriptionMixin.param_robot_description(mode="mock")
+
+ # robot state publisher
+ robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
+ robot_description=robot_description, use_sim_time=False
+ )
+ ld.add_action(robot_state_publisher)
+
+ # ros2 control node
+ ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False)
+ ld.add_action(ros2_control_node)
+
+ # joint state broad caster and controller on ros2 control node start
+ joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
+ controller="joint_state_broadcaster"
+ )
+ controller = LBRROS2ControlMixin.node_controller_spawner(
+ controller=LaunchConfiguration("ctrl")
+ )
+
+ controller_event_handler = RegisterEventHandler(
+ OnProcessStart(
+ target_action=ros2_control_node,
+ on_start=[
+ joint_state_broadcaster,
+ controller,
+ ],
+ )
+ )
+ ld.add_action(controller_event_handler)
+ return ld
diff --git a/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py
index 29f68bb3..3d1f0bdd 100644
--- a/lbr_bringup/launch/move_group.launch.py
+++ b/lbr_bringup/launch/move_group.launch.py
@@ -2,14 +2,17 @@
from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
from launch.actions import OpaqueFunction
+from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_mixins.lbr_bringup import LBRMoveGroupMixin
-from launch_mixins.lbr_description import LBRDescriptionMixin, RVizMixin
+from lbr_bringup.description import LBRDescriptionMixin
+from lbr_bringup.move_group import LBRMoveGroupMixin
+from lbr_bringup.rviz import RVizMixin
-def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
+def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
ld = LaunchDescription()
+ ld.add_action(LBRDescriptionMixin.arg_robot_name())
ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution())
ld.add_action(LBRMoveGroupMixin.arg_capabilities())
ld.add_action(LBRMoveGroupMixin.arg_disable_capabilities())
@@ -23,66 +26,69 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
)
move_group_params = LBRMoveGroupMixin.params_move_group()
- # MoveGroup:
- # - requires world frame
- # - urdf only has robot_name/world
- # This transform needs publishing
- robot_name = LaunchConfiguration("robot_name").perform(context)
- ld.add_action(
- LBRDescriptionMixin.node_static_tf(
- tf=[0, 0, 0, 0, 0, 0], # keep zero
- parent="world",
- child=PathJoinSubstitution(
- [
- robot_name,
- "world",
- ] # results in robot_name/world
- ),
- )
- )
+ mode = LaunchConfiguration("mode").perform(context)
+ use_sim_time = False
+ if mode == "gazebo":
+ use_sim_time = True
+ # MoveGroup
+ robot_name = LaunchConfiguration("robot_name")
ld.add_action(
LBRMoveGroupMixin.node_move_group(
parameters=[
moveit_configs_builder.to_dict(),
move_group_params,
- {"use_sim_time": LaunchConfiguration("sim")},
+ {"use_sim_time": use_sim_time},
],
namespace=robot_name,
)
)
- # RViz
+ # RViz if desired
rviz = RVizMixin.node_rviz(
rviz_config_pkg=f"{model}_moveit_config",
rviz_config="config/moveit.rviz",
parameters=LBRMoveGroupMixin.params_rviz(
moveit_configs=moveit_configs_builder.to_moveit_configs()
)
- + [{"use_sim_time": LaunchConfiguration("sim")}],
+ + [{"use_sim_time": use_sim_time}],
remappings=[
- ("display_planned_path", robot_name + "/display_planned_path"),
- ("joint_states", robot_name + "/joint_states"),
- ("monitored_planning_scene", robot_name + "/monitored_planning_scene"),
- ("planning_scene", robot_name + "/planning_scene"),
- ("planning_scene_world", robot_name + "/planning_scene_world"),
- ("robot_description", robot_name + "/robot_description"),
- ("robot_description_semantic", robot_name + "/robot_description_semantic"),
+ (
+ "display_planned_path",
+ PathJoinSubstitution([robot_name, "display_planned_path"]),
+ ),
+ ("joint_states", PathJoinSubstitution([robot_name, "joint_states"])),
+ (
+ "monitored_planning_scene",
+ PathJoinSubstitution([robot_name, "monitored_planning_scene"]),
+ ),
+ ("planning_scene", PathJoinSubstitution([robot_name, "planning_scene"])),
+ (
+ "planning_scene_world",
+ PathJoinSubstitution([robot_name, "planning_scene_world"]),
+ ),
+ (
+ "robot_description",
+ PathJoinSubstitution([robot_name, "robot_description"]),
+ ),
+ (
+ "robot_description_semantic",
+ PathJoinSubstitution([robot_name, "robot_description_semantic"]),
+ ),
],
+ condition=IfCondition(LaunchConfiguration("rviz")),
)
ld.add_action(rviz)
-
return ld.entities
def generate_launch_description() -> LaunchDescription:
ld = LaunchDescription()
+ ld.add_action(LBRDescriptionMixin.arg_mode())
ld.add_action(LBRDescriptionMixin.arg_model())
- ld.add_action(LBRDescriptionMixin.arg_robot_name())
- ld.add_action(LBRDescriptionMixin.arg_sim())
-
- ld.add_action(OpaqueFunction(function=launch_setup))
+ ld.add_action(RVizMixin.arg_rviz())
+ ld.add_action(OpaqueFunction(function=hidden_setup))
return ld
diff --git a/lbr_bringup/launch_mixins/__init__.py b/lbr_bringup/launch/moveit_servo.launch.py
similarity index 100%
rename from lbr_bringup/launch_mixins/__init__.py
rename to lbr_bringup/launch/moveit_servo.launch.py
diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py
deleted file mode 100644
index bc7f0c04..00000000
--- a/lbr_bringup/launch/real.launch.py
+++ /dev/null
@@ -1,171 +0,0 @@
-from typing import List
-
-from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler
-from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessStart
-from launch.substitutions import (
- AndSubstitution,
- LaunchConfiguration,
- NotSubstitution,
- PathJoinSubstitution,
-)
-from launch_mixins.lbr_bringup import LBRMoveGroupMixin
-from launch_mixins.lbr_description import LBRDescriptionMixin, RVizMixin
-from launch_mixins.lbr_ros2_control import LBRROS2ControlMixin
-
-
-def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
- ld = LaunchDescription()
-
- robot_description = LBRDescriptionMixin.param_robot_description(sim=False)
- world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero
-
- # robot state publisher
- robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
- robot_description=robot_description, use_sim_time=False
- )
- ld.add_action(robot_state_publisher)
-
- # ros2 control node
- ros2_control_node = LBRROS2ControlMixin.node_ros2_control()
- ld.add_action(ros2_control_node)
-
- # joint state broad caster and controller on ros2 control node start
- joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
- controller="joint_state_broadcaster"
- )
- force_torque_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
- controller="force_torque_broadcaster"
- )
- lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
- controller="lbr_state_broadcaster"
- )
- controller = LBRROS2ControlMixin.node_controller_spawner(
- controller=LaunchConfiguration("ctrl")
- )
-
- controller_event_handler = RegisterEventHandler(
- OnProcessStart(
- target_action=ros2_control_node,
- on_start=[
- joint_state_broadcaster,
- force_torque_broadcaster,
- lbr_state_broadcaster,
- controller,
- ],
- )
- )
- ld.add_action(controller_event_handler)
-
- # MoveIt 2
- ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution())
- ld.add_action(LBRMoveGroupMixin.arg_capabilities())
- ld.add_action(LBRMoveGroupMixin.arg_disable_capabilities())
- ld.add_action(LBRMoveGroupMixin.arg_monitor_dynamics())
- ld.add_action(LBRMoveGroupMixin.args_publish_monitored_planning_scene())
-
- # MoveGroup:
- # - requires world frame
- # - urdf only has robot_name/world
- # This transform needs publishing
- robot_name = LaunchConfiguration("robot_name").perform(context)
- ld.add_action(
- LBRDescriptionMixin.node_static_tf(
- tf=world_robot_tf,
- parent="world",
- child=PathJoinSubstitution(
- [
- robot_name,
- "world",
- ] # results in robot_name/world
- ),
- )
- )
-
- model = LaunchConfiguration("model").perform(context)
- moveit_configs_builder = LBRMoveGroupMixin.moveit_configs_builder(
- robot_name=model,
- package_name=f"{model}_moveit_config",
- )
- move_group_params = LBRMoveGroupMixin.params_move_group()
-
- ld.add_action(
- LBRMoveGroupMixin.node_move_group(
- parameters=[
- moveit_configs_builder.to_dict(),
- move_group_params,
- {"use_sim_time": False},
- ],
- condition=IfCondition(LaunchConfiguration("moveit")),
- namespace=robot_name,
- )
- )
-
- # RViz and MoveIt
- rviz_moveit = RVizMixin.node_rviz(
- rviz_config_pkg=f"{model}_moveit_config",
- rviz_config="config/moveit.rviz",
- parameters=LBRMoveGroupMixin.params_rviz(
- moveit_configs=moveit_configs_builder.to_moveit_configs()
- )
- + [{"use_sim_time": False}],
- condition=IfCondition(
- AndSubstitution(LaunchConfiguration("moveit"), LaunchConfiguration("rviz"))
- ),
- remappings=[
- ("display_planned_path", robot_name + "/display_planned_path"),
- ("joint_states", robot_name + "/joint_states"),
- ("monitored_planning_scene", robot_name + "/monitored_planning_scene"),
- ("planning_scene", robot_name + "/planning_scene"),
- ("planning_scene_world", robot_name + "/planning_scene_world"),
- ("robot_description", robot_name + "/robot_description"),
- ("robot_description_semantic", robot_name + "/robot_description_semantic"),
- ],
- )
-
- # RViz no MoveIt
- rviz = RVizMixin.node_rviz(
- rviz_config_pkg="lbr_bringup",
- rviz_config="config/config.rviz",
- condition=IfCondition(
- AndSubstitution(
- LaunchConfiguration("rviz"),
- NotSubstitution(LaunchConfiguration("moveit")),
- )
- ),
- )
-
- # RViz event handler
- rviz_event_handler = RegisterEventHandler(
- OnProcessStart(
- target_action=joint_state_broadcaster, on_start=[rviz_moveit, rviz]
- )
- )
- ld.add_action(rviz_event_handler)
-
- return ld.entities
-
-
-def generate_launch_description() -> LaunchDescription:
- ld = LaunchDescription()
- ld.add_action(LBRDescriptionMixin.arg_model())
- ld.add_action(LBRDescriptionMixin.arg_robot_name())
- ld.add_action(LBRDescriptionMixin.arg_port_id())
- ld.add_action(
- DeclareLaunchArgument(
- name="moveit",
- default_value="false",
- description="Whether to launch MoveIt 2.",
- )
- )
- ld.add_action(
- DeclareLaunchArgument(
- name="rviz", default_value="true", description="Whether to launch RViz."
- )
- )
- ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg())
- ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg())
- ld.add_action(LBRROS2ControlMixin.arg_ctrl())
- ld.add_action(OpaqueFunction(function=launch_setup))
- return ld
diff --git a/lbr_bringup/launch/rviz.launch.py b/lbr_bringup/launch/rviz.launch.py
new file mode 100644
index 00000000..cacc2faf
--- /dev/null
+++ b/lbr_bringup/launch/rviz.launch.py
@@ -0,0 +1,14 @@
+from launch import LaunchDescription
+from lbr_bringup.rviz import RVizMixin
+
+
+def generate_launch_description() -> LaunchDescription:
+ ld = LaunchDescription()
+
+ # launch arguments
+ ld.add_action(RVizMixin.arg_rviz_config())
+ ld.add_action(RVizMixin.arg_rviz_config_pkg())
+
+ # rviz
+ ld.add_action(RVizMixin.node_rviz())
+ return ld
diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/sim.launch.py
deleted file mode 100644
index edac639d..00000000
--- a/lbr_bringup/launch/sim.launch.py
+++ /dev/null
@@ -1,150 +0,0 @@
-from typing import List
-
-from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler
-from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import (
- AndSubstitution,
- LaunchConfiguration,
- NotSubstitution,
- PathJoinSubstitution,
-)
-from launch_mixins.lbr_bringup import LBRMoveGroupMixin
-from launch_mixins.lbr_description import GazeboMixin, LBRDescriptionMixin, RVizMixin
-from launch_mixins.lbr_ros2_control import LBRROS2ControlMixin
-
-
-def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
- ld = LaunchDescription()
-
- robot_description = LBRDescriptionMixin.param_robot_description(sim=True)
- world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero
- ld.add_action(GazeboMixin.include_gazebo()) # Gazebo has its own controller manager
- spawn_entity = GazeboMixin.node_spawn_entity(tf=world_robot_tf)
- ld.add_action(spawn_entity)
- joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
- controller="joint_state_broadcaster"
- )
- ld.add_action(joint_state_broadcaster)
- robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
- robot_description=robot_description, use_sim_time=True
- )
- ld.add_action(
- robot_state_publisher
- ) # Do not condition robot state publisher on joint state broadcaster as Gazebo uses robot state publisher to retrieve robot description
- ld.add_action(
- LBRROS2ControlMixin.node_controller_spawner(
- controller=LaunchConfiguration("ctrl")
- )
- )
-
- # MoveIt 2
- ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution())
- ld.add_action(LBRMoveGroupMixin.arg_capabilities())
- ld.add_action(LBRMoveGroupMixin.arg_disable_capabilities())
- ld.add_action(LBRMoveGroupMixin.arg_monitor_dynamics())
- ld.add_action(LBRMoveGroupMixin.args_publish_monitored_planning_scene())
-
- # MoveGroup:
- # - requires world frame
- # - urdf only has robot_name/world
- # This transform needs publishing
- robot_name = LaunchConfiguration("robot_name").perform(context)
- ld.add_action(
- LBRDescriptionMixin.node_static_tf(
- tf=world_robot_tf,
- parent="world",
- child=PathJoinSubstitution(
- [
- robot_name,
- "world",
- ] # results in robot_name/world
- ),
- parameters=[{"use_sim_time": True}],
- )
- )
-
- model = LaunchConfiguration("model").perform(context)
- moveit_configs_builder = LBRMoveGroupMixin.moveit_configs_builder(
- robot_name=model,
- package_name=f"{model}_moveit_config",
- )
- move_group_params = LBRMoveGroupMixin.params_move_group()
-
- ld.add_action(
- LBRMoveGroupMixin.node_move_group(
- parameters=[
- moveit_configs_builder.to_dict(),
- move_group_params,
- {"use_sim_time": True},
- ],
- condition=IfCondition(LaunchConfiguration("moveit")),
- namespace=robot_name,
- )
- )
-
- # RViz and MoveIt
- rviz_moveit = RVizMixin.node_rviz(
- rviz_config_pkg=f"{model}_moveit_config",
- rviz_config="config/moveit.rviz",
- parameters=LBRMoveGroupMixin.params_rviz(
- moveit_configs=moveit_configs_builder.to_moveit_configs()
- )
- + [{"use_sim_time": True}],
- condition=IfCondition(
- AndSubstitution(LaunchConfiguration("moveit"), LaunchConfiguration("rviz"))
- ),
- remappings=[
- ("display_planned_path", robot_name + "/display_planned_path"),
- ("joint_states", robot_name + "/joint_states"),
- ("monitored_planning_scene", robot_name + "/monitored_planning_scene"),
- ("planning_scene", robot_name + "/planning_scene"),
- ("planning_scene_world", robot_name + "/planning_scene_world"),
- ("robot_description", robot_name + "/robot_description"),
- ("robot_description_semantic", robot_name + "/robot_description_semantic"),
- ],
- )
-
- # RViz no MoveIt
- rviz = RVizMixin.node_rviz(
- rviz_config_pkg="lbr_bringup",
- rviz_config="config/config.rviz",
- condition=IfCondition(
- AndSubstitution(
- LaunchConfiguration("rviz"),
- NotSubstitution(LaunchConfiguration("moveit")),
- )
- ),
- )
-
- # RViz event handler
- rviz_event_handler = RegisterEventHandler(
- OnProcessExit(target_action=spawn_entity, on_exit=[rviz_moveit, rviz])
- )
- ld.add_action(rviz_event_handler)
-
- return ld.entities
-
-
-def generate_launch_description() -> LaunchDescription:
- ld = LaunchDescription()
- ld.add_action(LBRDescriptionMixin.arg_model())
- ld.add_action(LBRDescriptionMixin.arg_robot_name())
- ld.add_action(
- DeclareLaunchArgument(
- name="moveit",
- default_value="false",
- description="Whether to launch MoveIt 2.",
- )
- )
- ld.add_action(
- DeclareLaunchArgument(
- name="rviz", default_value="true", description="Whether to launch RViz."
- )
- )
- ld.add_action(
- LBRROS2ControlMixin.arg_ctrl()
- ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml
- ld.add_action(OpaqueFunction(function=launch_setup))
- return ld
diff --git a/lbr_bringup/launch/system_interface.launch.py b/lbr_bringup/launch/system_interface.launch.py
deleted file mode 100644
index efc53910..00000000
--- a/lbr_bringup/launch/system_interface.launch.py
+++ /dev/null
@@ -1,53 +0,0 @@
-from launch import LaunchDescription
-from launch.actions import RegisterEventHandler
-from launch.event_handlers import OnProcessStart
-from launch.substitutions import LaunchConfiguration
-from launch_mixins.lbr_description import LBRDescriptionMixin
-from launch_mixins.lbr_ros2_control import LBRROS2ControlMixin
-
-
-class LBRSystemInterface(LBRDescriptionMixin, LBRROS2ControlMixin):
- pass
-
-
-def generate_launch_description() -> LaunchDescription:
- ld = LaunchDescription()
- ld.add_action(LBRSystemInterface.arg_model())
- ld.add_action(LBRSystemInterface.arg_robot_name())
- ld.add_action(LBRSystemInterface.arg_port_id())
- robot_description = LBRSystemInterface.param_robot_description(sim=False)
- ld.add_action(LBRSystemInterface.arg_ctrl_cfg_pkg())
- ld.add_action(LBRSystemInterface.arg_ctrl_cfg())
- ld.add_action(LBRSystemInterface.arg_ctrl())
- robot_state_publisher = LBRSystemInterface.node_robot_state_publisher(
- robot_description=robot_description,
- use_sim_time=False,
- )
- ld.add_action(robot_state_publisher)
- ros2_control_node = LBRSystemInterface.node_ros2_control()
- ld.add_action(ros2_control_node)
- joint_state_broadcaster = LBRSystemInterface.node_controller_spawner(
- controller="joint_state_broadcaster"
- )
- force_torque_broadcaster = LBRSystemInterface.node_controller_spawner(
- controller="force_torque_broadcaster"
- )
- lbr_state_broadcaster = LBRSystemInterface.node_controller_spawner(
- controller="lbr_state_broadcaster"
- )
- controller = LBRSystemInterface.node_controller_spawner(
- controller=LaunchConfiguration("ctrl")
- )
- controller_event_handler = RegisterEventHandler(
- OnProcessStart(
- target_action=ros2_control_node,
- on_start=[
- joint_state_broadcaster,
- force_torque_broadcaster,
- lbr_state_broadcaster,
- controller,
- ],
- )
- )
- ld.add_action(controller_event_handler)
- return ld
diff --git a/lbr_bringup/lbr_bringup/__init__.py b/lbr_bringup/lbr_bringup/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/lbr_bringup/launch_mixins/lbr_description.py b/lbr_bringup/lbr_bringup/description.py
similarity index 52%
rename from lbr_bringup/launch_mixins/lbr_description.py
rename to lbr_bringup/lbr_bringup/description.py
index c91f2f7e..8d612d5a 100644
--- a/lbr_bringup/launch_mixins/lbr_description.py
+++ b/lbr_bringup/lbr_bringup/description.py
@@ -1,7 +1,6 @@
from typing import Dict, List, Optional, Union
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import DeclareLaunchArgument
from launch.substitutions import (
Command,
FindExecutable,
@@ -12,48 +11,6 @@
from launch_ros.substitutions import FindPackageShare
-class GazeboMixin:
- @staticmethod
- def include_gazebo(**kwargs) -> IncludeLaunchDescription:
- return IncludeLaunchDescription(
- PythonLaunchDescriptionSource(
- PathJoinSubstitution(
- [
- FindPackageShare("gazebo_ros"),
- "launch",
- "gazebo.launch.py",
- ]
- )
- ),
- **kwargs,
- )
-
- @staticmethod
- def node_spawn_entity(
- robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
- "robot_name", default="lbr"
- ),
- tf: List[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- **kwargs,
- ) -> Node:
- label = ["-x", "-y", "-z", "-R", "-P", "-Y"]
- tf = [str(x) for x in tf]
- return Node(
- package="gazebo_ros",
- executable="spawn_entity.py",
- arguments=[
- "-topic",
- "robot_description",
- "-entity",
- robot_name,
- ]
- + [item for pair in zip(label, tf) for item in pair],
- output="screen",
- namespace=robot_name,
- **kwargs,
- )
-
-
class LBRDescriptionMixin:
@staticmethod
def param_robot_description(
@@ -66,12 +23,10 @@ def param_robot_description(
port_id: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
"port_id", default="30200"
),
- sim: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration(
- "sim", default="true"
+ mode: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration(
+ "mode", default="mock"
),
) -> Dict[str, str]:
- if type(sim) is bool:
- sim = "true" if sim else "false"
robot_description = {
"robot_description": Command(
[
@@ -90,8 +45,8 @@ def param_robot_description(
robot_name,
" port_id:=",
port_id,
- " sim:=",
- sim,
+ " mode:=",
+ mode,
]
)
}
@@ -124,11 +79,16 @@ def arg_port_id(default_value: str = "30200") -> DeclareLaunchArgument:
)
@staticmethod
- def arg_sim(default_value: str = "true") -> DeclareLaunchArgument:
+ def arg_mode(default_value: str = "mock") -> DeclareLaunchArgument:
return DeclareLaunchArgument(
- name="sim",
+ name="mode",
default_value=default_value,
- description="Whether to use the simulation or not.",
+ description="The mode to launch in.",
+ choices=[
+ "mock",
+ "hardware",
+ "gazebo",
+ ],
)
@staticmethod
@@ -140,8 +100,8 @@ def param_port_id() -> Dict[str, LaunchConfiguration]:
return {"port_id": LaunchConfiguration("port_id", default="30200")}
@staticmethod
- def param_sim() -> Dict[str, LaunchConfiguration]:
- return {"sim": LaunchConfiguration("sim", default="true")}
+ def param_mode() -> Dict[str, LaunchConfiguration]:
+ return {"mode": LaunchConfiguration("mode", default="mock")}
@staticmethod
def node_static_tf(
@@ -150,6 +110,8 @@ def node_static_tf(
child: Optional[Union[LaunchConfiguration, str]] = None,
**kwargs,
) -> Node:
+ if len(tf) != 6:
+ raise ValueError("tf must be a list of 6 floats.")
label = ["--x", "--y", "--z", "--roll", "--pitch", "--yaw"]
tf = [str(x) for x in tf]
return Node(
@@ -165,51 +127,3 @@ def node_static_tf(
],
**kwargs,
)
-
-
-class RVizMixin:
- @staticmethod
- def arg_rviz_config_pkg(
- default_value: str = "lbr_description",
- ) -> DeclareLaunchArgument:
- return DeclareLaunchArgument(
- name="rviz_config_pkg",
- default_value=default_value,
- description="The RViz configuration file.",
- )
-
- @staticmethod
- def arg_rviz_config(
- default_value: str = "config/config.rviz",
- ) -> DeclareLaunchArgument:
- return DeclareLaunchArgument(
- name="rviz_config",
- default_value=default_value,
- description="The RViz configuration file.",
- )
-
- @staticmethod
- def node_rviz(
- rviz_config_pkg: Optional[
- Union[LaunchConfiguration, str]
- ] = LaunchConfiguration("rviz_config_pkg", default="lbr_description"),
- rviz_config: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
- "rviz_config", default="config/config.rviz"
- ),
- **kwargs,
- ) -> Node:
- return Node(
- package="rviz2",
- executable="rviz2",
- name="rviz2",
- arguments=[
- "-d",
- PathJoinSubstitution(
- [
- FindPackageShare(rviz_config_pkg),
- rviz_config,
- ]
- ),
- ],
- **kwargs,
- )
diff --git a/lbr_bringup/lbr_bringup/gazebo.py b/lbr_bringup/lbr_bringup/gazebo.py
new file mode 100644
index 00000000..145e052d
--- /dev/null
+++ b/lbr_bringup/lbr_bringup/gazebo.py
@@ -0,0 +1,49 @@
+from typing import List, Optional, Union
+
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+class GazeboMixin:
+ @staticmethod
+ def include_gazebo(**kwargs) -> IncludeLaunchDescription:
+ return IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ PathJoinSubstitution(
+ [
+ FindPackageShare("gazebo_ros"),
+ "launch",
+ "gazebo.launch.py",
+ ]
+ )
+ ),
+ **kwargs,
+ )
+
+ @staticmethod
+ def node_spawn_entity(
+ robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
+ "robot_name", default="lbr"
+ ),
+ tf: List[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ **kwargs,
+ ) -> Node:
+ label = ["-x", "-y", "-z", "-R", "-P", "-Y"]
+ tf = [str(x) for x in tf]
+ return Node(
+ package="gazebo_ros",
+ executable="spawn_entity.py",
+ arguments=[
+ "-topic",
+ "robot_description",
+ "-entity",
+ robot_name,
+ ]
+ + [item for pair in zip(label, tf) for item in pair],
+ output="screen",
+ namespace=robot_name,
+ **kwargs,
+ )
diff --git a/lbr_bringup/launch_mixins/lbr_bringup.py b/lbr_bringup/lbr_bringup/move_group.py
similarity index 100%
rename from lbr_bringup/launch_mixins/lbr_bringup.py
rename to lbr_bringup/lbr_bringup/move_group.py
diff --git a/lbr_bringup/launch_mixins/lbr_ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py
similarity index 95%
rename from lbr_bringup/launch_mixins/lbr_ros2_control.py
rename to lbr_bringup/lbr_bringup/ros2_control.py
index fd270768..a5a58627 100644
--- a/lbr_bringup/launch_mixins/lbr_ros2_control.py
+++ b/lbr_bringup/lbr_bringup/ros2_control.py
@@ -51,13 +51,16 @@ def node_ros2_control(
robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
"robot_name", default="lbr"
),
+ use_sim_time: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration(
+ "use_sim_time", default="false"
+ ),
**kwargs,
) -> Node:
return Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[
- {"use_sim_time": False},
+ {"use_sim_time": use_sim_time},
PathJoinSubstitution(
[
FindPackageShare(
diff --git a/lbr_bringup/lbr_bringup/rviz.py b/lbr_bringup/lbr_bringup/rviz.py
new file mode 100644
index 00000000..824bee74
--- /dev/null
+++ b/lbr_bringup/lbr_bringup/rviz.py
@@ -0,0 +1,62 @@
+from typing import Optional, Union
+
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+class RVizMixin:
+ @staticmethod
+ def arg_rviz(default_value: str = "false") -> DeclareLaunchArgument:
+ return DeclareLaunchArgument(
+ name="rviz",
+ default_value=default_value,
+ description="Whether to launch RViz.",
+ )
+
+ @staticmethod
+ def arg_rviz_config_pkg(
+ default_value: str = "lbr_bringup",
+ ) -> DeclareLaunchArgument:
+ return DeclareLaunchArgument(
+ name="rviz_config_pkg",
+ default_value=default_value,
+ description="The RViz configuration file.",
+ )
+
+ @staticmethod
+ def arg_rviz_config(
+ default_value: str = "config/mock.rviz",
+ ) -> DeclareLaunchArgument:
+ return DeclareLaunchArgument(
+ name="rviz_config",
+ default_value=default_value,
+ description="The RViz configuration file.",
+ )
+
+ @staticmethod
+ def node_rviz(
+ rviz_config_pkg: Optional[
+ Union[LaunchConfiguration, str]
+ ] = LaunchConfiguration("rviz_config_pkg", default="lbr_bringup"),
+ rviz_config: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
+ "rviz_config", default="config/mock.rviz"
+ ),
+ **kwargs,
+ ) -> Node:
+ return Node(
+ package="rviz2",
+ executable="rviz2",
+ name="rviz2",
+ arguments=[
+ "-d",
+ PathJoinSubstitution(
+ [
+ FindPackageShare(rviz_config_pkg),
+ rviz_config,
+ ]
+ ),
+ ],
+ **kwargs,
+ )
diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml
index cfd6656a..342ef1ef 100644
--- a/lbr_bringup/package.xml
+++ b/lbr_bringup/package.xml
@@ -2,7 +2,7 @@
lbr_bringup
- 2.0.0
+ 2.1.0
LBR launch files.
mhubii
Apache License 2.0
diff --git a/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt
index 14040864..b4a883db 100644
--- a/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt
+++ b/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt
@@ -20,34 +20,6 @@ find_package(orocos_kdl_vendor REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
-# admittance control node
-add_library(admittance_control_component
- SHARED
- src/admittance_control_node.cpp
-)
-
-target_include_directories(admittance_control_component
- PRIVATE src
-)
-
-ament_target_dependencies(admittance_control_component
- kdl_parser
- lbr_fri_idl
- lbr_fri_ros2
- orocos_kdl_vendor
- rclcpp
- rclcpp_components
-)
-
-target_link_libraries(admittance_control_component
- FRIClient::FRIClient
-)
-
-rclcpp_components_register_node(admittance_control_component
- PLUGIN lbr_demos::AdmittanceControlNode
- EXECUTABLE admittance_control
-)
-
# pose planning node
add_library(pose_planning_component
SHARED
@@ -99,7 +71,7 @@ rclcpp_components_register_node(pose_control_component
# install
install(
- TARGETS admittance_control_component pose_planning_component pose_control_component
+ TARGETS pose_planning_component pose_control_component
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
diff --git a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst
index e253e146..2cd26566 100644
--- a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst
+++ b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst
@@ -34,8 +34,7 @@ This demo implements a simple admittance controller.
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
- sim:=false \
+ ros2 launch lbr_bringup hardware.launch.py \
ctrl:=lbr_joint_position_command_controller \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
@@ -76,8 +75,7 @@ kinematics to move the robot's end-effector along the z-axis in Cartesian space.
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
- sim:=false \
+ ros2 launch lbr_bringup hardware.launch.py \
ctrl:=lbr_joint_position_command_controller \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
diff --git a/lbr_demos/lbr_demos_advanced_cpp/package.xml b/lbr_demos/lbr_demos_advanced_cpp/package.xml
index f8a74694..81e0b2e5 100644
--- a/lbr_demos/lbr_demos_advanced_cpp/package.xml
+++ b/lbr_demos/lbr_demos_advanced_cpp/package.xml
@@ -2,7 +2,7 @@
lbr_demos_advanced_cpp
- 2.0.0
+ 2.1.0
Advanced C++ demos for the lbr_ros2_control.
mhubii
Apache License 2.0
diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp
deleted file mode 100644
index e3e06741..00000000
--- a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp
+++ /dev/null
@@ -1,93 +0,0 @@
-#include
-
-#include "rclcpp/rclcpp.hpp"
-
-#include "lbr_fri_idl/msg/lbr_state.hpp"
-
-#include "admittance_controller.hpp"
-#include "lbr_base_position_command_node.hpp"
-
-namespace lbr_demos {
-class AdmittanceControlNode : public LBRBasePositionCommandNode {
-public:
- AdmittanceControlNode(const rclcpp::NodeOptions &options)
- : LBRBasePositionCommandNode("admittance_control", options) {
- this->declare_parameter("base_link", "link_0");
- this->declare_parameter("end_effector_link", "link_ee");
- this->declare_parameter>("f_ext_th", {2., 2., 2., 0.5, 0.5, 0.5});
- this->declare_parameter>("dq_gains", {2., 2., 2., 2., 2., 2., 2.});
- this->declare_parameter>("dx_gains", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1});
- this->declare_parameter("exp_smooth", 0.95);
-
- exp_smooth_ = this->get_parameter("exp_smooth").as_double();
- if (exp_smooth_ < 0. || exp_smooth_ > 1.) {
- throw std::runtime_error("Invalid exponential smoothing factor.");
- }
-
- admittance_controller_ = std::make_unique(
- this->robot_description_, this->get_parameter("base_link").as_string(),
- this->get_parameter("end_effector_link").as_string(),
- this->get_parameter("f_ext_th").as_double_array(),
- this->get_parameter("dq_gains").as_double_array(),
- this->get_parameter("dx_gains").as_double_array());
-
- // log parameters to terminal
- this->log_paramters_();
- }
-
-protected:
- void log_paramters_() {
- RCLCPP_INFO(this->get_logger(), "*** Parameters:");
- RCLCPP_INFO(this->get_logger(), "* base_link: %s",
- this->get_parameter("base_link").as_string().c_str());
- RCLCPP_INFO(this->get_logger(), "* end_effector_link: %s",
- this->get_parameter("end_effector_link").as_string().c_str());
- auto f_ext_th = this->get_parameter("f_ext_th").as_double_array();
- RCLCPP_INFO(this->get_logger(), "* f_ext_th: %.1f, %.1f, %.1f, %.1f, %.1f, %.1f", f_ext_th[0],
- f_ext_th[1], f_ext_th[2], f_ext_th[3], f_ext_th[4], f_ext_th[5]);
- auto dq_gains = this->get_parameter("dq_gains").as_double_array();
- RCLCPP_INFO(this->get_logger(), "* dq_gains: %.1f, %.1f, %.1f, %.1f, %.1f, %.1f, %.1f",
- dq_gains[0], dq_gains[1], dq_gains[2], dq_gains[3], dq_gains[4], dq_gains[5],
- dq_gains[6]);
- auto dx_gains = this->get_parameter("dx_gains").as_double_array();
- RCLCPP_INFO(this->get_logger(), "* dx_gains: %.1f, %.1f, %.1f, %.1f, %.1f, %.1f", dx_gains[0],
- dx_gains[1], dx_gains[2], dx_gains[3], dx_gains[4], dx_gains[5]);
- }
-
- void on_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) override {
- if (!lbr_state) {
- return;
- }
-
- smooth_lbr_state_(lbr_state);
-
- auto lbr_command = admittance_controller_->update(lbr_state_, dt_);
- lbr_joint_position_command_pub_->publish(lbr_command);
- };
-
- void smooth_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) {
- if (!init_) {
- lbr_state_ = *lbr_state;
- init_ = true;
- return;
- }
-
- for (int i = 0; i < 7; i++) {
- lbr_state_.measured_joint_position[i] =
- lbr_state->measured_joint_position[i] * (1 - exp_smooth_) +
- lbr_state_.measured_joint_position[i] * exp_smooth_;
- lbr_state_.external_torque[i] = lbr_state->external_torque[i] * (1 - exp_smooth_) +
- lbr_state_.external_torque[i] * exp_smooth_;
- }
- }
-
- double exp_smooth_;
- bool init_{false};
- lbr_fri_idl::msg::LBRState lbr_state_;
-
- std::unique_ptr admittance_controller_;
-};
-} // end of namespace lbr_demos
-
-#include "rclcpp_components/register_node_macro.hpp"
-RCLCPP_COMPONENTS_REGISTER_NODE(lbr_demos::AdmittanceControlNode)
diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp
deleted file mode 100644
index 3a12894d..00000000
--- a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp
+++ /dev/null
@@ -1,90 +0,0 @@
-#ifndef LBR_DEMOS_ADVANCED_CPP__ADMITTANCE_CONTROLLER_HPP_
-#define LBR_DEMOS_ADVANCED_CPP__ADMITTANCE_CONTROLLER_HPP_
-
-#include
-#include
-#include
-
-#include "kdl/chain.hpp"
-#include "kdl/chainjnttojacsolver.hpp"
-#include "kdl/tree.hpp"
-#include "kdl_parser/kdl_parser.hpp"
-
-#include "friLBRState.h"
-
-#include "lbr_fri_idl/msg/lbr_joint_position_command.hpp"
-#include "lbr_fri_idl/msg/lbr_state.hpp"
-#include "lbr_fri_ros2/pinv.hpp"
-
-namespace lbr_demos {
-class AdmittanceController {
- using joint_vector_t = Eigen::Vector;
- using cartesian_vector_t = Eigen::Vector;
-
-public:
- AdmittanceController(const std::string &robot_description,
- const std::string &base_link = "link_0",
- const std::string &end_effector_link = "link_ee",
- const std::vector &f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5},
- const std::vector &dq_gains = {2., 2., 2., 2., 2., 2., 2.},
- const std::vector &dx_gains = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1})
- : dq_gains_(dq_gains.data()), dx_gains_(dx_gains.data()), f_ext_th_(f_ext_th.data()) {
- if (!kdl_parser::treeFromString(robot_description, tree_)) {
- throw std::runtime_error("Failed to construct kdl tree from robot description.");
- }
- if (!tree_.getChain(base_link, end_effector_link, chain_)) {
- throw std::runtime_error("Failed to construct kdl chain from robot description.");
- }
-
- jacobian_solver_ = std::make_unique(chain_);
- jacobian_.resize(chain_.getNrOfJoints());
- q_.resize(chain_.getNrOfJoints());
- };
-
- const lbr_fri_idl::msg::LBRJointPositionCommand &
- update(const lbr_fri_idl::msg::LBRState &lbr_state, const double &dt) {
- std::memcpy(q_.data.data(), lbr_state.measured_joint_position.data(),
- sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
- std::memcpy(tau_ext_.data(), lbr_state.external_torque.data(),
- sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
-
- jacobian_solver_->JntToJac(q_, jacobian_);
-
- jacobian_inv_ = lbr_fri_ros2::pinv(jacobian_.data);
- f_ext_ = jacobian_inv_.transpose() * tau_ext_;
-
- for (int i = 0; i < 6; i++) {
- if (std::abs(f_ext_[i]) < f_ext_th_[i]) {
- f_ext_[i] = 0.;
- } else {
- f_ext_[i] = std::copysign(dx_gains_[i], f_ext_[i]) * (std::abs(f_ext_[i]) - f_ext_th_[i]);
- }
- }
-
- dq_ = dq_gains_.asDiagonal() * jacobian_inv_ * f_ext_;
-
- for (int i = 0; i < 7; i++) {
- lbr_joint_position_command_.joint_position[i] =
- lbr_state.measured_joint_position[i] + dq_[i] * dt;
- }
-
- return lbr_joint_position_command_;
- };
-
-protected:
- lbr_fri_idl::msg::LBRJointPositionCommand lbr_joint_position_command_;
- KDL::Tree tree_;
- KDL::Chain chain_;
- std::unique_ptr jacobian_solver_;
- KDL::Jacobian jacobian_;
- Eigen::Matrix jacobian_inv_;
- KDL::JntArray q_;
- joint_vector_t dq_;
- joint_vector_t tau_ext_;
- joint_vector_t dq_gains_;
- cartesian_vector_t dx_gains_;
- cartesian_vector_t f_ext_;
- cartesian_vector_t f_ext_th_;
-};
-} // end of namespace lbr_demos
-#endif // LBR_DEMOS_ADVANCED_CPP__ADMITTANCE_CONTROLLER_HPP_
diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp
index 8fec33db..e318586e 100644
--- a/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp
+++ b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp
@@ -64,5 +64,5 @@ class LBRBasePositionCommandNode : public rclcpp::Node {
return future.get()[0];
}
};
-} // end of namespace lbr_demos
+} // namespace lbr_demos
#endif // LBR_DEMOS_ADVANCED_CPP__LBR_BASE_POSITION_COMMAND_NODE_HPP_
diff --git a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst
index 2a70948d..fb37b371 100644
--- a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst
+++ b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst
@@ -34,8 +34,7 @@ This demo implements a simple admittance controller.
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
- sim:=false \
+ ros2 launch lbr_bringup hardware.launch.py \
ctrl:=lbr_joint_position_command_controller \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
@@ -75,8 +74,7 @@ This demo implements an admittance controller with a remote center of motion (RC
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
- sim:=false \
+ ros2 launch lbr_bringup hardware.launch.py \
ctrl:=lbr_joint_position_command_controller \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
diff --git a/lbr_demos/lbr_demos_advanced_py/package.xml b/lbr_demos/lbr_demos_advanced_py/package.xml
index cdd7ed21..ade1df84 100644
--- a/lbr_demos/lbr_demos_advanced_py/package.xml
+++ b/lbr_demos/lbr_demos_advanced_py/package.xml
@@ -2,7 +2,7 @@
lbr_demos_advanced_py
- 2.0.0
+ 2.1.0
Advanced Python demos for the lbr_ros2_control.
mhubii
cmower
diff --git a/lbr_demos/lbr_demos_advanced_py/setup.py b/lbr_demos/lbr_demos_advanced_py/setup.py
index 0a6e3644..d3dcdd20 100644
--- a/lbr_demos/lbr_demos_advanced_py/setup.py
+++ b/lbr_demos/lbr_demos_advanced_py/setup.py
@@ -6,7 +6,7 @@
setup(
name=package_name,
- version="2.0.0",
+ version="2.1.0",
packages=[package_name],
data_files=[
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
diff --git a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst
index 1fda2817..dafaa698 100644
--- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst
+++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst
@@ -34,9 +34,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
+ ros2 launch lbr_bringup hardware.launch.py \
ctrl:=lbr_joint_position_command_controller \
- sim:=false \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
#. Run the `joint_sine_overlay `_:octicon:`link-external` node:
@@ -57,8 +56,7 @@ Simulation
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
- sim:=true \
+ ros2 launch lbr_bringup gazebo.launch.py \
ctrl:=joint_trajectory_controller \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
@@ -90,7 +88,7 @@ Hardware
- ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL``
- ``FRI client command mode``: ``POSITION``
-#. Proceed with steps 1 and 2 from `Simulation`_ but with ``sim:=false``.
+#. Proceed with steps 1 and 2 from `Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py``.
LBR Torque Command Controller (Hardware only)
---------------------------------------------
@@ -118,9 +116,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
+ ros2 launch lbr_bringup hardware.launch.py \
ctrl:=lbr_torque_command_controller \
- sim:=false \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
#. Run the `torque_sine_overlay `_:octicon:`link-external` node:
@@ -157,9 +154,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
+ ros2 launch lbr_bringup hardware.launch.py \
ctrl:=lbr_wrench_command_controller \
- sim:=false \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
#. Run the `wrench_sine_overlay `_:octicon:`link-external` node:
diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml
index 24b6f2c3..f3d80e96 100644
--- a/lbr_demos/lbr_demos_cpp/package.xml
+++ b/lbr_demos/lbr_demos_cpp/package.xml
@@ -2,7 +2,7 @@
lbr_demos_cpp
- 2.0.0
+ 2.1.0
C++ demos for lbr_ros2_control.
mhubii
Apache License 2.0
diff --git a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst
index 41e39938..c15ed340 100644
--- a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst
+++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst
@@ -34,9 +34,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
+ ros2 launch lbr_bringup hardware.launch.py \
ctrl:=lbr_joint_position_command_controller \
- sim:=false \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
#. Run the `joint_sine_overlay `_:octicon:`link-external` node:
@@ -57,8 +56,7 @@ Simulation
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
- sim:=true \
+ ros2 launch lbr_bringup gazebo.launch.py \
ctrl:=joint_trajectory_controller \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
@@ -90,7 +88,7 @@ Hardware
- ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL``
- ``FRI client command mode``: ``POSITION``
-#. Proceed with steps 1 and 2 from `Simulation`_ but with ``sim:=false``.
+#. Proceed with steps 1 and 2 from `Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py``.
LBR Torque Command Controller (Hardware only)
---------------------------------------------
@@ -118,9 +116,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
+ ros2 launch lbr_bringup hardware.launch.py \
ctrl:=lbr_torque_command_controller \
- sim:=false \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
#. Run the `torque_sine_overlay `_:octicon:`link-external` node:
@@ -157,9 +154,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
+ ros2 launch lbr_bringup hardware.launch.py \
ctrl:=lbr_wrench_command_controller \
- sim:=false \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
#. Run the `wrench_sine_overlay `_:octicon:`link-external` node:
diff --git a/lbr_demos/lbr_demos_py/package.xml b/lbr_demos/lbr_demos_py/package.xml
index 67ddd0c4..6829f32e 100644
--- a/lbr_demos/lbr_demos_py/package.xml
+++ b/lbr_demos/lbr_demos_py/package.xml
@@ -2,7 +2,7 @@
lbr_demos_py
- 2.0.0
+ 2.1.0
Python demos for lbr_ros2_control.
mhubii
Apache License 2.0
diff --git a/lbr_demos/lbr_demos_py/setup.py b/lbr_demos/lbr_demos_py/setup.py
index d79f3692..c3af4e8a 100644
--- a/lbr_demos/lbr_demos_py/setup.py
+++ b/lbr_demos/lbr_demos_py/setup.py
@@ -4,7 +4,7 @@
setup(
name=package_name,
- version="2.0.0",
+ version="2.1.0",
packages=[package_name],
data_files=[
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
diff --git a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst
index bf0fc249..d7b52b15 100644
--- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst
+++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst
@@ -17,9 +17,16 @@ Simulation
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
+ ros2 launch lbr_bringup mock.launch.py \
moveit:=true \
- sim:=true \
+ model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
+
+#. Run MoveIt:
+
+ .. code-block:: bash
+
+ ros2 launch lbr_moveit_cpp move_group.launch.py \
+ mode:=mock \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
#. Run the `hello_moveit `_:octicon:`link-external` node:
@@ -27,7 +34,7 @@ Simulation
.. code-block:: bash
ros2 launch lbr_moveit_cpp hello_moveit.launch.py \
- sim:=true \
+ mode:=mock \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
Hardware
@@ -50,7 +57,7 @@ Hardware
- ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL``
- ``FRI client command mode``: ``POSITION``
-#. Proceed with steps 1 and 2 from `Simulation`_ but with ``sim:=false``.
+#. Proceed with steps 1, 2 and 3 from `Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py``.
Examining the Code
~~~~~~~~~~~~~~~~~~
@@ -69,7 +76,7 @@ The ``MoveGroup`` configurations are parsed conveniently through a mixin:
.. code-block:: python
- from launch_mixins.lbr_bringup import LBRMoveGroupMixin
+ from lbr_bringup.move_group import LBRMoveGroupMixin
...
diff --git a/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py
index 685b7014..fa080dfe 100644
--- a/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py
+++ b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py
@@ -3,15 +3,19 @@
from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
from launch.actions import OpaqueFunction
from launch.substitutions import LaunchConfiguration
-from launch_mixins.lbr_bringup import LBRMoveGroupMixin
-from launch_mixins.lbr_description import LBRDescriptionMixin
from launch_ros.actions import Node
+from lbr_bringup.description import LBRDescriptionMixin
+from lbr_bringup.move_group import LBRMoveGroupMixin
-def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
+def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
ld = LaunchDescription()
model = LaunchConfiguration("model").perform(context)
+ mode = LaunchConfiguration("mode").perform(context)
+ use_sim_time = False
+ if mode == "gazebo":
+ use_sim_time = True
# generate moveit configs
moveit_configs = LBRMoveGroupMixin.moveit_configs_builder(
@@ -26,7 +30,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
executable="hello_moveit",
parameters=[
moveit_configs.to_dict(),
- {"use_sim_time": LaunchConfiguration("sim")},
+ {"use_sim_time": use_sim_time},
LBRDescriptionMixin.param_robot_name(),
],
)
@@ -38,9 +42,8 @@ def generate_launch_description() -> LaunchDescription:
ld = LaunchDescription()
ld.add_action(LBRDescriptionMixin.arg_model())
- ld.add_action(LBRDescriptionMixin.arg_robot_name())
- ld.add_action(LBRDescriptionMixin.arg_sim())
+ ld.add_action(LBRDescriptionMixin.arg_mode())
- ld.add_action(OpaqueFunction(function=launch_setup))
+ ld.add_action(OpaqueFunction(function=hidden_setup))
return ld
diff --git a/lbr_demos/lbr_moveit_cpp/package.xml b/lbr_demos/lbr_moveit_cpp/package.xml
index 8d5f2804..dbda4e53 100644
--- a/lbr_demos/lbr_moveit_cpp/package.xml
+++ b/lbr_demos/lbr_moveit_cpp/package.xml
@@ -2,7 +2,7 @@
lbr_moveit_cpp
- 2.0.0
+ 2.1.0
Demo for using MoveIt C++ API.
mhubii
Apache-2.0
diff --git a/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst b/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst
index d1358852..31654530 100644
--- a/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst
+++ b/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst
@@ -3,14 +3,14 @@ lbr_demos_moveit_py
.. warning::
On hardware, do always execute in ``T1`` mode first.
-MoveIt via RViZ
+MoveIt via RViz
-----------------
.. image:: img/iiwa7_moveit_rviz.png
:align: center
- :alt: MoveIt via RViZ
-**IIWA 7 R800 in RViZ**
+ :alt: MoveIt via RViz
+**IIWA 7 R800 in RViz**
-To run MoveIt via RViZ, simply follow:
+To run MoveIt via RViz, simply follow:
Simulation
~~~~~~~~~~
@@ -18,12 +18,19 @@ Simulation
.. code-block:: bash
- ros2 launch lbr_bringup bringup.launch.py \
- moveit:=true \
- sim:=true \
+ ros2 launch lbr_bringup mock.launch.py \
model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
-#. You can now move the robot via MoveIt in RViZ!
+#. Run MoveIt with RViz:
+
+ .. code-block:: bash
+
+ ros2 launch lbr_bringup move_group.launch.py \
+ mode:=mock \
+ rviz:=true \
+ model:=iiwa7 # [iiwa7, iiwa14, med7, med14]
+
+#. You can now move the robot via MoveIt in RViz!
Hardware
~~~~~~~~
@@ -45,4 +52,4 @@ Hardware
- ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL``
- ``FRI client command mode``: ``POSITION``
-#. Proceed with steps 1 and 2 from `Simulation`_ but with ``sim:=false``.
+#. Proceed with steps 1 and 2 from `Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py``.
diff --git a/lbr_description/CMakeLists.txt b/lbr_description/CMakeLists.txt
index b2f9ac44..6debb078 100644
--- a/lbr_description/CMakeLists.txt
+++ b/lbr_description/CMakeLists.txt
@@ -11,7 +11,7 @@ find_package(ament_cmake_pytest REQUIRED)
# install
install(
- DIRECTORY config gazebo meshes urdf
+ DIRECTORY gazebo meshes urdf
DESTINATION share/${PROJECT_NAME}
)
diff --git a/lbr_description/package.xml b/lbr_description/package.xml
index f7445f5a..93829da1 100644
--- a/lbr_description/package.xml
+++ b/lbr_description/package.xml
@@ -2,7 +2,7 @@
lbr_description
- 2.0.0
+ 2.1.0
KUKA LBR description files
mhubii
Apache License 2.0
diff --git a/lbr_description/urdf/iiwa14/iiwa14.xacro b/lbr_description/urdf/iiwa14/iiwa14.xacro
index e936c2a6..7746f30b 100644
--- a/lbr_description/urdf/iiwa14/iiwa14.xacro
+++ b/lbr_description/urdf/iiwa14/iiwa14.xacro
@@ -7,7 +7,7 @@
-
+
@@ -25,6 +25,6 @@
\ No newline at end of file
diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.xacro
index 7e63d77f..712fc099 100644
--- a/lbr_description/urdf/iiwa14/iiwa14_description.xacro
+++ b/lbr_description/urdf/iiwa14/iiwa14_description.xacro
@@ -3,7 +3,7 @@
+ params="robot_name:=^|lbr mode:=^|mock system_parameters_path:=^|'$(find lbr_ros2_control)/config/lbr_system_parameters.yaml'">
@@ -19,10 +19,10 @@
-
+
diff --git a/lbr_description/urdf/iiwa7/iiwa7.xacro b/lbr_description/urdf/iiwa7/iiwa7.xacro
index a451cf70..501160d4 100644
--- a/lbr_description/urdf/iiwa7/iiwa7.xacro
+++ b/lbr_description/urdf/iiwa7/iiwa7.xacro
@@ -7,7 +7,7 @@
-
+
@@ -25,6 +25,6 @@
\ No newline at end of file
diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.xacro
index 6f4688c4..d177bb80 100644
--- a/lbr_description/urdf/iiwa7/iiwa7_description.xacro
+++ b/lbr_description/urdf/iiwa7/iiwa7_description.xacro
@@ -3,7 +3,7 @@
+ params="robot_name:=^|lbr mode:=^|mock system_parameters_path:=^|'$(find lbr_ros2_control)/config/lbr_system_parameters.yaml'">
@@ -19,10 +19,10 @@
-
+
diff --git a/lbr_description/urdf/med14/med14.xacro b/lbr_description/urdf/med14/med14.xacro
index c3abc0e9..89881aaa 100644
--- a/lbr_description/urdf/med14/med14.xacro
+++ b/lbr_description/urdf/med14/med14.xacro
@@ -7,7 +7,7 @@
-
+
@@ -25,6 +25,6 @@
\ No newline at end of file
diff --git a/lbr_description/urdf/med14/med14_description.xacro b/lbr_description/urdf/med14/med14_description.xacro
index e0fc40e2..0eac46fe 100644
--- a/lbr_description/urdf/med14/med14_description.xacro
+++ b/lbr_description/urdf/med14/med14_description.xacro
@@ -3,7 +3,7 @@
+ params="robot_name:=^|lbr mode:=^|mock system_parameters_path:=^|'$(find lbr_ros2_control)/config/lbr_system_parameters.yaml'">
@@ -19,10 +19,10 @@
-
+
diff --git a/lbr_description/urdf/med7/med7.xacro b/lbr_description/urdf/med7/med7.xacro
index 0e2fe380..e34caeb8 100644
--- a/lbr_description/urdf/med7/med7.xacro
+++ b/lbr_description/urdf/med7/med7.xacro
@@ -7,7 +7,7 @@
-
+
@@ -25,6 +25,6 @@
\ No newline at end of file
diff --git a/lbr_description/urdf/med7/med7_description.xacro b/lbr_description/urdf/med7/med7_description.xacro
index 43ef52df..58c885c4 100644
--- a/lbr_description/urdf/med7/med7_description.xacro
+++ b/lbr_description/urdf/med7/med7_description.xacro
@@ -3,7 +3,7 @@
+ params="robot_name:=^|lbr mode:=^|mock system_parameters_path:=^|'$(find lbr_ros2_control)/config/lbr_system_parameters.yaml'">
@@ -19,10 +19,10 @@
-
+
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp
index 3aea94f2..f66a8af0 100644
--- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp
+++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp
@@ -39,5 +39,5 @@ class App {
std::unique_ptr connection_ptr_;
std::unique_ptr app_ptr_;
};
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__APP_HPP_
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp
index 4c559905..db194729 100644
--- a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp
+++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp
@@ -50,5 +50,5 @@ class AsyncClient : public KUKA::FRI::LBRClient {
bool open_loop_;
};
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__ASYNC_CLIENT_HPP_
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp
index 7dcaa91d..9b963266 100644
--- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp
+++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp
@@ -76,5 +76,5 @@ class SafeStopCommandGuard : public CommandGuard {
std::unique_ptr
command_guard_factory(const CommandGuardParameters &command_guard_parameters,
const std::string &variant = "default");
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__COMMAND_GUARD_HPP_
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp
index bf72faa3..f14d5915 100644
--- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp
+++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp
@@ -139,5 +139,5 @@ class JointPIDArray {
PIDParameters pid_parameters_; /**< PID parameters for all joints.*/
pid_array_t pid_controllers_; /**< PID controllers for each joint.*/
};
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__FILTERS_HPP_
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp
index f605d599..a092b68a 100644
--- a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp
+++ b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp
@@ -76,5 +76,5 @@ struct EnumMaps {
}
};
};
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__ENUM_MAPS_HPP
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp
index 57c81292..b4257a9e 100644
--- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp
+++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp
@@ -66,5 +66,5 @@ class FTEstimator {
Eigen::Matrix tau_ext_;
Eigen::Matrix f_ext_;
};
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__FT_ESTIMATOR_HPP_
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp
index d2f345c5..24e8a324 100644
--- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp
+++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp
@@ -18,5 +18,5 @@ class PositionCommandInterface : public BaseCommandInterface {
void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) override;
};
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__INTERFACES__POSITION_COMMAND_HPP_
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp
index 9191d642..1a943bd7 100644
--- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp
+++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp
@@ -55,5 +55,5 @@ class StateInterface {
StateInterfaceParameters parameters_;
JointExponentialFilterArray external_torque_filter_, measured_torque_filter_;
};
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__INTERFACES__STATE_HPP_
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp
index fab800eb..e4ec7874 100644
--- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp
+++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp
@@ -18,5 +18,5 @@ class TorqueCommandInterface : public BaseCommandInterface {
void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) override;
};
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__INTERFACES__TORQUE_COMMAND_HPP_
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp
index 69b55b1a..0c846ee4 100644
--- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp
+++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp
@@ -18,5 +18,5 @@ class WrenchCommandInterface : public BaseCommandInterface {
void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) override;
};
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__INTERFACES__WRENCH_COMMAND_HPP_
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp
index 1616c27e..8d5d0b16 100644
--- a/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp
+++ b/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp
@@ -25,5 +25,5 @@ pinv(const MatT &mat,
});
return svd.matrixV() * dampedSingularValuesInv * svd.matrixU().adjoint();
}
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__PINV_HPP_
diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml
index a009f5d5..d5d2cc6c 100644
--- a/lbr_fri_ros2/package.xml
+++ b/lbr_fri_ros2/package.xml
@@ -2,7 +2,7 @@
lbr_fri_ros2
- 2.0.0
+ 2.1.0
The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS
2. Robot states can be extracted and commanded.
mhubii
diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp
index 54abde3f..7642a814 100644
--- a/lbr_fri_ros2/src/app.cpp
+++ b/lbr_fri_ros2/src/app.cpp
@@ -132,4 +132,4 @@ bool App::valid_port_(const int &port_id) {
}
return true;
}
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp
index 8a9bd587..14ad96bf 100644
--- a/lbr_fri_ros2/src/async_client.cpp
+++ b/lbr_fri_ros2/src/async_client.cpp
@@ -91,4 +91,4 @@ void AsyncClient::command() {
state_interface_ptr_->get_state()); // current state accessed via state interface (allows for
// open loop and is statically sized)
}
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp
index 8ba1e04a..92d06e5b 100644
--- a/lbr_fri_ros2/src/command_guard.cpp
+++ b/lbr_fri_ros2/src/command_guard.cpp
@@ -110,4 +110,4 @@ command_guard_factory(const CommandGuardParameters &command_guard_parameters,
ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC);
throw std::runtime_error(err);
}
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp
index f2e98052..4047e59a 100644
--- a/lbr_fri_ros2/src/filters.cpp
+++ b/lbr_fri_ros2/src/filters.cpp
@@ -80,4 +80,4 @@ void JointPIDArray::log_info() const {
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* antiwindup: %s",
pid_parameters_.antiwindup ? "true" : "false");
};
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp
index 35929be8..8ac0981c 100644
--- a/lbr_fri_ros2/src/ft_estimator.cpp
+++ b/lbr_fri_ros2/src/ft_estimator.cpp
@@ -56,4 +56,4 @@ void FTEstimator::reset() {
tau_ext_.setZero();
f_ext_.setZero();
}
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp
index b9eef3ef..b80530f3 100644
--- a/lbr_fri_ros2/src/interfaces/position_command.cpp
+++ b/lbr_fri_ros2/src/interfaces/position_command.cpp
@@ -57,4 +57,4 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command
// write joint position to output
command.setJointPosition(command_.joint_position.data());
}
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp
index f731fc9b..b658b266 100644
--- a/lbr_fri_ros2/src/interfaces/torque_command.cpp
+++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp
@@ -48,4 +48,4 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
command.setJointPosition(command_.joint_position.data());
command.setTorque(command_.torque.data());
}
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp
index 9ab34524..9dd9618a 100644
--- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp
+++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp
@@ -47,4 +47,4 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command,
command.setJointPosition(command_.joint_position.data());
command.setWrench(command_.wrench.data());
}
-} // end of namespace lbr_fri_ros2
+} // namespace lbr_fri_ros2
diff --git a/lbr_fri_ros2_stack/doc/hardware_setup.rst b/lbr_fri_ros2_stack/doc/hardware_setup.rst
index 15754c42..b7b63053 100644
--- a/lbr_fri_ros2_stack/doc/hardware_setup.rst
+++ b/lbr_fri_ros2_stack/doc/hardware_setup.rst
@@ -87,11 +87,12 @@ Install Applications to the Robot
.. thumbnail:: img/computer/01_lbr_fri_ros2_create_package_name.png
- #. .. dropdown:: Open a `Windows Terminal `_:octicon:`link-external` and clone the ``fri`` package
+ #. .. dropdown:: Open a `Windows Terminal `_:octicon:`link-external` and clone the ``fri`` package (make sure to replace ``$FRI_CLIENT_VERSION=1.15`` with your FRI version, which must be available as branch `here `_:octicon:`link-external`)
.. code-block:: bash
- git clone https://github.com/lbr-stack/fri.git -b ros2-fri-1.15 $HOME\Downloads\fri
+ $FRI_CLIENT_VERSION=1.15
+ git clone https://github.com/lbr-stack/fri.git -b fri-$FRI_CLIENT_VERSION $HOME\Downloads\fri
#. .. dropdown:: Open a `Windows Terminal `_:octicon:`link-external` as ``Administrator`` and create a symbolic link to ``LBRServer.java``
diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/package.xml
index a072dd50..8025516c 100644
--- a/lbr_fri_ros2_stack/package.xml
+++ b/lbr_fri_ros2_stack/package.xml
@@ -2,7 +2,7 @@
lbr_fri_ros2_stack
- 2.0.0
+ 2.1.0
ROS 2 stack for KUKA LBRs.
mhubii
Apache License 2.0
diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt
index 078cd0bf..6dc55918 100644
--- a/lbr_ros2_control/CMakeLists.txt
+++ b/lbr_ros2_control/CMakeLists.txt
@@ -29,10 +29,10 @@ find_package(realtime_tools REQUIRED)
add_library(
${PROJECT_NAME}
SHARED
- src/lbr_joint_position_command_controller.cpp
- src/lbr_torque_command_controller.cpp
- src/lbr_wrench_command_controller.cpp
- src/lbr_state_broadcaster.cpp
+ src/controllers/lbr_joint_position_command_controller.cpp
+ src/controllers/lbr_torque_command_controller.cpp
+ src/controllers/lbr_wrench_command_controller.cpp
+ src/controllers/lbr_state_broadcaster.cpp
src/system_interface.cpp
)
@@ -62,8 +62,7 @@ target_link_libraries(${PROJECT_NAME}
FRIClient::FRIClient
)
-pluginlib_export_plugin_description_file(controller_interface plugin_description_files/broadcasters.xml)
-pluginlib_export_plugin_description_file(controller_interface plugin_description_files/forward_command_controllers.xml)
+pluginlib_export_plugin_description_file(controller_interface plugin_description_files/controllers.xml)
pluginlib_export_plugin_description_file(hardware_interface plugin_description_files/system_interface.xml)
# Export for downstream usage, see https://docs.ros.org/en/foxy/Guides/Ament-CMake-Documentation.html
diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml
index a9e70793..c7939d25 100644
--- a/lbr_ros2_control/config/lbr_controllers.yaml
+++ b/lbr_ros2_control/config/lbr_controllers.yaml
@@ -2,7 +2,7 @@
# managers regardless of their namespace. Usefull in multi-robot setups.
/**/controller_manager:
ros__parameters:
- update_rate: 100
+ update_rate: 200
# ROS 2 control broadcasters
joint_state_broadcaster:
@@ -32,6 +32,10 @@
lbr_wrench_command_controller:
type: lbr_ros2_control/LBRWrenchCommandController
+ # Admittance controller
+ admittance_controller:
+ type: lbr_ros2_control/AdmittanceController
+
/**/force_torque_broadcaster:
ros__parameters:
frame_id: lbr/link_ee # namespace: https://github.com/ros2/rviz/issues/1103
diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro
index 3e9ec6cc..ab2e8664 100644
--- a/lbr_ros2_control/config/lbr_system_interface.xacro
+++ b/lbr_ros2_control/config/lbr_system_interface.xacro
@@ -1,19 +1,24 @@
+ params="mode joint_limits system_parameters_path">
-
-
+
+
+
+ mock_components/GenericSystem
+
+
+
gazebo_ros2_control/GazeboSystem
-
+
lbr_ros2_control::SystemInterface
${system_parameters['hardware']['fri_client_sdk']['major_version']}
@@ -33,11 +38,11 @@
${system_parameters['hardware']['measured_torque_cutoff_frequency']}
${system_parameters['hardware']['open_loop']}
-
+
-
+
@@ -83,16 +88,12 @@
-
+
+ params="name min_position max_position max_velocity max_torque mode">
- ${min_position}
- ${max_position}
- ${max_velocity}
- ${max_torque}
${min_position}
${max_position}
@@ -104,14 +105,18 @@
-
+
+ ${min_position}
+ ${max_position}
+ ${max_velocity}
+ ${max_torque}
-
+
@@ -120,43 +125,43 @@
max_position="${joint_limits['A1']['upper'] * PI / 180}"
max_velocity="${joint_limits['A1']['velocity'] * PI / 180}"
max_torque="${joint_limits['A1']['effort']}"
- sim="${sim}" />
+ mode="${mode}" />
+ mode="${mode}" />
+ mode="${mode}" />
+ mode="${mode}" />
+ mode="${mode}" />
+ mode="${mode}" />
+ mode="${mode}" />
\ No newline at end of file
diff --git a/lbr_ros2_control/doc/lbr_ros2_control.rst b/lbr_ros2_control/doc/lbr_ros2_control.rst
index 915dfcb0..36f2f7ba 100644
--- a/lbr_ros2_control/doc/lbr_ros2_control.rst
+++ b/lbr_ros2_control/doc/lbr_ros2_control.rst
@@ -34,18 +34,18 @@ The ``controller_manager::ControllerManager`` has a node, the `controller_manage
Hardware Plugin
---------------
-lbr_fri_ros2::SystemInterface
+lbr_ros2_control::SystemInterface
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-The ``lbr_fri_ros2::SystemInterface`` plugin implements a ``hardware_interface::SystemInterface`` and utilizes the :ref:`lbr_fri_ros2` package for communication with the robot. Overview :ref:`below ` (click to expand):
+The ``lbr_ros2_control::SystemInterface`` plugin implements a ``hardware_interface::SystemInterface`` and utilizes the :ref:`lbr_fri_ros2` package for communication with the robot. Overview :ref:`below ` (click to expand):
.. _lbr_ros2_control detailed software architecture figure:
.. thumbnail:: img/lbr_ros2_control_detailed_v2.0.0.svg
:alt: lbr_ros2_control_detailed
- The ``controller_manager::ControllerManager`` loads the correct plugin from the ```` tag in the ``robot_description``: `lbr_system_interface.xacro `_:octicon:`link-external`
-- FRI relevant parameters are forwarded to the ``lbr_fri_ros2::SystemInterface`` plugin from `lbr_system_paramters.yaml `_:octicon:`link-external`
+- FRI relevant parameters are forwarded to the ``lbr_ros2_control::SystemInterface`` plugin from `lbr_system_paramters.yaml `_:octicon:`link-external`
-The ``lbr_fri_ros2::SystemInterface`` is spun with the ``controller_manager`` node at a rate set by the ``update_rate`` parameter. The communication to the robot is run **asynchronously** at a rate set by the robot's sample time.
+The ``lbr_ros2_control::SystemInterface`` is spun with the ``controller_manager`` node at a rate set by the ``update_rate`` parameter. The communication to the robot is run **asynchronously** at a rate set by the robot's sample time.
**Why asynchronously**? KUKA designed the FRI that way, by adhering to this design choice, we can support multiple FRI versions, see :ref:`fri`!
diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp
new file mode 100644
index 00000000..6d28e3a1
--- /dev/null
+++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp
@@ -0,0 +1,63 @@
+#ifndef LBR_ROS2_CONTROL__ADMITTANCE_CONTROLLER_HPP_
+#define LBR_ROS2_CONTROL__ADMITTANCE_CONTROLLER_HPP_
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "controller_interface/controller_interface.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "friLBRState.h"
+#include "lbr_ros2_control/system_interface_type_values.hpp"
+
+namespace lbr_ros2_control {
+class Admittance {
+
+}
+
+class AdmittanceController : public controller_interface::ControllerInterface {
+ static constexpr uint8_t CARTESIAN_DOF = 6;
+
+public:
+ AdmittanceController();
+
+ controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+ controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+ controller_interface::CallbackReturn on_init() override;
+
+ controller_interface::return_type update(const rclcpp::Time &time,
+ const rclcpp::Duration &period) override;
+
+ controller_interface::CallbackReturn
+ on_configure(const rclcpp_lifecycle::State &previous_state) override;
+
+ controller_interface::CallbackReturn
+ on_activate(const rclcpp_lifecycle::State &previous_state) override;
+
+ controller_interface::CallbackReturn
+ on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
+
+protected:
+ bool reference_command_interfaces_();
+ bool reference_state_interfaces_();
+ void clear_command_interfaces_();
+ void clear_state_interfaces_();
+
+ std::array joint_names_ = {
+ "A1", "A2", "A3", "A4", "A5", "A6", "A7"};
+
+ std::vector>
+ joint_position_command_interfaces_;
+ std::vector>
+ estimated_ft_sensor_state_interface_;
+};
+} // namespace lbr_ros2_control
+#endif // LBR_ROS2_CONTROL__ADMITTANCE_CONTROLLER_HPP_
diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_joint_position_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp
similarity index 98%
rename from lbr_ros2_control/include/lbr_ros2_control/lbr_joint_position_command_controller.hpp
rename to lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp
index 5749b9f4..46eebecd 100644
--- a/lbr_ros2_control/include/lbr_ros2_control/lbr_joint_position_command_controller.hpp
+++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp
@@ -48,5 +48,5 @@ class LBRJointPositionCommandController : public controller_interface::Controlle
rclcpp::Subscription::SharedPtr
lbr_joint_position_command_subscription_ptr_;
};
-} // end of namespace lbr_ros2_control
+} // namespace lbr_ros2_control
#endif // LBR_ROS2_CONTROL__LBR_JOINT_POSITION_COMMAND_CONTROLLER_HPP_
diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp
similarity index 98%
rename from lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp
rename to lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp
index 15fa351c..877f71e6 100644
--- a/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp
+++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp
@@ -56,5 +56,5 @@ class LBRStateBroadcaster : public controller_interface::ControllerInterface {
std::shared_ptr>
rt_state_publisher_ptr_;
};
-} // end of namespace lbr_ros2_control
+} // namespace lbr_ros2_control
#endif // LBR_ROS2_CONTROL__LBR_STATE_BROADCASTER_HPP_
diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_torque_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp
similarity index 98%
rename from lbr_ros2_control/include/lbr_ros2_control/lbr_torque_command_controller.hpp
rename to lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp
index f86d1c25..7a4c5f7b 100644
--- a/lbr_ros2_control/include/lbr_ros2_control/lbr_torque_command_controller.hpp
+++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp
@@ -56,5 +56,5 @@ class LBRTorqueCommandController : public controller_interface::ControllerInterf
rclcpp::Subscription::SharedPtr
lbr_torque_command_subscription_ptr_;
};
-} // end of namespace lbr_ros2_control
+} // namespace lbr_ros2_control
#endif // LBR_ROS2_CONTROL__LBR_TORQUE_COMMAND_CONTROLLER_HPP_
diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp
similarity index 98%
rename from lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp
rename to lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp
index 183e4cf4..52b76f35 100644
--- a/lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp
+++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp
@@ -59,5 +59,5 @@ class LBRWrenchCommandController : public controller_interface::ControllerInterf
rclcpp::Subscription::SharedPtr
lbr_wrench_command_subscription_ptr_;
};
-} // end of namespace lbr_ros2_control
+} // namespace lbr_ros2_control
#endif // LBR_ROS2_CONTROL__LBR_WRENCH_COMMAND_CONTROLLER_HPP_
diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp
index 08a9b41f..ceb5157c 100644
--- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp
+++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp
@@ -166,5 +166,5 @@ class SystemInterface : public hardware_interface::SystemInterface {
// exposed command interfaces
lbr_fri_idl::msg::LBRCommand hw_lbr_command_;
};
-} // end of namespace lbr_ros2_control
+} // namespace lbr_ros2_control
#endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_HPP_
diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp
index 819e196f..bb750426 100644
--- a/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp
+++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp
@@ -38,5 +38,5 @@ constexpr char HW_IF_TORQUE_Z[] = "torque.z";
constexpr char HW_IF_WRENCH_PREFIX[] = "wrench";
constexpr char HW_IF_AUXILIARY_PREFIX[] = "auxiliary_sensor";
constexpr char HW_IF_ESTIMATED_FT_PREFIX[] = "estimated_ft_sensor";
-} // end of namespace lbr_ros2_control
+} // namespace lbr_ros2_control
#endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_TYPE_VALUES_HPP_
diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml
index 57fd12f8..08e32b2f 100644
--- a/lbr_ros2_control/package.xml
+++ b/lbr_ros2_control/package.xml
@@ -2,7 +2,7 @@
lbr_ros2_control
- 2.0.0
+ 2.1.0
ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI).
mhubii
Apache License 2.0
diff --git a/lbr_ros2_control/plugin_description_files/broadcasters.xml b/lbr_ros2_control/plugin_description_files/broadcasters.xml
deleted file mode 100644
index 0c2484f0..00000000
--- a/lbr_ros2_control/plugin_description_files/broadcasters.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-
-
-
- Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg.
-
-
\ No newline at end of file
diff --git a/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml b/lbr_ros2_control/plugin_description_files/controllers.xml
similarity index 67%
rename from lbr_ros2_control/plugin_description_files/forward_command_controllers.xml
rename to lbr_ros2_control/plugin_description_files/controllers.xml
index b5185800..82c4ae8e 100644
--- a/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml
+++ b/lbr_ros2_control/plugin_description_files/controllers.xml
@@ -1,5 +1,11 @@
+
+
+ Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg.
+
+
Forward command controller for LBRWrenchCommand message, see
lbr_fri_idl/msg/LBRWrenchCommand.msg.
+
+
+
+ A simple admittance controller.
+
\ No newline at end of file
diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp
new file mode 100644
index 00000000..147ad742
--- /dev/null
+++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp
@@ -0,0 +1,77 @@
+#include "lbr_ros2_control/controllers/admittance_controller.hpp"
+
+namespace lbr_ros2_control {
+AdmittanceController::AdmittanceController() {}
+
+controller_interface::InterfaceConfiguration
+AdmittanceController::command_interface_configuration() {
+ // reference joint position command interface
+}
+
+controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() {
+ // retrieve estimated ft state interface
+}
+
+controller_interface::CallbackReturn AdmittanceController::on_init() {}
+
+controller_interface::return_type AdmittanceController::update(const rclcpp::Time &time,
+ const rclcpp::Duration &period) {
+ // compute admittance
+ // add warning for high force-torques and refer to load data calibration
+}
+
+controller_interface::CallbackReturn
+AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) {
+ return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) {
+ if (!reference_command_interfaces_()) {
+ return controller_interface::CallbackReturn::ERROR;
+ }
+ if (!reference_state_interfaces_()) {
+ return controller_interface::CallbackReturn::ERROR;
+ }
+ return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+AdmittanceController::on_deactivate(const rclcpp_lifecycle::State &previous_state) {
+ clear_command_interfaces_();
+ clear_state_interfaces_();
+ return controller_interface::CallbackReturn::SUCCESS;
+}
+
+bool AdmittanceController::reference_command_interfaces_() {
+ for (auto &command_interface : command_interfaces_) {
+ if (command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) {
+ joint_position_command_interfaces_.emplace_back(std::ref(command_interface));
+ }
+ }
+ if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+ RCLCPP_ERROR(
+ this->get_node()->get_logger(),
+ "Number of joint position command interfaces '%ld' does not match the number of joints "
+ "in the robot '%d'.",
+ joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
+ return false;
+ }
+}
+
+bool AdmittanceController::reference_state_interfaces_() {
+ for (auto &state_interface : state_interfaces_) {
+ if (state_interface.get_interface_name() == HW_IF_ESTIMATED_FT_PREFIX) {
+ estimated_ft_sensor_state_interface_.emplace_back(std::ref(state_interface));
+ }
+ }
+}
+
+void AdmittanceController::clear_command_interfaces_() {
+ joint_position_command_interfaces_.clear();
+}
+
+void AdmittanceController::clear_state_interfaces_() {
+ estimated_ft_sensor_state_interface_.clear();
+}
+} // namespace lbr_ros2_control
diff --git a/lbr_ros2_control/src/lbr_joint_position_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp
similarity index 96%
rename from lbr_ros2_control/src/lbr_joint_position_command_controller.cpp
rename to lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp
index 46fb0170..f5217b82 100644
--- a/lbr_ros2_control/src/lbr_joint_position_command_controller.cpp
+++ b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp
@@ -1,4 +1,4 @@
-#include "lbr_ros2_control/lbr_joint_position_command_controller.hpp"
+#include "lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp"
namespace lbr_ros2_control {
LBRJointPositionCommandController::LBRJointPositionCommandController()
@@ -66,7 +66,7 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_deact
const rclcpp_lifecycle::State & /*previous_state*/) {
return controller_interface::CallbackReturn::SUCCESS;
}
-} // end of namespace lbr_ros2_control
+} // namespace lbr_ros2_control
#include "pluginlib/class_list_macros.hpp"
diff --git a/lbr_ros2_control/src/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp
similarity index 98%
rename from lbr_ros2_control/src/lbr_state_broadcaster.cpp
rename to lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp
index be79cc98..d858d220 100644
--- a/lbr_ros2_control/src/lbr_state_broadcaster.cpp
+++ b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp
@@ -1,4 +1,4 @@
-#include "lbr_ros2_control/lbr_state_broadcaster.hpp"
+#include "lbr_ros2_control/controllers/lbr_state_broadcaster.hpp"
namespace lbr_ros2_control {
controller_interface::InterfaceConfiguration
@@ -154,7 +154,7 @@ void LBRStateBroadcaster::init_state_msg_() {
rt_state_publisher_ptr_->msg_.time_stamp_sec = std::numeric_limits::quiet_NaN();
rt_state_publisher_ptr_->msg_.tracking_performance = std::numeric_limits::quiet_NaN();
}
-} // end of namespace lbr_ros2_control
+} // namespace lbr_ros2_control
#include "pluginlib/class_list_macros.hpp"
diff --git a/lbr_ros2_control/src/lbr_torque_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp
similarity index 97%
rename from lbr_ros2_control/src/lbr_torque_command_controller.cpp
rename to lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp
index 2892f8eb..a4f674f6 100644
--- a/lbr_ros2_control/src/lbr_torque_command_controller.cpp
+++ b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp
@@ -1,4 +1,4 @@
-#include "lbr_ros2_control/lbr_torque_command_controller.hpp"
+#include "lbr_ros2_control/controllers/lbr_torque_command_controller.hpp"
namespace lbr_ros2_control {
LBRTorqueCommandController::LBRTorqueCommandController()
@@ -102,7 +102,7 @@ void LBRTorqueCommandController::clear_command_interfaces_() {
joint_position_command_interfaces_.clear();
torque_command_interfaces_.clear();
}
-} // end of namespace lbr_ros2_control
+} // namespace lbr_ros2_control
#include "pluginlib/class_list_macros.hpp"
diff --git a/lbr_ros2_control/src/lbr_wrench_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp
similarity index 97%
rename from lbr_ros2_control/src/lbr_wrench_command_controller.cpp
rename to lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp
index 4f00a986..53445f1f 100644
--- a/lbr_ros2_control/src/lbr_wrench_command_controller.cpp
+++ b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp
@@ -1,4 +1,4 @@
-#include "lbr_ros2_control/lbr_wrench_command_controller.hpp"
+#include "lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp"
namespace lbr_ros2_control {
LBRWrenchCommandController::LBRWrenchCommandController()
@@ -108,7 +108,7 @@ void LBRWrenchCommandController::clear_command_interfaces_() {
joint_position_command_interfaces_.clear();
wrench_command_interfaces_.clear();
}
-} // end of namespace lbr_ros2_control
+} // namespace lbr_ros2_control
#include "pluginlib/class_list_macros.hpp"
diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp
index e058b7f0..30753044 100644
--- a/lbr_ros2_control/src/system_interface.cpp
+++ b/lbr_ros2_control/src/system_interface.cpp
@@ -683,7 +683,7 @@ void SystemInterface::compute_hw_velocity_() {
});
}
-} // end of namespace lbr_ros2_control
+} // namespace lbr_ros2_control
#include