Create a demo that uses transmissions (backport #226) (#241)

* Create a demo that uses transmissions (#226) * add copy for transmission demo and set in the urdf * Renamed to rrbot_transmissions * Changed reduction value of transmission2 * Added the rrbot_transmissions_system_position_only Exact copy of rrbot_system_position_only for now. Using it from the transmission demo * Moved logger into its own variable * Uncrustify * Renamed hw interface to joint. Added actuator interfaces * Formatting. Remove comments * Using transmissions for joint and actuator interfaces Added a helper object to manage the joint to transmissions to actuator data handling * Propagating data between all the interfaces * Cleanup. Added some consts * Simulate motor motion * Added logging to periodically show interface data * Refactored the parameters a bit. Removed unused ones * Added try/catches where needed Checking joint consistency in the description not required. Already done by the parser * Added transmission demo to README * Use clang-format instead * rename transmissions ros2_control macro * Remove unnecessary actuator parameters * Typo * Removed gazebo, mock hardware and fake sensors from the transmission demo * Missing 'explicit' from constructor * Further cleanup of gazebo, mock hardware and fake sensors from the transmission demo * branding fix * Remove pointer definitions Co-authored-by: Bence Magyar <[email protected]> * Fixed comment about reserve Co-authored-by: Bence Magyar <[email protected]> * Renamed member variable and added description --------- Co-authored-by: Noel Jimenez <[email protected]> Co-authored-by: Bence Magyar <[email protected]> (cherry picked from commit 1007b1d) * Fix preformatted block * fix header guard --------- Co-authored-by: Jordan Palacios <[email protected]> Co-authored-by: Bence Magyar <[email protected]>
ros-controls · Feb 16, 2023 · 0e71a38 · 0e71a38
1 parent a8959bb
commit 0e71a38
Show file tree

Hide file tree

Showing 11 changed files with 657 additions and 24 deletions.
diff --git a/README.md b/README.md
@@ -475,6 +475,30 @@ Available controllers:
 Commanding the robot: see the commands below.
 
 
+### Example 6: "Industrial Robots with an exposed transmission interface"
+
+Files:
+  - Launch file: [rrbot_transmissions_system_position_only.launch.py](ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py)
+  - Controllers yaml: [rrbot_controllers.yaml](ros2_control_demo_bringup/config/rrbot_controllers.yaml)
+  - URDF:  [rrbot_transmissions_system_position_only.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro)
+  - `ros2_control` URDF tag: [rrbot_transmissions_system_position_only.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro)
+
+Interfaces:
+  - Command interfaces:
+    - joint1/position
+    - joint2/position
+  - State interfaces:
+    - joint1/position
+    - joint2/position
+
+Available controllers:
+  - `joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`
+  - `forward_position_controller[forward_command_controller/ForwardCommandController]` (position)
+
+Moving the robot:
+  - see below description of `forward_position_controller`
+
+
 ## Controllers and moving hardware
 
 To move the robot you should load and start controllers.

diff --git a/example_1/README.rst b/example_1/README.rst
@@ -5,17 +5,17 @@ TODO(destogl): This is not adjusted yet!!
 
 *RRBot*, or ''Revolute-Revolute Manipulator Robot'', is a simple 3-linkage, 2-joint arm that we will use to demonstrate various features.
 It is essentially a double inverted pendulum and demonstrates some fun control concepts within a simulator and was originally introduced for Gazebo tutorials.
-The *RRBot* URDF files can be found in the `urdf` folder of `rrbot_description` package.
+The *RRBot* URDF files can be found in the ``urdf`` folder of ``rrbot_description`` package.
 
 1. To check that *RRBot* descriptions are working properly use following launch commands:
 
    *RRBot*
    ```
    ros2 launch rrbot_description view_robot.launch.py
    ```
-   **NOTE**: Getting the following output in terminal is OK: `Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist`.
-   This happens because `joint_state_publisher_gui` node need some time to start.
-   The `joint_state_publisher_gui` provides a GUI to generate  a random configuration for rrbot. It is immediately displayed in `Rviz`.
+   **NOTE**: Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``.
+   This happens because ``joint_state_publisher_gui`` node need some time to start.
+   The ``joint_state_publisher_gui`` provides a GUI to generate  a random configuration for rrbot. It is immediately displayed in ``Rviz``.
 
 
 1. To check that *RRBot* descriptions are working properly use following launch commands:
@@ -24,37 +24,37 @@ The *RRBot* URDF files can be found in the `urdf` folder of `rrbot_description`
    ```
    ros2 launch rrbot_description view_robot.launch.py
    ```
-   **NOTE**: Getting the following output in terminal is OK: `Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist`.
-   This happens because `joint_state_publisher_gui` node need some time to start.
-   The `joint_state_publisher_gui` provides a GUI to generate  a random configuration for rrbot. It is immediately displayed in `Rviz`.
+   **NOTE**: Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``.
+   This happens because ``joint_state_publisher_gui`` node need some time to start.
+   The ``joint_state_publisher_gui`` provides a GUI to generate  a random configuration for rrbot. It is immediately displayed in ``Rviz``.
 
 
 1. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with:
    ```
    ros2 launch ros2_control_demo_bringup rrbot.launch.py
    ```
-   The launch file loads and starts the robot hardware, controllers and opens `RViz`.
+   The launch file loads and starts the robot hardware, controllers and opens ``RViz``.
    In starting terminal you will see a lot of output from the hardware implementation showing its internal states.
    This is only of exemplary purposes and should be avoided as much as possible in a hardware interface implementation.
 
-   If you can see two orange and one yellow rectangle in in `RViz` everything has started properly.
+   If you can see two orange and one yellow rectangle in in ``RViz`` everything has started properly.
    Still, to be sure, let's introspect the control system before moving *RRBot*.
 
 1. Check if the hardware interface loaded properly, by opening another terminal and executing:
    ```
    ros2 control list_hardware_interfaces
    ```
-   You should get:
-   ```
-   command interfaces
+
+   You should get::
+
+    command interfaces
         joint1/position [claimed]
         joint2/position [claimed]
-   state interfaces
-         joint1/position
-         joint2/position
+    state interfaces
+        joint1/position
+        joint2/position
 
-   ```
-   Marker `[claimed]` by command interfaces means that a controller has access to command *RRBot*.
+   Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*.
 
 1. Check is controllers are running:
    ```
@@ -78,7 +78,7 @@ The *RRBot* URDF files can be found in the `urdf` folder of `rrbot_description`
    ```
    ros2 launch ros2_control_demo_bringup test_forward_position_controller.launch.py
    ```
-   You should now see orange and yellow blocks moving in `RViz`.
+   You should now see orange and yellow blocks moving in ``RViz``.
    Also, you should see changing states in the terminal where launch file is started.
 
 
@@ -94,5 +94,5 @@ Files used for this demos:
 
 
 Controllers from this demo:
-  - `Joint State Broadcaster` ([`ros2_controllers` repository](https://github.com/ros-controls/ros2_controllers)): [doc](https://ros-controls.github.io/control.ros.org/ros2_controllers/joint_state_broadcaster/doc/userdoc.html)
-  - `Forward Command Controller` ([`ros2_controllers` repository](https://github.com/ros-controls/ros2_controllers)): [doc](https://ros-controls.github.io/control.ros.org/ros2_controllers/forward_command_controller/doc/userdoc.html)
+  - ``Joint State Broadcaster`` ([``ros2_controllers`` repository](https://github.com/ros-controls/ros2_controllers)): [doc](https://ros-controls.github.io/control.ros.org/ros2_controllers/joint_state_broadcaster/doc/userdoc.html)
+  - ``Forward Command Controller`` ([``ros2_controllers`` repository](https://github.com/ros-controls/ros2_controllers)): [doc](https://ros-controls.github.io/control.ros.org/ros2_controllers/forward_command_controller/doc/userdoc.html)
diff --git a/example_1/hardware/rrbot.hpp b/example_1/hardware/rrbot.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_
-#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_
+#ifndef EXAMPLE_1__HARDWARE__RRBOT_HPP_
+#define EXAMPLE_1__HARDWARE__RRBOT_HPP_
 
 #include <memory>
 #include <string>
@@ -78,4 +78,4 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa
 
 }  // namespace ros2_control_demo_hardware
 
-#endif  // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_
+#endif  // EXAMPLE_1__HARDWARE__RRBOT_HPP_
diff --git a/ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py b/ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py
@@ -0,0 +1,70 @@
+# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt)
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "prefix",
+            default_value='""',
+            description="Prefix of the joint names, useful for \
+        multi-robot setup. If changed than also joint names in the controllers' configuration \
+        have to be updated.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "slowdown", default_value="50.0", description="Slowdown factor of the RRbot."
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_controller",
+            default_value="forward_position_controller",
+            description="Robot controller to start.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "start_rviz",
+            default_value="true",
+            description="Start RViz2 automatically with this launch file.",
+        )
+    )
+
+    # Initialize Arguments
+    prefix = LaunchConfiguration("prefix")
+    slowdown = LaunchConfiguration("slowdown")
+    robot_controller = LaunchConfiguration("robot_controller")
+    start_rviz = LaunchConfiguration("start_rviz")
+
+    base_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]),
+        launch_arguments={
+            "description_file": "rrbot_transmissions_system_position_only.urdf.xacro",
+            "prefix": prefix,
+            "slowdown": slowdown,
+            "robot_controller": robot_controller,
+            "start_rviz": start_rviz,
+        }.items(),
+    )
+
+    return LaunchDescription(declared_arguments + [base_launch])
diff --git a/...rbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro b/...rbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro
@@ -0,0 +1,50 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="rrbot_transmissions_system_position_only" params="name prefix use_gazebo:=^|false use_fake_hardware:=^|true mock_sensor_commands:=^|false slowdown:=2.0">
+
+    <ros2_control name="${name}" type="system">
+
+      <hardware>
+        <plugin>ros2_control_demo_hardware/RRBotTransmissionsSystemPositionOnlyHardware</plugin>
+        <param name="actuator_slowdown">${slowdown}</param>
+      </hardware>
+
+      <joint name="${prefix}joint1">
+        <command_interface name="position">
+          <param name="min">-1</param>
+          <param name="max">1</param>
+        </command_interface>
+        <state_interface name="position"/>
+      </joint>
+
+      <joint name="${prefix}joint2">
+        <command_interface name="position">
+          <param name="min">-1</param>
+          <param name="max">1</param>
+        </command_interface>
+        <state_interface name="position"/>
+      </joint>
+
+      <transmission name="transmission1">
+        <plugin>transmission_interface/SimpleTransmission</plugin>
+        <actuator name="actuator1" role="actuator1"/>
+        <joint name="joint1" role="joint1">
+          <mechanical_reduction>2.0</mechanical_reduction>
+          <offset>0.0</offset>
+        </joint>
+      </transmission>
+
+      <transmission name="transmission2">
+        <plugin>transmission_interface/SimpleTransmission</plugin>
+        <actuator name="actuator2" role="actuator2"/>
+        <joint name="joint2" role="joint2">
+          <mechanical_reduction>4.0</mechanical_reduction>
+          <offset>0.0</offset>
+        </joint>
+      </transmission>
+    </ros2_control>
+
+  </xacro:macro>
+
+</robot>
diff --git a/...mo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro b/...mo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<!-- Revolute-Revolute Manipulator -->
+<!--
+Copied and modified from ROS1 example -
+https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="2dof_robot">
+
+  <!-- Enable setting arguments from the launch file -->
+  <xacro:arg name="prefix" default="" />
+  <xacro:arg name="slowdown" default="100.0" />
+
+  <!-- Import RRBot macro -->
+  <xacro:include filename="$(find rrbot_description)/urdf/rrbot_description.urdf.xacro" />
+
+  <!-- Import Rviz colors -->
+  <xacro:include filename="$(find rrbot_description)/gazebo/rrbot.materials.xacro" />
+
+  <!-- Import RRBot ros2_control description -->
+  <xacro:include filename="$(find rrbot_description)/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro" />
+
+  <!-- Used for fixing robot -->
+  <link name="world"/>
+  <gazebo reference="world">
+    <static>true</static>
+  </gazebo>
+
+  <xacro:rrbot parent="world" prefix="$(arg prefix)">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </xacro:rrbot>
+
+  <xacro:rrbot_transmissions_system_position_only
+    name="RRBotTransmissionsSystemPositionOnly" prefix="$(arg prefix)"
+    use_fake_hardware="$(arg use_fake_hardware)"
+    mock_sensor_commands="$(arg mock_sensor_commands)"
+    slowdown="$(arg slowdown)" />
+
+</robot>
diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt
@@ -16,6 +16,7 @@ find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(transmission_interface REQUIRED)
 
 
 ## COMPILE
@@ -26,6 +27,7 @@ add_library(
   src/rrbot_system_position_only.cpp
   src/rrbot_system_multi_interface.cpp
   src/rrbot_system_with_sensor.cpp
+  src/rrbot_transmissions_system_position_only.cpp
   src/rrbot_actuator.cpp
   src/external_rrbot_force_torque_sensor.cpp
 )
@@ -40,13 +42,14 @@ ament_target_dependencies(
   pluginlib
   rclcpp
   rclcpp_lifecycle
+  transmission_interface
 )
 
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_HARDWARE_BUILDING_DLL")
 
-# Export hardware pligins
+# Export hardware plugins
 pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_hardware.xml)
 
 # INSTALL
@@ -75,5 +78,6 @@ ament_export_dependencies(
   pluginlib
   rclcpp
   rclcpp_lifecycle
+  transmission_interface
 )
 ament_package()