Robot Arm Kinematics (2D)

In [1]:

    

%matplotlib inline
from pylab import *
from numpy import sin, cos, pi, matrix, random
from __future__ import division
from math import atan2
rcParams['figure.figsize'] = 12, 10  # that's default image size for this interactive session
from IPython import display
from IPython.html.widgets import interact, fixed


    

    




---------------------------------------------------------------------------
ImportError                               Traceback (most recent call last)
<ipython-input-1-f2484913cece> in <module>()
----> 1 get_ipython().magic(u'matplotlib inline')
      2 from pylab import *
      3 from numpy import sin, cos, pi, matrix, random
      4 from __future__ import division
      5 from math import atan2

/usr/local/lib/python2.7/dist-packages/IPython/core/interactiveshell.pyc in magic(self, arg_s)
   2334         magic_name, _, magic_arg_s = arg_s.partition(' ')
   2335         magic_name = magic_name.lstrip(prefilter.ESC_MAGIC)
-> 2336         return self.run_line_magic(magic_name, magic_arg_s)
   2337 
   2338     #-------------------------------------------------------------------------

/usr/local/lib/python2.7/dist-packages/IPython/core/interactiveshell.pyc in run_line_magic(self, magic_name, line)
   2255                 kwargs['local_ns'] = sys._getframe(stack_depth).f_locals
   2256             with self.builtin_trap:
-> 2257                 result = fn(*args,**kwargs)
   2258             return result
   2259 

/usr/local/lib/python2.7/dist-packages/IPython/core/magics/pylab.pyc in matplotlib(self, line)

/usr/local/lib/python2.7/dist-packages/IPython/core/magic.pyc in <lambda>(f, *a, **k)
    191     # but it's overkill for just that one bit of state.
    192     def magic_deco(arg):
--> 193         call = lambda f, *a, **k: f(*a, **k)
    194 
    195         if callable(arg):

/usr/local/lib/python2.7/dist-packages/IPython/core/magics/pylab.pyc in matplotlib(self, line)
     98             print("Available matplotlib backends: %s" % backends_list)
     99         else:
--> 100             gui, backend = self.shell.enable_matplotlib(args.gui)
    101             self._show_matplotlib_backend(args.gui, backend)
    102 

/usr/local/lib/python2.7/dist-packages/IPython/core/interactiveshell.pyc in enable_matplotlib(self, gui)
   3118         """
   3119         from IPython.core import pylabtools as pt
-> 3120         gui, backend = pt.find_gui_and_backend(gui, self.pylab_gui_select)
   3121 
   3122         if gui != 'inline':

/usr/local/lib/python2.7/dist-packages/IPython/core/pylabtools.pyc in find_gui_and_backend(gui, gui_select)
    237     """
    238 
--> 239     import matplotlib
    240 
    241     if gui and gui != 'auto':

ImportError: No module named matplotlib

In [ ]:

    

def trans(x, y, a):
    '''create a 2D transformation'''
    s = sin(a)
    c = cos(a)
    return matrix([[c, -s, x],
                   [s,  c, y],
                   [0,  0, 1]])

def from_trans(m):
    '''get x, y, theta from transform matrix'''
    return [m[0, -1], m[1, -1], atan2(m[1, 0], m[0, 0])]


    

In [ ]:

    

trans(0, 0, 0)


    

In [ ]:

    

N = 3  # number of links
l = [0] + range(N, 0, -1)  # length of link, l[0] is ingored
max_len = sum(l)
a = random.random_sample(N)  # angles of joints
T0 = trans(0, 0, 0)  # base


    

In [ ]:

    

def forward_kinematics(T0, l, a):
    T = [T0]
    for i in range(len(a)):
        Ti = T[-1] * trans(l[i], 0, a[i])
        T.append(Ti)
    Te = T[-1] * trans(l[-1], 0, 0)  # end effector
    T.append(Te)
    return T


    

In [ ]:

    

