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 Determine What Torque You Need for Your Servo Motors

In this tutorial, I will show you how to calculate how much torque you need for the servo motors (i.e. joints) on a robotic arm you are trying to build.

Why is torque so important? When you want to build a robotic arm to perform some task in the world, you have to make sure that each joint of the arm (i.e. servo motor) is strong enough to lift whatever you want it to lift. In robotics jargon, the maximum weight that a robotic arm can lift is referred to as the maximum payload.

This six degree of freedom robotic arm we built here can lift a pair of sunglasses, but the servos don’t have enough torque to lift a car, for example.

What is Torque?

Consider this wooden rod below.

Let’s label its center of mass with a light blue circle. Imagine this blue circle is a nail that we have hammered into the wooden rod and a wall.

If we apply a force F on the end of the rod at a distance r from the center of mass, what do you think will happen?

Note that we are only concerned about the component of the force that is perpendicular to the position vector. The position vector is drawn from the axis of rotation (i.e. the blue nail) to the force vector (in blue below). The position vector must be perpendicular to the force vector.

The rod will rotate in a counterclockwise direction around the axis.

This force that is applied at a position r from the axis of rotation (which is directly out of the page) is known as torque. Torque is often represented by the Greek letter τ. The equation for torque is:

τ = r F

It is worth repeating, but with torque we are only concerned about the component of the force that is perpendicular to the position vector. Any force component that is parallel to the position vector doesn’t generate torque.

For example, imagine we applied force to the end of the rod at an angle like this:

The torque in this case just takes into account that red line above. Here, the equation for the torque is:

sin(θ) = F_{perpendicular}/ F … using trigonometry

Fsin(θ) = F_{perpendicular}

Therefore,

τ = r F sin(θ)

The official metric (SI) units of torque is the Newton-meter (Nm). However in datasheets for servo motors, you’ll often see ounce-force-inch (oz-in) or kilogram-force centimeter (kg-cm).

In the real world, a servo motor’s axis is the blue nail. This is the robotic arm’s joint. The orange bar is the link of a robotic arm. The force F is the force acting on an object (that the robotic arm is trying to lift) due to gravity.

F = mass * g

Where m is the mass the servo motor has to lift, and g is the acceleration due to gravity (9.80665 m/s²).

The torque for this setup above is:

τ = m * g * r

Example 1

If you see a servo with a torque of 35kg-cm, what does that mean?

Typically, when you see a value like 35 kg printed on a servo motor, what they are referring to is the stall torque, which, in this case, is 35 kg-cm. Stall torque is the torque load that causes a servo motor to “stall” or stop rotating.

A stall torque of 35 kg-cm means that the servo motor will stop rotating when it is trying to move a 35 kg weight at a radial distance of 1.0 cm.

In the example below, we will assume the orange link has a mass of 0 kg.

In case you’re wondering, what the force F is:

F = m * g = 35 kg* 9.80665 m/s²

F = 35 kg* 9.80665 m/s²

F = 343.2 N

Where N stands for newtons. Note that 1 N = 9.80665 kg.

Example 2

Let’s say the servo of the robotic arm rotates a bit, counterclockwise.

What is the torque in this case?

τ = r F

τ = (1.0 cm * cos(45°)) * (343.2 N)

τ = (.01 m * cos(45°)) * (343.2 N)

τ = (.00707m) * (343.2 N)

τ = 2.427 Nm

1 Nm is equal to 10.197162129779 kg-cm.

Therefore,

τ = 24.75 kg-cm

So, you can see that when the arm is extended out parallel to the ground surface, the torque is higher than when the arm is bent.

This makes a lot of sense. Imagine holding your arm stretched out horizontally and trying to hold a bucket of water in place while trying not to bend your elbow. It would be pretty difficult!

Accounting for the Force of Gravity

In the example above, we have accounted for the force of gravity when calculating our torque requirements. However, we assumed that the links have no weight. In reality they do.

Consider this diagram of a robotic arm below. It has four servo motors and a gripper on the end (at right). Each servo motor (i.e. joint) is connected by a link (typically a metal piece like you see in this six degree of freedom robotic arm here).

