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