def show_robot_arm(T):
    cla()
    x = [Ti[0,-1] for Ti in T]
    y = [Ti[1,-1] for Ti in T]
    plot(x, y, '-or', linewidth=5, markersize=10)
    xlim([-max_len, max_len])
    ylim([-max_len, max_len]) 
    ax = axes()
    ax.set_aspect('equal')
    t = atan2(T[-1][1, 0], T[-1][0,0])
    ax.annotate('[%f,%f,%f]' % (x[-1], y[-1], t), xy=(x[-1], y[-1]), xytext=(x[-1], y[-1] + 0.5))
    display.clear_output(wait=True)


    

In [ ]:

    

for i in range(N):
    @interact(value=(-pi/2, pi/2, 0.1), n=fixed(i))
    def set_joint_angle(n, value=0):
        global a
        a[n] = value
        T = forward_kinematics(T0, l, a)
        show_robot_arm(T)


    

In [ ]:

    

from sympy import init_printing, sin, cos, pi, asin, acos, atan2, atan, sqrt, simplify, solve, latex, symbols
from sympy import Matrix as matrix
init_printing()


    

In [ ]:

    

from sympy.abc import x, y, theta
trans(x, y, theta)


    

In [ ]:

    

N = 4
l = [symbols('l_%d' % i) for i in range(N + 1)]
l[0] = 0
a = [symbols('theta_%d' % i) for i in range(N)]
T0 = trans(0, 0, 0)


    

In [ ]:

    

l, a


    

In [ ]:

    

T0


    

In [ ]:

    

T = forward_kinematics(T0, l, a)
Te = T[-1]
Te


    

In [ ]:

    

Te = simplify(Te)
Te


    

In [2]:

    

eq = Te * trans(x, y, theta).inv() - T0
eq


    

    




---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-2-25ea52504e70> in <module>()
----> 1 eq = Te * trans(x, y, theta).inv() - T0
      2 eq

NameError: name 'Te' is not defined

In [3]:

    

eq = [a[0] + a[1] + a[2] - theta, eq[0, 2], eq[1, 2]]
eq


    

    




---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-3-cb09e6f0c139> in <module>()
----> 1 eq = [a[0] + a[1] + a[2] - theta, eq[0, 2], eq[1, 2]]
      2 eq

NameError: name 'a' is not defined

In [4]:

    

solve(eq, a)  # sympy is too stupid to solve it


    

    




---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-4-484eedeff113> in <module>()
----> 1 solve(eq, a)  # sympy is too stupid to solve it

NameError: name 'solve' is not defined

In [5]:

    

b = [symbols('theta_%d' % i) for i in range(N)]
T3 = trans(x, y, theta) * trans(l[3], 0, 0).inv()
l_1_2 = T3[0,-1] ** 2 + T3[1,-1] ** 2
b_1_2 = acos((l[1] ** 2 + l[2] ** 2 - l_1_2) / (2 * l[1] * l[2]))  # cosine rule
b[1] = pi - b_1_2
b_1 = acos((l[1] ** 2 - l[2] ** 2 + l_1_2) / (2 * l[1] * sqrt(l_1_2)))  # cosine rule
b_0 = atan(T3[1,-1] / T3[0,-1])
b[0] = b_0 - b_1
b[2] = atan(T3[1,0] / T3[0,0]) - b[0] - b[1]
B = matrix([b]).T
B = simplify(B)
B


    

    




---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-5-f07d5fa80632> in <module>()
----> 1 b = [symbols('theta_%d' % i) for i in range(N)]
      2 T3 = trans(x, y, theta) * trans(l[3], 0, 0).inv()
      3 l_1_2 = T3[0,-1] ** 2 + T3[1,-1] ** 2
      4 b_1_2 = acos((l[1] ** 2 + l[2] ** 2 - l_1_2) / (2 * l[1] * l[2]))  # cosine rule
      5 b[1] = pi - b_1_2

NameError: name 'N' is not defined

In [ ]:

    

print latex(B)


    

In [6]:

    

T_new = forward_kinematics(T0, l, b)
Te_new = T_new[-1]
Te_new


    

    




