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)
ros-controls · Feb 15, 2023 · 26034d2 · 26034d2
1 parent a8959bb
commit 26034d2
Show file tree

Hide file tree

Showing 9 changed files with 634 additions and 1 deletion.
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/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()
diff --git a/..._hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp b/..._hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp
@@ -0,0 +1,96 @@
+// Copyright 2020 ros2_control Development Team
+//
+// 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.
+
+#ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_TRANSMISSIONS_SYSTEM_POSITION_ONLY_HPP_
+#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_TRANSMISSIONS_SYSTEM_POSITION_ONLY_HPP_
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/logger.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "ros2_control_demo_hardware/visibility_control.h"
+#include "transmission_interface/transmission.hpp"
+
+namespace ros2_control_demo_hardware
+{
+class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface::SystemInterface
+{
+public:
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  hardware_interface::CallbackReturn on_init(
+    const hardware_interface::HardwareInfo & info) override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  hardware_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  hardware_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  hardware_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  hardware_interface::return_type read(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  hardware_interface::return_type write(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+private:
+  std::unique_ptr<rclcpp::Logger> logger_;
+  std::unique_ptr<rclcpp::Clock> clock_;
+
+  // parameters for the RRBot simulation
+  double actuator_slowdown_;
+
+  // transmissions
+  std::vector<std::shared_ptr<transmission_interface::Transmission>> transmissions_;
+
+  struct InterfaceData
+  {
+    explicit InterfaceData(const std::string & name);
+
+    std::string name_;
+    double command_;
+    double state_;
+
+    // this is the "sink" that will be part of the transmission Joint/Actuator handles
+    double transmission_passthrough_;
+  };
+  std::vector<InterfaceData> joint_interfaces_;
+  std::vector<InterfaceData> actuator_interfaces_;
+};
+
+}  // namespace ros2_control_demo_hardware
+
+#endif  // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_TRANSMISSIONS_SYSTEM_POSITION_ONLY_HPP_
diff --git a/ros2_control_demo_hardware/package.xml b/ros2_control_demo_hardware/package.xml
@@ -15,6 +15,7 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
+  <depend>transmission_interface</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
 

diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml
@@ -41,4 +41,11 @@
       The ros2_control RRBot example using external Force Torque Sensor where it is separate from the RRBot.
     </description>
   </class>
+  <class name="ros2_control_demo_hardware/RRBotTransmissionsSystemPositionOnlyHardware"
+         type="ros2_control_demo_hardware::RRBotTransmissionsSystemPositionOnlyHardware"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+      The ros2_control minimal robot protocol with transmissions.
+    </description>
+  </class>
 </library>