To calculate the torque requirement, we start at the end effector (i.e. gripper) of the robotic arm and then work our way to the base of the robot. We need to account for all the pieces of the robot that are impacted by gravity:

Weight of the link
Weight of the joint
Weight of the object being lifted (e.g. the box below)

Let’s start with joint 4. Joint 4 has to be strong enough to lift the box as well as link 4.

Box:

The center of mass of the box is located at a distance r₄from the axis of rotation of joint 4.
The force of gravity acting on the box is equal to m_box * g. (where m means mass).
The torque due to the box is therefore r₄ * m_box * g.

Link 4:

The center of mass of the link is that red dot above. It is located at a distance r₄/2from the axis of rotation of joint 4.
The force of gravity acting on link 4 is equal to m_link4 * g (note that the product of mass and the gravitational acceleration constant is also known as the “weight”).
The torque due to the box is therefore (1/2) * r₄ * m_link4 * g.

Therefore:

Torque₄= (r₄ * m_box * g) + ((1/2) * r₄ * m_link4 * g)

Let’s now do one more. We’ll do joint 3. We place a red dot in the center of mass of link 3.

Joint 3 has to be strong enough to lift the following components:

Box…………((r₃ + r₄) * m_box * g)
Link 4….…..((r₃ + r₄/2) * m_link4 * g)
Joint 4……..(r₃ * m_joint4 * g)
Link 3………(r₃/2 * m_link4 * g)

Therefore:

Torque₃= ((r₃ + r₄) * m_box * g) + ((r₃ + r₄/2) * m_link4 * g) + (r₃ * m_joint4 * g) + (r₃/2 * m_link4 * g)

You keep doing this for the other joints.

Accounting for Angular Acceleration

The force of gravity on the links and payload (i.e. the box) is only part of the calculation of the torque requirement for a motor. We want our robotic arm to move and do useful work in the world…not just sit there with its arm stretched out, holding a box in place.

There is a torque required for a joint to move (i.e. generate angular acceleration) a link (or payload) from a rest position. Therefore, the total torque requirement for a servo is:

Torque Required By a Servo Motor = (Torque Due to Force of Gravity on Links and Payload) + (Torque Due to Angular Acceleration of Links and Payload)

This torque due to angular acceleration is calculated using the following equation:

τ = Iα

where I is the rotational inertia (or moment of inertia), and α is the angular acceleration around an axis.

The value for I will vary depending on what is generating the angular acceleration (e.g. solid cylinder, thin rod, slab, etc.). Inertia is the “resistance an object has to any change in its velocity.” Therefore, rotational inertia in the case of a servo motor is the resistance the motor has to any change in its velocity.

The units for rotational inertia are kg-m². The unit for angular acceleration is rad/s².

Example

Consider this joint below that is connected to a link. In this case, the motor will rotate counterclockwise in a direction that is at a 90 degree angle to the force of gravity (which is straight down). The only torque we have here is the torque required to generate angular acceleration.

τ = Iα

I in this case will be the sum of the rotational inertia of the motor and the link.

I = I_motor + I_link1

You can find the rotational inertia of the motor (I_motor) in the datasheet for the motor (do a Google search for the datasheet). The rotational inertia of the link (I_link1) can be described as the rotational inertia of a rod of some length L and mass m, rotating about one end. The equation is as follows:

I_link1=(1/3)*m*L²

α, the angular acceleration, will be the same for both the motor and the link since they are connected to each other.

How do we find α?

Let’s assume that we want the motor to move 90° in 1.0 second and then stop (e.g. common in pick and place tasks). We can draw a graph of angular velocity ω (radians/second) vs. time t (seconds).

We want to minimize acceleration as much as possible so that we minimize the torque requirements for our servo motor. We could have any velocity curve we want, but the curve that minimizes acceleration is the one that increases linearly from rest, reaches a peak at the halfway point, and then decreases linearly from there.

The reason why this curve below gives the minimum acceleration is because acceleration is the slope of the velocity curve (i.e. change in angular velocity/change in time = angular acceleration).

You can see that the magnitude of the slope is minimized on the black curve. The red and green curves get steep in some parts, which means the angular acceleration is high (and thus more torque is needed for the servo motor).

