Puma Example

Kevin Walchko

created 7 Nov 2017


This is just an example of a more complex serial manipulator.


In [1]:
%matplotlib inline

# Let's grab some libraries to help us manipulate symbolic equations
from __future__ import print_function
from __future__ import division
import numpy as np
import sympy
from sympy import symbols, sin, cos, pi, simplify

def makeT(a, alpha, d, theta):
    # create a modified DH homogenious matrix
    return np.array([
        [           cos(theta),           -sin(theta),           0,             a],
        [sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha)],
        [sin(theta)*sin(alpha), cos(theta)*sin(alpha),  cos(alpha),  d*cos(alpha)],
        [                    0,                     0,           0,             1]
    ])

def simplifyT(tt):
    """
    This goes through each element of a matrix and tries to simplify it.
    """
    for i, row in enumerate(tt):
        for j, col in enumerate(row):
            tt[i,j] = simplify(col)
    return tt

Puma

Puma robot is an old, but classical serial manipulator. You can see Criag's example in section 3.7, pg 77. Once you have the DH parameters, you can use the above matrix to find the forward kinematics,


In [2]:
# craig puma
t1,t2,t3,t4,t5,t6 = symbols('t1 t2 t3 t4 t5 t6')
a2, a3, d3, d4 = symbols('a2 a3 d3 d4')

T1 = makeT(0,0,0,t1)
T2 = makeT(0,-pi/2,0,t2)
T3 = makeT(a2,0,d3,t3)
T4 = makeT(a3,-pi/2,d4,t4)
T5 = makeT(0,pi/2,0,t5)
T6 = makeT(0,-pi/2,0,t6)

ans = np.eye(4)
for T in [T1, T2, T3, T4, T5, T6]:
    ans = ans.dot(T)

print(ans)


[[(((-1.0*sin(t2)*sin(t3)*cos(t1) + 1.0*cos(t1)*cos(t2)*cos(t3))*cos(t4) + 1.0*sin(t1)*sin(t4))*cos(t5) + (-1.0*sin(t2)*cos(t1)*cos(t3) - 1.0*sin(t3)*cos(t1)*cos(t2))*sin(t5))*cos(t6) - ((-1.0*sin(t2)*sin(t3)*cos(t1) + 1.0*cos(t1)*cos(t2)*cos(t3))*sin(t4) - 1.0*sin(t1)*cos(t4))*sin(t6)
  -(((-1.0*sin(t2)*sin(t3)*cos(t1) + 1.0*cos(t1)*cos(t2)*cos(t3))*cos(t4) + 1.0*sin(t1)*sin(t4))*cos(t5) + (-1.0*sin(t2)*cos(t1)*cos(t3) - 1.0*sin(t3)*cos(t1)*cos(t2))*sin(t5))*sin(t6) - ((-1.0*sin(t2)*sin(t3)*cos(t1) + 1.0*cos(t1)*cos(t2)*cos(t3))*sin(t4) - 1.0*sin(t1)*cos(t4))*cos(t6)
  -((-1.0*sin(t2)*sin(t3)*cos(t1) + 1.0*cos(t1)*cos(t2)*cos(t3))*cos(t4) + 1.0*sin(t1)*sin(t4))*sin(t5) + (-1.0*sin(t2)*cos(t1)*cos(t3) - 1.0*sin(t3)*cos(t1)*cos(t2))*cos(t5)
  1.0*a2*cos(t1)*cos(t2) + a3*(-1.0*sin(t2)*sin(t3)*cos(t1) + 1.0*cos(t1)*cos(t2)*cos(t3)) - 1.0*d3*sin(t1) + d4*(-1.0*sin(t2)*cos(t1)*cos(t3) - 1.0*sin(t3)*cos(t1)*cos(t2))]
 [(((-1.0*sin(t1)*sin(t2)*sin(t3) + 1.0*sin(t1)*cos(t2)*cos(t3))*cos(t4) - 1.0*sin(t4)*cos(t1))*cos(t5) + (-1.0*sin(t1)*sin(t2)*cos(t3) - 1.0*sin(t1)*sin(t3)*cos(t2))*sin(t5))*cos(t6) - ((-1.0*sin(t1)*sin(t2)*sin(t3) + 1.0*sin(t1)*cos(t2)*cos(t3))*sin(t4) + 1.0*cos(t1)*cos(t4))*sin(t6)
  -(((-1.0*sin(t1)*sin(t2)*sin(t3) + 1.0*sin(t1)*cos(t2)*cos(t3))*cos(t4) - 1.0*sin(t4)*cos(t1))*cos(t5) + (-1.0*sin(t1)*sin(t2)*cos(t3) - 1.0*sin(t1)*sin(t3)*cos(t2))*sin(t5))*sin(t6) - ((-1.0*sin(t1)*sin(t2)*sin(t3) + 1.0*sin(t1)*cos(t2)*cos(t3))*sin(t4) + 1.0*cos(t1)*cos(t4))*cos(t6)
  -((-1.0*sin(t1)*sin(t2)*sin(t3) + 1.0*sin(t1)*cos(t2)*cos(t3))*cos(t4) - 1.0*sin(t4)*cos(t1))*sin(t5) + (-1.0*sin(t1)*sin(t2)*cos(t3) - 1.0*sin(t1)*sin(t3)*cos(t2))*cos(t5)
  1.0*a2*sin(t1)*cos(t2) + a3*(-1.0*sin(t1)*sin(t2)*sin(t3) + 1.0*sin(t1)*cos(t2)*cos(t3)) + 1.0*d3*cos(t1) + d4*(-1.0*sin(t1)*sin(t2)*cos(t3) - 1.0*sin(t1)*sin(t3)*cos(t2))]
 [((1.0*sin(t2)*sin(t3) - 1.0*cos(t2)*cos(t3))*sin(t5) + (-1.0*sin(t2)*cos(t3) - 1.0*sin(t3)*cos(t2))*cos(t4)*cos(t5))*cos(t6) - (-1.0*sin(t2)*cos(t3) - 1.0*sin(t3)*cos(t2))*sin(t4)*sin(t6)
  -((1.0*sin(t2)*sin(t3) - 1.0*cos(t2)*cos(t3))*sin(t5) + (-1.0*sin(t2)*cos(t3) - 1.0*sin(t3)*cos(t2))*cos(t4)*cos(t5))*sin(t6) - (-1.0*sin(t2)*cos(t3) - 1.0*sin(t3)*cos(t2))*sin(t4)*cos(t6)
  (1.0*sin(t2)*sin(t3) - 1.0*cos(t2)*cos(t3))*cos(t5) - (-1.0*sin(t2)*cos(t3) - 1.0*sin(t3)*cos(t2))*sin(t5)*cos(t4)
  -1.0*a2*sin(t2) + a3*(-1.0*sin(t2)*cos(t3) - 1.0*sin(t3)*cos(t2)) + d4*(1.0*sin(t2)*sin(t3) - 1.0*cos(t2)*cos(t3))]
 [0 0 0 1.00000000000000]]

