A Very Simple 2-Joint Robot Control Model

This post will describe a very simple position control loop for a simple robot arm with just two joints. Multiple robot control frameworks are available, both commercially and as free and open code. They all use the same basic control equations and models for robot arms, land robots, and air and sea drones. This is a very basic visual introduction to robot arm positioning.

We will start by importing the required modules, numpy, math, and matplotlib. The required trigonometric functions are, in our humble opinion, easier to access through the math module than through numpy module, thus justifying the inclusion of math. We will suppress the scientific notation representation of numbers and limit the floating point precision to 3 digits as we do not require it for such a simple demonstration.

import numpy as np
import math
import matplotlib.pyplot as plt

np.set_printoptions(precision=3)
np.set_printoptions(suppress=True)

We will extensively use the value of pi, so we bring it in as an upper-case constant.

PI = np.pi

Our robot is defined by two arms (1, 2) with lengths L1 and L2 that can be rotated on the X-Y plane by angles a and b:

In Python, we could define the angular displacements as α and β, but these characters belong to UNICODE extensions and may be problematic when porting the script. We will set our initial arm lengths to 0.5 m and 0.3 m, with initial displacement angles of pi/6 (30º) and pi/3 (60º):

L1 = 0.5
L2 = 0.3

α = PI/6
β = PI/3

a = PI/6
b = PI/3

Thus, the position of the arm joints, the gray circle in the diagram, can be defined as:

arm_1 = np.array([L1*np.cos(a), L1*np.sin(a)]).T
arm_2 = np.array([L2*np.cos(a+b), L2*np.sin(a+b)]).T

print(f"Arm 1 position: {arm_1}")
print(f"Arm 2 position: {arm_2}")

Making the final position of the end-effector the sum of both displacements:

p = arm_1 + arm_2
print(f"Final end-effector position: {p}")

The complete position calculation can be transformed into a function so that we can calculate and plot multiple robot arm positions:

def effector_position(L1=1, L2=1, a=PI/4, b=PI/4, ratio=1.1):

  # Origin coordinates:
  x_0 = 0
  y_0 = 0

  # Joint 1 displacement:
  x_1 = (math.cos(a))*L1
  y_1 = (math.sin(a))*L1

  # Joint 2 displacement:
  x_2 = (x_1 + ((math.cos(a+b))*L2))
  y_2 = (y_1 + ((math.sin(a+b))*L2))  
  
  # Create an empty figure:
  plt.figure()

  # Add the position to the figure:
  plt.plot(x_0, y_0, marker="o", markersize=10,
               markeredgecolor="black", markerfacecolor="white")
  plt.plot(x_1, y_1, marker="o", markersize=10,
               markeredgecolor="black", markerfacecolor="white")
  plt.plot([x_0, x_1], [y_0, y_1], color = 'black', linewidth = 5)
  plt.plot([x_1, x_2], [y_1, y_2], color = 'black', linewidth = 5)
  plt.plot(x_2, y_2, marker="o", markersize=10,
               markeredgecolor="red", markerfacecolor="green")

  # Limit the image axes to 1.1 max and min values:
  min = -L1-L2
  max = L1+L2
  plt.axis(np.array([min, max, 0, max])*ratio)

The default position returns an image with the position of the arms:

This "direct kinematics" calculation is not extremely exciting; it is begging us to extend it so that we can plot all the possible end effector locations that a given robotic arm could reach. Usually, mechanical joints, not unlike human joints, will have angular displacement limitations, so our arm position plot will take the movement limitations to plot all the possible locations the arm could reach:

def coverage(L1=1, L2=1, a_lims=[0, 2*PI], b_lims=[0, 2*PI], ratio=1.1, N=5):

  # Origin coordinates:
  x_0 = 0
  y_0 = 0

  # Empty figure:
  plt.figure()

  a_s = np.linspace(a_lims[0], a_lims[1], N)
  b_s = np.linspace(b_lims[0], b_lims[1], N)

  # Iterate for every value for a angle:
  for a in a_s:
    # First joint location.
    x_1 = (math.cos(a))*L1
    y_1 = (math.sin(a))*L1

    # Iterate for every value for b angle:
    for b in b_s: 
      # Second joint location:
      x_2 = (x_1 + ((math.cos(a+b))*L2))
      y_2 = (y_1 + ((math.sin(a+b))*L2))   
      
      # Add the position to the figure:
      plt.plot(x_0, y_0, marker="o", markersize=10,
                  markeredgecolor="black", markerfacecolor="white")
      plt.plot(x_1, y_1, marker="o", markersize=10,
                  markeredgecolor="black", markerfacecolor="white")
      plt.plot([x_0, x_1], [y_0, y_1], color = 'black', linewidth = 5)
      plt.plot([x_1, x_2], [y_1, y_2], color = 'black', linewidth = 5)
      plt.plot(x_2, y_2, marker="o", markersize=10,
                  markeredgecolor="red", markerfacecolor="green")

      # Limit the image axes to 1.1 max and min values:
      min = -L1-L2
      max = L1+L2
      plt.axis(np.array([min, max, 0, max])*ratio)

By passing as keyword arguments the lengths of the robotic arms, the limits of angular displacement for these arms, the size ratio for the plot, and the number of iterations we want to see displayed on the graph, we can obtain a "cloud" of possible end effector positions:

coverage(a_lims=[PI/12, PI/2], b_lims=[-PI/2, PI/2], N=12)

The result is this:

The forward kinematic calculation is relatively simple. However, the exciting part, the complex part, and the instrumental part of kinematics is that reverse calculation. Reverse kinematics will give us the position of the joint angles required to reach a given place with our end effectors. We will see the reverse kinematics for this simple robot in a following post.

Do not hesitate to contact us if you require quantitative model development, deployment, verification, or validation. We will also be glad to help you with your machine learning or artificial intelligence challenges when applied to asset robotics, automation, or intelligence gathering from satellite, drone, or fixed-point imagery.

The notebook for this publication is located here.