├── .github └── workflows │ └── reviewer_lottery.yml ├── README.md ├── design_drafts ├── async_controller.md ├── async_controller_impl.md ├── cascade_control.md ├── components_architecture_and_urdf_examples.md ├── controller_chaining.md ├── controller_components.md ├── controller_execution_management.md ├── design_of_controllers_and_their_status.md ├── draws │ ├── README.md │ └── components_architecture.xml ├── fallback_controllers.md ├── flexible_joint_states_msg.md ├── hardware_access.md ├── images │ ├── async_controller.png │ ├── chaining_example1.png │ ├── chaining_example2.png │ ├── components_architecture.png │ ├── control_loop_naming.png │ ├── four_bar_linkage_transmission.png │ ├── nested_execution.png │ ├── parallel_execution.png │ ├── sequential_execution.png │ └── simple_transmission.png ├── interfaces_remapping.md ├── joint_interfaces_and_transmissions.md ├── mode_switching_and_conflict_check.md ├── non_joint_command_interfaces.md ├── ros2_control_boilerplate.md ├── ros2_control_documentation.rst └── transmission_loading_and_parametrization.md └── documentation_resources.md /.github/workflows/reviewer_lottery.yml: -------------------------------------------------------------------------------- 1 | name: Reviewer lottery 2 | # pull_request_target takes the same events as pull_request, 3 | # but it runs on the base branch instead of the head branch. 4 | on: 5 | pull_request_target: 6 | types: [opened, ready_for_review, reopened] 7 | 8 | jobs: 9 | assign_reviewers: 10 | uses: ros-controls/ros2_control_ci/.github/workflows/reusable-reviewer-lottery.yml@master 11 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Items to showcase on the roadmap for ros_control in ROS2 2 | 3 | For ROS2 and ros2_control, head over to the `design_drafts` folder! 4 | 5 | If you are looking for general documentation on `ros2_control`, click [here](https://control.ros.org). 6 | 7 | If you are looking for resources on `ros_control` in ROS Noetic and friends, check [here](documentation_resources.md) 8 | -------------------------------------------------------------------------------- /design_drafts/async_controller.md: -------------------------------------------------------------------------------- 1 | # Asynchronous Controllers in ros2_control 2 | 3 | ## What is an asynchronous controller? 4 | 5 | An asynchronous controller is a controller that for some reason cannot (or we don’t want to) perform the operations needed in an update() call. 6 | For instance if ros control is running at 100Hz, the sum of the execution time of all controllers update() calls must be below 10ms. If a controller requires 15ms it cannot be executed synchronously without affecting the whole ros control update rate. 7 | 8 | ## Abstract implementation description 9 | Create an AsyncControllerWrapper class that can wrap any Controller without modifying it and makes it run asynchronously seamlessly. 10 | It will receive a Controller and store it, and act as an intermediary between the ros_control loop and the Controller. 11 | On the update() call, the AsyncControllerWrapper will read data from the interfaces, store it in an intermediate object, and write data from another intermediate object to the interfaces. 12 | The wrapped Controller, running on a separate thread, will read and write from these intermediate objects. 13 | This data must be protected by mutexes or similar. 14 | When new state data is available, the AsyncControllerWrapper will notify the wrapped controller so it is processed on its own thread. 15 | 16 | ## Implementation guidelines: 17 | 18 | ### Concurrency risks 19 | The main risk is handling concurrency, since we want to wrap any controller, which was probably not implemented to support concurrent calls to some of its methods. 20 | The only method of the wrapped controller that we’ll run in a separate thread, is update(). 21 | The rest of the method calls will be forwarded from the AsyncControllerWrapper to the wrapped controller in the main thread. 22 | We must make sure that the asynchronous update() is not being executed when forwarding other methods. 23 | 24 | ### StateInterfaces and CommandInterfaces 25 | The second risk we want to control, is that our asynchronous controller has access to State and Command Interfaces, and uses them from its own thread, causing for instance concurrent write of a CommandInterface while the Actuator is reading it. 26 | 27 | To avoid this, we can create intermediate Interfaces, stored in the AsyncControllerWrapper, that act as a bridge between the wrapped controller and the real State and Command Interfaces. 28 | This bridge should allow thread safe data storage, and non blocking operations on the ros_control thread. Some containers for this purpose exist in: https://github.com/ros-controls/realtime_tools/tree/ros2_devel/include/realtime_tools 29 | 30 | These interfaces should be created on the assign_interfaces method, and removed on the release_interfaces method. But at the moment those methods are not virtual, that will need to change. 31 | 32 | Sample implementation (not tested), we create our own State and Command interfaces, and assign them to the wrapped controller. 33 | 34 | ```c++ 35 | using ControllerInterface = controller_interface::ControllerInterface; 36 | class AsyncControllerWrapper : public controller_interface::ControllerInterface 37 | { 38 | public: 39 | AsyncControllerWrapper(std::shared_ptr wrapped_controller); 40 | void assign_interfaces( 41 | std::vector && command_interfaces, 42 | std::vector && state_interfaces) 43 | { 44 | std::vector loaned_bridge_command_interfaces_; 45 | for (const auto & ci : command_interfaces) { 46 | bridge_command_data_.push_back(ci.get_value()); 47 | bridge_command_interfaces_.emplace_back( 48 | ci.get_name(), ci.get_interface_name(), 49 | &bridge_command_data_.back()); 50 | loaned_bridge_command_interfaces_.emplace_back(bridge_command_interfaces_.back()); 51 | } 52 | std::vector loaned_bridge_state_interfaces_; 53 | 54 | for (const auto & si : state_interfaces) { 55 | bridge_state_data_.push_back(si.get_value()); 56 | bridge_state_interfaces_.emplace_back( 57 | hardware_interface::StateInterface( 58 | si.get_name(), si.get_interface_name(), 59 | &bridge_state_data_.back())); 60 | loaned_bridge_command_interfaces_.emplace_back(bridge_command_interfaces_.back()); 61 | } 62 | wrapped_controller_->assign_interfaces( 63 | std::move(loaned_bridge_command_interfaces_), 64 | std::move(loaned_bridge_state_interfaces_)); 65 | } 66 | 67 | void release_interfaces() 68 | { 69 | wrapped_controller_->release_interfaces(); 70 | } 71 | 72 | virtual controller_interface::return_type update() override 73 | { 74 | // Caution, this is just an incomplete example on how to copy the data from the real interfaces 75 | // to our bridge interfaces 76 | // This access must be protected so it does not concurrently with the asynchronous update() 77 | 78 | for (size_t i = 0; i < state_interfaces_.size(); ++i) { 79 | bridge_state_data_[i] = state_interfaces_[i].get_value(); 80 | } 81 | 82 | for (size_t i = 0; i < bridge_command_interfaces_.size(); ++i) { 83 | bridge_command_interfaces_[i].set_value(bridge_command_data_[i]); 84 | } 85 | } 86 | 87 | protected: 88 | std::shared_ptr wrapped_controller_; 89 | std::vector bridge_command_interfaces_; 90 | std::vector bridge_command_data_; 91 | std::vector bridge_state_interfaces_; 92 | std::vector bridge_state_data_; 93 | }; 94 | ``` 95 | 96 | ### The `update()` method 97 | The `update()` method needs to: 98 | Write commands from the bridge command interfaces to the real command interfaces. 99 | Read real state interfaces and write them to the bridge state interfaces. 100 | Notify the thread that it has new data to run. 101 | 102 | And do all this ensuring: 103 | That the thread is not running and actively reading/writing from the data at the same time 104 | That the caller thread is not blocked on a mutex waiting for the asynchronous update() to end (as this defeats the purpose of asynchronous controller). 105 | 106 | 107 | #### Implementation considerations 108 | For the asynchronous `update()` calls on the wrapped controller, avoid using std::async as it would create a new thread on each call. This can have a significant overhead on the wrapper. 109 | A better idea is probably to create a thread, that we wake up when there’s new data. 110 | For waking up a thread, std::condition_variable is a good solution, although it is not real time safe. For the moment it can be implemented like this and in the future, when the rest of the code is made RT safe, this can be reviewed. 111 | Alternatively the wrapped controller could run in a non-blocking periodic thread at a certain rate. 112 | -------------------------------------------------------------------------------- /design_drafts/async_controller_impl.md: -------------------------------------------------------------------------------- 1 | ## Implementation of Asynchronous Controllers 2 | 3 | 4 | #### Short description 5 | 6 | Asynchronous controllers are an upcoming addition to the ros2_control framework. As the name suggests, they make it possible to run the update calls of these controllers on a separate thread. 7 | For synchronization, the standard library's std::atomic interface was used. 8 | 9 | The purpose of this design draft is to explain how the synchronization works between the Controller Manager's thread and an asynchronous controller. 10 | The asynchronous controllers' threads are entirely independent of each other, which means that no data sharing occurs between them. Synchronization happens only between the Controller Manager's thread and an async controller - so in the following drawings only these two threads will be visualized, even though it's possible to use multiple async controllers at once. 11 | 12 | 13 | #### Implementation 14 | 15 | 16 | The critical sections are: 17 | 18 | * The state interface writes from the Controller Manager's read function 19 | * The command interface writes of the async controller's update function 20 | 21 | The former has to occur and be seen by the async thread before the update, while the latter, ideally, has to finish and be seen before the controller manager's write. 22 | The implementation is based on the synchronization between the atomic operations' release and acquire orderings. In short, no operations sequenced before a release-ordered atomic store may move beyond the release store itself. Acquire is the opposite; no operation sequenced after the acquire load may move before the load itself. These guarantees are necessary because even though operation A is sequenced before operation B, the result of operation A might be visible later in the other thread than the result of operation B. 23 | 24 | In practice, this is how it would work: 25 | 26 | ```c++ 27 | void cm_thread_read() { 28 | int state_if_one = 12; // may be visible after the store to state_if_two but not the atomic flag (release) 29 | int state_if_two = 13; // may be visible after the store to state_if_one but not the atomic flag (release) 30 | state_interfaces_ready.store(true, std::memory_order_release); // std::atomic, if the controller thread sees the result of this store, 31 | } 32 | 33 | void controller_thread_update() { 34 | if (state_interfaces_ready.load(std::memory_order_acquire) { // only use the state_if values if the state_interfaces_ready flag is true 35 | int local_var_used_by_update = state_if_one; // may be reordered with the load in the next operation, but not the atomic flag (acquire) 36 | int other_local_var_used_by_update = state_if_two; // may be reordered with the load in the previous operation, but not the atomic flag (acquire) 37 | } 38 | } 39 | 40 | ``` 41 | 42 |
43 | 44 | The code of the current implementation is a bit different (e.g., instead of loads atomic exchange operations are used to set the flag to false immediately), but also uses this principle. The next diagram shows the complete architecture, along with four different concurrency cases to account for. 45 | 46 |
47 | 48 | ![async_controller](https://user-images.githubusercontent.com/25421074/214663116-b0de8240-7efd-4ec4-bbc2-5a87d76430ca.png) 49 | 50 | 51 | 1. The state interfaces aren't written yet by the CM read. In this case, we can be sure that the CM's release flag isn't set, thus the acquire load on the other side will return false - so we'll skip the update and the asynchronous controller will sleep until the next iteration. 52 | 53 | 2. The state interfaces are written in the CM read function, and the release flag is set. It can happen that the state interfaces are visible on the controller's side, but the store to the flag isn't - in this case, we'll do the same as the first scenario. On the other hand, if the store to the flag is visible, then the thread's acquire load will return true if it isn't currently sleeping, and the respective orderings guarantee that the async update will be able to work with the latest state interface values. 54 | 55 | 3. After the async update, a release store is made to signal that the command interfaces are written by the controller's thread. Again, the release ordering guarantees that if this store is visible on the CM thread, then so are the command interfaces. If the acquire load of this variable is true right before the cm write function, then we're again working with the latest values, which is the intended outcome. 56 | 57 | 4. If the store to the async controller's flag isn't visible in the Controller Manager, and the corresponding load returns false, it means that the asynchronous update isn't finished before the next CM write. This is not ideal, since it means that two threads might access the same variable in the same time, leading to a race condition. Currently, if this happens, only a warning is given to the user, and we continue with the CM write function as if nothing happened. In practice, probably nothing dangerous will happen, since the aligned loads and stores of plain pointers (similarly to ints and bools) are atomic on most CPUs, and this can only occur if the async update is slower than all synchronous updates combined, but even then, this is an issue and should be handled differently. 58 | 59 | Here are are few options that could alleviate the problem: 60 | 61 | * At the end of the CM update call, wait for the asynchronous controller to finish. As locking is out of the question, busy waiting could also be tried. However, this could potentially defeat the purpose of using an asynchronous controller. 62 | 63 | * Handles should work with atomic primitives, if they are added in the future. The set_value() and get_value() functions could be relaxed stores and loads of the value. This ensures that we're working on tear-free variables without relying on platform specific implementations, with minimal overhead compared to a normal, non-atomic variable. 64 | 65 | * There could be a mechanism to check which interfaces are used by which controllers and components. This could let us know if the async update is finished or not, and simply skip the following write, or address it in another manner. 66 | -------------------------------------------------------------------------------- /design_drafts/cascade_control.md: -------------------------------------------------------------------------------- 1 | # Cascade Control (Serial Chaining of Controllers) 2 | 3 | This document proposes a minimal-viable-implementation of serial controllers chaining as described in [Chaining Controllers design document](controller_chaining.md). 4 | Cascade control is a specific type of controller chaining. 5 | 6 | 7 | ## Scope of the Document and Background Knowledge 8 | 9 | The proposal focuses only on serial chaining of controller and tries to reuse existing mechanisms for it. 10 | It focuses on [inputs and outputs of a controller](controller_chaining.md#input--outputs-of-a-controller) and their management in controller manager. 11 | The concept of [controller groups](controller_chaining.md#controller-group) will be introduced only for clarity reasons, and its only meaning is that controllers in that group can be updated in arbitrary order. 12 | This doesn't mean that the controller groups as described [in the controller chaining document](controller_chaining.md#controller-group) will not be introduced and used in the future. 13 | Nevertheless, the author is convinced that this would add only unnecessary complexity at this stage, although in the long term they *could* provide clearer structure and interfaces. 14 | 15 | ## Motivation, Purpose and Use 16 | 17 | To describe the intent of this document, let focus on simple yet sufficient example ([Example 2 from 'controllers_chaining' design docs](controller_chaining.md#example-2)): 18 | 19 | ![Example2](images/chaining_example2.png) 20 | 21 | In this example, we want to chain 'position_tracking' controller with 'diff_drive_controller' and two PID controllers. 22 | Let's now imagine a use case that we don't only want to run all those controllers as a group, but also flexibly add preceding steps. 23 | This means the following: 24 | 1. When a robot is stared, we want to check if motor velocity control is working properly and therefore only PID controllers are activated. 25 | This means we can control the input of PID controller also externally using topics. 26 | But these controllers also provide virtual interfaces, so we can chain them. 27 | 2. Then "diff_drive_controller" is activated and attach itself to the virtual input interfaces of PID controllers. 28 | PID controllers also get informed that they are working in chained mode and therefore disable their external interface through subscriber. 29 | Now we check if kinematics of differential robot is running properly. 30 | 3. After that, "position_tracking" can be activated and attached to "diff_drive_controller" that disables its external interfaces. 31 | 4. If any of the controllers is deactivated, also all preceding controllers are deactivated. 32 | 33 | 34 | ## Implementation 35 | 36 | ### A new Controller Base-Class: Chainable Controller 37 | 38 | A `ChainableController` extends `ControllerInterface` class with `virtual InterfaceConfiguration input_interface_configuration() const = 0` method. 39 | This method should implement for each controller that **can be preceded** by another controller exporting all the input interfaces. 40 | For simplicity reasons, it is assumed for now that controller's all input interfaces are used. 41 | Therefore, do not try to implement any exclusive combinations of input interfaces, but rather write multiple controllers if you need exclusivity. 42 | 43 | The `ChainableController` base class implements `void set_chained_mode(bool activate)` that sets an internal flag that a controller is used by another controller (in chained mode) and calls `virtual void on_set_chained_mode(bool activate) = 0` that implements controller's specific actions when chained modes is activated or deactivated, e.g., deactivating subscribers. 44 | 45 | #### Example 46 | 47 | PID controllers export one virtual interface `pid_reference` and stop their subscriber `/pid_reference` when used in chained mode. 48 | 'diff_drive_controller' controller exports list of virtual interfaces `/v_x`, `/v_y`, and `/w_z`, and stops subscribers from topics `/cmd_vel` and `/cmd_vel_unstamped`. Its publishers can continue running. 49 | 50 | ### Inner Resource Management 51 | After configuring a chainable controller, controller manager calls `input_interface_configuration` method and takes ownership over controller's input interfaces. 52 | This is the same process as done by `ResourceManager` and hardware interfaces. 53 | Controller manager maintains "claimed" status of interface in a vector (the same as done in `ResourceManager`). 54 | 55 | ### Activation and Deactivation Chained Controllers 56 | Controller Manager has an additional parameter that describes how controllers are chained. 57 | In the first version, the parameter-structure would have some semantic meaning embedded into it, as follows: 58 | ``` 59 | controller_manager: 60 | ros__parameters: 61 | chained_controllers: 62 | 63 | - parallel_group_1: 64 | - controller1_1 65 | - controller1_2 66 | 67 | - parallel_group_2: 68 | - controller2_1 69 | 70 | - parallel_group_3: 71 | - controller3_1 72 | - controller3_2 73 | - controller3_3 74 | 75 | ... 76 | 77 | - parallel_group_N: 78 | - controllerN_1 79 | - ... 80 | - controllerN_M 81 | ``` 82 | 83 | 84 | This structure is motivated by `filter_chain` structure from [ros/filters repository](https://github.com/ros/filters/tree/ros2) (see [this file for implementation](https://github.com/ros/filters/blob/ros2/include/filters/filter_chain.hpp)). 85 | 86 | This structure is stored internally by controller manager into an ordered map (`std::map>`) with group name as key. 87 | When a controller should be deactivated, the controller manager deactivates all the controllers in the preceding groups first. 88 | All other controllers from the group stay active, as well as all controllers in the following groups. 89 | NOTE: In the future this could be done more intelligently, i.e., deactivate only controllers in the preceding groups that actually precede the controller that should be deactivated. 90 | 91 | On the other hand, the controller should be manually activated in the reverse order, i.e., from the those closer to the hardware toward those preceding them. 92 | 93 | 94 | ## Closing remarks 95 | 96 | - Maybe addition of the new controller's type `ChainableController` is not necessary. It would also be feasible to add implementation of `input_interface_configuration()` method into `ControllerInterface` class with default result `interface_configuration_type::NONE`. 97 | -------------------------------------------------------------------------------- /design_drafts/components_architecture_and_urdf_examples.md: -------------------------------------------------------------------------------- 1 | # ROS2 Control Components Architecture and URDF-Description Examples 2 | 3 | ## Architecture 4 | 5 | ![ros2_control Components Achitecture][ros2_control_arch_resource_manager] 6 | 7 | [ros2_control_arch_resource_manager]: images/components_architecture.png "ROS2 Control - Components Architecture" 8 | 9 | ## URDF Examples 10 | 11 | ros2_control implementation examples are presented for the following robot/robot-cell architectures: 12 | 13 | 1. Industrial Robots with only one interface 14 | 2. Industrial Robots with multiple interfaces (can not be written at the same time) 15 | 2.1. Robots with multiple interfaces used at the same time - the same structure as in (2) 16 | 3. Industrial Robots with a sensor integrated into the robot's control box 17 | 4. Industrial Robots with a sensor connected to ROS computer 18 | 5. Modular Robots with separate communication to each actuator 19 | 6. Modular Robots with actuators not providing states and additional sensors (simple Transmissions) 20 | 7. Modular Robots with separate communication to each "actuator" with multi joints (Transmission Example) - (system component is used) 21 | 8. Sensor only 22 | 9. Actuator Only 23 | 24 | Note: 25 | * Everything within the `` tag is implemented as a plugin. 26 | * The examples below have some `` tags defined. The names in those tags are primarily for demonstration, not part of a pre-defined XML schema. Each component may define their names inside the `` tag. 27 | 28 | #### 1. Industrial Robots with only one interface 29 | * the communication is done using proprietary API to communicate with robot control box 30 | * Data for all joints is exchanged in batch (at once) 31 | * Examples: KUKA RSI 32 | 33 | ```xml 34 | 35 | 36 | ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware 37 | 2.0 38 | 3.0 39 | 2.0 40 | 41 | 42 | 43 | -1 44 | 1 45 | 46 | 47 | 48 | 49 | 50 | -1 51 | 1 52 | 53 | 54 | 55 | 56 | ``` 57 | 58 | #### 2. Industrial Robots with multiple interfaces (can not be written at the same time) 59 | * the communication is done using proprietary API to communicate with robot control box 60 | * Data for all joints is exchanged in batch (at once) 61 | * Examples: KUKA FRI, ABB Yumi, Schunk LWA4p, etc. 62 | 63 | ```xml 64 | 65 | 66 | ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware 67 | 2 68 | 2 69 | 70 | 71 | 72 | -1 73 | 1 74 | 75 | 76 | -1 77 | 1 78 | 79 | 80 | -0.5 81 | 0.5" 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | -1 90 | 1 91 | 92 | 93 | 94 | 95 | 96 | 97 | ``` 98 | 99 | Note: 100 | * For `joint2` the "velocity" and "effort" command interfaces are intentionally left out to show another common use-case where only one interface can be commanded, but robot provides state of multiple types. 101 | 102 | 103 | #### 2.1 Robots with multiple interfaces used at the same time - the same structure as in (2) 104 | * the communication is done using proprietary API to communicate with robot control box 105 | * Data for all joints is exchanged in batch (at once) 106 | * Multiple values can be commanded, e.g. goal position and the maximal velocity on the trajectory allowed 107 | * Examples: humanoid robots (TALOS), 4 legged robot [solo](https://github.com/open-dynamic-robot-initiative/master-board/blob/master/documentation/BLMC_%C2%B5Driver_SPI_interface.md) 108 | 109 | ```xml 110 | 111 | 112 | ros2_control_demo_hardware/RRBotSystemMultiInterfaceMultiWriteHardware 113 | 2 114 | 2 115 | 116 | 117 | 118 | -1 119 | 1 120 | 121 | 122 | -1 123 | 1 124 | 125 | 126 | -0.5 127 | 0.5" 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | -1 136 | 1 137 | 138 | 139 | 140 | 141 | 142 | 143 | ``` 144 | 145 | 146 | #### 3. Industrial Robots with integrated sensor 147 | * the communication is done using proprietary API 148 | * Data for all joints is exchanged in batch (at once) 149 | * Sensor data are exchanged together with joint data 150 | * Examples: KUKA RSI with sensor connected to KRC (KUKA control box) 151 | 152 | ```xml 153 | 154 | 155 | ros2_control_demo_hardware/RRBotSystemWithSensorHardware 156 | 2.0 157 | 3.0 158 | 2.0 159 | 5.0 160 | 161 | 162 | 163 | -1 164 | 1 165 | 166 | 167 | 168 | 169 | 170 | -1 171 | 1 172 | 173 | 174 | 175 | 176 | 177 | 178 | rrbot_tcp 179 | 100 180 | 15 181 | 182 | 183 | ``` 184 | 185 | #### 4. Industrial Robots with externally connected sensor 186 | * the communication is done using proprietary API 187 | * Data for all joints is exchanged at once 188 | * Sensor data are exchanged independently 189 | * Examples: KUKA RSI and FTS connected to ROS-PC 190 | 191 | ```xml 192 | 193 | 194 | ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware 195 | 2 196 | 2 197 | 198 | 199 | 200 | -1 201 | 1 202 | 203 | 204 | 205 | 206 | 207 | -1 208 | 1 209 | 210 | 211 | 212 | 213 | 214 | 215 | ros2_control_demo_hardware/ForceTorqueSensor2DHardware 216 | 0.43 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | kuka_tcp 226 | -100 227 | 100 228 | 229 | 230 | ``` 231 | 232 | #### 5. Modular Robots with separate communication to each actuator 233 | * the communication is done on actuator level using proprietary or standardized API (e.g., canopen_402, Modbus, RS232, RS485) 234 | * Data for all actuators is exchanged separately from each other 235 | * Examples: Mara, Arduino-based-robots 236 | 237 | ```xml 238 | 239 | 240 | ros2_control_demo_hardware/PositionActuatorHardware 241 | 1.23 242 | 3 243 | 244 | 245 | 246 | -1 247 | 1 248 | 249 | 250 | 251 | 252 | 253 | 254 | ros2_control_demo_hardware/PositionActuatorHardware 255 | 1.23 256 | 3 257 | 258 | 259 | 260 | -1 261 | 1 262 | 263 | 264 | 265 | 266 | ``` 267 | 268 | #### 6. Modular Robots with actuators not providing states and with additional sensors (simple Transmissions) 269 | * the communication is done on actuator level using proprietary or standardized API (e.g., canopen_402, modbus, RS232, RS485) 270 | * Data for all actuators and sensors is exchanged separately from each other 271 | * Examples: Arduino-based-robots, custom robots 272 | 273 | ```xml 274 | 275 | 276 | ros2_control_demo_hardware/VelocityActuatorHardware 277 | 1.23 278 | 3 279 | 280 | 281 | 282 | -1 283 | 1 284 | 285 | 286 | 287 | 288 | transmission_interface/SimpleTansmission 289 | ${1024/PI} 290 | 291 | 292 | 293 | 294 | ros2_control_demo_hardware/VelocityActuatorHardware 295 | 1.23 296 | 3 297 | 298 | 299 | 300 | -1 301 | 1 302 | 303 | 304 | 305 | 306 | 307 | 308 | ros2_control_demo_hardware/PositionSensorHardware 309 | 2 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | ros2_control_demo_hardware/PositionSensorHardware 318 | 2 319 | 320 | 321 | 322 | 323 | 324 | ``` 325 | 326 | #### 7. Modular Robots with separate communication to each "actuator" with multi joints (Transmission Example) - (system component is used) 327 | * the communication is done on actuator level using proprietary or standardized API (e.g., canopen_402) 328 | * Data for all actuators is exchanged separately from each other 329 | * There is a many-to-many connection between joint and actuator values resolved by a transmission 330 | * Examples: Wrist of a humanoid robot 331 | 332 | ```xml 333 | 334 | 335 | ros2_control_demo_hardware/ActuatorHardwareMultiDOF 336 | 1.23 337 | 3 338 | 339 | 340 | 341 | -1 342 | 1 343 | 344 | 345 | 346 | 347 | 348 | -1 349 | 1 350 | 351 | 352 | 353 | 354 | transmission_interface/SomeComplex2by2Transmission 355 | {joint1, joint2} 356 | {output2, output2} 357 | 1.5 358 | 3.2 359 | 3.1 360 | 1.4 361 | 362 | 363 | ``` 364 | 365 | #### 8. Sensor only 366 | * the communication is done on a sensor level using proprietary or standardized API 367 | * Examples: Camera, ForceTorqueSensor, Distance Sensors, IMU, etc. 368 | 369 | ```xml 370 | 371 | 372 | ros2_control_demo_hardware/CameraWithIMUSensor 373 | 2 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | ``` 385 | 386 | #### 9. Actuator Only 387 | * the communication is done on actuator level using proprietary or standardized API 388 | * There may be a mechanical reduction or other type of 1-1 mapping between joint and actuator resolved by a transmission - there is no need to name the joint in the transmission tag 389 | * Examples: Small Conveyor, Motor, etc. 390 | 391 | ```xml 392 | 393 | 394 | ros2_control_demo_hardware/VelocityActuatorHardware 395 | 1.13 396 | 3 397 | 398 | 399 | 400 | -1 401 | 1 402 | 403 | 404 | 405 | 406 | transmission_interface/RotationToLinerTansmission 407 | ${1024/PI} 408 | 409 | 410 | ``` 411 | 412 | #### 10. Real robot examples 413 | 414 | ##### [Solo: a 4 legged robot](https://github.com/open-dynamic-robot-initiative/master-board/blob/master/documentation/BLMC_%C2%B5Driver_SPI_interface.md) 415 | 416 | ```xml 417 | 418 | 419 | solo_control/SoloRobotHardware 420 | 0.001 421 | 0.001 422 | 423 | 424 | 425 | -1 426 | 1 427 | 428 | 429 | -1 430 | 1 431 | 432 | 433 | -0.5 434 | 0.5 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | -1 448 | 1 449 | 450 | 451 | -1 452 | 1 453 | 454 | 455 | -0.5 456 | 0.5 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | -1 470 | 1 471 | 472 | 473 | -1 474 | 1 475 | 476 | 477 | -0.5 478 | 0.5 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | 492 | -1 493 | 1 494 | 495 | 496 | -1 497 | 1 498 | 499 | 500 | -0.5 501 | 0.5 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | -1 514 | 1 515 | 516 | 517 | -1 518 | 1 519 | 520 | 521 | -0.5 522 | 0.5 523 | 524 | 525 | 526 | 527 | 528 | 529 | 530 | 531 | 532 | 533 | 534 | 535 | -1 536 | 1 537 | 538 | 539 | -1 540 | 1 541 | 542 | 543 | -0.5 544 | 0.5 545 | 546 | 547 | 548 | 549 | 550 | 551 | 552 | 553 | 554 | 555 | 556 | 557 | 558 | -1 559 | 1 560 | 561 | 562 | -1 563 | 1 564 | 565 | 566 | -0.5 567 | 0.5 568 | 569 | 570 | 571 | 572 | 573 | 574 | 575 | 576 | 577 | 578 | 579 | 580 | -1 581 | 1 582 | 583 | 584 | -1 585 | 1 586 | 587 | 588 | -0.5 589 | 0.5 590 | 591 | 592 | 593 | 594 | 595 | 596 | 597 | 598 | 599 | 600 | 601 | 602 | -1 603 | 1 604 | 605 | 606 | -1 607 | 1 608 | 609 | 610 | -0.5 611 | 0.5 612 | 613 | 614 | 615 | 616 | 617 | 618 | 619 | 620 | 621 | 622 | 623 | 624 | -1 625 | 1 626 | 627 | 628 | -1 629 | 1 630 | 631 | 632 | -0.5 633 | 0.5 634 | 635 | 636 | 637 | 638 | 639 | 640 | 641 | 642 | 643 | 644 | 645 | 646 | -1 647 | 1 648 | 649 | 650 | -1 651 | 1 652 | 653 | 654 | -0.5 655 | 0.5 656 | 657 | 658 | 659 | 660 | 661 | 662 | 663 | 664 | 665 | 666 | 667 | 668 | -1 669 | 1 670 | 671 | 672 | -1 673 | 1 674 | 675 | 676 | -0.5 677 | 0.5 678 | 679 | 680 | 681 | 682 | 683 | 684 | 685 | 686 | 687 | 688 | 689 | 690 | 691 | -54 692 | 23 693 | 694 | 695 | 696 | -10 697 | 10 698 | 699 | -54 700 | 23 701 | -10 702 | 10 703 | 704 | 705 | ``` 706 | -------------------------------------------------------------------------------- /design_drafts/controller_chaining.md: -------------------------------------------------------------------------------- 1 | # Chaining Controllers 2 | 3 | > **DISCLAIMER WORK IN PROGRESS!!**: This document serves as a support to discuss the challenges and possible implementation for chaining controllers. It is **NOT** a final solution. 4 | 5 | 6 | ## Motivation 7 | 8 | The goal of chaining controller is not to create a complete framework like Simulink, but to provide developers with a solution to chain controllers input/output to each others. Controllers can be chain in [Sequence or in Parallel.](controller_execution_management.md) 9 | 10 | ### Example 1 11 | 12 | A typical use case for this is a safety `JointLimitsController` which is being linked after a `JointEffortController` to enforce an operation within appropriate min/max torque values 13 | 14 | 15 | ![Example1](images/chaining_example1.png) 16 | 17 | ### Example 2 18 | 19 | Another use case would be to add a PID at the output of a diff_drive_controller for each wheel. And even adding a position_tracker controller at the input of the diff_driver_controller. 20 | 21 | 22 | ![Example2](images/chaining_example2.png) 23 | 24 | In this example, we want the `position_tracking` controller to output a `cmd_vel` to an interface that will be an input of the diff_drive_controller. 25 | 26 | ## Input & Outputs of a controller 27 | 28 | In order to chain controllers, inputs and outputs must be defined. 29 | 30 | ### Current controllers and ROS interfaces 31 | 32 | The existing controllers usually implement ros interfaces to communicate with ROS. Often those interfaces provide inputs to the controller. 33 | It is not clear how we could handle chaining two controllers of that kind, as this would require replacing the ROS interfaces with something else. Another Handle, maybe ? 34 | 35 | > For example the diff_driver_controller takes a `cmd_vel` Twist Message input. We want to chain a position_tracker that outputs a `cmd_vel` Data to the diff_drive_controller. 36 | 37 | ### The other inputs: a new interface type ? 38 | 39 | How should we define inputs of controllers that are neither CommandInterface nor StateInterface. 40 | Should we create a new type of interface (`InputInterface`) and a new virtual `input_interface_configuration()` method? 41 | 42 | Since it is up to the user to decide wether he wants to implement a chainable controller, we can introduce a `ChainableController` class that implements those new types and virtual methods. 43 | 44 | Controllers, which inherits from `ChainableController` would have to implement this method to define non-ros interfaces 45 | 46 | > question: should we make all controllers chainable? 47 | > idea: maybe ros-interfaces can inherit from `InputInterface` ? 48 | 49 | ## Reusability of a sub-controller 50 | 51 | ### Granularity 52 | 53 | While it could be possible to split controllers such as a PID controller into three sub-controllers (P, I, D) which can be re-used individually. Would that really be useful? While there might be a need for P-Controller, is there any for an I-Controller or even a D-Controller? 54 | 55 | ## Controller Group 56 | 57 | 58 | A `ControllerGroup` which inherits from `ControllerInterface` or `ChainableController` class could allow adding sub-controllers via an `add_controller(ChainableController& )` method. The `ControllerGroup::update()` method will implement an order for chaining controllers. For a `ControllerSequenceGroup`, the method will simply call in sequence the update of each sub-controller. 59 | 60 | ### Inner Resource Management 61 | 62 | The controller group will hold a FakeResourceManager object. For each claimed Interface of each sub-controller, the ControllerGroup will create a FakeResource inside of the FakeResourceManager. During assigment, sub-controllers will be loaned a FakeStateInterface linked to the FakeResource. 63 | 64 | 65 | ### Interface Configuration of a group 66 | 67 | The command and state interfaces claimed by a `ControllerGroup` will be a concatenation of all sub-controllers interfaces. It will store an `std::map` 68 | 69 | ### Chaining 70 | 71 | Chaining can be achieved by matching interface names. 72 | Example: the diff_drive_controller claims an `InputInterface` called *cmd_vel*, the pose_tracking claims a `CommandInterface` called *cmd_vel* as well. 73 | Those two Interface can then be linked. 74 | 75 | ## Nesting controllers 76 | 77 | Since a ControllerGroup inherit from `ChainableController`, they can be chained and nested. 78 | 79 | ## Possible Implementations 80 | 81 | ### ros2 as an inter-controller middleware 82 | 83 | One idea was to used ros2 topics to communicate between two controllers. 84 | However, this is not real-time safe, and ros2 topics are asynchronous. 85 | 86 | ### Using a new InputInterface. 87 | 88 | A new method called `input_interface_configuration()` would allow a controller to define the `InputInterface` of the controller. 89 | -------------------------------------------------------------------------------- /design_drafts/controller_components.md: -------------------------------------------------------------------------------- 1 | # Loading Controller as Components 2 | 3 | ## Components in a Nutshell 4 | 5 | ROS2 Eloquent introduced the concept of components. 6 | A component represents a node which can be conveniently loaded into a generic components container and so being executed without the node to compile the node in its own executable. 7 | Throughout the rest of this document, the word `node` is equivalent to `component`. 8 | The component container (a.k.a `ComponentManager`) provides functionalities such as services and command line interfaces to dynamically load and unload components. 9 | It takes ownership over the loaded components and can thus control their execution. 10 | 11 | That should be enough background for the scope of this document. 12 | For more details see https://index.ros.org/doc/ros2/Tutorials/Composition/ 13 | 14 | ## Controller Manager as a Component Manager 15 | 16 | The `ComponentManager` is in a sense very similar to the `ControllerManager` in which the controller manager holds the same logic of dynamically loading and unloading controllers. 17 | We therefore propose to leverage the functionality of the `rclcpp_components` within the controller manager, which provides all necessary services and command line tools needed to load components. 18 | The current implementation is the component manager is non-public and has to be made public for inheritance. 19 | A top-level issue tracking this can be found here: https://github.com/ros2/rclcpp/issues/1010 20 | 21 | ### Type Safety of Components 22 | 23 | The component loading functionality differs from the `rclcpp_components` in a way that the controller manager has to make sure to load only components which adhere to the `ControllerInterface` and not load any arbitrary nodes. 24 | The current component container interface does not allow to filter for specific types and has to be modified accordingly. 25 | The proposed design lets the controller manager being a child class of the component manager and gives so the change to override the appropriate functions to guarantee a strict type safety. 26 | An alternative, more speculative, approach could be to modify the original component manager to support the type filtering. 27 | 28 | ### Implications for Controllers 29 | 30 | In order for a node to qualify as a component, it really only has to inherit from a _node_-like such as `rclcpp::Node` or `rclcpp_lifecycle::LifecycleNode`. 31 | It further requires a constructor which accepts a single `const rclcpp::NodeOptions & options` argument which is then passed to the parent constructor in order to parse things such parameters correctly. 32 | At last, each controller has to be explicitly exported via `cmake` and so registered in the `ament_resource_index` to be found at runtime. 33 | 34 | All these requirements are easily being fulfilled by a controller and thus doesn't require disruptive changes to its current design. 35 | 36 | ## Extended Functionality of the Controller Manager 37 | 38 | The `ComponentManager` really only loads and unloads components at runtime and provides the necessary services for doing so. 39 | The functionality of the controller manager exceeds these by managing hardware resources and as well as the life cycle of individual controllers. 40 | 41 | ### Life Cycle Management of Controllers 42 | 43 | Each loaded controller inherits from the `ControllerInterface` and therefore from a `LifecycleNode`. 44 | The responsibilities for the controller manager differs from the component manager such that loaded controllers have to be configured and activated appropriately. 45 | It further has to react to life cycle events in case controllers run into failure behaviors and might unload the failing controller. 46 | 47 | ### Resource Management 48 | 49 | Controllers have the ability to claim hardware resources. 50 | These resources might be potentially in conflict when multiple controllers try to acquire ownership of these resources concurrently. 51 | It shall further be the responsibility of the controller manager to react appropriately to these conflicts and unload either one or all conflicting controllers. 52 | 53 | ### Execution Management of Controllers 54 | 55 | One important aspect of the controller manager is its execution model which is responsible for in which order the loaded controllers are being processed. 56 | The component manager in its current implementation accepts a pointer to an `rclcpp::Executor` which could be leverage to provide an executor tailored for the controller manager. 57 | Details on how this execution management can look like is outside of the scope of this document and shall be discussed in its dedicated design document. 58 | The point here being made is solely that a component manager must not interfere with the execution management applied in the controller manager. 59 | -------------------------------------------------------------------------------- /design_drafts/controller_execution_management.md: -------------------------------------------------------------------------------- 1 | # Controller Execution Management 2 | 3 | The following shall discuss the execution of loaded controllers inside the `ControllerManager`. 4 | As of today, the loaded controllers are processed in a sequential order which doesn't allow for much flexibility or interoperability between the controllers. 5 | We identified a few essential criteria when it comes to the execution of controllers: 6 | 7 | * Sequential Execution of Controllers 8 | * Parallel Execution of Controllers 9 | * Nesting Controllers 10 | * Variable Frequencies of Controllers 11 | 12 | We'll go into more details below for each aspect. 13 | 14 | 15 | ## Prerequisites 16 | 17 | ### Flexible Joint State Message 18 | 19 | Over the course of this document, we are referring to a "Flexible Joint State Message". 20 | This data type is precisely described in [here](flexible_joint_states_msg.md). 21 | 22 | What is important to understand for this article is that this joint state message serves as a general container structure for various, dynamically specified key values pairs. 23 | Every controller can hereby access values which are specific to itself without being limited to a set of pre-defined keys. 24 | For example can a `JointEffortController` access the robot's input state via keys like `joint_position` and `joint_velocity` and write its output torque under a key such as `torque_command`. 25 | This further allows ease of extensibility for various controllers which could access data completely independent of their underlying hardware. 26 | Various components such as camera systems or third-party peripherals can therefore be combined and shared between controllers. 27 | A typical example for this are visuomotor controllers where camera images are used to generate torque values. 28 | Relevant features extracted from camera inputs can be shared between multiple controllers to eventually calculate the appropriate control command. 29 | 30 | ### Controller and Hardware Interface 31 | 32 | Each controller gets the previously mentioned joint state message attached during their initialization phase. 33 | The controller manager is responsible for initializing the joint state message appropriately and routing it correctly to each controller. 34 | It is further the task of the controller manager to fill the message with a call to `read` from the connected hardware interface and apply the results from the controllers accordingly with a call to `write`. 35 | 36 | Each individual implementation for a hardware then reads the appropriate values out of the flexible joint state message and applies these on the actual hardware. 37 | 38 | To gain some insights about which keys each controller requires, the `ControllerInterface` specifies two abstract methods each controller implementation has to define. 39 | With `get_input_keys` the controller has to specify a list of keys which are being used to fetch its input state from the attached joint state message. 40 | Respectively, with `get_output_keys` the controller specifies a list of keys it is modifying inside the joint state message. 41 | 42 | ### Resource Management 43 | 44 | While the general container structure with key-value pairs offers a great deal of flexibility, it has to be guaranteed that no concurrent access of the same key can happen during runtime. 45 | The controller manager has to make sure that controllers don't collide with their input and output keys. 46 | 47 | One could argue that input keys should generally be flagged as read-only to keep the input state of the hardware in a valid state. 48 | This however disables basic functionality such as clamping where the input key would be the same as the output key or any other use case where a key-value pair is getting modified in place. 49 | 50 | ## Sequential Execution of Controllers 51 | 52 | All loaded controllers are currently executed in a strict round-robin order. 53 | That is, there is a tight order starting from `Controller_1`, `Controller_2`, …, `Controller_N`, where all controllers are running independent from each other. 54 | 55 | ### Controller Chaining 56 | 57 | One requirement we'd like to see is a more flexible controller chaining. 58 | Controller chaining still implies a fixed configuration in which order the controllers are being executed, however with shared resources. 59 | A typical use case for this is a safety `JointLimitsController` which is being linked after a `JointEffortController` to enforce an operation within appropriate min/max torque values. 60 | Another use case could be to clamp a `DiffDriveController` velocity values to optimize for battery lifetime or top acceleration. 61 | 62 | In order to realize such a controller chaining, the output of one's controller must be linked as the input to its consecutive controller. 63 | This further motivates a relaxation that resources such as the `JointHandle` can only be uniquely claimed. 64 | 65 | ### Sequential Controller Group 66 | 67 | Instead of loading each controller individually as an independent controller, we propose a parent group which holds and connects controllers which are configured in a specific order. 68 | The group has a single input and a single output which is being scoped within the group. 69 | That is, all controllers loaded within a group don't see any input/output states outside the group. 70 | The input and output states can then be safely re-routed and linked between individual controllers. 71 | After all controllers are executed, the final output state is being returned from the group. 72 | 73 | The image below shows how this execution model could look like: 74 | ![sequential_execution](images/sequential_execution.png "Sequential Execution group") 75 | 76 | ## Parallel Execution of Controllers 77 | 78 | Complementary to sequential execution, there are use cases where it makes sense to operate controllers in a parallel fashion. 79 | `Controller_1`, `Controller_2`, …, `Controller_N` are being executed in parallel, each operates on a copy of the group's input and individual output states are being fused with a specific strategy. 80 | 81 | ### Separation of Tasks 82 | 83 | Having controllers being run in parallel allows a better separation of tasks per controller. 84 | One easy to grasp example (also if not the most sensible) is a simple `PID-Controller`. 85 | We could break down each gain (`P`, `I`, `D`) in its own controller and let them calculate in isolation. 86 | By being able to execute multiple controllers in parallel, the calculation of the actual output can then be to simply add the `P`, `I`, `D` outputs - according to the control law of `p * (x_d - x) + p * (x_d - x) / dt + i * sum(x_d -x) * dt`. 87 | A second use case could be a differential drive controller which computes rotational and translational velocities and a second controller calculates dynamic friction values which then again can be fused together. 88 | 89 | ### Parallel Controller Group 90 | 91 | Just like in the sequential execution, we propose having a group structure which holds a set of loaded controllers which are being executed in parallel and primarily independent from each other. 92 | When all controllers were executed, each individual calculated values are being joined. 93 | As already mentioned previously, this parallel group can be initialized with a custom designed operation which dictates how all commonly calculated values (e.g. `torques`) are being combined. 94 | 95 | The parallel group also has only one input and one output state. 96 | However, the input state has to be multiplexed (essentially copied) for each controller and can't be shared in order to guarantee and isolated execution where no other controller within the same group can interfere. 97 | 98 | The image below shows how this execution model could look like: 99 | ![parallel_execution](images/parallel_execution.png "Parallel Execution group") 100 | 101 | ## Nested Controllers 102 | 103 | In both cases, the sequential as well as the parallel group can be controllers in itself. 104 | That gives the flexibility to easily nest the group within another group transparently. 105 | Again, a very simple scenario could be a parallel `PID` controller setup which is then being configured within a sequential group to enforce joint limits. 106 | This further allows to consider maybe very complex configured groups as black boxes, yet have the freedom to extend these fairly easy. 107 | In principle, this feature allows to create cascading controller schemes. 108 | 109 | The image below shows how this execution model could look like: 110 | ![nested_execution](images/nested_execution.png "Nested Execution group") 111 | 112 | ## Variable Controller Frequencies 113 | 114 | One big constraint of the round-robin approach is the constant update frequency for all loaded controllers. 115 | There are plenty of use-cases however where it makes sense to have variable update frequencies, such as running a robot controller with 1 kHz whereas a controller operating on a slower sensor only receives new data at e.g. 20 Hz. 116 | Every controller should therefore be attached with its own update frequency. 117 | 118 | Looking at the `rclcpp executor` model, one approach could be to start a timer for each controller where its callback will call the respective `update()` function. 119 | In order to establish a deterministic behavior of the timers, the controller manager has to make sure that really only timers are started and no other blocking calls such as subscriptions or services are part of it. 120 | There is quite some discussion about this at the moment of writing here: https://answers.ros.org/question/327477/ros2-uses-6-times-more-cpu-than-fastrtps/ 121 | 122 | The potential options are having a separate executor designed for ros control with adheres to the interface of the `rclcpp::executor` or to neglect the concept and implement a custom execution model for ros_control. 123 | 124 | ## Implementation - Proof of Concept 125 | 126 | for full details see https://github.com/Karsten1987/cm_poc 127 | -------------------------------------------------------------------------------- /design_drafts/design_of_controllers_and_their_status.md: -------------------------------------------------------------------------------- 1 | # Design proposals for controllers 2 | 3 | When writing controllers - there is no consistent naming convention we are using for different variables and this is quite confusing, especially, when debugging things. 4 | 5 | Proposal is to have the following names for the 4 important values in controllers: 6 | 7 | - **Reference** - input to controller from a topic or preceding controller. 8 | - **Feedback** - measured state of the system controller controls. 9 | - **Error** - control error (*Reference* - *Feedback*) 10 | - **Output** - controller's output, i.e., values written to command interfaces. This are either values of references for the following controller or input to the controlled system. 11 | 12 | This is summarized in the following figure: 13 | 14 | ![Control loop with names](images/control_loop_naming.png) 15 | 16 | Beside this, every controller **must have** `~/controller_state` topic where at least the four values mentioned above have to be output (of course if applicable). 17 | -------------------------------------------------------------------------------- /design_drafts/draws/README.md: -------------------------------------------------------------------------------- 1 | Note: draws created with [app.diagrams.net](https://app.diagrams.net/) 2 | -------------------------------------------------------------------------------- /design_drafts/draws/components_architecture.xml: -------------------------------------------------------------------------------- 1 | 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 -------------------------------------------------------------------------------- /design_drafts/fallback_controllers.md: -------------------------------------------------------------------------------- 1 | # Fallback Controllers 2 | 3 | ## Motivation 4 | 5 | Right now, we don't have a way to manage controllers when they fail to perform their tasks. We might want the robot to perform a different control scheme for the safety of the robot. The goal of the fallback controllers is to take control of the joints when the primary controller fails. 6 | 7 | ### Example 1 8 | A walking controller for a humanoid is also responsible for maintaining the balance of the robot along with footstep execution, however, there might be cases where the robot might fall after a few steps due to miscalibration or due to flexibility in some joints or it could be a different reason. In this case, we might want the robot to go to a safe position and be compliant enough for the impact, if not it might damage some joints or sensors onboard. 9 | 10 | ### Example 2 11 | A mobile manipulator is performing a trajectory hits an obstacle and continues to apply force. In this case, we might want to switch to be more compliant mode, so that it doesn't damage the robot's joints. 12 | 13 | ## Current implementation 14 | 15 | The current implementation fetches the return type from the current controller update cycle and then decides the return type status of the controller_manager's update cycle. 16 | 17 | ```c++ 18 | auto controller_ret = loaded_controller.c->update( 19 | time, (controller_update_factor != 1u) 20 | ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate) 21 | : period); 22 | 23 | if (controller_ret != controller_interface::return_type::OK) 24 | { 25 | ret = controller_ret; 26 | } 27 | ``` 28 | 29 | ## Implementation proposal 30 | 31 | Currently, we have a way to handle hardware errors in both `read` and `write` methods by deactivating the controller, but not when the controller returns an **error** type. The proposal focuses on the handling the controller return type properly along with activating a new set of controllers, known as fallback controllers. 32 | 33 | ### ControllerInfo with fallback controllers list 34 | 35 | When loading the controller by retrieving its info from the paramserver, along with it we also can get the list of fallback controllers. This list of fallback controllers can be stored in the `ControllerInfo`. 36 | 37 | ```yaml 38 | controller_manager: 39 | ros__parameters: 40 | update_rate: 100 # Hz 41 | 42 | joint1_position_controller: 43 | type: fallback_controller/FallBackController 44 | 45 | joint2_position_controller: 46 | type: fallback_controller/FallBackController 47 | 48 | position_controller: 49 | type: forward_command_controller/ForwardCommandController 50 | fallback_controllers: ["joint1_position_controller", "joint2_position_controller"] 51 | ``` 52 | 53 | ``` c++ 54 | struct ControllerInfo 55 | { 56 | /// Controller name. 57 | std::string name; 58 | 59 | /// Controller type. 60 | std::string type; 61 | 62 | /// List of claimed interfaces by the controller. 63 | std::vector claimed_interfaces; 64 | 65 | std::vector fallback_controllers; 66 | }; 67 | ``` 68 | 69 | ### Activating the controller with fallback controllers 70 | 71 | When trying to activate the controller with fallback controllers, the following checks are needed: 72 | 73 | * All the corresponding fallback controllers should be already configured 74 | * The fallback controllers list should be able to activate without any dependency on other controllers (in case of chaining with a different controller) outside the fallback controllers list 75 | * All the primary controller command interfaces should be present in the fallback controllers command interfaces 76 | 77 | If any of the above checks fail, then activation of the primary controller fails. The user will always be able to run the primary controller without configuring the fallback controller. 78 | 79 | 80 | ### The `update()` method 81 | 82 | Deactivate the controllers that have a return type other than OK, and then activate the already configured fallback controllers from the list, so that in the next update cycle, the fallback controllers can act. 83 | 84 | ```c++ 85 | std::vector stop_request = {}; 86 | std::vector start_request = {}; 87 | ... 88 | auto controller_ret = loaded_controller.c->update( 89 | time, (controller_update_factor != 1u) 90 | ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate) 91 | : period); 92 | 93 | if (controller_ret != controller_interface::return_type::OK) 94 | { 95 | stop_request.push_back(loaded_controller.info.name); 96 | cont auto &fallback_controllers_list = loaded_controller.info.fallback_controllers; 97 | start_request.insert(start_request.end(), fallback_controllers_list.begin(), fallback_controllers_list.end()); 98 | ret = controller_ret; 99 | } 100 | ... 101 | if(!stop_request.empty()) 102 | deactivate_controllers(rt_controller_list, stop_request); 103 | if(!start_request.empty()) 104 | activate_controllers(rt_controller_list, start_request); 105 | ``` 106 | 107 | ## Closing remarks 108 | 109 | - In the future, we should be able to disable all the preceding control chains prior to the controller that returns other than OK, so that it can be integrated well with controller chaining. The initial implementation can be limited to the non-chainable controllers. 110 | -------------------------------------------------------------------------------- /design_drafts/flexible_joint_states_msg.md: -------------------------------------------------------------------------------- 1 | # Flexible Joint State Messages 2 | 3 | ## Motivation 4 | 5 | Throughout `ros_control`, only the three common fields are used for each joint and actuator. 6 | These are position, velocity and effort. 7 | This is also reflected in [`sensor_msgs/JointState`](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/JointState.html), cementing this setup in place. 8 | This design is proposing to break with this practice and provide a message that can accommodate reported readings for an arbitrary set of joints or hardware interfaces. 9 | The proposed setup should both allow for reporting values for user-defined interfaces as well as making reporting position, velocity and effort optional. 10 | The rationale behind the latter is that it is currently left entirely to user-policy whether e.g. velocity and effort should be filled with zeroes when only position is reported and how to tell if this is the case. 11 | The flexible joint state message shall encompass a complex hardware setup, covering a mix of different actuators and sensors. 12 | A set of mixed-type joints would e.g. be a mobile manipulator. 13 | One could think of a diff drive controller only providing velocity values, whereas the manipulator provides the full joint state (`position`, `velocity`, `effort`) for its joints. 14 | In the example given, it would be possible to provide pre-defined fields in a statically defined joint state message, however this brings two problems with it: 15 | 1. How can the user indicate that a certain field (e.g. `effort`) is not provided for the joint? 16 | Setting the value to a specific defined value such as `NaN` brings problems, latest when trying to serialize that message. 17 | 2. What if neither of the provided fields are reported by the hardware? 18 | One example could be a suction gripper, which may report only the applied pressure or vacuum level, in which case it's again up to the user's creativity to violate one of the existing fields. 19 | 20 | ## Goal 21 | 22 | With the given motivation, we therefore propose to introduce a flexible way of reporting values in the joint state message to address the given problems mentioned above. 23 | 24 | Further in this text `value-identifier` is used to refer to a type of value reported in joint states, e.g. `position`, `velocity` and `effort`. 25 | 26 | The requirements for a flexible joint state message are fulfilled, if it 27 | 1. does not enforce defining position, velocity and effort for every joint, 28 | 2. supports different hardware interface(s) additionally to position, velocity and effort, 29 | 3. does not enforce defining values for every joint in every value-identifier, 30 | 4. provides a realtime-friendly way of use (some assumptions are fair to use), 31 | 32 | If the requirements are all fulfilled, the flexible joint state message can replace the currently used `JointStateHandles`. 33 | One can consider the flexible joint state message to represent not only a ROS message, which being used for communication. 34 | The same message type can be used internally in `ros_control` as a storage instance which is used to propagate the robot's state to the loaded controllers and to convey desired joint/actuator commands to the robot hardware. 35 | That is, the hardware interface can fill the joint state message with all information the underlying hardware reports. 36 | This would traditionally be joint states such as `position` and `velocity`, but also provides the flexibility to be extended by non-classical hardware interfaces such as grippers or custom sensors/actuators. 37 | The joint state message would then be passed to each individual controller to read the appropriate information about of the message and perform their calculation. 38 | The result of each controller can similarly be stored in the same instance of the joint state message which will be eventually passed back to the hardware interface. 39 | The hardware interface then can extract the result of the controller's computation from the message and apply their values to the hardware. 40 | That not only allows to easily combine multiple controllers, but also a dynamic composition of the hardware interfaces to contain multiple interfaces to various hardware components such as robots with interchangeable grippers. 41 | This composition can become extremely handy when dealing with external components to a robot, such as a gripper or the previously mentioned mobile manipulator, where the base platform is not necessarily coupled with the robotic arm. 42 | 43 | For more details on how the flexible joint state message can be used to create complex controller schemes, please refer to [controller execution management](controller_execution_management.md). 44 | 45 | ## Design 46 | 47 | The main advantage of `sensor_msgs/JointState` is that it provides a fairly flat structure which, given some assumptions are met, it is easy to process in realtime-safe code. 48 | Obviously, with the associate array it is easy to look up joint values for a name as its indices are aligned, i.e. the values in `position`, `velocity` and `effort` are aligned with the joint names in `name`. 49 | 50 | ``` 51 | Header header 52 | 53 | string[] name 54 | float64[] position 55 | float64[] velocity 56 | float64[] effort 57 | ``` 58 | 59 | The following proposal reflects a map-like (similar to what `std::unordered_map` represents in C++ or a hash table in other languages) datatype where not only three statically chosen value-identifiers are attached to a joint name, but can be set dynamically. 60 | 61 | ### Proposal for a Flexible Joint State Message 62 | 63 | ``` 64 | Header header 65 | 66 | string[] interface_name 67 | InterfaceValue[] interface_value 68 | string[] value_identifier 69 | float64[] value 70 | ``` 71 | 72 | where `interface_name` stands for a list of joint or hardware interface names, `interface_value` is a vector of a new message, being essentially a key-value pair, e.g. `effort` and `1.0`. 73 | 74 | In the example of the mobile manipulator, a possible configuration would look like the following: 75 | 76 | ``` 77 | Header header 78 | 79 | interface_name = ['wheel_left', 'wheel_right', 'joint_1', 'joint_2', …, 'joint_N', 'gripper_1'] 80 | interface_value = [wheel_left_iv, wheel_right_iv, joint_1_iv, joint_2_iv, …, joint_n_iv, gripper_1_iv] 81 | ``` 82 | where the `InterfaceValue` messages can be composed individually per `interface_name`. 83 | The name is similar to the already existing joint state message coming from the URDF. 84 | 85 | The `InterfaceValue` for the left wheel `wheel_left_iv` could be composed as such, reporting only its velocity: 86 | ``` 87 | value_identifier = ['velocity'] 88 | value = [1.5] 89 | ``` 90 | 91 | The `InterfaceValue` for the first joint `joint_1_iv` reports a more complete interface: 92 | ``` 93 | value_identifier = ['position', 'velocity', 'effort'] 94 | value = [1.1, 2.2, 3.3] 95 | ``` 96 | 97 | Additionally, the `InterfaceValue` for the gripper can be completely custom: 98 | ``` 99 | value_identifier = ['vacuum_level', 'voltage'] 100 | value = [4.4, 5.5] 101 | ``` 102 | 103 | That allows a relatively straight forward lookup of dynamically attached values per interface name. 104 | Each individual field is thus indexable by a tuple of `interface_name` and `value_identifier`. 105 | 106 | ## Issues Considered 107 | 108 | ### Name Lookup 109 | 110 | *Value Identifier Convention* 111 | With a dynamically allocated message values, the value identifier might not comply to any standard. 112 | Imagine a torque controller working with a particular hardware setup (and thus a particular joint state message configuration). 113 | This controller needs to know whether it can compute a torque value based on `position`, `velocity` or something completely different. 114 | An additional drawback of this key-value pair is that the keys are not necessarily defined correctly. 115 | It can potentially be very cumbersome to debug the difference between a keys like `position`, or `pos`, or `pos_val`. 116 | 117 | *Constants within the Message* 118 | A potential solution to this is to provide constants of the most applicable keys and use as such. 119 | Secondly, in order to pre-process whether all key-values pairs are available before, we propose a change to the controller interface to provide the used keys during startup time. 120 | This would allow to perform a sanity check before actually running the controller and potentially crash. 121 | For more details on this, we refer to the [Execution Management Design Doc](controller_execution_management.md). 122 | Additionally, we could provide a set of helper functions in the form of header-only files along with `control_msgs`. 123 | Nothing says that a messages package cannot ship some util code, too. 124 | 125 | ### Realtime Access Control 126 | 127 | *Concurrent Access* 128 | With multiple controllers being loaded, this could potentially lead to concurrent access of the same value in the joint state map. 129 | In order to avoid this, the current ROS1 features `JointInterfaceHandles` which essentially look the resource to avoid multiple access to the same hardware interface. 130 | However, as previously motivated this has drawbacks when designing complex control schemes, such as controller chaining or cascading. 131 | Instead of `mutex`ing and therefore locking the resources, we can provide a lock-free method. 132 | The idea is to introduce so-called controller groups, which either run controllers in parallel or sequentially. 133 | In the first case, a copy of the joint state map is attached to each controller and eventually their results will be combined into a new joint map. 134 | In the second case, the sequential controller group is responsible to attach the joint state map to the controllers in order - this can be done without any additional copies. 135 | For more information on this, the reader is again referred to [Execution Management](controller_execution_management.md). 136 | 137 | *Read-Only vs Read-Write Access* 138 | In certain cases, one might want to flag certain value identifiers as read-only, such as `joint_1_position` or `joint_1_velocity` as these values are directly measured by the robot's hardware and should not be modified during the execution of the control loop. 139 | Controllers trying to write a read-only value are thus denied and the integrity of the read-only values can be guaranteed. 140 | This could potentially be achieved by requiring that each controller has to declare its identifiers as `input` and `output` where all `input` values are by default read-only. 141 | Similarly, `output` identifiers are `read-write`, which would also cope with cases where an `input` topic is getting modified, e.g. when clamping joint torque commands, where input and output are the same identifier. 142 | 143 | However, the method mentioned here is a sole implementation detail and is not reflected in the actual message. 144 | For this, one could thing of creating a third field to the `InterfaceValue` message, which has a `enum` value for IO: 145 | ``` 146 | Header header 147 | 148 | string[] interface_name 149 | InterfaceValue[] interface_value 150 | int8 IO_READ_ONLY = 0 # read-only constant 151 | int8 IO_READ_WRITE = 1 # read-write constant 152 | string[] value_identifier 153 | float64[] value 154 | int8[] io_flag 155 | ``` 156 | 157 | ## Discarded Designs 158 | 159 | ### Flat Structure 160 | ``` 161 | Header header 162 | 163 | string[] value_identifier 164 | float64[] value 165 | ``` 166 | 167 | Alternatively to the chosen one, this proposal has one less indirection when indexing the values. 168 | Each value has to be precisely identified by its name. 169 | 170 | While being small and flat, this proposal was discarded as the indexing entails more issues than benefits as for every value per hardware interface, a unique name has to be designed. 171 | This design would most likely mean to prefix the value with its interface name, e.g. `joint_1_torque_command`, which is no better than having a level of indirection more in the index. 172 | Furthermore, this even more leads to confusion and non-standard value identifiers. 173 | 174 | ### Matrix configuration 175 | ``` 176 | Header header 177 | 178 | string[] joint_name 179 | string[] interface_name 180 | Matrix2D value 181 | ``` 182 | 183 | where ideally `Matrix2D` is a double indexable type, where the rows/columns are defined by `joint_name` and the columns/rows are defined by `interface_name`. 184 | -------------------------------------------------------------------------------- /design_drafts/hardware_access.md: -------------------------------------------------------------------------------- 1 | # Hardware Access through Controllers 2 | 3 | The following article describe how controllers can exclusively claim hardware resources yet stay as flexible as possible to avoid a strongly typed triple of 'position', 'velocity' and 'effort' [c.f. [Flexible Joint States Message](https://github.com/ros-controls/roadmap/blob/master/design_drafts/flexible_joint_states_msg.md)]. 4 | We firstly describe how hardware resources are classified and loaded. 5 | We then provide a real-time safe way of accessing these resources in controllers. 6 | 7 | ## Hardware Resources 8 | A hardware resource describes a physical component which is being considered through ros2_control. 9 | We hereby distinguish between three classes of hardware resources, Actuators, Sensor and System. 10 | Each individual hardware is loaded at runtime and thus allows a flexible and dynamic composition of the to-be-controlled setup. 11 | The hardware is composed and configured solely through the URDF. 12 | 13 | **Joint (Interface)** 14 | 15 | A joint is considered a logical component and is being actuated by at least one actuator (generally, it might be under- or over-actuated depending on the actual hardware setup). 16 | The joint is meant to be abstraction layer between a controller instance and the underlaying hardware. 17 | The abstraction is needed to shim over a potentially complex hardware setup in order to control a joint. 18 | A single joint might be controlled by multiple motors with a non-trivial transmission interface, yet a controller only takes care about joint values. 19 | 20 | A joint is configured in conjunction with a hardware resource such as **Actuator** or **System**. 21 | The joint component is used through command and state interfaces which are declared in the URDF. 22 | The command interfaces describe the value in which this joint can be controlled (e.g. effort or velocity) where as the state interfaces are considered read-only feedback interfaces. 23 | The interfaces itself can be further specified by passing in parameters. 24 | An example URDF: 25 | ```xml 26 | ... 27 | 28 | 29 | 1.5 30 | 31 | 32 | 33 | 34 | ... 35 | ``` 36 | 37 | **Sensor (Interface)** 38 | 39 | A sensor is a second logical component which represents an interface to a hardware resource which has read-only state feedback. 40 | Similar to the Joint interface, the sensor interface shims over the physical hardware. 41 | 42 | ```xml 43 | 44 | 45 | 46 | 47 | 48 | ``` 49 | A sensor interface can only be configured within a **Sensor** or **System** hardware tag. 50 | 51 | **Actuator (Hardware)** 52 | 53 | An actuator describes a single *physical actuator* instance with at max 1DoF and is strictly tight to a **Joint**. 54 | It might hereby take a single command value for its appropriate mode of operation, such as a desired joint velocity or effort - in rare cases an actuator might actually take a precise joint position value. 55 | The implementation of this actuator might then convert the desired value into PWM or other hardware specific commands and control the hardware. 56 | Similarly, an actuator might provide state feedback. 57 | Depending on the setup, the motor encoders might provide position, velocity, effort or current feedback. 58 | 59 | The URDF snippet for an actuator might look like the following: 60 | ```xml 61 | 62 | 63 | simple_servo_motor_pkg/SimpleServoMotor 64 | /dev/tty0 65 | ... 66 | 67 | 68 | 69 | -1.57 70 | 1.57 71 | 72 | 73 | 74 | ... 75 | 76 | 77 | ``` 78 | The snippet above depicts a simple hardware setup, with a single actuator which controls one logical joint. 79 | The joint here is configured to be commanded in position values, whereas the state feedback is also position. 80 | 81 | If a joint is configured with a command or state interface the hardware is not supporting, a runtime error shall occur during startup. 82 | Opposite to it, a joint might be configured with only the minimal required interfaces even though the hardware might support additional interfaces (such as "current" or "voltage"). 83 | Those shall simply be not instantiated and thus ignored. 84 | 85 | **Sensor (Hardware)** 86 | 87 | A sensor is a hardware component which only has state feedback. 88 | It can be considered as a read-only hardware resource and thus does not require exclusive access management - that is it can be used by multiple controllers concurrently. 89 | ```xml 90 | 91 | 92 | simple_sensor_pkg/SimpleSensor 93 | /dev/tty0 94 | ... 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | ``` 103 | Note that we technically have a separation between a physical hardware resource (``) and a logical component (``), both called *Sensor*. 104 | We don't individually specify them further in this document as they don't have a significant semantic interpretation for the user. 105 | 106 | **System (Hardware)** 107 | 108 | A system is meant to be a more complex hardware setup which contains multiple joints and sensors. 109 | This is mostly used for third-party robotic systems such as robotic arms or industrial setups, which have their own (proprietary) API. 110 | The implementation of the system hardware resource serves as an interface between the hardware and the controller to provide this API. 111 | ```xml 112 | 113 | 114 | complex_robot_pkg/ComplexRobot 115 | ... 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | ``` 133 | 134 | ## Resource Manager 135 | The resource manager is responsible for parsing the URDF and instantiating the respective hardware resources and logical components. 136 | It has ownership over the lifetime of these hardware resources and their associated logical joints. 137 | It serves as the storage backend for the controller manager which can loan resources to the controller. 138 | The resource manager keeps a ledger of controllers and their respective claimed resources. 139 | If a controller no longer needs access to the claimed resource, it is released and returns to the ResourceManager where it may be offered for other controllers. 140 | 141 | The resource manager internally maintains a mapping of each individual hardware resource and their interfaces. 142 | This mapping can be indexed through a simple `_logical_component_/_interface_name_` lookup. 143 | `ResourceManager` abstracts the individual hardware resources from their logical components, such that a controller does not have to know which hardware is responsible for commanding which joint. 144 | 145 | In the examples above, the actuator command interfaces are being mapped to `joint1/position`, the state interfaces equivalently to `joint1/position`. 146 | Likewise for the sensor, their interfaces are being mapped to `my_sensor/roll`, `my_sensor/pitch`, `my_sensor/yaw`. 147 | 148 | ## Controller Interface 149 | Once the system is bootstrapped and a controller is loaded, it can claim logical components and access their interfaces. 150 | 151 | **Generic Access** 152 | 153 | A controller has the chance to access a single interface value via a query to the resource manager for the respective key, such as `joint1/effort`, which - if available - claims the handle that allows to set `effort` values on `joint1` during the execution of this controller. 154 | ```c++ 155 | void MyController::init(... resource_manager) 156 | { 157 | InterfaceCommandHandle joint1_effort_cmd = resource_manager->claim_command_interface("joint1/effort"); 158 | InterfaceStateHandle joint1_position_state = resource_manager->claim_state_interface("joint1/position"); 159 | } 160 | ``` 161 | 162 | **Semantic Components** 163 | 164 | While the above example might suffice for simple setups, one can imagine that there might be quite some handles accumulated, e.g. when dealing with 6D FT/Sensors or IMUs. 165 | We therefore propose semantic components which wrap a non-zero amount of keys and provide a more meaningful API on top of it. 166 | ```c++ 167 | void MyController::init(... resource_manager) 168 | { 169 | FTSensor6D ft_sensor(resource_manager, 170 | "sensor1/fx", "sensor1/fy", "sensor1/fz", // force values 171 | "sensor1/tx", "sensor1/ty", "sensor1/tz"); // torque values 172 | 173 | std::vector torque = ft_sensor.get_torque_values(); 174 | geometry_msgs::msg::Wrench wrench_msg = ft_sensor.as_wrench_msg(); 175 | } 176 | ``` 177 | 178 | As an outlook, one could think of semantic components to provide more insights to the hardware resources. 179 | An example would be a camera sensor, where it wouldn't make much sense to provide a key for every pixel. 180 | A solution would be provide keys which describe the camera sufficiently, such as a generic `data` and `size` key. 181 | The implementation of this `Camera` class would require insights on how to interpret the `camera1/data` pointer to not treat it as an individual `double` value, but as a pointer address or similar. 182 | ```c++ 183 | Camera cam(resource_manager, "camera1/data", "camera1/size"); 184 | cv::Mat img = cam.get_image(); 185 | ``` 186 | -------------------------------------------------------------------------------- /design_drafts/images/async_controller.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/roadmap/3ad26197ce3ea6d748222c70ec8f5c68b3a6a8e1/design_drafts/images/async_controller.png -------------------------------------------------------------------------------- /design_drafts/images/chaining_example1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/roadmap/3ad26197ce3ea6d748222c70ec8f5c68b3a6a8e1/design_drafts/images/chaining_example1.png -------------------------------------------------------------------------------- /design_drafts/images/chaining_example2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/roadmap/3ad26197ce3ea6d748222c70ec8f5c68b3a6a8e1/design_drafts/images/chaining_example2.png -------------------------------------------------------------------------------- /design_drafts/images/components_architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/roadmap/3ad26197ce3ea6d748222c70ec8f5c68b3a6a8e1/design_drafts/images/components_architecture.png -------------------------------------------------------------------------------- /design_drafts/images/control_loop_naming.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/roadmap/3ad26197ce3ea6d748222c70ec8f5c68b3a6a8e1/design_drafts/images/control_loop_naming.png -------------------------------------------------------------------------------- /design_drafts/images/four_bar_linkage_transmission.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/roadmap/3ad26197ce3ea6d748222c70ec8f5c68b3a6a8e1/design_drafts/images/four_bar_linkage_transmission.png -------------------------------------------------------------------------------- /design_drafts/images/nested_execution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/roadmap/3ad26197ce3ea6d748222c70ec8f5c68b3a6a8e1/design_drafts/images/nested_execution.png -------------------------------------------------------------------------------- /design_drafts/images/parallel_execution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/roadmap/3ad26197ce3ea6d748222c70ec8f5c68b3a6a8e1/design_drafts/images/parallel_execution.png -------------------------------------------------------------------------------- /design_drafts/images/sequential_execution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/roadmap/3ad26197ce3ea6d748222c70ec8f5c68b3a6a8e1/design_drafts/images/sequential_execution.png -------------------------------------------------------------------------------- /design_drafts/images/simple_transmission.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/roadmap/3ad26197ce3ea6d748222c70ec8f5c68b3a6a8e1/design_drafts/images/simple_transmission.png -------------------------------------------------------------------------------- /design_drafts/interfaces_remapping.md: -------------------------------------------------------------------------------- 1 | # Controller Interfaces Remapping 2 | 3 | The idea of this approach is similar to the Topics, Parameters, Services and nodes in traditional ROS 2 approach. Remapping interfaces allows reusing the interfaces requested by the controller's ``state`` and ``command`` interface configuration and remap it to another interface instance. This helps to reduce the complexity of the controllers and the interfaces they can be used. 4 | 5 | For instance, a controller that uses ``effort`` as the interface type in combination with the ``joint`` names, however, there might be cases where the joints use a combination of ``prismatic`` and ``revolute`` joint types with the corresponding ``force`` and ``torque`` interfaces from the hardware. By allowing the remapping of interfaces, this exact controller can be used and these ``force`` and ``torque`` interfaces are remapped to the ``effort`` interfaces of the controller. Thereby, reduing the complexity at the controller design to support various interfaces. 6 | 7 | The proposed remapping approach is for each of the controllers to be able to define the ``remap`` parameter namespace and define the ``state`` and ``command`` interfaces. The following would be an example of the controller that remaps its interfaces 8 | 9 | ```yaml 10 | arm_controller: 11 | ros__parameters: 12 | type: joint_trajectory_controller/JointTrajectoryController 13 | joints: 14 | - arm_1_joint 15 | - arm_2_joint 16 | - arm_3_joint 17 | - arm_4_joint 18 | - arm_5_joint 19 | - arm_6_joint 20 | - arm_7_joint 21 | command_interfaces: 22 | - effort 23 | state_interfaces: 24 | - position 25 | - velocity 26 | remap: 27 | state_interfaces: 28 | arm_2_joint/position: "arm_2_joint/absolute_encoder/position" 29 | command_interfaces: 30 | arm_1_joint/effort: "arm_1_joint/torque" 31 | arm_2_joint/effort: "arm_2_joint/torque" 32 | arm_3_joint/effort: "arm_3_joint/force" 33 | arm_4_joint/effort: "arm_4_joint/torque" 34 | arm_5_joint/effort: "arm_5_joint/force" 35 | arm_6_joint/effort: "arm_6_joint/force" 36 | arm_7_joint/effort: "arm_7_joint/torque" 37 | 38 | ``` 39 | 40 | The idea is to just change the multiple ``state_interface_configuration`` and ``command_interface_configuration`` method called inside the Controller Manager with a new method that just shows the remapped part, this way it is very transparent to the user as no modifications are really needed by the controller itself. It is also better to print the remappings at the configure stage, so that the user is aware of any possible issues if it may cause. I believe the usecases from this approach is beyond the above explained usecase, and simple design of the controllers does go in favor with this approach. -------------------------------------------------------------------------------- /design_drafts/joint_interfaces_and_transmissions.md: -------------------------------------------------------------------------------- 1 | # Joint/actuator interfaces and transmission parsing 2 | 3 | In ROS Melodic and previous distributions, `ros_control` operates with statically-typed joint handles as the primary mean of communication within components of the framework. 4 | These handles are stored in joint-name to handle maps and are made available to controllers inheriting (or using other means to acquire) the relevant controller interface class e.g. 5 | ``` 6 | class DiffDriveController : 7 | public controller_interface::Controller 8 | { ... }; 9 | ``` 10 | 11 | Commands on joint interfaces are transformed into commands for actuator interfaces using transmissions. 12 | An example of such a transmission is simple reducer which applies a multiplier between commands and readings. 13 | Users of `ros_control` can extend the framework by providing their own transmissions as all transmissions are loaded as plugins. 14 | When defining a robot setup, joints and actuators are connected via transmissions declared through the URDF along with the required interfaces. 15 | At startup, the required interfaces for transmissions are validated by the `TransmissionLoader`. 16 | Transmissions use a [common interface](https://github.com/ros-controls/ros_control/blob/melodic-devel/transmission_interface/include/transmission_interface/transmission.h) to propagate values. 17 | This however already shows a shortcomings of this design as a whole as with a fixed set of interfaces, a fixed set of functions were created, most of them defined empty, for example: 18 | ``` 19 | virtual void jointToActuatorEffort(const JointData& jnt_data, 20 | ActuatorData& act_data) = 0; 21 | ``` 22 | All transmissions inheriting this interface has to define empty functions for those it does not operate on. Additionally, this set of functions is not extendable in a clean way, without incurring additional interface inheritance ([and a general mess...](https://github.com/ros-controls/ros_control/pull/395)) on the derived classes. 23 | 24 | ## Scope: 25 | This design document proposes changes to 26 | 1) joint and actuator interfaces 27 | 2) transmission parsing 28 | 3) controller interfaces (as a consequence of 1) 29 | 30 | and ties in with the use of the [flexible joint states message](https://github.com/ros-controls/roadmap/blob/master/design_drafts/flexible_joint_states_msg.md) 31 | 32 | 4) in the context of replacing `ActuatorData` and `JointData` from `ros_control` on ROS Melodic in how transmission loading and runtime is handled 33 | 5) the `joint_state_controller` which depends on the "read-only" `JointStateHandle` class to report position, velocity and effort values. 34 | 35 | ## Proposal 36 | 37 | ### urdf 38 | Over the years we didn't receive any feedback about the URDF notation being good/bad which means it's not currently a pain point for anyone at the moment, this design will not propose any changes to that. 39 | Let's stick with the notation: 40 | ``` 41 | 42 | transmission_interface/ExtendedSimpleTransmission 43 | 44 | 0.0 45 | hardware_interface/PositionJointInterface 46 | hardware_interface/VelocityJointInterface 47 | hardware_interface/EffortJointInterface 48 | awesome_interface/FooJointInterface 49 | 50 | 51 | 52 | 50 53 | 54 | 55 | ``` 56 | The content of a `hardwareInterface` XML attribute should match up with strings provided by `ros2_control` in the shape of familiar names: 57 | `hardware_interface::PositionJointInterface, hardware_interface::VelocityJointInterface, hardware_interface::EffortJointInterface`. 58 | By relying on strings internally in the framework but using them mostly through shared names, a standard nomenclature is kept all the while allowing custom extensions in an easy and transparent way. 59 | Note the in the example above, since all names are merely strings, `awesome_interface/FooJointInterface` - a custom interface - can be used if it is provided by the `RobotHardware`. 60 | 61 | ### RobotHardware 62 | 63 | ``` 64 | void RobotHardware::declareInterface(string name); 65 | ``` 66 | or 67 | ``` 68 | void RobotHardware::registerInterface(string interface_name, string joint_name); 69 | ``` 70 | 71 | a possible automated way adding these to the `RobotHardware` is to declare them when 72 | ``` 73 | template 74 | class RobotHardware{}; 75 | ... 76 | MyRobotHW : public RobotHardware 77 | ``` 78 | 79 | Initially, controller and transmission loader checks could happen through a list of registered interfaces. 80 | Once all requirements are there, this can also manifest in a few entries being added to the shared flexible joint state message instance, removing the need for a separate "registry" of interfaces. 81 | 82 | ### Transmissions 83 | 84 | The Transmission interface and base class should be merged and simplified. 85 | There is little benefit in providing multitudes of classes, requiring multitudes of checks all following the same pattern. 86 | I am proposing a much simpler setup as outlined below. 87 | 88 | ``` 89 | template 90 | class Transmission 91 | { 92 | public: 93 | virtual ~Transmission() {} 94 | 95 | virtual void actuatorToJoint(const std::string& interface_name, 96 | const ActuatorData& act_data, 97 | JointData& jnt_data) = 0; 98 | 99 | virtual void jointToActuator(const std::string& interface_name, 100 | const ActuatorData& act_data, 101 | JointData& jnt_data) = 0; 102 | }; 103 | 104 | 105 | template 106 | class ActuatorToJointTransmissionHandle 107 | { 108 | public: 109 | ActuatorToJointTransmissionHandle(const std::string& name, 110 | Transmission* transmission, 111 | const ActuatorData& actuator_data, 112 | const JointData& joint_data) 113 | : TransmissionHandle(name, transmission, actuator_data, joint_data) {} 114 | 115 | void propagate() 116 | { 117 | transmission_->actuatorToJoint("hardware_interface::PositionJointInterface", actuator_data_, joint_data_); 118 | transmission_->actuatorToJoint("hardware_interface::VelocityJointInterface", actuator_data_, joint_data_); 119 | transmission_->actuatorToJoint("hardware_interface::EffortJointInterface", actuator_data_, joint_data_); 120 | 121 | if(my_custom_check) 122 | { 123 | transmission_->actuatorToJoint("awesome_interface/FooJointInterface", actuator_data_, joint_data_); 124 | } 125 | 126 | } 127 | }; 128 | ``` 129 | 130 | The template parameters in this case may be uncalled for in a first implementation but could serve as a base for supporting other underlying data representations, not only a single one (whether it's flexible joint states or not). 131 | 132 | Transmission parsing should involve a much lighter weight setup as before. 133 | The initial implementation should be a simple "registry" of required interfaces which is fairly close to how it is done at the moment. 134 | We should aim to remove as many intermediate handlers, loaders and interfaces as possible as their added value is often minimal at a cost of large code complexity. 135 | 136 | 137 | ## Related possible changes 138 | 139 | The proposed changes open a couple of possibilities in the framework. 140 | 141 | Registered interfaces can also be interpreted in the scope of controller groups or controllers themselves, providing virtual joints, implementing data passing between chained controllers while offering some accounting. 142 | 143 | Actuator and joint interfaces need not be separate things. 144 | On top of this, transmissions could also be thought of as controllers, only much simpler. 145 | Perhaps not at the top level, but the underlying implementation of transmissions could leverage that of controllers, reducing the amount of code to be maintained. 146 | (Of course provided that it doesn't come with a high code complexity.) 147 | -------------------------------------------------------------------------------- /design_drafts/mode_switching_and_conflict_check.md: -------------------------------------------------------------------------------- 1 | # Mode switching and conflict checking 2 | 3 | # Problem 4 | 5 | ## Summary 6 | 7 | Joints can have multiple, mutually exclusive command interfaces. 8 | We need to ensure that 9 | * only one of them can be claimed at a time, 10 | * the System that provides them is aware which one is active to facilitate actuator mode switching. 11 | At the same time we should also allow multiple, non-exclusive command interfaces to be implemented. 12 | 13 | With the current implementation of `ResourceManager`, all command interfaces are made available by each System when they `export_command_interfaces()`. This setup does not allow for returning/swapping out command interfaces on-the-fly nor any communication to System components other than through command interfaces. Furthermore, the `ResourceManager` has no knowledge which System component a given command interface was exported from. 14 | 15 | ## Example system definitions 16 | 17 | These system definitions were taken from `components_archutecture_and_urdf_examples.md` for reference. 18 | 19 | ### 2. Industrial Robots with multiple interfaces (can not be written at the same time) 20 | * the communication is done using proprietary API to communicate with robot control box 21 | * Data for all joints is exchanged in batch (at once) 22 | * Examples: KUKA FRI, ABB Yumi, Schunk LWA4p, etc. 23 | 24 | ```xml 25 | 26 | 27 | ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware 28 | 2 29 | 2 30 | 31 | 32 | 33 | -1 34 | 1 35 | 36 | 37 | -1 38 | 1 39 | 40 | 41 | -0.5 42 | 0.5" 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -1 51 | 1 52 | 53 | 54 | 55 | 56 | 57 | 58 | ``` 59 | 60 | ### 2.1 Robots with multiple interfaces used at the same time - the same structure as in (2) 61 | * the communication is done using proprietary API to communicate with robot control box 62 | * Data for all joints is exchanged in batch (at once) 63 | * Multiple values can be commanded, e.g. goal position and the maximal velocity on the trajectory allowed 64 | * Examples: humanoid robots (TALOS), 4 legged robot [solo](https://github.com/open-dynamic-robot-initiative/master-board/blob/master/documentation/BLMC_%C2%B5Driver_SPI_interface.md) 65 | 66 | 67 | ## Background for joint control mode 68 | 69 | ### Classical control mode 70 | 71 | A control mode for a joint is the low level control mode applied to an actuator. It can be realized by a power electronics board with a micro controller, an embedded computer with a real-time operating system, a composition of system. 72 | 73 | The simplest system is usually the power electronics with a micro controller. 74 | For a DC motor it typically implements a Pulse Width Modulation system taking as an input a voltage and providing current as an output. 75 | 76 | The simplest control mode in this case is the so called effort mode in ROS-1 which is voltage multiply by a scalar to get current. 77 | 78 | If an encoder is present in the motor side, one can compute the difference between the motor position and the desired one, implementing a position control mode. 79 | 80 | With a sufficient encoder precision and with a high-frequency sampling (1KHz-10KHz) it is possible to compute the shaft velocity and implement a velocity control mode. 81 | 82 | To summarize it is possible to have the following three modes in one actuator: 83 | * position 84 | * velocity 85 | * effort 86 | 87 | However other control schemes exist. They are explained later on. 88 | 89 | ### Control mode in simulation 90 | 91 | An important note is that position and velocity means a desired value send to a low level control system. 92 | 93 | The time response, frequency range and stability margin depends heavily of the underlying power electronics and the gains of the control loop. 94 | In simulation those two schemes can be simulated by a PID sending a joint torque which is closer to what one will obtain on the real robot. 95 | 96 | Simulation can be used to check various level of reality. 97 | At first, when one wants to check geometrical consistency and not the underlying subsystem of the actuator, the desired position can be set as a constraint in the simulation algorithm. 98 | Then the PID simulation is irrelevant. Right now there is no way to make the difference between such two control modes. 99 | 100 | The same is true for robot using a joint torque measurement to perform torque control. 101 | 102 | 103 | ### Control mode using a closed source controller 104 | 105 | Industrial robot provides their own controller based on their integration of the power electronics and the best low level control scheme they found. 106 | 107 | ### Control mode using an open source controller 108 | When the micro controller can be programmed, it is possible to implement a control mode where desired position, desired velocity, desired effort and low level control gains are specified. The control gains can be computed using a LQR approach for instance. 109 | This is typically the case when using AC motor with power electronics (recent approach). 110 | 111 | ### General properties 112 | A control mode is a control scheme applied to an actuator. It can use one or several state and control interfaces. 113 | It is *NOT* possible to mix control mode together for one actuator. 114 | 115 | To switch from one control mode to another control mode on one actuator, it is necessary to check if this is feasible from the viewpoint of the actuator itself. 116 | 117 | Often it is dangerous to switch control mode for several actuators at once (due to self-collision for instance). 118 | The robot has to be in a specific state. This is usually done at the system level (controllers + hardware) that such safety is enforced. 119 | This can not be done at the control mode switching. 120 | 121 | # Solutions 122 | 123 | ## Proposal 1 124 | 125 | Supplied by @mahaarbo in https://github.com/ros-controls/ros2_control/pull/322 126 | 127 | This approach broadcasts all resource claims and allows System components to act on it as they see fit. The API consists of the following two main calls: 128 | 129 | ``` 130 | return_type 131 | ResourceManager::notify_command_resource_claim(const std::vector & interfaces); 132 | ``` 133 | and 134 | ``` 135 | return_type 136 | System::accept_command_resource_claim(const std::vector & interfaces); 137 | ``` 138 | where the set of control interfaces needed for the mode is provided in the interface. 139 | 140 | ## Proposal 2 141 | 142 | The mode switching logic introduced to `ros_control` in ROS Indigo using [`canSwitch()` and `doSwitch()`](https://github.com/ros-controls/ros_control/pull/200). 143 | 144 | ``` 145 | virtual bool canSwitch(const std::list &start_list, 146 | const std::list &stop_list) const; 147 | 148 | virtual void doSwitch(const std::list &start_list, 149 | const std::list &stop_list); 150 | ``` 151 | 152 | TBC -------------------------------------------------------------------------------- /design_drafts/non_joint_command_interfaces.md: -------------------------------------------------------------------------------- 1 | # Definition of command interfaces for non-joint related information 2 | 3 | ## Motivation 4 | Already multiple times, users raised the question about the handling of non-joint-related interfaces. 5 | Such interfaces are not considerd during [design of the ros2_control URDF-tag structure](./components_architecture_and_urdf_examples.md). 6 | The design proposes two possibilities to classify (group) the interfaces, using `` and `` tags. 7 | ``-tag groups the interfaces associated with the joints of physical robots and actuators. 8 | They have command and state interfaces to set the goal values for hardware and read its current state. 9 | ``-tag groups multiple state interfaces describing, e.g., internal states of hardware. 10 | 11 | Most modern robots, especially industrial manipulators, also have other physical ports to control external hardware. 12 | The most usual ones are General Purpose Inputs/Outputs (GPIOs). 13 | Although the strict definition considers only digital ports, the term GPIO in this document describes any (one-dimensional) input/output port of physical hardware, e.g., digital or analog input/output. 14 | 15 | ## Problem 16 | To use GPIOs in the ros2_control framework, they have to be listed in the robot's URDF: inputs (state interfaces) under a ``-tag and output (command and state interfaces) under a ``-tag. 17 | This could lead to confusion since non-existing (virtual) joints have to be defined in the ros2_control part of the URDF structure. 18 | "Joint" is a well-defined term in robotics. Therefore this naming should be avoided, i.e., another keyword should be used describing GPIOs, semantically separating them from the kinematics joints. 19 | 20 | ## Proposed solution 21 | Use of keyword "gpio" when describing input and output ports of a robotic device that cannot be associated with any joint or sensor. 22 | Parsing of `-tag` is similar to this of a ``-tag having command and state interfaces. 23 | The tag must have at least one ``- or ``-tag as a child. 24 | 25 | The keyword "gpio" is chosen for its generality. 26 | Although strictly used for digital signals, in this document describes any electrical analog, digital signal, or physical value. 27 | 28 | The `` tag can be used as a child of all three types of hardware interfaces, i.e., system, sensor, or actuator. 29 | Semantically, they describe values that can not be associated with a joint or a sensor. 30 | 31 | ### Examples 32 | 33 | 1. Robot with multiple GPIO interfaces 34 | - RRBot System 35 | - Digital: 4 inputs and 2 outputs 36 | - Analog: 2 inputs and 1 output 37 | - Vacuum valve at the flange (on/off) 38 | 39 | ``` 40 | 41 | 42 | ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware 43 | 2.0 44 | 3.0 45 | 2.0 46 | 47 | 48 | 49 | -1 50 | 1 51 | 52 | 53 | 54 | 55 | 56 | -1 57 | 1 58 | 59 | 60 | 61 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 71 | 72 | 73 | 74 | 75 | 77 | 78 | 79 | 80 | ``` 81 | 82 | 2. Gripper with electrical and suction grasping possibilities 83 | - Multimodal gripper 84 | - 1-DoF parallel gripper 85 | - suction on/off 86 | 87 | ``` 88 | 89 | 90 | ros2_control_demo_hardware/MultimodalGripper 91 | 92 | 93 | 94 | 0 95 | 100 96 | 97 | 98 | 99 | 101 | 102 | 103 | 104 | ``` 105 | 106 | 3. Force-Torque-Sensor with temperature feedback and adjustable calibration 107 | - 2D FTS 108 | - Temperature feedback in °C 109 | - Choice between 3 calibration matrices, i.e., calibration ranges 110 | 111 | ``` 112 | 113 | 114 | ros2_control_demo_hardware/ForceTorqueSensor2DHardware 115 | 0.43 116 | 117 | 118 | 119 | 120 | kuka_tcp 121 | 100 122 | 100 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | ``` 133 | 134 | -------------------------------------------------------------------------------- /design_drafts/ros2_control_boilerplate.md: -------------------------------------------------------------------------------- 1 | This file is a description of `ros2_control_boilerplate` repository, which implements a simple simulation of ros2_control concepts. 2 | The repository has two goals: 3 | 4 | 1. It provides templates for faster start of implementing own hardware and controllers; 5 | 2. The repository is a validation environment for `ros2_control` concepts, which can only be tested during run-time (e.g., execution of controllers by the controller manager, communication between robot hardware and controllers). 6 | 7 | When finalized, this document becomes `README.md` of the `ros2_control_boilerplate` repository. 8 | Therefore is the remainder of this document structured as such. 9 | 10 | 11 | # ROS2 Control Boilerplate 12 | 13 | This repository provides templates for the development of `ros2_control`-enabled robots and a simple simulation of a robot to demonstrate and prove `ros2_control` concepts. 14 | The repository is inspired by [ros_control_boilerplate](https://github.com/PickNikRobotics/ros_control_boilerplate) repository from Dave Coleman. 15 | The simulation has three parts/packages: 16 | 1. The first package, `boilerplate_robot_headless`, uses scripts to simulate communication to and movements of virtual hardware. 17 | This package does not have any dependencies except on the `ros2` core packages and can, therefore, be used on SoC-hardware of headless systems. 18 | 2. The second package, `boilerplate_robot_gazebo`, uses a gazebo simulator to simulate the *boilerplate robot* and its physics. 19 | This package is useful to test the connection of `ros2_control` to the gazebo simulator and to detect any missing plugins. 20 | 3. The third package holds template/example files for `ros2_control` enabled robots and controllers. 21 | The intention of those files is to simplify start with `ros2_control` and to enable faster integration of new robots and controllers. 22 | 23 | This repository demonstrates the following `ros2_control` concepts: 24 | 25 | * Creating of `hardware_interface` for a Robot, Sensor, and Actor 26 | * Creating a robot description in the form of YAML files 27 | * Loading configuration and starting robot using launch files 28 | * Control of two joints of *boilerplate robot* 29 | * Using simulated robots and starting `ros_control` with gazebo simulator 30 | * Implementing of controller switching strategy for a robot 31 | * Using joint limits and transmission concepts in `ros2_control` 32 | * TBD 33 | -------------------------------------------------------------------------------- /design_drafts/ros2_control_documentation.rst: -------------------------------------------------------------------------------- 1 | =========================== 2 | ros2_control Framework 3 | =========================== 4 | 5 | The ros2_control is a framework for (real-time) control of robots using `ROS` (`Robot Operating System `__). 6 | Its packages are a rewrite of `ros_control `__ packages to simplify integrating new hardware and overcome some drawbacks. 7 | 8 | If you are not familiar with the control theory, please get some idea about it (e.g., at `Wikipedia `_) to get familiar with the terms used in this manual. 9 | 10 | .. contents:: Table of Contents 11 | :depth: 2 12 | 13 | Overview 14 | ======== 15 | The ros2_control framework's source can be found in `ros-controlls/ros2_control`_ and `ros-controls/ros2_controllers`_ GitHub-repositories. 16 | The following figure shows the Architecture of the ros2_control framework. 17 | 18 | |ros2_control_architecture| 19 | 20 | Controller Manager 21 | ------------------ 22 | The `Controller Manager`_ (CM) connects the controllers' and hardware-abstraction sides of the ros2_control framework. 23 | It also serves as the entry-point for users through ROS services. 24 | The CM implements a node without an executor so it can be integrated into a custom setup. 25 | Still, for a standard user, it is recommended to use the default node-setup implemented in `ros2_control_node `_ file from the ``controller_manager`` package. 26 | This manual assumes that you use this default node-setup. 27 | 28 | On the one side, CM manages (e.g., loading, activating, deactivating, unloading) controllers and from them required interfaces. 29 | On the other side, it has access to the hardware components (through Resource Manager), i.e., their interfaces. 30 | The Controller Manager matches *required* and *provided* interfaces, gives controllers access to hardware when activated, or reports an error if there is an access conflict. 31 | 32 | The execution of the control-loop is managed by the CM's ``update()`` method. 33 | The method reads data from the hardware components, updates outputs of all active controllers, and writes the result to the components. 34 | 35 | Resource Manager 36 | ---------------- 37 | The `Resource Manager`_ (RM) abstracts physical hardware and its drivers (called *hardware components*) for the ros2_control framework. 38 | The RM loads the components using ``pluginlib``-library, manages their lifecycle and components' state and command interfaces. 39 | This abstraction provided by RM enables re-usability of implemented hardware components, e.g., robot and gripper, without any implementation and flexible hardware application for state and command interfaces, e.g., separate hardware/communication libraries for motor control and encoder reading. 40 | 41 | In the control loop execution, the RM's ``read()`` and ``write()`` methods deal with communication to the hardware components. 42 | 43 | .. _overview-controllers: 44 | Controllers 45 | ----------- 46 | The controllers in the ros2_control framework have the same functionality as defined in the control theory. They compare the reference value with the measured output and, based on this error, calculate a system's input (for more details, visit `Wikipedia `_). 47 | The controlles are objects derived from `ControllerInterface`_ (``controller_interface`` package in `ros-controls/ros2_control`_) and exported as plugins using ``pluginlib``-library. 48 | For example of on controller check `ForwardCommandController implementation`_ in the `ros-controls/ros2_controllers`_ repository. 49 | The controllers' lifecycle is based on the `LifecycleNode-Class`_ implementing the state machine as described in the `Node Lifecycle Design`_ document. 50 | 51 | When executing the control-loop ``update()`` method is called. 52 | The method can access the latest hardware states and enable the controller to write the hardware's command interfaces. 53 | 54 | User Interfaces 55 | --------------- 56 | Users interact with the ros2_control framework using `Controller Manager`_'s services. 57 | Those can be used directly or through the "cli"-interface (base command: ``ros2 control``). 58 | The "cli"-interface is more user-friendly for direct interaction, i.e., outside of a node. 59 | 60 | For a list of services and their definitions, check the ``srv`` folder in the `controller_manager_msgs`_ package. 61 | 62 | For the description of the "cli"-interface, see the ``README.md`` file of the `ros2controlcli`_ package. 63 | 64 | 65 | Hardware Components 66 | =================== 67 | The *hardware components* realize communication to physical hardware and represent its abstraction in the ros2_control framework. 68 | The components have to be exported as plugins using ``pluginlib``-library. 69 | The `Resource Manager`_ dynamically loads those plugins and manages their lifecycle. 70 | 71 | There are three basic types of components: 72 | 73 | System 74 | Complex (multi-DOF) robotic hardware like industrial robots. 75 | The main difference between the *Actuator* component is the possibility to use complex transmissions like needed for humanoid robot's hands. 76 | This component has reading and writing capabilities. 77 | It is used when the is only one logical communication channel to the hardware (e.g., KUKA-RSI). 78 | 79 | Sensor 80 | Robotic hardware is used for sensing its environment. 81 | A sensor component is related to a joint (e.g., encoder) or a link (e.g., force-torque sensor). 82 | This component type has only reading capabilities. 83 | 84 | Actuator 85 | Simple (1 DOF) robotic hardware like motors, valves, and similar. 86 | An actuator implementation is related to only one joint. 87 | This component type has reading and writing capabilities. Reading is not mandatory if not possible (e.g., DC motor control with Arduino board). 88 | The actuator type can also be used with a multi-DOF robot if its hardware enables modular design, e.g., CAN-communication with each motor independently. 89 | 90 | 91 | A detailed explanation of hardware components is given in the `Hardware Access through Controllers design document`_. 92 | 93 | Hardware Description in URDF 94 | ---------------------------- 95 | The ros2_control framework uses the ````-tag in the robot's URDF file to describe its components, i.e., the hardware setup. 96 | The chosen structure enables tracking together multiple `xacro`-macros into one without any changes. 97 | The example hereunder shows a position-controlled robot with 2-DOF (RRBot), an external 1-DOF force-torque sensor, and an externally controlled 1-DOF parallel gripper as its end-effector. 98 | For more examples and detailed explanations, check `ros-controls/ros2_control_demos`_ repository and `ROS2 Control Components URDF Examples design document`_. 99 | 100 | .. code:: xml 101 | 102 | 103 | 104 | ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware 105 | 2 106 | 2 107 | 108 | 109 | 110 | -1 111 | 1 112 | 113 | 114 | 115 | 116 | 117 | -1 118 | 1 119 | 120 | 121 | 122 | 123 | 124 | 125 | ros2_control_demo_hardware/ForceTorqueSensor1DHardware 126 | 0.43 127 | 128 | 129 | 130 | rrbot_tcp 131 | -100 132 | 100 133 | 134 | 135 | 136 | 137 | ros2_control_demo_hardware/PositionActuatorHardware 138 | 1.23 139 | 3 140 | 141 | 142 | 143 | 0 144 | 50 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | Running the Framework for Your Robot 153 | ------------------------------------ 154 | To run the ros2_control framework, do the following. 155 | The example files can be found in the `ros2_control_demos`_ repository. 156 | 157 | #. Create a YAML file with the configuration of the controller manager and controllers. (`Example for RRBot `_) 158 | #. Extend the robot's URDF description with needed ```` tags. 159 | It is recommended to use macro files instead of pure URDF. (`Example for RRBot `_) 160 | #. Create a launch file to start the node with `Controller Manager`_. 161 | You can use a default `ros2_control node`_ (recommended) or integrate the controller manager in your software stack. 162 | (`Example launch file for RRBot `_) 163 | 164 | Repositories 165 | ============ 166 | The ros2_control framework consist of the following repositories: 167 | 168 | ros2_control 169 | The `ros2_control`_ repository implements the main interfaces and components of the framework mentioned in the previous sections. 170 | 171 | ros2_controllers 172 | The `ros2_controllers`_ repository implements widely used controllers, e.g., forward controller, joint trajectory controller, differential drive controller, etc. 173 | 174 | ros2_control_demos 175 | The `ros2_control_demos`_ repository provides examples of using the framework and templates for a smooth start with it. 176 | 177 | Differences to ros_control (ROS1) 178 | ================================= 179 | 180 | Hardware Structures - classes 181 | ----------------------------- 182 | 183 | The ros_control framework uses the ``RobotHW`` class as a rigid structure to handle any hardware. 184 | This makes it impossible to extend the existing robot with additional hardware, like sensors, actuators, and tools, without coding. 185 | The ``CombinedRobotHardware`` corrects this drawback. 186 | Still, this solution is not optimal, especially when combining robots with external sensors. 187 | 188 | The ros2_control framework defines three types of hardware ``Actuator``, ``Sensor`` and ``System``. 189 | Using a combination (composition) of those basic components, any physical robotic cell (robot and its surrounding) can be described. 190 | This also means that multi-robot, robot-sensor, robot-gripper combinations are supported out of the box. 191 | Section `Hardware Components <#hardware-components>`__ describes this in detail. 192 | 193 | Hardware Interfaces 194 | ------------------- 195 | 196 | The ros_control allows only three types of interfaces (joints), i.e., ``position``, ``velocity``, and ``effort``. The ``RobotHW`` class makes it very hard to use any other data to control the robot. 197 | 198 | The ros2_control does not mandate a fixed set of interface types, but they are defined as strings in `hardware's description <#hardware-description-in-urdf>`__. 199 | To ensure compatibility of standard controllers, standard interfaces are defined as constants in `hardware_interface package `__. 200 | 201 | Controller's Access to Hardware 202 | ------------------------------- 203 | 204 | In ros_control, the controllers had direct access to the ``RobotHW`` class requesting access to its interfaces (joints). 205 | The hardware itself then took care of registered interfaces and resource conflicts. 206 | 207 | In ros2_control, ``ResourceManager`` takes care of the state of available interfaces in the framework and enables controllers to access the hardware. 208 | Also, the controllers do not have direct access to hardware anymore, but they register their interfaces to the `ControllerManager`. 209 | 210 | Migration Guide to ros2_control 211 | =============================== 212 | 213 | RobotHardware to Components 214 | --------------------------- 215 | #. The implementation of ``RobotHW`` is not used anymore. 216 | This should be migrated to SystemInterface`_ class or, for more granularity, `SensorInterface`_ and `ActuatorInterface`_. 217 | See the above description of "Hardware Components" to choose a suitable strategy. 218 | #. Decide which component type is suitable for your case. Maybe it makes sense to separate ``RobotHW`` into multiple components. 219 | #. Implement `ActuatorInterface`_, `SensorInterface`_ or `SystemInterface`_ classes as follows: 220 | 221 | #. In the constructor, initialize all variables needed for communication with your hardware or define the default one. 222 | #. In the configure function, read all the parameters your hardware needs from the parsed URDF snippet (i.e., from the `HardwareInfo`_ structure). Here you can cross-check if all joints and interfaces in URDF have allowed values or a combination of values. 223 | #. Define interfaces to and from your hardware using ``export_*_interfaces`` functions. 224 | The names are ``/`` (e.g., ``joint_a2/position``). 225 | This can be extracted from the `HardwareInfo`_ structure or be hard-coded if sensible. 226 | #. Implement ``start`` and ``stop`` methods for your hardware. 227 | This usually includes changing the hardware state to receive commands or set it into a safe state before interrupting the command stream. 228 | It can also include starting and stopping communication. 229 | #. Implement `read` and `write` methods to exchange commands with the hardware. 230 | This method is equivalent to those from `ŔobotHW`-class in ROS1. 231 | #. Do not forget the ``PLUGINLIB_EXPORT_CLASS`` macro at the end of the .cpp file. 232 | #. Create .xml library description for the pluginlib, for example see `RRBotSystemPositionOnlyHardware `_. 233 | 234 | 235 | Controller Migration 236 | -------------------- 237 | An excellent example of a migrated controller is the `JointTrajectoryController`_. 238 | The real-time critical methods are marked as such. 239 | 240 | #. Implement `ControllerInterface`_ class as follows: 241 | 242 | #. If there are any member variables, initialized those in the constructor. 243 | #. In the `init` method, first call ``ControllerInterface::init`` initialize lifecycle of the controller. 244 | Then declare all parameters defining their default values. 245 | #. Implement the ``state_interface_configuration()`` and ``command_interface_configuration()`` methods. 246 | #. Implement ``update()`` function for the controller. (**real-time**) 247 | #. Then implement required lifecycle methods (others are optional): 248 | * ``on_configure`` - reads parameters and configures controller. 249 | * ``on_activate`` - called when controller is activated (started) (**real-time**) 250 | * ``on_deactivate`` - called when controller is deactivated (stopped) (**real-time**) 251 | #. Do not forget ``PLUGINLIB_EXPORT_CLASS`` macro at the end of the .cpp file. 252 | #. Create .xml library description for the pluginlib, for example see `JointTrajectoryController `_. 253 | 254 | 255 | 256 | .. _ros-controls/ros2_control: https://github.com/ros-controls/ros2_control 257 | .. _ros-controls/ros2_controllers: https://github.com/ros-controls/ros2_controllers 258 | .. _ros-controls/ros2_control_demos: https://github.com/ros-controls/ros2_control_demos 259 | .. _controller_manager_msgs: https://github.com/ros-controls/ros2_control/tree/master/controller_manager_msgs 260 | .. _Controller Manager: https://github.com/ros-controls/ros2_control/blob/master/controller_manager/src/controller_manager.cpp 261 | .. _ControllerInterface: https://github.com/ros-controls/ros2_control/blob/master/controller_interface/include/controller_interface/controller_interface.hpp 262 | .. _ros2_control node: https://github.com/ros-controls/ros2_control/blob/master/controller_manager/src/ros2_control_node.cpp 263 | .. _ForwardCommandController implementation: https://github.com/ros-controls/ros2_controllers/blob/master/forward_command_controller/src/forward_command_controller.cpp 264 | .. _Resource Manager: https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/src/resource_manager.cpp 265 | .. _LifecycleNode-Class: https://github.com/ros2/rclcpp/blob/master/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp 266 | .. _JointTrajectoryController: https://github.com/ros-controls/ros2_controllers/blob/master/joint_trajectory_controller/src/joint_trajectory_controller.cpp 267 | .. _Node Lifecycle Design: https://design.ros2.org/articles/node_lifecycle.html 268 | .. _ros2controlcli: https://github.com/ros-controls/ros2_control/tree/master/ros2controlcli 269 | .. _Hardware Access through Controllers design document: https://github.com/ros-controls/roadmap/blob/master/design_drafts/hardware_access.md 270 | .. _ROS2 Control Components URDF Examples design document: https://github.com/ros-controls/roadmap/blob/master/design_drafts/components_architecture_and_urdf_examples.md 271 | 272 | .. _ActuatorInterface: https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/actuator_interface.hpp 273 | .. _SensorInterface: https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/sensor_interface.hpp 274 | .. _SystemInterface: https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/system_interface.hpp 275 | .. _HardwareInfo: https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/hardware_info.hpp 276 | 277 | 278 | .. |ros2_control_architecture| image:: images/components_architecture.png 279 | :alt: "ros2_control Architecture" 280 | -------------------------------------------------------------------------------- /design_drafts/transmission_loading_and_parametrization.md: -------------------------------------------------------------------------------- 1 | # Transmission loading and parametrization 2 | 3 | ## Motivation 4 | 5 | The role of transmissions in `ros_control` and `ros2_control` is to implement value transformations that reflect the effect of different mechanical transmissions. 6 | These implement the same underlying mathematical formulas that are used when modeling mechanical transmissions. 7 | 8 | The value transformation can be responsible for something as simple as a reducer transmission, where a certain *joint-level* value is divided by a factor accounting for a mechanical reduction rate to compute the *actuator-level* command to be sent to the actuator while the inverse is applied for feedback from the actuator. 9 | 10 | 11 | ![ros2_control Simple Transmission][simple_transmission] 12 | 13 | 14 | More complex mechanical transmissions involve more than one joint where per-joint reduction rate depends on the state of other joints in the system, such as a four-bar linkage transmission. 15 | 16 | ![ros2_control FourBarLinkage Transmission][four_bar_linkage_transmission] 17 | 18 | Disclaimer: The `transmission_interface` library is not always required. It mainly depends on the level of abstraction the hardware implements and makes available through the hardware-level communication interfaces. An industrial arm for instance will typically expose *joint-level* interfaces through their comms and will have individually actuated joints. A humanoid robot on the other hand may employ differential transmissions for implementing multi-axis joints often seen in wrists, four-bar-linkages for larger parts of the body, such as torso movement, or a complex leg with a single actuator but multiple joints with feedback from them, etc. 19 | 20 | 21 | ## How it works in ROS Noetic and problems 22 | 23 | To facilitate extendability, transmissions are implemented as plugins that are loaded and configured at startup time. This allows anyone to implement their custom transmission plugins in any ROS package without having to change `ros_control`. 24 | 25 | ### XML format 26 | 27 | An additional XML description for each joint has to be added to the URDF in order to define transmissions on them. 28 | 29 | ``` 30 | 31 | 32 | 33 | 34 | 35 | transmission_interface/FourBarLinkageTransmission 36 | 37 | joint2 38 | hardware_interface/PositionJointInterface 39 | 40 | 41 | joint1 42 | hardware_interface/PositionJointInterface 43 | 44 | 45 | actuator1 46 | 50 47 | 48 | 49 | actuator2 50 | -50 51 | 52 | 53 | 54 | 55 | ``` 56 | 57 | Let's dissect the XML above: 58 | 59 | * `transmission_interface/FourBarLinkageTransmission` defines the transmission plugin to be loaded, parameters are following 60 | * `` defines the role and hardware interface of joints. The joint `bar_joint` should already be defined in the URDF. 61 | * `joint2` is internal to transmission implementations, used to determine where in a transmission a certain joint or actuator takes it's place in the underlying formulas. Typically this is implemented by ordering joints and actuators according to their role (e.g. `[actuator1, actuator2]` and `[joint1, joint2]`) in the vector that gets passed in the configure stage. 62 | * `hardware_interface/PositionJointInterface` defines the *joint interface type* which the transmission needs to know for picking the right formula to apply to the values, e.g. a reduction rate may have a different effect to position, velocity or effort values 63 | * the section enclosed by `` defines the actuator name, role and mechanical reduction rate to be used in the underlying formula. 64 | 65 | Let's look at some issues now. 66 | 67 | ### More than one role 68 | 69 | This is the first and only place *joint interface type* is defined for a joint. It should not be the responsibility of transmissions to carry this information but since the URDF has no support for it, it had to be added here. 70 | 71 | Throughout ROS1 there are many robots using `SimpleTransmission` with a reduction rate of 1 only to be able to define the type of joint interfaces they use so `gazebo_ros_control` can use the intended joint interfaces and simulate their robot properly. 72 | 73 | ### Actuator names are not really defined 74 | 75 | Actuators are virtually non-existent in the URDF, the first and only place they are shown is in the transmission definition. It is entirely up to the one implementing the `RobotHW` to make sense of these names. 76 | 77 | Common policies are to match `bar_joint` to `bar_actuator` or `joint1` to `joint1_motor`. 78 | 79 | ### Joint and actuator tags 80 | 81 | There are some minor issues that cause confusion from time to time: 82 | * Why is the `hardwareInterface` tag paired with the joint? Could be defined for both if we want to allow different interfaces or if we want to ensure that they are not different for compatibility... 83 | * Why is the `mechanicalReduction` tag paired with the actuator? The reduction is applied to one of them when the information flows from actuator to controller but the inverse is applied when it flows from controller to actuator. 84 | 85 | 86 | 87 | ## Proposals for ROS2 XML format 88 | 89 | The plugin-based system should be kept for transmissions. The tag `type` was replaced with `plugin` in the xml format for clarity and consistency with the rest of `ros2_control`. 90 | 91 | The parametrization should become simpler as it doesn't need to concern itself anymore with the joint command interface types as they are defined elsewhere. 92 | 93 | It is however still unclear if there is any use to define actuator names outside the transmission scope. 94 | 95 | Where to place `mechanical_reduction`? 96 | 97 | The mandatory parameters within the `` tag should `` and at least one `` and `` tag. 98 | 99 | The parametrization of `` and `` should contain `name` and `role` at all times but additional parameters should be allowed to be freely defined as different transmission may require/support different ones, let them report if they are unhappy with the configuration they were given. 100 | 101 | ### Proposal 1 102 | 103 | Merging the current ros2_control xml format with the ROS1 transmissions: 104 | 105 | ```xml 106 | 107 | 108 | ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware 109 | 2.0 110 | 3.0 111 | 2.0 112 | 113 | 114 | 115 | -1 116 | 1 117 | 118 | 119 | 120 | 121 | 122 | -1 123 | 1 124 | 125 | 126 | 127 | 128 | 129 | transmission_interface/DifferentialTransmission 130 | 131 | actuator1 132 | 133 | 134 | actuator2 135 | 136 | 137 | joint1 138 | 0.5 139 | 10 140 | 141 | 142 | joint2 143 | 50 144 | 145 | 146 | 147 | ``` 148 | 149 | ### Proposal 2 150 | 151 | Merging the current ros2_control xml format with the ROS1 transmissions but making `role` and `mechanical_reduction` tags mandatory and one-liners: 152 | 153 | ```xml 154 | 155 | 156 | ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware 157 | 2.0 158 | 3.0 159 | 2.0 160 | 161 | 162 | 163 | -1 164 | 1 165 | 166 | 167 | 168 | 169 | 170 | -1 171 | 1 172 | 173 | 174 | 175 | 176 | 177 | transmission_interface/DifferentialTransmission 178 | 179 | 180 | 181 | 0.5 182 | 183 | 184 | 185 | 186 | ``` 187 | 188 | 189 | 190 | [simple_transmission]: images/simple_transmission.png "SimpleTransmission" 191 | [four_bar_linkage_transmission]: images/four_bar_linkage_transmission.png "FourBarLinkageTransmission" 192 | -------------------------------------------------------------------------------- /documentation_resources.md: -------------------------------------------------------------------------------- 1 | # Documentation resources 2 | 3 | A collection of external resources, aka blog posts, tutorials, videos created by the community. 4 | Any level of complexity is appreciated as long as it's an effective way to explain concepts of `ros_control`. 5 | 6 | * The quintessential presentation by Adolfo: https://vimeo.com/107507546 7 | * [`ros_control`](http://wiki.ros.org/ros_control) and [`ros_controllers`](http://wiki.ros.org/ros_controllers) ROS wiki pages 8 | * [ROS Control, An Overview](https://fjp.at/posts/ros/ros-control/): summary of Adolfo's ROSCon presentation ([edit on github](https://github.com/fjp/fjp.github.io/blob/master/collections/_posts/ros/2020-03-13-ros-controls.md)). 9 | * `joint_trajectory_controller`: [ROS wiki](http://wiki.ros.org/joint_trajectory_controller?distro=melodic), [Understanding trajectory replacement](http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement) 10 | * ros_control tutorials [ROS wiki page](http://wiki.ros.org/ros_control/Tutorials) 11 | * ros_control [github wiki on basics](https://github.com/ros-controls/ros_control/wiki) 12 | * [`diff_drive_controller` github wiki](https://github.com/ros-controls/ros_controllers/wiki/diff_drive_controller) 13 | * [`combined_robot_hw` ROS wiki](http://wiki.ros.org/combined_robot_hw?distro=melodic) 14 | * [`controller_manager` ROS wiki](http://wiki.ros.org/controller_manager?distro=melodic) 15 | * [`realtime_tools` ROS wiki](http://wiki.ros.org/realtime_tools?distro=melodic) 16 | * `transmission interface` [ROS wiki](http://wiki.ros.org/transmission_interface?distro=melodic) and [C++ doxy pages](http://docs.ros.org/melodic/api/transmission_interface/html/c++/index.html) 17 | * NASA's [How To Write A Controller For Valkyrie](https://gitlab.com/nasa-jsc-robotics/valkyrie/-/wikis/How-To-Write-A-Controller-For-Valkyrie) 18 | * Getting started with a custom RobotHW: https://slaterobots.com/blog/5abd8a1ed4442a651de5cb5b/how-to-implement-ros_control-on-a-custom-robot 19 | --------------------------------------------------------------------------------