In [3]:
ans = simplifyT(ans)
print(ans)


[[1.0*((sin(t1)*sin(t4) + cos(t1)*cos(t4)*cos(t2 + t3))*cos(t5) - sin(t5)*sin(t2 + t3)*cos(t1))*cos(t6) + 1.0*(sin(t1)*cos(t4) - sin(t4)*cos(t1)*cos(t2 + t3))*sin(t6)
  1.0*(-(sin(t1)*sin(t4) + cos(t1)*cos(t4)*cos(t2 + t3))*cos(t5) + sin(t5)*sin(t2 + t3)*cos(t1))*sin(t6) + 1.0*(sin(t1)*cos(t4) - sin(t4)*cos(t1)*cos(t2 + t3))*cos(t6)
  -1.0*(sin(t1)*sin(t4) + cos(t1)*cos(t4)*cos(t2 + t3))*sin(t5) - 1.0*sin(t2 + t3)*cos(t1)*cos(t5)
  1.0*a2*cos(t1)*cos(t2) + 1.0*a3*cos(t1)*cos(t2 + t3) - 1.0*d3*sin(t1) - 1.0*d4*sin(t2 + t3)*cos(t1)]
 [1.0*((sin(t1)*cos(t4)*cos(t2 + t3) - sin(t4)*cos(t1))*cos(t5) - sin(t1)*sin(t5)*sin(t2 + t3))*cos(t6) - 1.0*(sin(t1)*sin(t4)*cos(t2 + t3) + cos(t1)*cos(t4))*sin(t6)
  1.0*((-sin(t1)*cos(t4)*cos(t2 + t3) + sin(t4)*cos(t1))*cos(t5) + sin(t1)*sin(t5)*sin(t2 + t3))*sin(t6) - 1.0*(sin(t1)*sin(t4)*cos(t2 + t3) + cos(t1)*cos(t4))*cos(t6)
  1.0*(-sin(t1)*cos(t4)*cos(t2 + t3) + sin(t4)*cos(t1))*sin(t5) - 1.0*sin(t1)*sin(t2 + t3)*cos(t5)
  1.0*a2*sin(t1)*cos(t2) + 1.0*a3*sin(t1)*cos(t2 + t3) + 1.0*d3*cos(t1) - 1.0*d4*sin(t1)*sin(t2 + t3)]
 [-1.0*(sin(t5)*cos(t2 + t3) + sin(t2 + t3)*cos(t4)*cos(t5))*cos(t6) + 1.0*sin(t4)*sin(t6)*sin(t2 + t3)
  1.0*(sin(t5)*cos(t2 + t3) + sin(t2 + t3)*cos(t4)*cos(t5))*sin(t6) + 1.0*sin(t4)*sin(t2 + t3)*cos(t6)
  1.0*sin(t5)*sin(t2 + t3)*cos(t4) - 1.0*cos(t5)*cos(t2 + t3)
  -1.0*a2*sin(t2) - 1.0*a3*sin(t2 + t3) - 1.0*d4*cos(t2 + t3)]
 [0 0 0 1.00000000000000]]

In [4]:
print('position x: {}'.format(ans[0,3]))
print('position y: {}'.format(ans[1,3]))
print('position z: {}'.format(ans[2,3]))


position x: 1.0*a2*cos(t1)*cos(t2) + 1.0*a3*cos(t1)*cos(t2 + t3) - 1.0*d3*sin(t1) - 1.0*d4*sin(t2 + t3)*cos(t1)
position y: 1.0*a2*sin(t1)*cos(t2) + 1.0*a3*sin(t1)*cos(t2 + t3) + 1.0*d3*cos(t1) - 1.0*d4*sin(t1)*sin(t2 + t3)
position z: -1.0*a2*sin(t2) - 1.0*a3*sin(t2 + t3) - 1.0*d4*cos(t2 + t3)

Looking at the position, this is the same position listed in Craig, eqn 3.14.

Also, this is the simplified version!!!. As you get more joints and degrees of freedom, the equations get nastier. You also can run into situations where you end up with singularities (like division by zero) and send your robot into a bad place!