Calculating Wheel Odometry for a Differential Drive Robot

Have you ever wondered how robots navigate their environments so precisely? A key component for many mobile robots is differential drive. This type of robot uses two independently controlled wheels, allowing for maneuvers like moving forward, backward, and turning. 

But how does the robot itself know where it is and how far it’s traveled? This is where wheel odometry comes in.

Wheel odometry is a technique for estimating a robot’s position and orientation based on the rotations of its wheels. By measuring the number of revolutions each wheel makes, we can calculate the distance traveled and any changes in direction. This information is important for tasks like path planning, obstacle avoidance, and overall robot control.

This tutorial will guide you through calculating wheel odometry for a differential drive robot. We’ll explore how to convert raw wheel encoder data – the number of revolutions for each wheel – into the robot’s displacement in the x and y directions (relative to a starting point) and the change in its orientation angle. Buckle up and get ready to dive into the fascinating world of robot self-localization!

Prerequisites

Calculate Wheel Displacements

First, we calculate the distance each wheel has traveled based on the number of revolutions since the last time step. This requires knowing:

1-calculate-wheel-displacement-distance

Calculate the Robot’s Average Displacement and Orientation Change

Next, we determine the robot’s average displacement and the change in its orientation. The average displacement (Davg) is the mean of the distances traveled by both wheels:

2-robot-displacement

The change in orientation (Δθ), measured in radians, is influenced by the difference in wheel displacements and the distance between the wheels (L):

3-change-in-orientation-robot

Calculate Changes in the Global Position

Now, we can calculate the robot’s movement in the global reference frame. Assuming the robot’s initial orientation is θ, and using Davg and Δθ, we find the changes in the x and y positions as follows:

4b-change-in-global-position

You will often see the following equation instead:

4-change-in-global-position

This simplification assumes Δθ is relatively small, allowing us to approximate the displacement direction using the final orientation θnew without significant loss of accuracy. It’s a useful approximation for small time steps or when precise integration of orientation over displacement is not critical.

To find the robot’s new orientation:

5-robot-new-orientation

Note, for robotics projects, it is common for us to modify this angle so that it is always between -pi and +pi. Here is what that code would look like:

# Keep angle between -PI and PI  
if (self.new_yaw_angle > PI):
    self.new_yaw_angle = self.new_yaw_angle - (2 * PI)
if (self.new_yaw_angle < -PI):
    self.new_yaw_angle = self.new_yaw_angle + (2 * PI)

For ROS 2, you would then convert this new yaw angle into a quaternion.

Update the Robot’s Global Position

Finally, we update the robot’s position in the global reference frame. If the robot’s previous position was (x, y), the new position (xnew, ynew) is given by:

6-update-robot-global-position

Practical Example

Let’s apply these steps with sample values:

7-define-values

Using these values, we’ll calculate the robot’s new position and orientation.

Step 1: Wheel Displacements

8-step-1

Step 2: Average Displacement and Orientation Change

9-step-2

Step 3: Changes in Global Position

10-step-3

Step 4: New Global Position and Orientation

Therefore the new orientation of the robot is 2.52 radians, and the robot is currently located at (x=-4.34, y = 4.80).

11-step-4

Important Note on Assumptions

The calculations for wheel odometry as demonstrated in the example above are made under two crucial assumptions:

  1. No Wheel Slippage: It’s assumed that the wheels of the robot do not slip during movement. Wheel slippage can occur due to loss of traction, often caused by slick surfaces or rapid acceleration/deceleration. When slippage occurs, the actual distance traveled by the robot may differ from the calculated values, as the wheel rotations do not accurately reflect movement over the ground.
  2. Adequate Friction: The calculations also assume that there is adequate friction between the wheels and the surface on which the robot is moving. Adequate friction is necessary for the wheels to grip the surface effectively, allowing for precise control over the robot’s movement. Insufficient friction can lead to wheel slippage, which, as mentioned, would result in inaccuracies in the odometry data.

