How To Convert a Quaternion Into Euler Angles in Python

Given a quaternion of the form (x, y, z, w) where w is the scalar (real) part and x, y, and z are the vector parts, how do we convert this quaternion into the three Euler angles:

Rotation about the x axis = roll angle = α
Rotation about the y-axis = pitch angle = β
Rotation about the z-axis = yaw angle = γ

Doing this operation is important because ROS2 (and ROS) uses quaternions as the default representation for the orientation of a robot in 3D space. Roll, pitch, and yaw angles are a lot easier to understand and visualize than quaternions.

Here is the Python code:

import math

def euler_from_quaternion(x, y, z, w):
		"""
		Convert a quaternion into euler angles (roll, pitch, yaw)
		roll is rotation around x in radians (counterclockwise)
		pitch is rotation around y in radians (counterclockwise)
		yaw is rotation around z in radians (counterclockwise)
		"""
		t0 = +2.0 * (w * x + y * z)
		t1 = +1.0 - 2.0 * (x * x + y * y)
		roll_x = math.atan2(t0, t1)
    
		t2 = +2.0 * (w * y - z * x)
		t2 = +1.0 if t2 > +1.0 else t2
		t2 = -1.0 if t2 < -1.0 else t2
		pitch_y = math.asin(t2)
    
		t3 = +2.0 * (w * z + x * y)
		t4 = +1.0 - 2.0 * (y * y + z * z)
		yaw_z = math.atan2(t3, t4)
    
		return roll_x, pitch_y, yaw_z # in radians

Example

Suppose a robot is on a flat surface. It has the following quaternion:

Quaternion [x,y,z,w] = [0, 0, 0.7072, 0.7072]

What is the robot’s orientation in Euler Angle representation in radians?

The program shows that the roll, pitch, and yaw angles in radians are (0.0, 0.0, 1.5710599372799763).

Which is the same as:

Euler Angle (roll, pitch, yaw) = (0.0, 0.0, π/2)

And in Axis-Angle Representation, the angle is:

Axis-Angle {[x, y, z], angle} = { [ 0, 0, 1 ], 1.571 }

So we see that the robot is rotated π/2 radians (90 degrees) around the z axis (going counterclockwise).

And that’s all there is to it folks. That’s how you convert a quaternion into Euler angles.

You can use the code in this tutorial for your work in ROS2 since, as of this writing, the tf.transformations.euler_from_quaternion method isn’t available for ROS2 yet.

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.

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 = K_p * (Desired – Actual)

Where:

Control Output is what you want to control (e.g. steering intensity, speed, etc.)
K_pis 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 = K_p * Error + K_i * 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 = K_p * Error + K_i * Sum of Errors Over Time + K_d * 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:

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).
Integral gain that scales the control output based on the accumulated error.
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

You have ROS2 installed and working in a Linux environment.

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>