Tag: ground

PID Control Made Simple

In this tutorial, I’ll explain how PID (Proportional-Integral-Derivative) Control works using an analogy.

Imagine we are the captain of a ship. We want the boat to maintain a heading of due north automatically, without any human intervention.

compass_direction_magnetic_compass

We go out and hire a smart robotics software engineer to write a control algorithm for the ship. 

The variable that we want to control is the ship’s heading, and we want to make sure the heading maintains a “set point” of due north (e.g. 0 degrees).

Proportional Control

  • If the ship is heading slightly off course to the left, we want the ship to steer slightly to the right.
  • If the ship is heading slightly off course to the right, we want the ship to steer slightly to the left.
  • If the ship is heading strongly off course to the left, we want the ship to steer strongly to the right.
  • If the ship is heading strongly off course to the right, we want the ship to steer strongly to the left.

In short, the steering magnitude is directly proportional to the difference between the desired heading and the current heading. The further you are off course, the harder the ship steers.

Mathematically, proportional control is: 

Control Output  = Kp * (Desired – Actual)

Where:

  • Control Output is what you want to control (e.g. steering intensity, speed, etc.)
  • Kp is some non-negative constant
  • Desired = Set Point (e.g. the heading you want to achieve…i.e. 0 degrees north)
  • Actual = Process Variable (e.g. the heading you measured)
  • Error = Desired – Actual

The Proportional term is all about adjusting the output based on the present error.

Integral Control

Now let’s suppose that there is a crosswind. Even though the ship is adjusting the steering in proportion to the heading error, there are still differences between the actual and desired heading that are accumulating over time as a result of this ongoing crosswind. 

The ship continues to drift off course even though proportional control is being applied.

The integral term looks for the residual error that is generated even after proportional control is applied. In this example, the residual error is caused by the wind, and it keeps adding up over time. The integral term seeks to get rid of this residual error by incorporating the historical cumulative value of the error. As the error decreases, the integral term will decrease.

Integral control looks for factors that keep pushing the ship off course and adjusts accordingly.

Mathematically, we now have to add the Integral term.

Control Output  = Kp * Error + Ki * Sum of Errors Over Time

The Integral term is all about adjusting the output based on the past error.

Derivative Control

Let’s suppose that the ship is steering hard to the left in order to correct the heading. You don’t want the ship to steer so hard that momentum causes the ship to overshoot your desired heading. 

What you want to do is have the ship slow down the steering as it approaches the heading. 

The derivative term of PID control does this. It is used to slow down the heading correction as the measured heading approaches the desired heading. This derivative term helps the ship avoid overshooting.

Mathematically, we now have to add the Derivative term to get the final PID Control equation:

Control Output  = Kp * Error + Ki * Sum of Errors Over Time + Kd * Rate of Change of the Error with Respect to Time

1-pid-control
How all of the PID control constants combine to generate the desired control response

The Derivative term is all about adjusting the output based on the future (predicted) error. If the error is decreasing too quickly with respect to time, this term will reduce the control output.

Summary

PID is a control method that is made up of three terms:

  1. Proportional gain that scales the control output based on the difference between the actual state of the system and the desired state of the system (i.e. the error).
  2. Integral gain that scales the control output based on the accumulated error.
  3. Derivative gain that scales the control output based on the rate of change of the error (i.e. how fast the error is changing).

How To Set up a ROS2 Project for Python – Foxy Fitzroy

In this tutorial, we will learn how to set up a ROS2 project from scratch.

Prerequisites

Source Your ROS2 Installation

We first need to create a new workspace. A workspace is a folder that will contain all your ROS packages.

Here is the official tutorial, but I’ll walk through all the steps below.

Open a new terminal window.

To find out which version of ROS2 you have installed, type the following command:

printenv ROS_DISTRO

I’m using the foxy distribution of ROS2.

Now, let’s source the ROS2 environment. Type the following command:

source /opt/ros/foxy/setup.bash

Create a Workspace

Let’s create a workspace directory. “-p” is short for “–parents”. This flag makes sure the mkdir (“make directory”) command makes the parent /dev_ws/ folder as well as the /src/ folder within it (Feel free to check out my Linux commands tutorial).

mkdir -p ~/dev_ws/src
cd ~/dev_ws/src

The syntax is:

mkdir -p ~/<yourworkspace>_ws/src

cd ~/<yourworkspace>_ws/src

Create a Package

Now let’s create a ROS package.

We will use the following syntax.

ros2 pkg create –build-type ament_python <package_name>

Type this command:

ros2 pkg create --build-type ament_python my_package

Your package named my_package has now been created.

Build Your Package

Return to the root of your workspace:

cd ~/dev_ws

Build all packages in the workspace.

colcon build

If you only want to build my_package, you can type

colcon build --packages-select my_package

Source the Setup File

Open a new terminal window.

Source your ROS2 installation.

source /opt/ros/foxy/setup.bash

Move to your workspace folder.

cd ~/dev_ws

Add the files you just created to the path.

source install/setup.bash

If you don’t have gedit installed, install it using this command:

sudo apt-get install gedit

Open your .bashrc file.

gedit ~/.bashrc

Add this line of code to the very bottom of the file.

source /opt/ros/foxy/setup.bash

You now don’t have to source your ROS2 installation every time you open a new terminal window.