What is the required area underneath this curve below? The area of the curve (i.e. triangle) is equal to the distance the servo motor needs to move.

The formula for the area of a triangle (A_triangle) is:

A_triangle = (1/2)*(base)*(height)

The distance the servo motor needs to move is 90°, which is equal to π/2 radians.

π/2 = (1/2)*(1.0)*(ω_max)

ω_max= π radians/second

Since α is the slope of the curve, we know that it is:

α = (change in y/ change in x) = (π/0.5) = 2π rad/s²

Now, we have all the numbers we need to calculate the required torque:

τ = (I_motor + (1/3)*m*L²) * (2π)

Let’s assume:

I_motor = 0 kg-m²(we’ll assume the rotational inertia of the motor is negligible compared to the link, but you would typically get this value in the motor’s datasheet)
L = 0.75 m
m = 1.2 kg

τ = ((1/3)*(1.2kg)*(0.75 meters)²) * (2π rad/s²)

τ = 1.414 kg-m²/s²= 1.414 Nm = 14.410 kg-cm = 200 oz-in

Now, that we’ve identified the torque requirements for the motor in motion (we covered the stationary motor case in the previous section of this post), we need to make sure we select a motor that will be able to exert 14.410 kg-cm of torque at all of the speeds in the curve below.

So the next step is to take a motor that has a stall torque greater than 14.410 kg-cm and plot the torque vs. speed curve. We need to make sure that both the maximum torque and maximum speed (i.e. ω_max) fall under this curve; otherwise, we could damage our motors.

Let’s suppose we searched around on the Internet at various electronics stores and found a motor with a no load speed of 45 RPM and a stall torque of 250 oz-in (18 kg-cm). Let’s draw the torque vs. speed curve. Note that the “no load speed” is the revolutions per minute of a motor when it is running at top speed with nothing attached to it.

From our analysis, we calculated that we need 200 oz-in of torque, so I’ll draw that on the graph as a blue line.

ω_max= π radians/second

1 Radians Per Second = 9.5493 Revolutions Per Minute

ω_max= π radians/second * 9.5493 rpm/(radians/second) = 30 rpm

Let’s draw that on the curve.

It looks like that pink dot is above the curve, so we might need a stronger motor. Let’s double check to see if that point is, in fact, above our torque vs. speed curve.

Torque = (-250/45)*(30) + 250 = 83 oz-in

We see that the motor generates 83 oz-in of torque when the speed is 30rpm. However, we need to have 200 oz-in of torque, so this motor is not strong enough for our project.

Now, let’s see if we found a motor with the following specifications.

No-load speed = 100 rpm
Stall Torque = 350 oz-in

Do this work for our purposes? Let’s do the math.

Torque = (-350/100)*(30) + 350 = 245 oz-in

Since 245 > 200, this motor works for our purposes.

However, in our analysis we have not taken into account the rotational inertia of the motor, and we have assumed that there is no payload attached to the link. So, in a real-world use case, we need to account for those by incorporating them into our original calculation of I (i.e. the rotational inertia).

Accounting for Angular Acceleration and the Force of Gravity

Now let’s look at an example where we need to take both angular acceleration and the force of gravity into account in order to calculate the torque requirement.

We will reposition the joint-link combination we worked with in the previous section so that the body of the motor is now parallel to the surface. Here is how that looks:

Here we have two torques:

Acceleration of the Link: τ = Iα
Gravity: τ = r F

Adding the two torques we have:

τ = Iα + mass * g * (r/2) * cos(θ)

Where r/2 is the distance from the axis of rotation to the center of mass of the link. The center of mass of the link is noted with a pink circle below.

The mass of the link is 1.2 kg, and the link length is 0.75 meters. In the worst case, cos(θ) = 1. Therefore, the amount of torque that the motor needs to have the link overcome the downward force of gravity is:

mass * g * (r/2) * cos(θ) = (1.2 kg)(9.8 m/s²)(0.75/2) = 4.41 Nm = 624 oz-in

You can see how the torque required to overcome the force of gravity is more than 3x the required torque to accelerate a link from rest (which we calculated as 200 oz-in).

Total torque required = 200 oz-in + 624 oz-in = 824 oz-in

