From 3c156be15b0314e3a49428733a02850b31881271 Mon Sep 17 00:00:00 2001 From: Franz Pucher <1286618+fjp@users.noreply.github.com> Date: Sat, 3 Sep 2022 22:02:00 +0200 Subject: [PATCH] Feat: Document low-level and high-level PIDs - Add pid.md about PIDs, tuning and gain trim model - Add low-level.md and high-level.md to document both approaches - Re-structure diffbot_base package documentation using index pages - Move related microcontroller documention into package/diffbot_base folder - Add svg images showing low and high level interfaces of ROS Control Feat: Update navigation - Re-structure navigation using new content pages - Use navigation.indexes feature from mkdocs material --- .../diffbot_base/high-level.md} | 7 +- docs/packages/diffbot_base/index.md | 136 ++++++ docs/packages/diffbot_base/low-level.md | 413 ++++++++++++++++++ docs/packages/diffbot_base/pid.md | 253 +++++++++++ .../diffbot_base/scripts}/teensy-mcu.md | 0 docs/{packages.md => packages/index.md} | 26 +- .../block-diagram-low-high-level.svg | 1 + .../diffbot_base/roscontrol-sim-reality.svg | 1 + mkdocs.yml | 21 +- 9 files changed, 846 insertions(+), 12 deletions(-) rename docs/{diffbot_base.md => packages/diffbot_base/high-level.md} (99%) create mode 100644 docs/packages/diffbot_base/index.md create mode 100644 docs/packages/diffbot_base/low-level.md create mode 100644 docs/packages/diffbot_base/pid.md rename docs/{ => packages/diffbot_base/scripts}/teensy-mcu.md (100%) rename docs/{packages.md => packages/index.md} (72%) create mode 100644 docs/resources/packages/diffbot_base/block-diagram-low-high-level.svg create mode 100644 docs/resources/packages/diffbot_base/roscontrol-sim-reality.svg diff --git a/docs/diffbot_base.md b/docs/packages/diffbot_base/high-level.md similarity index 99% rename from docs/diffbot_base.md rename to docs/packages/diffbot_base/high-level.md index dc48efce..1e72e85d 100644 --- a/docs/diffbot_base.md +++ b/docs/packages/diffbot_base/high-level.md @@ -1,11 +1,12 @@ --- -title: Autonomous Differential Drive Mobile Robot - Base Package -description: "ROS Base Package for ROS Noetic running on a Raspberry Pi 4 for an autonomous 2WD Robot to act in an environment according to sensor information." +title: Base Package - High Level Approach +description: "ROS Base Package for ROS Noetic running on a Raspberry Pi 4 for an autonomous 2WD Robot to act in an environment according to sensor information. +This describes the implementation of the diffbot_base package following the high-level approach." categories: [robotics] tags: [2wd, differential drive, robot, ros, noetic, raspberry, pi, autonomous, ubuntu, focal, package, gazebo, simulation, hardware_interfacem, hardware, interface, ros-control, control, controllers, diff_drive_controller] --- -## DiffBot Base Package +## DiffBot Base Package - High Level Approach This package contains the so called hardware interface of DiffBot which represents the real hardware in software to work with [ROS Control](http://wiki.ros.org/ros_control). diff --git a/docs/packages/diffbot_base/index.md b/docs/packages/diffbot_base/index.md new file mode 100644 index 00000000..8669c41b --- /dev/null +++ b/docs/packages/diffbot_base/index.md @@ -0,0 +1,136 @@ +--- +title: Autonomous Differential Drive Mobile Robot - Base Package +description: "ROS Base Package for ROS Noetic running on a Raspberry Pi 4 for an autonomous 2WD Robot to act in an environment according to sensor information." +categories: [robotics] +tags: [2wd, differential drive, robot, ros, noetic, raspberry, pi, autonomous, ubuntu, focal, package, gazebo, simulation, hardware_interfacem, hardware, interface, ros-control, control, controllers, diff_drive_controller] +--- + +## DiffBot Base Package + +This package contains the so called hardware interface of DiffBot which represents the real hardware in software to work with +[ROS Control](http://wiki.ros.org/ros_control). + +
+ +
ROS Control Overview.
+
+ +This package contains the platform-specific code for the base controller component required by the ROS Navigation Stack. It consists of the firmware based on rosserial +for the Teensy MCU and the C++ node running on the SBC that instantiates the +ROS Control hardware interface, which includes the `controller_manager` control loop +for the real robot. + +The low-level `base_controller` component reads the encoder ticks from the hardware, +calculates angular joint positions and velocities, and publishes them to the +ROS Control hardware interface. Making use of this interface makes it possible +to use the [`diff_drive_controller`](http://wiki.ros.org/diff_drive_controller) package +from ROS Control. It provides a controller (`DiffDriveController`) for a differential +drive mobile base that computes target joint velocities from commands received by either +a teleop node or the ROS Navigation Stack. The computed target joint velocities are +forwarded to the low-level base controller, where they are compared to the measured +velocities to compute suitable motor PWM signals using two separate PID controllers, +one for each motor. + +Another part of this package is a launch file that will + +- Load the robot description from `diffbot_description` to the paramter server +- Run the hardware interface of this package `diffbot_base` +- Load the controller configuration yaml from the `diffbot_control` package to the [parameter server](http://wiki.ros.org/Parameter%20Server) +- Load the controllers with the [controller manager](http://wiki.ros.org/controller_manager?distro=noetic) +- Load the value of the encoder resolution to the parameter server + + +## Low-Level vs High-Level PID + +There exist (at least) two commonly used approaches to control a robot base. + +The difference between the two presented approaches here is where the PID controller(s) +that control each motor are kept. One possibility is to run these PIDs on the high level hardware interface on the SBC and sending the computed output commands to a motor driver node. Another option operates the PIDs on the low-level microcontroller hardware. For DiffBot these approaches are refered to as + +- High-Level PIDs running on the hardware interface on the SBC +- Low-Level PIDs running on the firmware of the microcontroller + +The DiffBot project initially used the PID controllers in the high level hardware interface. From release [1.0.0](https://github.com/ros-mobile-robots/diffbot/tree/1.0.0) on two low-level PIDs are operating on the low-level base controller hardware. + +This section focuses on the current approach (low-level PIDs). +The previous approach (high-level PID) is documented in [High-Level Approach](high-level.md). + +## Developing a low-level controller firmware and a high-level ROS Control hardware interface for a differential drive robot + +In the following two sections, the base controller, mentioned in the Navigation +Stack, will be developed. For DiffBot/Remo, this platform-specific node is split +into two software components. + +The first component is the high-level `diffbot::DiffBotHWInterface` that +inherits from `hardware_interface::RobotHW`, acting as an interface between +robot hardware and the packages of ROS Control that communicate with the +Navigation Stack and provide +[`diff_drive_controller`](http://wiki.ros.org/diff_drive_controller) – one of +many available controllers from ROS Control. With the `gazebo_ros_control` +plugin, the same controller including its configuration can be used in +simulation and the real robot. + +An overview of ROS Control in simulation and the real world is given in the +following figure (http://gazebosim.org/tutorials/?tut=ros_control): + +
+ +
ROS Control in Simulation and Reality
+
+ +The second component is the low-level base controller that measures angular +wheel joint positions and velocities and applies the commands from the +high-level interface to the wheel joints. The following figure shows the +communication between the two components: + +
+ +
Block diagram of the low-level controller and the high-level hardware interface
+
+ +The low-level base controller uses two PID controllers to compute PWM signals +for each motor based on the error between measured and target wheel velocities. +`RobotHW` receives measured joint states (angular position (rad) and angular +velocity (rad/s)) from which it updates its joint values. With these measured +velocities and the desired command velocity (`geometry_msgs/Twist` message on the +`cmd_vel` topic), from the Navigation Stack, the `diff_drive_controller` computes +the target angular velocities for both wheel joints using the mathematical +equations of a differential drive robot. This controller works with continuous +wheel joints through a `VelocityJointInterface` class. The computed target +commands are then published within the high-level hardware interface inside the +robots `RobotHW::write` method. Additionally, the controller computes and +publishes the odometry on the odom topic (`nav_msgs/Odometry`) and the transform +from `odom` to `base_footprint`. + + +Having explained the two components of the base +controller, the low-level firmware is implemented first. The high-level hardware +interface follows the next section. + +But before this an introduction to the PID controllers are given in [PID Controllers](pid.md). + +### diffbot_base Package + +The `diffbot_base` package is created with `catkin-tools`: + +```console +fjp@diffbot:/home/fjp/catkin_ws/src$ catkin create pkg diffbot_base --catkin-deps diff_drive_controller hardware_interface roscpp sensor_msgs rosparam_shortcuts +Creating package "diffbot_base" in "/home/fjp/catkin_ws/src"... +Created file diffbot_base/package.xml +Created file diffbot_base/CMakeLists.txt +Created folder diffbot_base/include/diffbot_base +Created folder diffbot_base/src +Successfully created package files in /home/fjp/catkin_ws/src/diffbot_base. +``` + +To work with this package the specified dependencies must be installed either using the available Ubuntu/Debian packages for ROS Noetic or they have to be built from source first. The following table lists the dependencies should have been install during the initial setup phase. These dependencies are not already part of the ROS Noetic desktop full installation but required for the `diffbot_base` package. + +!!! note + + Follow the instructions at [ROS Noetic Setup](https://fjp.at/projects/diffbot/ros-noetic/) on how to setup ROS and the [obtain (system) dependencies](../index.md#obtain-system-dependencies) section on how to install all required dependencies. Performing these steps avoids installing any dependencies manually. + +| Dependency | Source | Ubuntu/Debian Package | +|:-----------------------------:|:-----------------------------------------------------:|:--------------------------------:| +| `rosparam_shortcuts` | https://github.com/PickNikRobotics/rosparam_shortcuts | `ros-noetic-rosparam-shortcuts` | +| `hardware_interface` | https://github.com/ros-controls/ros_control | `ros-noetic-ros-control` | +| `diff_drive_controller` | https://github.com/ros-controls/ros_controllers | `ros-noetic-ros-controllers` | diff --git a/docs/packages/diffbot_base/low-level.md b/docs/packages/diffbot_base/low-level.md new file mode 100644 index 00000000..f9c62f89 --- /dev/null +++ b/docs/packages/diffbot_base/low-level.md @@ -0,0 +1,413 @@ +--- +title: Base Package - Low Level Approach +description: "ROS Base Package for ROS Noetic running on a Raspberry Pi 4 for an autonomous 2WD Robot to act in an environment according to sensor information. +This describes the implementation of the diffbot_base package following the low-level approach." +categories: [robotics] +tags: [2wd, differential drive, robot, ros, noetic, raspberry, pi, autonomous, ubuntu, focal, package, gazebo, simulation, hardware_interfacem, hardware, interface, ros-control, control, controllers, diff_drive_controller] +--- + +# DiffBot Base Package - Low Level Approach + +Having explained the two components of the base +controller in [DiffBot Base](index.md), the low-level firmware is implemented first, followed by the high-level hardware, detailed in the next section. + +## Implementing the low-level base controller for Remo + +The low-level base controller is implemented on the Teensy microcontroller using +[PlatformIO](https://platformio.org/). The programming language in PlatformIO is +the same as Arduino (based on Wiring) and it is available as a plugin for the +Visual Studio Code editor. On the development PC, we can flash the robot +firmware to the board with this plugin. We will get the firmware code from the +`diffbot_base` ROS package, located in the +[`scripts/base_controller`](https://github.com/ros-mobile-robots/diffbot/tree/noetic-devel/diffbot_base/scripts/base_controller) +subfolder. Opening this folder in Visual Studio Code will recognize it as a +PlatformIO workspace because it contains the `platformio.ini` file. This file +defines the required dependencies and makes it straightforward to flash the +firmware to the Teensy board after compilation. Inside this file, the used +libraries are listed: + +```console +lib_deps = frankjoshua/Rosserial Arduino Library@^0.9.1 + adafruit/Adafruit Motor Shield V2 Library@^1.0.11 + Wire +``` + +As you can see, the firmware depends on `rosserial`, the `Adafruit Motor Shield +V2` library, and `Wire`, an I2C library. PlatformIO allows using custom +libraries defined in the local `./lib` folder, which are developed in this +section. + +The firmware is used to read from encoders and ultrasonic and IMU sensors, and +receive wheel velocity commands from the high-level +`hardware_interface::RobotHW` class, discussed in the next section. The +following code snippets are part of the low-level base controller’s `main.cpp` +file and show the used libraries, found in +`diffbot_base/scripts/base_controller`, in the `lib` and `src` folders. `src` +contains `main.cpp` consisting of the `setup()` and `loop()` functions, common +to every Arduino sketch and starts off by including the following headers: + +```cpp +#include +#include "diffbot_base_config.h" +``` + +Besides the ros header file, it includes definitions specific to Remo, which are +defined in the `diffbot_base_config.h` header. It contains constant parameter +values such as the following: + +- Encoder pins: Defines to which pins on the Teensy microcontroller the Hall + effect sensors are connected. +- Motor I2C address and pins: The Adafruit motor driver can drive four DC + motors. Due to cable management, motor terminals `M3` and `M4` are used for + the left and right motors, respectively. +- PID: The tuned constants for both PID controllers of `base_controller`. +- PWM_MAX and PWM_MIN: The minimum and maximum possible PWM values that can be + sent to the motor driver. +- Update rates: Defines how often functions of `base_controller` are executed. + For example, the control portion of the low-level base controller code reads + encoder values and writes motor commands at a specific rate. + +After including Remo-specific definitions, next follows the custom libraries in +the `lib` folder: + + ```cpp + #include "base_controller.h" + #include "adafruit_feather_wing/adafruit_feather_wing.h" + ``` + +These include directives and the libraries that get included with them are +introduced next: + +- `base_controller`: Defines the `BaseController` template class, defined in the + `base_controller.h` header, and acts as the main class to manage the two + motors, including each motor’s encoder, and communicate with the high-level + hardware interface. +- `motor_controller_intf`: This library is indirectly included with + `adafruit_feather_wing.h` and defines an abstract base class, named + `MotorControllerIntf`. It is a generic interface used to operate a single + motor using arbitrary motor drivers. It is meant to be implemented by other + specific motor controller subclasses and therefore avoids changing code in + classes that know the `MotorControllerIntf` interface and call its + `setSpeed(int value)` method, such as done by `BaseController`. The only + requirement for this to work is for a subclass to inherit from this + `MotorControllerIntf` interface and implement the `setSpeed(int value)` class + method. +- `adafruit_feather_wing`: This library, in the `motor_controllers` folder, + implements the `MotorControllerIntf` abstract interface class and defines a + concrete motor controller. For Remo, the motor controller is defined in the + `AdafruitMotorController` class. This class has access to the motor driver + board and serves to operate the speed of a single motor, which is why two + instances are created in the `main.cpp` file. +- `encoder`: This library is used in the `BaseController` class and is based on + `Encoder.h` from https://www.pjrc.com/teensy/td_libs_Encoder.html that allows + reading encoder tick counts from quadrature encoders, like the DG01D-E motors + consist of. The encoder library also provides a method `jointState()` to + directly obtain the joint state, which is returned by this method in the + `JointState` struct, that consists of the measured angular position (rad) and + angular velocity (rad/s) of the wheel joints: + + ```cpp + diffbot::JointState diffbot::Encoder::jointState() { + long encoder_ticks = encoder.read(); + ros::Time current_time = nh_.now(); + ros::Duration dt = current_time - prev_update_time_; + double dts = dt.toSec(); + double delta_ticks = encoder_ticks - prev_encoder_ticks_; + double delta_angle = ticksToAngle(delta_ticks); + joint_state_.angular_position_ += delta_angle; + joint_state_.angular_velocity_ = delta_angle / dts; + prev_update_time_ = current_time; + prev_encoder_ticks_ = encoder_ticks; + return joint_state_; + } + ``` + +- `pid`: Defines a PID controller to compute PWM signals based on the velocity + error between measured and commanded angular wheel joint velocities. For more + infos about PIDs and the tuning sections refer to [PID Controllers](pid.md) + + +With these libraries, we look at the `main.cpp` file. Inside it exists only a +few global variables to keep the code organized and make it possible to test the +individual components that get included. The main code is explained next: + +1. First, we define the global ROS node handle, which is referenced in other + classes, such as BaseController, where it is needed to publish, subscribe, or + get the current time, using `ros::NodeHandle::now()`, to keep track of the + update rates: + + ```cpp + ros::NodeHandle nh; + ``` + +2. For convenience and to keep the code organized, we declare that we want to + use the `diffbot` namespace, where the libraries of the base controller are + declared: + + ```cpp + using namespace diffbot; + ``` + +3. Next, we define two concrete motor controllers of type + `AdafruitMotorController` found in the `motor_controllers` library: + + ```cpp + AdafruitMotorController motor_controller_right = AdafruitMotorController(3); + AdafruitMotorController motor_controller_left = AdafruitMotorController(4); + ``` + + This class inherits from the abstract base class `MotorControllerIntf`, + explained above. It knows how to connect to the Adafruit motor driver using its + open source `Adafruit_MotorShield` library + (https://learn.adafruit.com/adafruit-stepper-dc-motor-featherwing/library-reference) + and how to get a C++ pointer to one of its DC motors (`getMotor(motor_num)`). + Depending on the integer input value to `AdafruitMotorController::setSpeed(int + value)`, the DC motor is commanded to rotate in a certain direction and at a + specified speed. For Remo, the range is between –255 and 255, specified by the + `PWM_MAX` and `PWM_MIN` identifiers. + +4. The next class that is defined globally inside main is `BaseController`, + which incorporates most of the main logic of this low-level base controller: + + ```cpp + BaseController base_controller(nh, &motor_controller_left, &motor_controller_right); + ``` + + As you can see, it is a template class that accepts different kinds of motor + controllers (`TMotorController`, which equals `AdafruitMotorController` in the + case of Remo) that operate on different motor drivers (`TMotorDriver`, which + equals `Adafruit_MotorShield`), using the `MotorControllerIntf` interface as + explained previously. The `BaseController` constructor takes a reference to the + globally defined ROS node handle and the two motor controllers to let it set the + commanded speeds computed through two separate PID controllers, one for each + wheel. + + In addition to setting up pointers to the motor controllers, the + `BaseController` class initializes two instances of type `diffbot::Encoder`. Its + measured joint state, returned from `diffbot::Encoder::jointState()`, is used + together with the commanded wheel joint velocities in the `diffbot::PID` + controllers to compute the velocity error and output an appropriate PWM signal + for the motors. + +After defining the global instances, the firmware’s `setup()` function is +discussed next. The low-level BaseController class communicates with the +high-level interface `DiffBotHWInterface` using ROS publishers and subscribers. +These are set up in the `Basecontroller::setup()` method, which is called in the +`setup()` function of `main.cpp`. In addition to that, the +`BaseController::init()` method is here to read parameters stored on the ROS +parameter server, such as the wheel radius and distance between the wheels. +Beside initializing `BaseController`, the communication frequency of the motor +driver is configured: + +```cpp +void setup() { + base_controller.setup(); + base_controller.init(); + motor_controller_left.begin(); + motor_controller_right.begin(); +} +``` + +The `begin(uint16_t freq)` method of the motor controllers has to be called +explicitly in the main `setup()` function because `MotorControllerIntf` doesn't +provide a `begin()` or `setup()` method in its interface. This is a design +choice that, when added, would make the `MotorControllerIntf` less generic. + +After the `setup()` function follows the `loop()` function, to read from sensors +and write to actuators, which happens at specific rates, defined in the +`diffbot_base_config.h` header. The bookkeeping of when these read/write +functionalities occurred is kept in the `BaseController` class inside its +`lastUpdateRates` struct. Reading from the encoders and writing motor commands +happens in the same code block as the control rate: + +```cpp +void loop() { +ros::Duration command_dt = nh.now() - base_controller.lastUpdateTime().control; +if (command_dt.toSec() >= ros::Duration(1.0 / base_controller.publishRate().control_, 0).toSec()) { + base_controller.read(); + base_controller.write(); + base_controller.lastUpdateTime().control = nh.now(); +} +``` + +The following steps in this code block happen continuously at the control rate: + +1. Encoder sensor values are read through the `BaseController::read()` method + and the data is published inside this method for the high-level + `DiffbotHWInterface` class, on the `measured_joint_states` topic of message + type `sensor_msgs::JointState`. +2. The `BaseController` class subscribes to `DiffBotHWInterface` from which it + receives the commanded wheel joint velocities (topic `wheel_cmd_velocities`, + type `diffbot_msgs::WheelsCmdStamped`) inside the + `BaseController::commandCallback(const diffbot_msgs::WheelsCmdStamped&)` + callback method. In `BaseController::read()`, the PID is called to compute + the motor PWM signals from the velocity error and the motor speeds are set + with the two motor controllers. +3. To keep calling this method at the desired control rate, the + `lastUpdateTime().control` variable is updated with the current time. + +After the control loop update block, if an IMU is used, its data could be read +at the imu rate and published for a node that fuses the data with the encoder +odometry to obtain more precise odometry. Finally, in the main `loop()`, all the +callbacks waiting in the ROS callback queue are processed with a call to +`nh.spinOnce()`. + +MThis describes the low-level base controller. For more details and the complete +library code, please refer to the `diffbot_base/scripts/base_controller` +package. + +In the following section, the `diffbot::DiffBotHWInterface` class is described. + + +## ROS Control high-level hardware interface for a differential drive robot + +The [ros_control](http://wiki.ros.org/ros_control) meta package contains the +hardware interface class `hardware_interface::RobotHW`, which needs to be +implemented to leverage many available controllers from the `ros_controllers` +meta package. First, we’ll look at the `diffbot_base` node that instantiates and +uses the hardware interface: + +1. The `diffbot_base` node includes the `diffbot_hw_interface.h` header, as well + as the `controller_manager`, defined in `controller_manager.h`, to create the + control loop (read, update, write): + + ```cpp + #include + #include + #include + ``` + +2. Inside the main function of this `diffbot_base` node, we define the ROS node + handle, the hardware interface (`diffbot_base::DiffBotHWInterface`), and pass + it to the `controller_manager`, so that it has access to its resources: + + ```cpp + ros::NodeHandle nh; + diffbot_base::DiffBotHWInterface diffBot(nh); + controller_manager::ControllerManager cm(&diffBot); + ``` + +3. Next, set up a separate thread that will be used to service ROS callbacks. + This runs the ROS loop in a separate thread as service callbacks can block + the control loop: + + ```cpp + ros::AsyncSpinner spinner(1); + spinner.start(); + ``` + +4. Then define at which rate the control loop of the high-level hardware + interface should run. For Remo, we choose 10 Hz: + + ```cpp + ros::Time prev_time = ros::Time::now(); + ros::Rate rate(10.0); rate.sleep(); // 10 Hz rate + ``` + +5. Inside the blocking while loop of the `diffbot_base` node, we do basic + bookkeeping to get the system time to compute the control period: + + ```cpp + while (ros::ok()) { + const ros::Time time = ros::Time::now(); + const ros::Duration period = time - prev_time; + prev_time = time; + ... + ``` + +6. Next, we execute the control loop steps: read, update, and write. The + `read()` method is here to get sensor values, while `write()` writes commands + that were computed by `diff_drive_controller` during the `update()` step: + + ```cpp + ... + diffBot.read(time, period); + cm.update(time, period); + diffBot.write(time, period); + ... + ``` + +7. These steps keep getting repeated with the specified rate using + `rate.sleep()`. + +After having defined the code that runs the main control loop of the +`diffbot_base` node, we’ll take a look at the implementation of +`diffbot::DiffBotHWInterface`, which is a child class of +`hardware_interface::RobotHW`. With it, we register the hardware and implement +the `read()` and `write()` methods, used above in the control loop. + +The constructor of the `diffbot::DiffBotHWInterface` class is used to get +parameters from the parameter server, such as the `diff_drive_controller` +configuration from the `diffbot_control` package. Inside the constructor, the +wheel command publisher and measured joint state subscriber are initialized. +Another publisher is `pub_reset_encoders_`, which is used in the +`isReceivingMeasuredJointStates` method to reset the encoder ticks to zero after +receiving measured joint states from the low-level base controller. + +After constructing `DiffBotHWInterface`, we create instances of +`JointStateHandles` classes (used only for reading) and `JointHandle` classes +(used for read, and write access) for each controllable joint and register them +with the `JointStateInterface` and `VelocityJointInterface` interfaces, +respectively. This enables the `controller_manager` to manage access for joint +resources of multiple controllers. Remo uses `DiffDriveController` and +`JointStateController`: + +```cpp +for (unsigned int i = 0; i < num_joints_; i++) { + hardware_interface::JointStateHandle joint_state_handle(joint_names_[i], &joint_positions_[i], &joint_velocities_[i], &joint_efforts_[i]); + joint_state_interface_.registerHandle(joint_state_handle); + + hardware_interface::JointHandle joint_handle(joint_state_handle, &joint_velocity_commands_[i]); + velocity_joint_interface_.registerHandle(joint_handle); +} +``` + +The last step that is needed to initialize the hardware resources is to register +the `JointStateInterface` and the `VelocityJointInterface` interfaces with the +robot hardware interface itself, thereby grouping the interfaces together to +represent Remo robot in software: + +```cpp +registerInterface(&joint_state_interface_); +registerInterface(&velocity_joint_interface_); +``` + +Now that the hardware joint resources are registered and the controller manager +knows about them, it’s possible to call the `read()` and `write()` methods of +the hardware interface. The controller manager update happens in between the +read and write steps. Remo subscribes to the `measured_joint_states` topic, +published by the low-level base controller. The received messages on this topic +are stored in the `measured_joint_states_ array` of type +`diffbot_base::JointState` using the `measuredJointStateCallback` method, and +are relevant in the `read()` method: + +1. The `read()` method is here to update the measured joint values with the current sensor readings from the encoders – angular positions (rad) and velocities (rad/s): + + ```cpp + void DiffBotHWInterface::read() { + for (std::size_t i = 0; i < num_joints_; ++i) { + joint_positions[i] = measured_joint_states[i].angular_position; + joint_velocity[i] = measured_joint_states[i].angular_velocity; + } + ``` + +2. The final step of the control loop is to call the `write()` method of the `DiffBotHWInterface` class to publish the angular wheel velocity commands of each joint, computed by `diff_drive_controller`: + + ```cpp + void DiffBotHWInterface::write() { + diffbot_msgs::WheelsCmdStamped wheel_cmd_msg; + for (int i = 0; i < NUM_JOINTS; ++i) { + wheel_cmd_msg.wheels_cmd.angular_velocities.joint.push_back(joint_velocity_commands_[i]); + } + pub_wheel_cmd_velocities_.publish(wheel_cmd_msg); + } + ``` + +In this method, it would be possible to correct for steering offsets due to +model imperfections and slight differences in the wheel radii. See [gain / trim +model](pid.md#gain--trim-model). + +This concludes the important parts of the `DiffBotHWInterface` class and enables +Remo to satisfy the requirements to work with the ROS Navigation Stack. In the +next section, we’ll look at how to bring up the robot hardware and how the +started nodes interact with each other. diff --git a/docs/packages/diffbot_base/pid.md b/docs/packages/diffbot_base/pid.md new file mode 100644 index 00000000..25b10c84 --- /dev/null +++ b/docs/packages/diffbot_base/pid.md @@ -0,0 +1,253 @@ + +# PID Controllers + +DiffBot uses two PID controllers, one for each motor. +Depending on which software version you use the two PID controllers are located in +different locations in the source code. Either the two PIDs are in the + +- hardware interface: high-level approach until version 0.0.2, or +- base controller firmware: low-level approach starting from version 1.0.0 + +The current approach (starting from version 1.0.0) operates the PIDs in the +low-level base controller firmware. + +Regardless of the location (high or low-level), each PID is passed the error +between velocity measured by the encoders and the target velocity computed by +the `diff_drive_controller` for a specific wheel joint. The +`diff_drive_controller` doesn't have a PID controller integrated, and doesn't +take care if the wheels of the robot are actually turning. As mentioned above, +ROS Control expects that the commands sent by the controller are actually +applied on the real robot hardware and that the joint states are always up +to date. This means that the `diff_drive_controller` just uses the `twist_msg` +on the `cmd_vel` topic for example from the `rqt_robot_steering` and converts it +to a velocity command for the motors. It doesn't take the actual velocity of the +motors into account. + +!!! note + + See [the code of `diff_drive_controller`](https://github.com/ros-controls/ros_controllers/blob/698f85b2c3467dfcc3ca5743d68deba03f3fcff2/diff_drive_controller/src/diff_drive_controller.cpp#L460) where the `joint_command_velocity` is calculated. + +For this reason a PID controller can help to avoid situations such as the following where the robot moves not straigth although it's commanded to do so: + + + +!!! note + + The video runs version 0.0.2, operating the PIDs in the high-level hardware interface. + +The PIDs used in the video inherits from the ROS Control +[`control_toolbox::Pid`](http://wiki.ros.org/control_toolbox) that provides +Dynamic Reconfigure out of the box to tune the proportional, integral and +derivative gains. The behaviour when using only the P, I and D gains is that the +output can overshoot and even change between positive and negative motor percent +values because of a P gain that is too high. To avoid this, a feed forward gain F +can help to reach the setpoint faster. In the high-level approach this feed forward gain is present as a dynamic reconfigure parameter, defined in the in the `pid.yml` configuration file in the `cfg` folder of this package. + +For more details on ROS dynamic reconfigure see [the official tutorials](http://wiki.ros.org/dynamic_reconfigure/Tutorials). + +With the use of the PID controller the robot is able to drive straight: + + + +In case of using inexpensive motors like the [DG01D-E](https://www.sparkfun.com/products/16413) of DiffBot, +you have to take inaccurate driving behaviour into account. The straight driving behaviour can be improved +with motors that start spinning at the same voltage levels. To find suitable motors do a voltage sweep test by slightly increasing the voltage and +note the voltage level where each motor starts to rotate. Such a test was done on DiffBot's motors. + +Using six [DG01D-E](https://www.sparkfun.com/products/16413) motors the following values were recorded (sorted by increasing voltage): + +| Motor | Voltage (V) | +|:-----:|:-----------:| +| 01 | 2.5 | +| 02 | 2.8 - 3.0 | +| 03 | 3.1 | +| 04 | 3.2 | +| 05 | 3.2 | +| 06 | 3.3 | + + +!!! note + + In the videos above, motors numbered 01 and 03 were used coincidencely and I + wasn't aware of the remarkable differences in voltage levels. Using the motors + 04 and 05 improved the driving behaviour significantly. + +To deal with significant differences in the motors it would also help to tune +the two PIDs individually, which is not shown in the [video +above](https://youtu.be/fdn5Mu0Qhl8). + + +!!! note + + Make also sure that the motor driver outputs the same voltage level on both channels when the robot is commanded to move straight. + The used Grove i2c motor driver was tested to do this. + Another problem of not driving straight can be weight distribution or the orientation of the caster wheel. + +A good test to check the accuracy is to fix two meters of adhesive tape on the +floor in a straight line. Then, place the robot on one end oriented in the +direction to the other end. Now command it to move straight along the line and +stop it when it reaches the end of the tape. Record the lateral displacement +from the tape. Measuring a value below 10 cm is considered precise for these +motors. + + + +The video shows the real DiffBot robot as well as running the +`gazebo_ros_control` plugin with `diff_drive_controller` from ROS Control. The +robot is commanded to drive straight on a 2 meter test track. Five runs show +different offsets after 2 meter, which can be cause by: + +- Orientation of the start pose +- Inaccurate motors start to spin at different voltage levels +- Low encoder resolution (geared output resolution at wheel) +- Wheel slip or alignment of the left/right and caster wheel +- Weight distribution +- Manufacturing tolerances +- Deformable material of the wheels generating different surfaces of contact with the ground depending on their assembly + +## Gain / Trim Model + +In case the robot constantly drives towards one side (e.g., to the left), this +issue might be overcome by tuning each PID individually. An alternative solution +can be gain and trim parameters, as implemented in the [`write()` method of the +high-level hardware +interface](https://github.com/ros-mobile-robots/diffbot/blob/e7b6c8c5206235a338ef15e20084630793813a4f/diffbot_base/src/diffbot_hw_interface.cpp#L169). + +```cpp +double motor_constant_right_inv = (gain_ + trim_) / motor_constant_; +double motor_constant_left_inv = (gain_ - trim_) / motor_constant_; + +joint_velocity_commands_[0] = joint_velocity_commands_[0] * motor_constant_left_inv; +joint_velocity_commands_[1] = joint_velocity_commands_[1] * motor_constant_right_inv; +``` + +The idea is that the trim coefficient $t$ is a "quick and dirty" way to account for +various nuisances of the physical robot, including differences in motors, +wheels, and the asymmetry of the robot. It's simple and easily interpretable, +too. + +The two PIDs work to ensure that the wheels spin at the desired speed, but not +that the robot drives the desired way though. There are other factors such as +slightly different wheel raddi $R$ which lead to different rotational velocities +$\dot{\phi}_{l/r}$ (translational velocity $v_{l/r} = \dot{\phi}_{l/r} \cdot R$) and therefore different traveled distances for each wheel (left and right) $d_{l/r}$: + +$$ +\begin{aligned} +d_{l/r} &= v_{l/r} \cdot \Delta t \\ +d_{l/r} &= \frac{\dot{\phi}_{l/r}}{R} \cdot \Delta t +\end{aligned} +$$ + +These mechanical variations "can't be seen" by the encoders alone. For example +assume you want the robot to go straight, you would set the two desired +rotational wheel velocities $\dot{\phi}_{l/r}$'s to be equal - but still the +robot would go left if, e.g., $R_r > R_l$, because, for the same wheel rotation +(ticks), the right wheel has traveled a bigger distance. + +So the question is: what is the control objective? + +When the control objective is to follow a desired reference position (angular or +lateral), it is more meaningful to measure that as error driving the PID +controller. But nothing stops us from having several PID controllers in cascade +sequence. One for the, e.g., lateral position, that computes the desired $(v_0, +\omega)$ of the robot, and two more in parallel for the wheel velocities. Still, +there would be a trim coefficient to compensate for differences in traveled +distances of the two wheels. + + +$d_{l/r}$ are the distances traveled by the left and right wheels, respectively, in a $\Delta t$: + +$$ +d_{l/r} = R_{l/r} \cdot \Delta \phi_{l/r} +$$ + +where $\Delta \phi_{l/r}$ are the rotations of each wheel (more accurately, of each motor), and $R_{l/r}$ is the radius of each wheel. If the radii are exactly the same, then $R_l = R_r = R$ and the approach using only two PID controllers would work because controlling $\dot{\phi_{l/r}} \approx \frac{\Delta \phi_{l/r,k}}{\Delta t_k}$ is equivalent to controlling $\Delta \phi_{l/r,k}$, as the time interval we can assume known. + +In practice, $R_l = R_r = R$ does not hold because of manufacturing tolerances, deformable material of the wheels generating different surfaces of contact with the ground depending on their assembly and robot mass distribution, etc. + +The trim parameter $t$ we define as something that adjusts the relationship between the commands sent to the motors (imagine voltage $V$) and the angular speed of each wheel ($\dot{\phi}$): + +$$ +\begin{aligned} +V_l = k(1-t) \dot \phi_l \\ +V_r = k(1+t) \dot \phi_r +\end{aligned} +$$ + + +Note that it's the same value of $t$ for the two motors ($k$, just assume to be +a given constant). The way we tweak it is to find the value such that the robot +goes straight, that is, $d_l = d_r$ (over some distance), when we command it to go +straight, that is, we send the same voltages to the two wheels ($V_l = V_r$). + +$$ +\begin{aligned} +V_l &= V_r \\ +k(1-t) \dot\phi_l &= k(1+t) \dot\phi_r +\end{aligned} +$$ + +From the two equations ($V_l$, $V_r$), by imposing the assumptions above and using the +definition of $d_{l/r}$, we can derive $t = \frac{R_r - R_l}{R_r + R_l}$, which basically shows how the trim is there to +(empirically) compensate for the difference in the wheel radii. + +!!! info "source" + + The trim / gain approach was customized for [Duckiebots](https://docs.duckietown.org/daffy/opmanual_duckiebot/out/wheel_calibration.html). The broader context is + system identification: a good resource for further reading could be [Russ + Tedrake's underactuated robotics + course](http://underactuated.mit.edu/sysid.html). + + +After the robot can drive on the straight line without deviating from it too far the final error can be overcome with addtional sensors, such as IMUs, cameras and LiDARs, and then fuse those sensor data to follow a goal location more accurately by constantly adjusting wheel velocities. An even simpler example are IR sensors to follow the line and adjust the wheel velocities according to stay near the line. + +### PID Tuning + +To tune the PID controller, the following video provides a helpful overview of +the process. It gives some insights which behavior each PID parameter influences +for a vehicle trying to follow a lane, thereby reducing the cross track error. +However, the same process can be followed to reduce the error $e(t)$ being the +difference between the desired (target) angular wheel velocity $\dot +\phi_{desired,l/r}$ and the one measured $\dot \phi_{measured,l/r}$ by the +encoders: + +$$ +e_{l/r} = \dot{\phi}_{desired,l/r} - \dot{\phi}_{measured,l/r} +$$ + + + +In case of oscillations (wheel spin direction keeps changing), might happen +because of a too high proportional gain $K_P$ or a too low derivative gain +$K_D$. Changing gains should be done one at a time because all are related by +influencing the overall calculated output command $u(t)$ via the following equation + +$$ +u(t) = K_\text{p} e(t) + K_\text{i} \int_0^t e(\tau) \,\mathrm{d}\tau + K_\text{d} \frac{\mathrm{d}e(t)}{\mathrm{d}t} +$$ + +One option to start the tuning process is by setting these values: + +P = something small until the motors start to spin and no major oscillations are observable +I = 0 +D = 0 + +To overcome oscillations increase the D gain first but usually it will be a value below P (might be one or more magnitudes lower). The derivative term tracks how fast the velocity changes over time. A higher D gain can dampen oscillations and therefore lead to more stable behavior. The D term best estimate of the future trend of the error $e(t)$. +If the derivative gain is too low then the system is called underdamped and will pull too quickly towards the desired velocity, which can easily result in overshooting the target value. On the other hand if the D gain is too high the system is known to be overdamped and it will take a long time to reach the desired velocity. +A properlly chosen D gain will allow to reach the desired velocity quickly with reduced overshoot and therefore a near zero error rate (known as critically damped). + +Term I accounts for past values of the error and integrates them over time. This term should be adjust last and increased slightly to tune the behavior further. + +In code the values to tune these PID gains are found in the [`base_controller_config.h`](https://github.com/ros-mobile-robots/diffbot/blob/e7b6c8c5206235a338ef15e20084630793813a4f/diffbot_base/scripts/base_controller/lib/config/diffbot_base_config.h#L18-L20): + +```cpp +#define K_P 0.6 // P constant +#define K_I 0.3 // I constant +#define K_D 0.5 // D constant +``` + +## Related Issues to PIDs + +- https://github.com/ros-mobile-robots/diffbot/issues/13 +- https://github.com/ros-mobile-robots/diffbot/issues/35 +- [Duckietown EDX MOOC Forum question](https://courses.edx.org/courses/course-v1:ETHx+DT-01x+1T2021/discussion/forum/90b2fa2c07d84c70926270dd995f1d57d007f56a/threads/60874666e136290498b20825) \ No newline at end of file diff --git a/docs/teensy-mcu.md b/docs/packages/diffbot_base/scripts/teensy-mcu.md similarity index 100% rename from docs/teensy-mcu.md rename to docs/packages/diffbot_base/scripts/teensy-mcu.md diff --git a/docs/packages.md b/docs/packages/index.md similarity index 72% rename from docs/packages.md rename to docs/packages/index.md index ccc4c2b6..c67a13d0 100644 --- a/docs/packages.md +++ b/docs/packages/index.md @@ -79,7 +79,7 @@ Inside the workspace use [`catkin-tools`](https://catkin-tools.readthedocs.io/en catkin build ``` -Now source the catkin workspace either using the [created alias](ros-setup.md#environment-setup) or the full command for the bash shell: +Now source the catkin workspace either using the [created alias](../ros-setup.md#environment-setup) or the full command for the bash shell: ``` source devel/setup.bash @@ -91,3 +91,27 @@ Now you are ready to follow the examples listed in the readme. !!! info TODO extend documentation with examples + + +## Optional Infos + +### Manual Dependency Installation + +To install a package from source clone (using git) or download the source files from where they are located (commonly hosted on GitHub) into the `src` folder of a ros catkin workspace and execute the [`catkin build`](https://catkin-tools.readthedocs.io/en/latest/verbs/catkin_build.html) command. Also make sure to source the workspace after building new packages with `source devel/setup.bash`. + +```console +cd /homw/fjp/git/diffbot/ros/ # Navigate to the workspace +catkin build # Build all the packages in the workspace +ls build # Show the resulting build space +ls devel # Show the resulting devel space +``` + +!!! note + + Make sure to clone/download the source files suitable for the ROS distribtion + you are using. If the sources are not available for the distribution you are + working with, it is worth to try building anyway. Chances are that the package + you want to use is suitable for multiple ROS distros. For example if a package + states in its docs, that it is only available for + [kinetic](http://wiki.ros.org/kinetic) it is possible that it will work with a + ROS [noetic](http://wiki.ros.org/noetic) install. diff --git a/docs/resources/packages/diffbot_base/block-diagram-low-high-level.svg b/docs/resources/packages/diffbot_base/block-diagram-low-high-level.svg new file mode 100644 index 00000000..366cef8c --- /dev/null +++ b/docs/resources/packages/diffbot_base/block-diagram-low-high-level.svg @@ -0,0 +1 @@ +ROS Navigation StackRealityInput/OutputHardwareController ManagerLoads, unloads, and calls updates to controllersjoint_state_controllerdiff_drive_controllerEncodersActuatorsMotors, Servos, etc.Embedded Controllerse.g., Teensy, operates PID controllers to follow set pointsubscribe/wheel_cmd_velocitiesdiffbot_msgs/WheelsCmdpublish/measured_joint_statessensor_msgs/JointStateJoint Statesangular positions / velocitiespublish/wheel_cmd_velocitiessubscribe/measured_joint_stateshardware_interface::RobotHWProvides position, velocity, and effort interfacesbetweenhardwareand ros_controlread()write()High-levelHardwareInterfaceLow-levelBaseControllerupdate joint interface valuesROS Interface/joint_statesfor robot_state_publisherROS Interface/cmd_vel \ No newline at end of file diff --git a/docs/resources/packages/diffbot_base/roscontrol-sim-reality.svg b/docs/resources/packages/diffbot_base/roscontrol-sim-reality.svg new file mode 100644 index 00000000..4921a39c --- /dev/null +++ b/docs/resources/packages/diffbot_base/roscontrol-sim-reality.svg @@ -0,0 +1 @@ +GazeboSimulatorSimulationHardwareHardware ResourceInterface LayerEncodersActuatorsRealityInput/Outputhardware_interface::RobotHWSimProvides position, velocity, and effort interfacesbetweenGazeboand ros_controlwriteSim()readSim()read()write()Joint State Interfacee.g.JointStateInterfaceJoint Command Interfacee.g.VelocityJointInterfaceController Managerjoint_state_controllerdiff_drive_controllerROS Interface/joint_statesROS Interface/cmd_velEmbedded controllersLoads, unloads, and calls updates to controllershardware_interface::RobotHWProvides position, velocity, and effort interfacesbetweenhardwareand ros_control \ No newline at end of file diff --git a/mkdocs.yml b/mkdocs.yml index 8b0051bd..5df83325 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -6,7 +6,7 @@ theme: features: - navigation.tabs - navigation.top -# - navigation.indexes + - navigation.indexes palette: # Light mode - media: "(prefers-color-scheme: light)" @@ -128,22 +128,27 @@ nav: - Power Supply: power-supply.md - Laser Range Scanner: laser-range-scanner.md - Processing Units: - - Raspberry Pi Setup: rpi-setup.md - - Jetson Nano Setup: jetson-nano-setup.md - - Git Setup: git-setup.md - - ROS Setup: ros-setup.md - - Teensy MCU: teensy-mcu.md + - Single Board Computer: + - Raspberry Pi Setup: rpi-setup.md + - Jetson Nano Setup: jetson-nano-setup.md + - Git Setup: git-setup.md + - ROS Setup: ros-setup.md + - Microcontroller: packages/diffbot_base/scripts/teensy-mcu.md - Hardware Interfaces: hardware-interfaces.md - ROS Network Setup: ros-network-setup.md - Packages: - - Packages: packages.md + - packages/index.md - Robot Package: diffbot_robot.md - Robot Description: - Robot Description: robot-description.md - Remo Description: packages/remo_description.md - Simulation: diffbot_gazebo.md - Control: diffbot_control.md - - Base Hardware Interface: diffbot_base.md + - Base Hardware Interface: + - packages/diffbot_base/index.md + - PID Controllers: packages/diffbot_base/pid.md + - Low-Level PID Approach: packages/diffbot_base/low-level.md + - High-Level PID Approach: packages/diffbot_base/high-level.md - Messages: diffbot_msgs.md - Motor Driver: grove_motor_driver.md - Motor and Encoder: DG01D-E-motor-with-encoder.md