| | | | | |

Inverse Kinematics Mathematica® Source Code (GPL 3)

Source Code

(* R || R || R Inverse Kinematics *)

(* Copyright (C)2012 Sasan Ardalan *)
(* This program is free software:you can redistribute it and/or modify *)
(* it under the terms of the GNU General Public License as published by *)
(* the Free Software Foundation,either version 3 of the License,or *)
(* (at your option) any later version.This program is distributed *)

(* in the hope that it
will be useful,but WITHOUT ANY WARRANTY * )

(* without even the implied warranty of *)
(* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the *)
(* GNU General Public License for more details.You should have *)
(* received a copy of the GNU General Public License *)
(* along with this program.If not,see<http://www.gnu.org/licenses/>. *)

(* Author: Sasan Ardalan *)
(* Date: February 17, 2012 *)
(* http://www.ionotronics.com *)

(* For Inverse Kinematics Theory see "Theory of Applied Robotics", Reza N. Jazar, Springer 2010 *)

(* Forward Kinematic Matrix 0T3 *)

FWD0T3[theta1_, theta2_, theta3_, L1_, L2_, L3_] := Module[{FWD},

FWD = {{Cos[
theta1 + theta2 + theta3], -Sin[theta1 + theta2 + theta3], 0,
L1*Cos[theta1] + L2*Cos[theta1 + theta2] +
L3*Cos[theta1 + theta2 + theta3]},
{Sin[theta1 + theta2 + theta3], Cos[theta1 + theta2 + theta3], 0,
L1*Sin[theta1] + L2*Sin[theta1 + theta2] +
L3*Sin[theta1 + theta2 + theta3]},
{0, 0, 1, 0},
{0, 0, 0, 1}};
FWD
]
(* Jacobian *)

CalcJacobian[theta1_, theta2_, theta3_, L1_, L2_, L3_] :=
Module[{Jcbn},
Jcbn = {{-L1*Sin[theta1] - L2*Sin[theta1 + theta2] -
L3*Sin[theta1 + theta2 + theta3],
-L2*Sin[theta1 + theta2] - L3*Sin[theta1 + theta2 + theta3],
-L3*Sin[theta1 + theta2 + theta3]},
{L1*Cos[theta1] + L2*Cos[theta1 + theta2] +
L3*Cos[theta1 + theta2 + theta3],
L2*Cos[theta1 + theta2] + L3*Cos[theta1 + theta2 + theta3],
L3*Cos[theta1 + theta2 + theta3]}};
Jcbn
]

InvKinematicsIteration[theta1_, theta2_, theta3_, L1_, L2_, L3_,
Tp_] :=
Module[{qn},
t0T3 = FWD0T3[theta1, theta2, theta3, L1, L2, L3];

(* setup Jacobi matrix *)

J = CalcJacobian [theta1, theta2, theta3, L1, L2, L3];

JInv = PseudoInverse[J];

V = {0, 0, 0, 1};

T = V.Transpose[t0T3];

deltaT = Tp - T;
deltaT2 = {deltaT[[1]], deltaT[[2]]};

qn = {theta1, theta2, theta3};
qnp1 = qn + N[deltaT2.Transpose[JInv]];


qn = qnp1;
qn
]
(* Desired X Y Position *)
Tp = {17, -2.5, 0, 1}

(* Links *)
L1 = 9.2
L2 = 9.2
L3 = 3
(* Initial Joint Parameters *)
qn = {1*Pi/180, 20*Pi/180, 20*Pi/180}

 

track = {}
grObjects = {}

For[i = 0, i < 30, i++,
Tp = {17 - i*.5, -2.5 + 0.5*i, 0, 1};
Print[Tp];



qn = InvKinematicsIteration[qn[[1]], qn[[2]], qn[[3]], L1, L2, L3,
Tp];
qn = InvKinematicsIteration[qn[[1]], qn[[2]], qn[[3]], L1, L2, L3,
Tp];
qn = InvKinematicsIteration[qn[[1]], qn[[2]], qn[[3]], L1, L2, L3,
Tp];
qn = InvKinematicsIteration[qn[[1]], qn[[2]], qn[[3]], L1, L2, L3,
Tp];
qn = InvKinematicsIteration[qn[[1]], qn[[2]], qn[[3]], L1, L2, L3,
Tp];
qn = InvKinematicsIteration[qn[[1]], qn[[2]], qn[[3]], L1, L2, L3,
Tp];



qdeg = 180.0/Pi*qn;

Print[qdeg];


(* Draw Configuration Store joint parameters and graphic objects \
into lists *)

(* R || R || R Manipulator *)
(* See Pages 260-261 Forward \
Kinematics Chaper in "Theory of Applied Robotics", Reza N. Jazar, \
Springer 2010 *)


theta1 = qn[[1]];
theta2 = qn[[2]];
theta3 = qn[[3]];


t0T1 = {{Cos [theta1], -Sin[ theta1], 0, L1*Cos[theta1]},
{Sin [theta1], Cos[theta1], 0, L1* Sin[theta1]},
{0 , 0 ,
1, 0 },
{0 , 0 ,
0, 1 }};


t1T2 = {{Cos [theta2], -Sin[ theta2], 0, L2*Cos[theta2]},
{Sin [theta2], Cos[theta2], 0, L2* Sin[theta2]},
{0 , 0 ,
1, 0 },
{0 , 0 ,
0, 1 }};

t2T3 = {{Cos [theta3], -Sin[ theta3], 0, L3*Cos[theta3]},
{Sin [theta3], Cos[theta3], 0, L3* Sin[theta3]},
{0 , 0 ,
1, 0 },
{0 , 0 ,
0, 1 }};

t0T2 = t0T1.t1T2;
t0T3 = t0T2.t2T3;

V1 = {0, 0, 0, 1};
V0 = V1.Transpose [t0T1];
X1 = V0[[1]];
Y1 = V0[[2]];
V2 = {0, 0, 0, 1};
V0 = V2.Transpose[t0T2];
X2 = V0[[1]];
Y2 = V0[[2]];

V3 = {0, 0, 0, 1};
V0 = V3.Transpose[t0T3];
X3 = V0[[1]];
Y3 = V0[[2]];

(* Coordinates to Draw Gripper Base *)
(* See \
http://www.okc.cc.ok.us/maustin/EquationsofLine/Equations%20 of %20 a \
%20 Line.htm for equations of line and perpendicular to line etc *)

slope3 = (Y3 - Y2)/(X3 - X2);
slopeGripper = -1/slope3;
x1Gripper = X3 - 0.01*X3;
y1Gripper = slopeGripper*(x1Gripper - X3) + Y3;

x2Gripper = X3 + 0.01*X3;
y2Gripper = slopeGripper*(x2Gripper - X3) + Y3;

Print["Results"];
Print [X3, Y3];

track = Append[track, qdeg];

Print [track];

grObjects = Append[grObjects, Circle[{0.0, 0.0}, 1.0]];

grObjects =
Append[grObjects, {Thickness[0.005],
Line[{{0.0, 0.0}, {X1, Y1}}]}];
grObjects = Append[grObjects, Circle[{X1, Y1}, 1.0]];
grObjects =
Append[grObjects, {Thickness[0.005], Line[{{X1, Y1}, {X2, Y2}}]}];
grObjects = Append[grObjects, Circle[{X2, Y2}, 1.0]];
grObjects =
Append[grObjects, {Thickness[0.005], Line[{{X2, Y2}, {X3, Y3}}]}];
grObjects =
Append[grObjects, {Thickness[0.005],
Line[{{x1Gripper, y1Gripper}, {x2Gripper, y2Gripper}}]}];


Graphics[{grObjects}

 

 

 

 
 
 
 
Shopping cart icon credit
Contact | © 2006-2018 IONOTRONICS Corporation All Rights Reserved