Adding a Payload to the Robotic Arm

Now, let’s assume the robotic arm has something at the end of it that it needs to carry. We call this a payload.

If you go to this list at Wikipedia, you will see that this object can be considered a “point mass”. The equation for rotational inertia for a point mass is:

I = Mr²

Where M = mass of the object, and r is the distance from the rotational axis to the center of mass of the object.

Earlier we found the torque required to produce angular acceleration for the link. That was:

τ = (I_motor + (1/3)*m*L²) * (2π)

τ = ((1/3)*(1.2kg)*(0.75 meters)²) * (2π rad/s²)

τ = 1.414 kg-m²/s²= 1.414 Nm = 14.410 kg-cm = 200 oz-in

Now we need to add the rotational inertia of the object attached to the link. Let’s assume the object has a mass of 1.2kg (just like the mass of the link). The equation becomes:

τ = ((1/3)*(1.2kg)*(0.75 meters)²+ 1.2 kg * (0.75)²) * (2π rad/s²)

τ = 5.655 kg-m²/s²= 5.655 Nm = 57.665 kg-cm = 800.8 oz-in

Now let’s go back here:

We had:

τ = Iα + mass * g * (r/2) * cos(θ)

τ = Iα + 4.41 Nm

We also have to add the torque of the payload due to gravity.

τ = Iα + 4.41 Nm + (1.2 kg)(9.8 m/s²)(0.75)

τ = Iα + 4.41 Nm + 8.82 Nm

τ = Iα + 13.23 Nm

τ = Iα + 1873.5 oz-in

Therefore,

τ = Total amount of torque required to move the link and payload

= (torque needed for angular acceleration) + (torque to overcome gravity)

= 800.8 oz-in + 1873.5 oz-in

= 2674.3 oz-in

You now have the fundamentals to expand this calculation to multi-link robotic arms. That’s it for now. Keep building!

References

Here is a handy calculator that helps you select the appropriate servo motor for each joint of your robotic arm.
Khan Academy has a good comprehensive short course on torque.
Professor Angela Sodemann has a great tutorial on her site RoboGrok.com that covers torque requirements for manipulators

Detect the Corners of Objects Using Harris Corner Detector

In this tutorial, we will write a program to detect corners on random objects.

Our goal is to build an early prototype of a vision system that can make it easier and faster for robots to identify potential grasp points on unknown objects.

Real-World Applications

The first real-world application that comes to mind is Dr. Pieter Abbeel (a famous robotics professor at UC Berkley) and his laundry-folding robot.

Dr. Abbeel used the Harris Corner Detector algorithm as a baseline for evaluating how well the robot was able to identify potential grasp points on a variety of laundry items. You can check out his paper at this link.

If the robot can identify where to grasp a towel, for example, inverse kinematics can then be calculated so the robot can move his arm so that the gripper (i.e. end effector) grasps the corner of the towel to begin the folding process.

Prerequisites

You have Python 3.7 or higher

Requirements

Here are the requirements:

Create a program that can detect corners on an unknown object using the Harris Corner Detector algorithm.

Detecting Corners on Jeans

We’ll start with this pair of jeans above. Save that image to some folder on your computer.

Now, in the same folder you saved that image above (we’ll call the file jeans.jpg), open up a new Python program.

Name the program harris_corner_detection.py.

Write the following code:

# Project: Detect the Corners of Objects Using Harris Corner Detector
# Author: Addison Sears-Collins
# Date created: October 7, 2020
# Reference: https://stackoverflow.com/questions/7263621/how-to-find-corners-on-a-image-using-opencv/50556944

import cv2 # OpenCV library
import numpy as np # NumPy scientific computing library
import math # Mathematical functions

# The file name of your image goes here
fileName = 'jeans.jpg'

# Read the image file
img = cv2.imread(fileName)

# Convert the image to grayscale 
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)   

######## The code in this block is optional #########
## Turn the image into a black and white image and remove noise
## using opening and closing

#gray = cv2.threshold(gray, 75, 255, cv2.THRESH_BINARY)[1]

#kernel = np.ones((5,5),np.uint8)

#gray = cv2.morphologyEx(gray, cv2.MORPH_OPEN, kernel)
#gray = cv2.morphologyEx(gray, cv2.MORPH_CLOSE, kernel)

## To create a black and white image, it is also possible to use OpenCV's
## background subtraction methods to locate the object in a real-time video stream 
## and remove shadows.
## See the following links for examples 
## (e.g. Absolute Difference method, BackgroundSubtractorMOG2, etc.):
## https://automaticaddison.com/real-time-object-tracking-using-opencv-and-a-webcam/
## https://automaticaddison.com/motion-detection-using-opencv-on-raspberry-pi-4/

############### End optional block ##################

# Apply a bilateral filter. 
# This filter smooths the image, reduces noise, while preserving the edges
bi = cv2.bilateralFilter(gray, 5, 75, 75)

# Apply Harris Corner detection.
# The four parameters are:
#   The input image
#   The size of the neighborhood considered for corner detection
#   Aperture parameter of the Sobel derivative used.
#   Harris detector free parameter 
#   --You can tweak this parameter to get better results 
#   --0.02 for tshirt, 0.04 for washcloth, 0.02 for jeans, 0.05 for contour_thresh_jeans
#   Source: https://docs.opencv.org/3.4/dc/d0d/tutorial_py_features_harris.html
dst = cv2.cornerHarris(bi, 2, 3, 0.02)

# Dilate the result to mark the corners
dst = cv2.dilate(dst,None)

# Create a mask to identify corners
mask = np.zeros_like(gray)

# All pixels above a certain threshold are converted to white         
mask[dst>0.01*dst.max()] = 255

# Convert corners from white to red.
#img[dst > 0.01 * dst.max()] = [0, 0, 255]

# Create an array that lists all the pixels that are corners
coordinates = np.argwhere(mask)

# Convert array of arrays to lists of lists
coordinates_list = [l.tolist() for l in list(coordinates)]

# Convert list to tuples
coordinates_tuples = [tuple(l) for l in coordinates_list]

# Create a distance threshold
thresh = 50

# Compute the distance from each corner to every other corner. 
def distance(pt1, pt2):
    (x1, y1), (x2, y2) = pt1, pt2
    dist = math.sqrt( (x2 - x1)**2 + (y2 - y1)**2 )
    return dist

# Keep corners that satisfy the distance threshold
coordinates_tuples_copy = coordinates_tuples
i = 1    
for pt1 in coordinates_tuples:
    for pt2 in coordinates_tuples[i::1]:
        if(distance(pt1, pt2) < thresh):
            coordinates_tuples_copy.remove(pt2)      
    i+=1

# Place the corners on a copy of the original image
img2 = img.copy()
for pt in coordinates_tuples:
    print(tuple(reversed(pt))) # Print corners to the screen
    cv2.circle(img2, tuple(reversed(pt)), 10, (0, 0, 255), -1)
cv2.imshow('Image with 4 corners', img2) 
cv2.imwrite('harris_corners_jeans.jpg', img2) 

# Exit OpenCV
if cv2.waitKey(0) & 0xff == 27:
    cv2.destroyAllWindows()

Run the code. Here is what you should see:

Now, if you uncomment the optional block in the code, you will see a window that pops up that shows a binary black and white image of the jeans. The purpose of the optional block of code is to remove some of the noise that is present in the input image.

Notice how when we remove the noise, we get a lot more potential grasp points (i.e. corners).

Detecting Corners on a T-Shirt

To detect corners on a t-shirt, you’ll need to tweak the fourth parameter on this line of the code. I use 0.02 typically, but you can try another value like 0.04:

# 0.02 for tshirt, 0.04 for washcloth, 0.02 for jeans, 0.05 for contour_thresh_jeans
#   Source: https://docs.opencv.org/3.4/dc/d0d/tutorial_py_features_harris.html
dst = cv2.cornerHarris(bi, 2, 3, 0.02)

Let’s detect the corners on an ordinary t-shirt.

Here is the input image (tshirt.jpg):

Change the fileName variable in your code so that it is assigned the name of the image (‘tshirt.jpg’).

Here is the output image:

Detecting Corners on a Washcloth

Input Image (washcloth.jpg):

Output image:

That’s it. Keep building!