These assumptions are essential for the accuracy of wheel odometry calculations. In real-world scenarios, various factors such as floor material, wheel material, and robot speed can affect these conditions.

Therefore, while the mathematical model provides a foundational understanding of how to calculate a robot’s position and orientation based on wheel rotations, you should be aware of these limitations and consider implementing corrective measures or additional sensors to account for potential discrepancies in odometry data due to slippage or inadequate friction.

Calculating Wheel Velocities for a Differential Drive Robot

Have you ever wondered how those robots navigate around restaurants or hospitals, delivering food or fetching supplies? These little marvels of engineering often rely on a simple yet powerful concept: the differential drive robot.

A differential drive robot uses two independently powered wheels and often has one or more passive caster wheels. By controlling the speed and direction of each wheel, the robot can move forward, backward, turn, or even spin in place. 

1-differential-drive-robot-base

This tutorial will delve into the behind-the-scenes calculations that translate desired robot motion (linear and angular velocities) into individual wheel speeds (rotational velocities in revolutions per second).

Real-World Applications

Differential drive robots, with their two independently controlled wheels, are widely used for their maneuverability and simplicity. Here are some real-world applications:

Delivery and Warehousing:

  • Inventory management:  Many warehouse robots use differential drive designs. They can navigate narrow aisles, efficiently locate and transport goods, and perform stock checks.  
  • Last-mile delivery:  Delivery robots on sidewalks and in controlled environments often employ differential drive. They can navigate crowded areas and deliver packages autonomously. 

Domestic and Public Use:

  • Floor cleaning robots:  These robots navigate homes and offices using differential drive. They can efficiently clean floors while avoiding obstacles.
  • Disinfection robots:  In hospitals and public areas, differential drive robots can be used for disinfecting surfaces with UV light or other methods.
  • Security robots:  These robots patrol buildings or outdoor spaces, using differential drive for maneuverability during surveillance.

Specialized Applications:

  • Agricultural robots:  Differential drive robots can be used in fields for tasks like planting seeds or collecting data on crops.
  • Military robots:  Small reconnaissance robots often use differential drive for navigating rough terrain.
  • Entertainment robots:  Robots designed for entertainment or education may utilize differential drive for movement and interaction.
  • Restaurant robots: Robots that deliver from the kitchen to the dining room.

Why Differential Drive for these Applications?

Differential drive robots offer several advantages for these tasks:

  • Simple design:  Their two-wheeled platform makes them relatively easy and inexpensive to build.
  • Maneuverability:  By controlling the speed of each wheel independently, they can turn sharply and navigate complex environments.
  • Compact size:  Their design allows them to operate in tight spaces.

Prerequisites

Convert Commanded Velocities to Wheel Linear Velocities

The robot’s motion is defined by two control inputs: the linear velocity (V) and the angular velocity (ω) of the robot in its reference frame.

2-differential-drive-diagram-robot-reference-frame

The linear velocity is the speed at which you want the robot to move forward or backward, while the angular velocity defines how fast you want the robot to turn clockwise and counterclockwise.

The linear velocities of the right (Vr) and the left (Vl) wheels can be calculated using the robot’s wheelbase width (L), which is the distance between the centers of the two wheels:

3-equation-1-linear-velocities-right-left-wheel

Convert Linear Velocity to Rotational Velocity

Once we have the linear velocities of the wheels, we need to convert them into rotational velocities. The rotational velocity (θ) in radians per second for each wheel is given by:

4-equation-rotational-velocity-wheel

Thus, the rotational velocities of the right and left wheels are:

5-rotational-velocities-right-left-wheels

Convert Radians per Second to Revolutions per Second

The final step is to convert the rotational velocities from radians per second to revolutions per second (rev/s) for practical use. Since there are 2π radians in a full revolution:

6-revolutions-per-second

Example

Let’s calculate the wheel velocities for a robot with a wheel radius of 0.05m, wheelbase width of 0.30m, commanded linear velocity of 1.0m/s, and angular velocity of 0.5rad/s.

7-full-calculation

The Complete Guide to Parameters – ROS 2 C++

In this tutorial, we’ll explore parameters in ROS 2 using C++. Parameters are important for creating flexible and dynamic robot software. 

You’ll learn how to define, access, and use parameters within your nodes, enabling you to customize your robot’s behavior without altering its code. 

Join me as we dive into the essentials of parameters, setting the foundation for more complex robotics applications.

Real-World Applications

Here are some real-world examples of how ROS 2 parameters can be used in robotic arm applications:

  • Joint Limits and Velocities: Parameters can be used to define the safe operating range (minimum and maximum) for each joint of the arm in terms of angles or positions. Additionally, they can set the maximum allowed speed for each joint during movement. This ensures safe and controlled operation, preventing damage to the robot or its surroundings.
  • Payload Capacity: This parameter specifies the maximum weight the arm can safely carry while maintaining stability and avoiding damage to its motors or gears. This parameter is important for tasks like picking and placing objects or performing assembly operations.
  • Path Planning Parameters: These parameters influence how the arm plans its trajectory to reach a target point. They can include factors like joint acceleration/deceleration rates, smoothness of the path, and collision avoidance settings. 
  • End-effector Calibration: Parameters can store calibration data for the gripper or any tool attached to the end of the arm. This data can include offsets, transformations, or specific configurations for different tools, ensuring accurate tool positioning during tasks.

As you can see, pretty much any variable that applies to your robot can be declared as a parameter in ROS 2. Parameters are used to configure nodes at startup (and during runtime), without changing the code.

Remember, a ROS 2 node is a mini-program (written in either Python or C++) that performs a specific task within a robot system. 

Prerequisites

Here is the GitHub repository that contains the code we will develop in this tutorial.

Write the Code

Let’s start by creating a ROS 2 node that declares two parameters (a floating-point value and a string) and includes a function that executes anytime one of the parameters changes.

Open a terminal, and type these commands to open VS Code.

cd ~/ros2_ws
code .

Right-click on src/cobot_arm_examples/src folder, and create a new file called “minimal_cpp_parameters.cpp”.

Type the following code inside minimal_cpp_parameters.cpp:

/**
 * @file minimal_cpp_parameters.cpp
 * @brief Demonstrates the basics of declaring parameters in ROS 2.
 *
 * Description: Demonstrates the basics of declaring parameters in ROS 2.
 * 
 * -------
 * Subscription Topics:
 *   None
 * -------
 * Publishing Topics:
 *   String message
 *   /topic_cpp - std_msgs/String
 * -------
 * @author Addison Sears-Collins
 * @date 2024-03-06
 */
 
#include <memory> // Include for smart pointer utilities
#include <rclcpp/rclcpp.hpp> // ROS 2 C++ client library
#include <rcl_interfaces/msg/parameter_descriptor.hpp> // Enables parameters' metadata such as their names and types.
#include <rclcpp/parameter.hpp> // Include for manipulating parameters within a node.

/**
 * @class MinimalParameter
 * @brief Defines a minimal ROS 2 node that declares parameters.
 *
 * This class inherits from rclcpp::Node and demonstrates the process of declaring
 * parameters within a ROS 2 node. It showcases how to declare parameters with
 * default values and descriptions, and how to handle parameter change requests
 * via a callback mechanism.
 */
 class MinimalParameter : public rclcpp::Node
{
public:
    /**
     * @brief Constructs a MinimalParameter node.
     *
     * Initializes the node, declares two parameters (`velocity_limit` and `robot_name`)
     * with default values and descriptions, and registers a callback for handling
     * parameter change requests.
     */
    MinimalParameter() : Node("minimal_parameter_cpp_node")
    {
        // Describe parameters
        rcl_interfaces::msg::ParameterDescriptor velocity_limit_descriptor;
        velocity_limit_descriptor.description = "Maximum allowed angular velocity in radians per second (ignored for fixed joints)";

        rcl_interfaces::msg::ParameterDescriptor robot_name_descriptor;
        robot_name_descriptor.description = "The name of the robot";

        // Declare parameters
        this->declare_parameter("velocity_limit", 2.792527, velocity_limit_descriptor);
        this->declare_parameter("robot_name", "Automatic Addison Bot", robot_name_descriptor);
    
        // Register a callback function that will be called whenever there is an attempt 
        // to change one or more parameters of the node.  
        parameter_callback_handle = this->add_on_set_parameters_callback(
            std::bind(&MinimalParameter::parameter_change_callback, this, std::placeholders::_1));		
    }
	
private:
    /**
     * @brief Callback function for handling parameter change requests.
     *
     * This method is invoked automatically whenever there is an attempt to change
     * one or more of the node's parameters. It checks each parameter change request,
     * logs the new value of recognized parameters, and validates the change.
     *
     * @param params A vector containing the parameters attempted to be changed.
     * @return An instance of rcl_interfaces::msg::SetParametersResult indicating
     *         whether the parameter changes were successful or not.
     */

