Section 3: Iterative inverse kinematics using Jacobian transpose

DH parameters

DH = {{0, 0, 0, θ1, 0}, {0, L1, 0, θ2, 0}, {0, L2, 0, 0, 0}} ; DH //TraditionalForm

( 0 0 0 θ1 0 ) 0 L1 0 θ2 0 0 L2 0 0 0

Numeric values for simulation

nDH = DH /. {L1 -> 1, L2 -> 1/2} // N ; nDH // TraditionalForm

( 0.` 0.` 0.` θ1 0.` ) 0.` 1.` 0.` θ2 0.` 0.` 0.5` 0.` 0.` 0.`

Inverse kinematic solution, example #1

pick = {1, 1} ; xd = {0, .7} ; RowBox[{RowBox[{initAngles, , =, , RowBox[{RowBox[{{, RowBox[{0, ,, 0.01}], }}], , //, , N}]}], ;}]

RowBox[{RowBox[{RowBox[{traj, , =, , RowBox[{IKTJ, [, RowBox[{nDH, ,, pick, ,, initAngles, ,, , xd, ,, , 0.1}], ]}]}], ;}], }]

RowBox[{RowBox[{AnimateTrajectory, [, RowBox[{nDH, ,, , traj, ,, , 2, ,, , 20, ,, , 20.}], ]}], ;}]

[Graphics:../anim5.gif]

Plot[#[t], {t, 0, #[[1, 1, 2]]}, FrameTrue] & /@ traj ;

[Graphics:../HTMLFiles/index_177.gif]

[Graphics:../HTMLFiles/index_178.gif]

Inverse kinematic solution, example #2

pick = {1, 1} ; xd = {.7, 0} ; initAngles = (#[#[[1, 1, 2]]]) & /@ traj ;

initAngles

RowBox[{{, RowBox[{1.08606, ,, 2.42954}], }}]

RowBox[{RowBox[{RowBox[{traj, , =, , RowBox[{IKTJ, [, RowBox[{nDH, ,, pick, ,, initAngles, ,, , xd, ,, , 0.1}], ]}]}], ;}], }]

RowBox[{RowBox[{AnimateTrajectory, [, RowBox[{nDH, ,, , traj, ,, , 2, ,, , 20, ,, , 30.}], ]}], ;}]

[Graphics:../anim6.gif]

Plot[#[t], {t, 0, #[[1, 1, 2]]}, FrameTrue] & /@ traj ;

[Graphics:../HTMLFiles/index_207.gif]

[Graphics:../HTMLFiles/index_208.gif]

Created by Mathematica  (October 1, 2003)