While you’re at it add this line of code to the very bottom of the .bashrc file.

source ~/dev_ws/install/setup.bash

The syntax is

source ~/<your_workspace>/install/setup.bash

Save the file, and close it.

Your system now knows where to find your ROS2 packages.

Write Node(s)

Move to the dev_ws/src/my_package/my_package folder.

cd dev_ws/src/my_package/my_package

Write a Python program (i.e. node), and add it to this folder you’re currently in. This post has some sample code you can use. Copy the Publisher Node code, and paste it into a file named my_python_script.py.

gedit my_python_script.py

Save the file and close it.

Now change the permissions on the file.

chmod +x my_python_script.py

Make sure that the names of your python nodes are not the same as the name of the package. Otherwise, you will have ImportErrors.

You can add as many Python programs as you like. For example, you might have one node that defines a class, and then you have another node that imports that class and uses it to make calculations, which are then published to a ROS2 topic as some data type (e.g. Float64, geometry_msgs/Twist, etc.).

Add Dependencies

Navigate one level back.

cd ..

You are now in the dev_ws/src/my_package/ directory. 

Type 

ls

You should see the setup.py, setup.cfg, and package.xml files.

Open package.xml with your text editor.

gedit package.xml

Fill in the <description>, <maintainer>, and <license> tags.

Add a new line after the ament_python build_type dependency and add the following dependencies which will correspond to other packages the packages in this workspace needs (this will be in your node’s import statements):

<exec_depend>rclpy</exec_depend>
<exec_depend>geometry_msgs</exec_depend>

This let’s the system know that this package needs the rclpy and geometry_msgs packages when its code is executed. 

The code doesn’t need the geometry_msgs package, but I’m having you add it anyway just to show you that you can add all sorts of message types in this package.xml file.

Save the file.

Add an Entry Point

Now we need to add the entry points for the node(s) by opening the setup.py file.

gedit setup.py

Make sure this code is in there:

entry_points={
        'console_scripts': [
                'my_python_script = my_package.my_python_script:main',
        ],
},

The syntax is:

‘my_python_script = my_package.my_python_script:main’

my_python_script will be the name of the executable.

‘my_package.my_python_script:main’ tells the system to execute the main() function inside my_python_script.py. Therefore, this will be the entry point when you run your node.

The executable script will go to:

~/dev_ws/install/my_package/lib/my_package/

Save setup.py and close it.

Check for Missing Dependencies

Check for any missing dependencies before you build the package.

Move to your workspace folder.

cd ~/dev_ws

Run the following command:

rosdep install -i --from-path src --rosdistro <distro> -y

For example:

rosdep install -i --from-path src --rosdistro foxy -y

I get a message that says:

“#All required rosdeps installed successfully”

Build and Run

Now, build your package by first moving to the src folder.

cd src

Then build the package.

colcon build --packages-select my_package

You should get a message that says:

“Summary: 1 package finished [<time it took in seconds>]”

Open a new terminal tab.

cd ~/dev_ws

Run the node:

ros2 run my_package my_python_script

The syntax is:

ros2 run <package_name> <node name without the .py>

Open a new terminal tab, and move to the root directory of the workspace.

cd ~/dev_ws

List the active topics.

ros2 topic list -t

To listen to any topic, type:

ros2 topic echo /topic_name

Where you replace “/topic_name” with the name of the topic. 

When you’ve had enough, type Ctrl+C in each terminal tab to stop the nodes from spinning.

Zip the Workspace for Distribution

If you ever want to zip the whole workspace and send it to someone, open a new terminal window.

Move to the directory containing your workspace.

cd ~/dev_ws
cd ..
zip -r myros2workspace.zip /dev_ws

The syntax is:

zip -r <filename.zip> <foldername>

How To Multiply Two Quaternions Together Using Python

Here is how we multiply two quaternions together using Python. Multiplying two quaternions together has the effect of performing one rotation around an axis and then performing another rotation about around an axis.

import numpy as np
import random

def quaternion_multiply(Q0,Q1):
    """
    Multiplies two quaternions.

    Input
    :param Q0: A 4 element array containing the first quaternion (q01,q11,q21,q31) 
    :param Q1: A 4 element array containing the second quaternion (q02,q12,q22,q32) 

    Output
    :return: A 4 element array containing the final quaternion (q03,q13,q23,q33) 

    """
    # Extract the values from Q0
    w0 = Q0[0]
    x0 = Q0[1]
    y0 = Q0[2]
    z0 = Q0[3]
	
    # Extract the values from Q1
    w1 = Q1[0]
    x1 = Q1[1]
    y1 = Q1[2]
    z1 = Q1[3]
	
    # Computer the product of the two quaternions, term by term
    Q0Q1_w = w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1
    Q0Q1_x = w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1
    Q0Q1_y = w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1
    Q0Q1_z = w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1
	
    # Create a 4 element array containing the final quaternion
    final_quaternion = np.array([Q0Q1_w, Q0Q1_x, Q0Q1_y, Q0Q1_z])
	
    # Return a 4 element array containing the final quaternion (q02,q12,q22,q32) 
    return final_quaternion
    
Q0 = np.random.rand(4) # First quaternion
Q1 = np.random.rand(4) # Second quaternion
Q = quaternion_multiply(Q0, Q1)
print("{0} x {1} = {2}".format(Q0, Q1, Q))