    // Member variable to store the callback handle
    rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle;

    rcl_interfaces::msg::SetParametersResult parameter_change_callback(const std::vector<rclcpp::Parameter> &params)
    {
        // Create a result object to report the outcome of parameter changes.
	    auto result = rcl_interfaces::msg::SetParametersResult();

        // Assume success unless an unsupported parameter is encountered.
        result.successful = true;

        // Iterate through each parameter in the change request.    
        for (const auto &param : params) 
        {
            // Check if the changed parameter is 'velocity_limit' and of type double.
            if (param.get_name() == "velocity_limit" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) 
            {
                RCLCPP_INFO(this->get_logger(), "Parameter velocity_limit has changed. The new value is: %f", param.as_double());
            } 
            // Check if the changed parameter is 'robot_name' and of type string.
            else if (param.get_name() == "robot_name" && param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) 
            {
                RCLCPP_INFO(this->get_logger(), "Parameter robot_name has changed. The new value is: %s", param.as_string().c_str());
            } 
            // Handle any unsupported parameters.
            else 
            {
                // Mark the result as unsuccessful and provide a reason.
                result.successful = false;
                result.reason = "Unsupported parameter";
            }
        } 
        // Return the result object, indicating whether the parameter change(s) were successful or not.		
        return result;
    }
};

/**
 * @brief Main function.
 *
 * This function initializes the ROS 2 system, creates and runs an instance of the
 * MinimalParameter node. It keeps the node running until it is manually terminated,
 * ensuring that the node can react to parameter changes or perform its intended
 * functions during its lifecycle. Finally, it shuts down the ROS 2 system before
 * terminating the program.
 *
 * @param argc The number of command-line arguments.
 * @param argv The array of command-line arguments.
 * @return int Returns 0 upon successful completion of the program.
 */
int main(int argc, char * argv[])
{
    // Initialize ROS 2.
    rclcpp::init(argc, argv); 
  
    // Create an instance of the MinimalParameter node and keep it running.
    auto minimal_parameter_cpp_node = std::make_shared<MinimalParameter>();
    rclcpp::spin(minimal_parameter_cpp_node);

    // Shutdown ROS 2 upon node termination.
    rclcpp::shutdown(); 

    // End of program.
    return 0; 
}

Modify the CMakeLists.txt File

Now let’s configure the CMakeLists.txt file. Here is what it should look like:

cmake_minimum_required(VERSION 3.8)
project(cobot_arm_examples)

# Check if the compiler being used is GNU's C++ compiler (g++) or Clang.
# Add compiler flags for all targets that will be defined later in the 
# CMakeLists file. These flags enable extra warnings to help catch
# potential issues in the code.
# Add options to the compilation process
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# Locate and configure packages required by the project.
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(rcl_interfaces REQUIRED)
find_package(std_msgs REQUIRED)

# Define a CMake variable named dependencies that lists all
# ROS 2 packages and other dependencies the project requires.
set(dependencies
  rclcpp
  rcl_interfaces
  std_msgs
)

# Add the specified directories to the list of paths that the compiler
# uses to search for header files. This is important for C++
# projects where you have custom header files that are not located
# in the standard system include paths.
include_directories(
  include
)

