fn_Invki.py

##%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
##% EECS 498: Hands-On Robotics
##% Project 2 - Robotic Manipulator
##% Kurt Lundeen, Bharadwaj Mantha
##% 2014-11-25
##%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
import sympy as sym
from RobotArm import*
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def goToPoint(self, p):


  



    th1= sym.Symbol('th1')
    th2= sym.Symbol('th2')
    th3= sym.Symbol('th3')
    th4= sym.Symbol('th4')
    th5= sym.Symbol('th5')
    L1 = sym.Symbol('L1')
    L2 = sym.Symbol('L2')
    L3 = sym.Symbol('L3')
    L4 = sym.Symbol('L4')

    th = [th1,th2,th3,th4, th5];
    numJoints = (th.size)[1];
    spaDim = 3;
    gst0 = [npxeye(spaDim), [0 , L2+L3+L4, L1];zeros(1,spaDim) 1];

    w[1] = [0;0;1];
    w[2] = [-1;0;0];
    w[3] = w[2];
    w[4] = [0;1;0];
    w[5] = w[2];

    q = cell(1,numJoints);
    q[1] = [0;0;L1];
    q[2] = q[1];
    q[3] = [0;L2;L1];
    q[4] = [0;L2+L3;L1];
    q[5] = q[4];

    expwth = cell(1,numJoints);
    expwth[1] = [cos(th1) -sin(th1) 0
                 sin(th1)  cos(th1) 0
                        0         0 1];
    expwth[2] = [1         0        0
                 0  cos(th2) sin(th2)
                 0 -sin(th2) cos(th2)];
    expwth[3] = [1         0        0
                 0  cos(th3) sin(th3)
                 0 -sin(th3) cos(th3)];
    expwth[4] = [ cos(th4) 0 sin(th4)
                         0 1        0
                 -sin(th4) 0 cos(th4)];
    expwth[5] = [1         0        0
                 0  cos(th5) sin(th5)
                 0 -sin(th5) cos(th5)];          

    v = cell(1,numJoints);
    xi = cell(1,numJoints);
    for i = 1:numJoints
        v[i] = np.cross(-w[i],q[i]);
        xi[i] = [v[i];w[i]];
        expxith[i] = [expwth[i] (eye(spaDim)-expwth[i])*np.cross(w[i],v[i])+w[i]*np.transpose(w[i])*v[i]*th(i);zeros(1,spaDim) 1];
    end

    gst = simplify(expxith[1]*expxith[2]*expxith[3]*expxith[4]*expxith[5]*gst0);

    ##% Inverse kinematics

    gst = self.getEffectorMatrix();

    Tr = gst(1:3,4); ##% the translational matrix of the end effector
    ##% #%partial derivaties with respective to each joint rotation th
    '''
    **********THIS SYMBOLIC DIFFERENTIATION MAY NOT WORK HERE. CONSIDER USING A NUMERIC APPROACH******
    '''
    j1=sym.diff(Tr,th1); ##%#w.r.t th1
    j2=sym.diff(Tr,th2); ##%w.r.t th2
    j3=sym.diff(Tr,th3); ##%w.r.t th3
    j4=sym.diff(Tr,th4); ##%w.r.t th4
    j5=sym.diff(Tr,th5); ##%w.r.t th5
    Jac=[j1 j2 j3 j4 j5]; #% generalized Jacobian matrix

    
    th1=0;th2=-pi/4;th3=pi/2;th4=0;th5=pi/2;
    
    L1=150; L2=240; L3=300; L4=80; ##% beginning point (current location of the robot)
    subs(Tr); ##%initial coordinates of the robot arm
    #% Jac=subs(Jac); ##% jacobian matrix 
    #% Jac_inv=pinv(Jac); #% inverse jacobian matrix
    x=2;
    a=0.001;
    while x>0.2
    l_d=p;#%[0;400;35]; #%end point (desired location)
    l_c=subs(Tr);#% current coordinates of the robot arm 
    e=l_d-l_c; #%error b/w the desired and actual position
    Jac=subs(Jac); #% jacobian matrix 
    Jac_inv=pinv(Jac); #% inverse jacobian matrix
    th_d=[th1;th2;th3;th4;th5]+Jac_inv*e+a*randn(5,1); #%desired joint angles
    th1=th_d(1);th2=th_d(2);th3=th_d(3);th4=th_d(4);th5=th_d(5);
    subs(Tr); #%to check for intermediate coordinate points
    x=norm(l_d-l_c); #% calculating the norm for running the loop
    end 
    #%subs(Tr)
    end