Author: Michael Greer
This notebook introduces iterative inverse kinematics via jacobian inversion.
import numpy as np
from numpy import cos, sin, pi
import matplotlib.pyplot as plt
import sympy as sp
First, we define a function to plot the planar robot
def plot_robot(links, joints):
# Convert to list if it's an ndarray
if (isinstance(joints, np.ndarray)):
joints = joints.flatten().tolist()
xs = [0]
ys = [0]
total_len = sum(links)
total_q = 0
for l,q in zip(links, joints):
total_q += q
xs.append(xs[-1] + l * cos(total_q))
ys.append(ys[-1] + l * sin(total_q))
plt.figure(figsize=(8,8))
plt.plot(xs, ys)
plt.ylim(-1 * total_len, total_len)
plt.xlim(-1 * total_len, total_len)
plt.title('Robot Manipulator Pose')
return
# Change the joint values to see how the arm moves
plot_robot([5, 5, 5], [0, pi/2, -pi/2])
From this, we can see that the forward kinematics are simply:
$$ x = \sum_{i=1}^n l_{i}cos(\sum_{j=1}^{i}q_{j})$$$$ y = \sum_{i=1}^n l_{i}sin(\sum_{j=1}^{i}q_{j})$$The forward kinematics give us the cartesian end effector position at a certain configuration, implemented in the function below.
def f_kine(links, joints):
# Convert to list if it's an ndarray
if (isinstance(joints, np.ndarray)):
joints = joints.flatten().tolist()
x = 0
y = 0
total_len = sum(links)
total_q = 0
for l,q in zip(links, joints):
total_q += q
x += l * cos(total_q)
y += l * sin(total_q)
return np.array([[x],[y]])
The jacobian is simply an array comprised of the derivative of the forward kinematic function with respect to each of the joints, evaluated at those joint values. For the 2-D case, this is:
$$\begin{bmatrix} \frac{\partial f_{x}}{\partial q_{1}} & \frac{\partial f_{x}}{\partial q_{2}} & ... & \frac{\partial f_{x}}{\partial q_{n}} \\ \frac{\partial f_{y}}{\partial q_{1}} & \frac{\partial f_{y}}{\partial q_{2}} & ... & \frac{\partial f_{y}}{\partial q_{n}} \end{bmatrix}$$Where n is the total number of joints in the robot. For simplicity in the code, the link and joint values are indexed at 0 instead of 1.
def jacobian(links, joints):
# Convert to list if it's an ndarray
if (isinstance(joints, np.ndarray)):
joints = joints.flatten().tolist()
# Form symbolic q matrix
qs = []
for i in range(0,len(joints)):
qs.append(sp.Symbol('q{}'.format(i)))
jac = np.zeros((2, len(links)))
x = 0
y = 0
# Form forward kinematics
for i in range(len(links)):
total_q = 0
for j in range(i + 1):
total_q += qs[j]
x += links[i] * sp.cos(total_q)
y += links[i] * sp.sin(total_q)
# Differentiate to find jacobian
for i in range(len(links)):
Jx = sp.diff(x, qs[i])
Jy = sp.diff(y, qs[i])
for k in range(len(links)):
Jx = Jx.subs(qs[k], joints[k])
Jy = Jy.subs(qs[k], joints[k])
jac[0,i] = Jx.evalf()
jac[1,i] = Jy.evalf()
return jac
The significance of the jacobian is that it shows us each joint's ability to move the end effector in the x and y directions. The larger the value in the jacobian matrix, the greater that joint's ability to move the end effector in that direction. This also brings us to another method for calculating the jacobian which involves taking the cross product between the joint axis and the vector between the joint origin and the end effector, but that's a subject for another time. The relationship between joint movement and end effector movement can be shown as: $$\dot{x}=J\dot{\theta}$$ We can rearrange this to form: $$\dot{\theta}=J^{+}\dot{x}$$ Since the jacobian is not square in the redundant case, we use the psuedoinverse instead of the inverse (denotated by the superscript +). Now we can specify a desired movement in the cartesian space, and use this to find what the corresponding movement should be in the joint space. Since this is a linear approximation, we limit the size of any individual $\dot{x}$ and repeat this process until we get close to our target.
# Note: this ONLY works for targets within the workspace. For points outside the workspace this function will not exit
def i_kine(links, joints, target, error_trace=False):
current_q = joints
# This term limits the maximum delta x per move
max_move = 2
e_trace = []
while(1):
# Get the current end effector position
current_x = f_kine(links, current_q)
# Get the vector to the target
delta_x = target - current_x
# Find the magnitude of the movement required
delta_x_norm = np.linalg.norm(delta_x)
e_trace.append(delta_x_norm)
# Limit the maximum magnitude of delta_x
if (delta_x_norm > max_move):
delta_x /= (delta_x_norm / max_move)
# Stop if the end effector is sufficently close to target
if (delta_x_norm < 0.001):
break
jac = jacobian(links, current_q)
# Jacobian pseudoinversion
jac_pinv = np.linalg.pinv(jac)
# Find the required movements in the joint space
alpha = 0.8
delta_q = alpha * jac_pinv @ delta_x
# Add the change to find the updated joint angles
current_q += delta_q
return (current_q, e_trace) if error_trace else current_q
links = [5.0, 5.0, 5.0, 5.0, 5.0, 5.0]
joints = np.array([[0.0],[0.0],[0.0],[0.0],[0.0],[0.0]])
target = np.array([[15.0],[-15.0]])
joints, e_trace = i_kine(links, joints, target, error_trace=True)
plot_robot(links, joints)
plt.figure(figsize=(8,8))
plt.plot(e_trace)
plt.title('Error Trace')
We can clearly see that this method is suitable to reduce end effector error. Compared to the direct inverse kinematics approach this iterative method takes more time, but remember that the direct approach is not applicable to redundant manipulators. This iterative approach also has the benefit of incorporating multi-objective solutions. These objectives can be joint movement reduction, joint limit avoidance, and collision avoidance. This method can also generalize to any structure that is an open kinematic chain; test this by changing the structure of the robot in the previous cell.
The main downside to this is that it is an iterative operation, whereas a direct approach exists for non-redundant systems. Again, this approach does not apply to redundant robots. Redundant kinematics and multi-objective inverse kinematics will be explored more in future notebooks.