Section 2: Three-link planar manipulator

DH parameters

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

( 0 0 0 θ1 0 ) 0 L1 ... 0 L2 0 θ3 0 0 L3 0 0 0

Numeric values for simulation

nVals =    {L1 -> 1, L2 -> 3/4, L3->1/2} // N ;

nDH = DH /. nVals ; nDH // TraditionalForm

( 0 0 0 θ1 0 ) 0 1.` ... 0 0.75` 0 θ3 0 0 0.5` 0 0 0

Construct pseudo-inverse Jacobian trajectory [J relates (vx, vy) and (θ1, θ2 and θ3)]

j1 = Take[Jacobian1[DH, 0, 4] // Simplify, 2] ; j1 = j1 /. nVals ; j2 = j1 . Transpose[j1] // Simplify ; jp = Transpose[j1] . Inverse[j2] // Chop ;

Simulate pseudo-inverse Jacobian trajectory

This section simulates the relationship Overscript[θ, .] = J^T(J J^T)^(-1) Overscript[X, .]for a straight-line end-effector trajectory that results in a singularity.

path1 = { 1, .2} ; init1 = {Pi/4, -Pi/2, -Pi/2} // N ; RowBox[{RowBox[{maxTime, , =, , 10.}], ;}]

traj = SimulateVelocityTrajectory[ jp, nDH, path1, init1, maxTime] ;

NDSolve :: ndsz : At t == 1.3640657950760342`, step size is effectively zero; singularity or stiff system suspected. More…

AnimateTrajectory[nDH, traj, 2] ;

[Graphics:../anim3.gif]

Joint velocities Overscript[θ, .] _1 , Overscript[θ, .] _2 andOverscript[θ, .] _3

endTime = traj[[1, 1, 1, 2]] ;

{vθ1, vθ2, vθ3} = {D[traj[[1]][t], t], D[traj[[2]][t], t], D[traj[[3]][t], t]} ;

Plot[{vθ1, vθ2, vθ3}, {t, 0, endTime}, FrameTrue, PlotStyle {RGBColor[1, 0, 0], RGBColor[0, 0, 1], RGBColor[1, 0, 1]}] ;

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

Full-rank three-DOF manipulator example [J relates (vx, vy, ωz) and (θ1, θ2 and θ3)]

This section simulates the relationship Overscript[θ, .] = J^(-1) Overscript[X, .]for a straight-line end-effector trajectory with constant end-effector rotation that results in a singularity.

pick =    {1, 1, 0, 0, 0, 1} ; path2 = {1, 0, 1} ; RowBox[{RowBox[{init2, , =, , RowBox[{{, RowBox[{0, ,, , 3., ,, , 1.5}], }}]}], ;}] maxTime = 10 ;

traj = SIJVT[ nDH, pick, path2, init2, maxTime] ;

Square Jacobian: Using Inverse[ ]

NDSolve :: ndsz : At t == 2.012794113448484`, step size is effectively zero; singularity or stiff system suspected. More…

AnimateTrajectory[nDH, traj, 2] ;

[Graphics:../anim4.gif]

Joint velocities Overscript[θ, .] _1 , Overscript[θ, .] _2 andOverscript[θ, .] _3

endTime = traj[[1, 1, 1, 2]] ;

{vθ1, vθ2, vθ3} = {D[traj[[1]][t], t], D[traj[[2]][t], t], D[traj[[3]][t], t]} ;

Plot[{vθ1, vθ2, vθ3}, {t, 0, endTime}, FrameTrue, PlotStyle {RGBColor[1, 0, 0], RGBColor[0, 0, 1], RGBColor[1, 0, 1]}] ;

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

Created by Mathematica  (October 1, 2003)