---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-6-57759ca7f77a> in <module>()
----> 1 T_new = forward_kinematics(T0, l, b)
      2 Te_new = T_new[-1]
      3 Te_new

NameError: name 'forward_kinematics' is not defined

In [7]:

    

simplify(Te_new[0, 0])


    

    




---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-7-f9f0def762e9> in <module>()
----> 1 simplify(Te_new[0, 0])

NameError: name 'simplify' is not defined

In [8]:

    

simplify(Te_new[1, 0])


    

    




---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-8-e3f3bb645c6a> in <module>()
----> 1 simplify(Te_new[1, 0])

NameError: name 'simplify' is not defined

In [9]:

    

simplify(Te_new[0, -1])


    

    




---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-9-bf7b730f49ba> in <module>()
----> 1 simplify(Te_new[0, -1])

NameError: name 'simplify' is not defined

In [10]:

    

from numpy import sin, cos, pi, matrix
from math import atan2, acos

T0 = trans(0, 0, 0)
lv = [0] + range(N, 0, -1)  # length of link, l[0] is ingored
bf = B
for i in range(N):
    bf = bf.subs(l[i + 1], lv[i + 1])

def inverse_kinematics(x_e, y_e, theta_e):
    b = bf.subs(x, x_e).subs(y, y_e).subs(theta, theta_e)
    b = (b.subs('I', 1).subs('pi', pi).tolist())
    b = [float(i[0]) for i in b]
    return b
    
@interact(x_e=(0, max_len, 0.1), y_e=(-max_len, max_len, 0.1), theta_e=(-pi, pi, 0.1))
def set_end_effector(x_e=6, y_e=0, theta_e=0):
    b = inverse_kinematics(x_e, y_e, theta_e)
    T = forward_kinematics(T0, lv, b)
    show_robot_arm(T)


    

    




---------------------------------------------------------------------------
ImportError                               Traceback (most recent call last)
<ipython-input-10-5d4c6615606a> in <module>()
----> 1 from numpy import sin, cos, pi, matrix
      2 from math import atan2, acos
      3 
      4 T0 = trans(0, 0, 0)
      5 lv = [0] + range(N, 0, -1)  # length of link, l[0] is ingored

ImportError: No module named numpy

In [11]:

    

from numpy import asarray
theta = asarray([0.1] * N)
def inverse_kinematics(x_e, y_e, theta_e, theta):
    target = matrix([[x_e, y_e, theta_e]])
    while True:
        T = forward_kinematics(T0, lv, theta)
        Te = matrix([from_trans(T[-1])])
        e = target - Te
        T = matrix([from_trans(i) for i in T[1:-1]])
        J = Te - T
        J = J.T
        J[-1, :] = 1
        JJT = J * J.T
        d_theta = 0.001 * J.T * JJT.I * e.T
        #print (d_theta.T * d_theta)[0, 0]
        theta += asarray(d_theta.T)[0]
        if (d_theta.T * d_theta)[0, 0] < 1e-6:
            break
    return theta

T = forward_kinematics(T0, lv, theta)
Te = matrix([from_trans(T[-1])])

@interact(x_e=(0, max_len, 0.01), y_e=(-max_len, max_len, 0.01), theta_e=(-pi, pi, 0.01), theta=fixed(theta))
def set_end_effector(x_e=Te[0,0], y_e=Te[0,1], theta_e=Te[0,2], theta=theta):
    theta = inverse_kinematics(x_e, y_e, theta_e, theta)
    T = forward_kinematics(T0, lv, theta)
    show_robot_arm(T)


    

    




---------------------------------------------------------------------------
ImportError                               Traceback (most recent call last)
<ipython-input-11-42579ee25b5f> in <module>()
----> 1 from numpy import asarray
      2 theta = asarray([0.1] * N)
      3 def inverse_kinematics(x_e, y_e, theta_e, theta):
      4     target = matrix([[x_e, y_e, theta_e]])
      5     while True:

ImportError: No module named numpy

In [ ]: