http://www.csg.ethz.ch/education/lectures/ATCN/ws06_07

http://www.uoftbookstore.com/online/merchant.ihtml?id=452&step=2

 http://www.iis.ee.ethz.ch/~zimmi/publications/comp_arith_notes.pdf

http://www.ibr.cs.tu-bs.de/papers/

http://www.soe.uoguelph.ca/webfiles/rmuresan/ENGG3640MicroInterfacing.htm

http://www.stanford.edu/class/ee281/

http://inst.eecs.berkeley.edu/~cs162/fa08/

 http://inst.eecs.berkeley.edu/~cs152/sp08/

 http://www.tik.ee.ethz.ch/tik/education/lectures/DRS/DRS.html

http://www.ida.liu.se/labs/eslab/teaching.shtml

 https://ccnet.stanford.edu/ee108b/

http://intranet.cs.man.ac.uk/Study_subweb/Ugrad/coursenotes/CS1211/

http://www.cs.uiuc.edu/class/fa07/cs232/lectures/

 http://www.pads.uwaterloo.ca/ece324/protected/lectures/lectures.html

http://web.abo.fi/~lpetre/teleprot/teleprot.html

http://camars.kaist.ac.kr/~hyoon/courses/cs440/

 https://www.iu.hio.no/~mark/lectures/MS005A/

 http://thydzik.com/academic/Robotics%20315/chap1.pdf

http://thydzik.com/academic/Robotics%20315/chap2.pdf

http://thydzik.com/academic/Robotics%20315/chap3.pdf

http://thydzik.com/academic/Robotics%20315/chap4.pdf

http://thydzik.com/academic/Robotics%20315/chap5.pdf

http://thydzik.com/academic/Robotics%20315/chap6.pdf

http://thydzik.com/academic/Robotics%20315/chap7.pdf

http://www.cs.cmu.edu/~biorobotics/book/

1) X cos(θi) + Y sin(θi) = K

X, Y, and K are constants.

Then θi=2 atan(Y+(+/-)sqrt(X^2+Y^2+K^2), X+K)

2) sin(θi)=K1
    cos(θi)=K2

where K1, and K2 are constants. Then K1/K2 yields

θi=atan(K1, K2)

3) Xj sin(θi)=K1
    Xj cos(θi)=K2

K1/K2 yields θi=atan(K1, K2)   Xj>0
                      Θi= θi+pi              Xj<0

4) X sin(θij) + Y cos(θij) + Z cos(θi) = K1
   -X cos(θij) + Y sin(θij) + Z sin(θi) = K2

 

This solution is implemented in a MATLAB program IK.m.
Programming

With the analytical values found above, a program was created to output the calculated θi values.  The program IK.m checks for invalid inputs such as a co-ordinate outside of the workspace and also verified the integrity the values.  The forward kinematic model and the QTRPY model were implemented in FK.m and QTRPY.m respectively.




Forward kinematic model code:

FK.m

l1=95;
l2=95;
l3=150;
l4=46;
l5=0;

d1=104;
d2=41;
d3=21;
d4=-30;
d5=53;

t5=-2.4279;
t3=-1.1225;
t1=1.0861;
t2=-1.4195;
t4=3.5413;

exp1=l1*cos(t1)+l2*cos(t1 + t2)+(d3 + d4 + l5*sin(t5))*sin(t1+t2)+cos(t1+t2)*(l3*cos(t3)+l4*cos(t3 + t4)+cos(t3+t4)*l5*cos(t5)+d5*sin(t3 + t4));
exp2=-(d3+d4)*cos(t1+t2)-l5*sin(t5)*cos(t1+t2)+l1*sin(t1)+l2*sin(t1+t2)+sin(t1+t2)*(l3*cos(t3)+l4*cos(t3+t4)+l5*cos(t5)*cos(t3+t4)+d5*sin(t3+t4));
exp3=d1+d2-d5*cos(t3+t4)+l3*sin(t3)+l4*sin(t3+t4)+l5*cos(t5)*sin(t3+t4);
FK=[cos(t1+t2)*cos(t3+t4)*cos(t5)+sin(t1+t2)*sin(t5), cos(t5)*sin(t1+t2)-cos(t1+t2)*cos(t3+t4)*sin(t5), cos(t1+t2)*sin(t3+t4) exp1; cos(t3+t4)*cos(t5)*sin(t1+t2)-cos(t1+t2)*sin(t5), -cos(t1+t2)*cos(t5)-cos(t3+t4)*sin(t1+t2)*sin(t5), sin(t1+t2)*sin(t3+t4), exp2; cos(t5)*sin(t3+t4), -sin(t3+t4)*sin(t5), -cos(t3+t4), exp3; 0 0 0 1]