Jacobian mapping examples

This notebook simulates inverse velocity mappings for simple two- and three- link planar manipulators, and assumes that the notebook "velocity_kinematics_defs.nb" has already been evaluated. In Section 1 we simulate the inverse velocity relationship  \(TraditionalForm\`θ\& .  = \(J\^\(-1\)\) X\& . \) for a non-singular circular end-effector path and  for a singular, straight-line end-effector path for a two-link planar manipulator. In Section 2 we simulate some inverse velocity relationships for a three-link planar manipulator; specifically,  \(TraditionalForm\`θ\& .  = \(\(\(J\^T\)(J\ J\^T)\)\^\(-1\)\) 
          X\& . \), where J relates ((Subscript[v, x]),  (Subscript[v, y]))to ((Subscript[θ, 1]),  (Subscript[θ, 2]), (Subscript[θ, 3])), and \(TraditionalForm\`θ\& .  = \(J\^\(-1\)\) X\& . \), where  J relates ((Subscript[v, x]),  (Subscript[v, y]),  (Subscript[ω, z]))to ((Subscript[θ, 1]),  (Subscript[θ, 2]), (Subscript[θ, 3])). In both cases the specified straight-line trajectories result in singularities. Finally, in Section 3 we illustrate the use of Δθ = 
        (J^T) . ΔXfor iteratively computing the inverse kinematics of a robot manipulator

Section 1: Two-link planar manipulator

DH parameters

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

Numeric values for simulation

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

Nonsingular inverse Jacobian trajectory

This section simulates the relationship \(TraditionalForm\`\(θ\& . \) = \(J\^\(-1\)\) \(X\& . \)\)for a non-singular, circular, end-effector trajectory.

\(\(\(\(pick\  = \ {1, 1};\)\  (*\ 
      select\ first\ two\ rows\ of\ Jacobian\ *) 

    \(path1\  = \ {\ 0.2  Cos[t], \ 0.2  Sin[t]};\)\  (*\ 
      end - effector\ velocity\ as\ a\ function\ of\ time\ *) 

    \(init1\  = \ {Pi/4, \ \(-Pi\)/2}\  // \ N;\)\  (*\ 
      initial\ joint\ angles\ *) 

    \(maxTime\  = \ 2  Pi\  // \ N;\)\)\( (*\ 
      simulation\ length\ *) \)\)\)

traj  =  SIJVT[ nDH,  pick, path1, init1, maxTime];

AnimateTrajectory[nDH,  traj, 2];

Singular inverse Jacobian trajectory

This section simulates the relationship \(TraditionalForm\`\(θ\& . \) = \(J\^\(-1\)\) \(X\& . \)\)for a straight-line end-effector trajectory that results in a singularity.

pick  =  {1, 1};
path2  =  { 1,  0};
init2  =  {Pi/6,  -Pi/2}  //  N;
maxTime  =  10;

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

AnimateTrajectory[nDH,  traj, 2];

Joint velocities \(TraditionalForm\`\(\(θ\& . \)\_\(\(1\)\(\ \)\)\) and\)\(TraditionalForm\`\(θ\& . \)\_\(\(2\)\(\ \)\)\)

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

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

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

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

Numeric values for simulation

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

nDH  =  DH  /.  nVals; 
nDH  //  TraditionalForm

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 \(TraditionalForm\`θ\& .  = \(\(\(J\^T\)(J\ J\^T)\)\^\(-1\)\) 
          X\& . \)for a straight-line end-effector trajectory that results in a singularity.

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

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

AnimateTrajectory[nDH,  traj, 2];

Joint velocities \(TraditionalForm\`\(θ\& . \)\_\(\(1\)\(\ \)\), \ \(\(θ\& . \)\_\(\(2\)\(\ \)\)\) and\)\(TraditionalForm\`\(θ\& . \)\_\(\(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]}];

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

This section simulates the relationship \(TraditionalForm\`\(θ\& . \) = \(J\^\(-1\)\) \(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};
init2  =  {0,  3. ,  1.5};
maxTime  =  10;

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

AnimateTrajectory[nDH,  traj, 2];

Joint velocities \(TraditionalForm\`\(θ\& . \)\_\(\(1\)\(\ \)\), \ \(\(θ\& . \)\_\(\(2\)\(\ \)\)\) and\)\(TraditionalForm\`\(θ\& . \)\_\(\(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]}];

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

Numeric values for simulation

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

Inverse kinematic solution, example #1

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

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

AnimateTrajectory[nDH,  traj,  2,  20,  20. ];

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

Inverse kinematic solution, example #2

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

initAngles

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

AnimateTrajectory[nDH,  traj,  2,  20,  30. ];

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