# Tells CMake to create an executable target named minimal_cpp_publisher
# from the source file src/minimal_cpp_publisher.cpp. Also make sure CMake
# knows about the program's dependencies.
add_executable(minimal_cpp_publisher src/minimal_cpp_publisher.cpp)
ament_target_dependencies(minimal_cpp_publisher ${dependencies})

add_executable(minimal_cpp_subscriber src/minimal_cpp_subscriber.cpp)
ament_target_dependencies(minimal_cpp_subscriber ${dependencies})

add_executable(minimal_cpp_parameters src/minimal_cpp_parameters.cpp)
ament_target_dependencies(minimal_cpp_parameters ${dependencies})

# Copy necessary files to designated locations in the project
install (
  DIRECTORY cobot_arm_examples scripts
  DESTINATION share/${PROJECT_NAME}
)

install(
  DIRECTORY include/
  DESTINATION include
)

# Install cpp executables
install(
  TARGETS
  minimal_cpp_publisher
  minimal_cpp_subscriber
  minimal_cpp_parameters
  DESTINATION lib/${PROJECT_NAME}
)

# Install Python modules for import
ament_python_install_package(${PROJECT_NAME})

# Install Python executables
install(
  PROGRAMS
  scripts/minimal_py_publisher.py
  scripts/minimal_py_subscriber.py
  scripts/minimal_py_parameters.py
  #scripts/example3.py
  #scripts/example4.py
  #scripts/example5.py
  #scripts/example6.py
  #scripts/example7.py
  DESTINATION lib/${PROJECT_NAME}
)

# Automates the process of setting up linting for the package, which
# is the process of running tools that analyze the code for potential
# errors, style issues, and other discrepancies that do not adhere to
# specified coding standards or best practices.
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

# Used to export include directories of a package so that they can be easily
# included by other packages that depend on this package.
ament_export_include_directories(include)

# Generate and install all the necessary CMake and environment hooks that 
# allow other packages to find and use this package.
ament_package()

Modify the package.xml File

Add the rcl_interfaces dependency to package.xml:

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>cobot_arm_examples</name>
  <version>0.0.0</version>
  <description>Basic examples demonstrating ROS 2</description>
  <maintainer email="automaticaddison@example.com">Addison Sears-Collins</maintainer>
  <license>Apache-2.0</license>

  <!--Specify build tools that are needed to compile the package-->
  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>ament_cmake_python</buildtool_depend>

  <!--Declares package dependencies that are required for building the package-->
  <depend>rclcpp</depend>
  <depend>rclpy</depend>
  <depend>std_msgs</depend>
  <depend>rcl_interfaces</depend>

  <!--Specifies dependencies that are only needed for testing the package-->
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

Build the Workspace

Open a new terminal window, and type the following commands:

cd ~/ros2_ws/
colcon build
source ~/.bashrc

Run the Node 

In this section, we will finally run our node. Open a terminal window, and type:

ros2 run cobot_arm_examples minimal_cpp_parameters

Now, press Enter.

Open a new terminal window.

What are the currently active nodes?

ros2 node list

List all the parameters for all nodes in the ROS2 system.

ros2 param list

Retrieve the value of a specific parameter for a node.

ros2 param get /minimal_parameter_cpp_node robot_name
1-get-min-param-cpp-node-robot-name
ros2 param get /minimal_parameter_cpp_node velocity_limit
2-get-min-param-cpp-node-vel-limit

Set the value of a specific parameter for a specified node.

ros2 param set /minimal_parameter_cpp_node robot_name '!!str New Automatic Addison Bot'

You should see “Set parameter successful”.

Double check the new value:

ros2 param get /minimal_parameter_cpp_node robot_name
3-get-new-node-name
ros2 param set /minimal_parameter_cpp_node velocity_limit 1.0

Now go back to the terminals where your scripts are running and press CTRL + C to stop the execution.

To clear the terminal window, type:

clear

That’s it! Keep building!