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
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)
In [3]:
ans = simplifyT(ans)
print(ans)
In [4]:
print('position x: {}'.format(ans[0,3]))
print('position y: {}'.format(ans[1,3]))
print('position z: {}'.format(ans[2,3]))
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!
This work is licensed under a Creative Commons Attribution-ShareAlike 4.0 International License.