Static forces example

This notebook illustrates the computation of static joint torques required to support static forces at the end-effector of the robot manipulator, and follows example 5.7 in Craig closely. This notebook assumes that the definitions in "velocity_kinematics_defs.nb" have already been evaluated.

Short hand display function

Nice[m_] := m  //  MatrixForm  //  
          TraditionalForm;
TF :=  TraditionalForm;

Static force definitions

\(\(fs[dh_, i_, i_, \ fend_: {fx, fy, 0}] := 
	
        If[i\  == \ Length[dh], 
          fend, 
		R[dh, i, i + 1] . 
            fs[dh, \ i + 1, \ i + 1, \ fend]];\)\)

\(\(ns[dh_, i_, i_, \ fend_: {fx, fy, 0}] := 
	
        If[i\  == \ Length[dh], \ {0, 0, 
            0}, 
		R[dh, i, i + 1] . 
              ns[dh, i + 1, i + 1, \ fend] + \((P[dh, i, i + 1]~
                Cross~\((R[dh, i, i + 1] . 
                    fs[dh, i + 1, i + 1, \ fend])\))\)];\)\)

τs[dh_, i_, 
        fend_: {fx, fy, 0}] := ns[dh, i, i,  fend][[3]]

Static force propagation

Two-link planar manipulator example (Craig, example 5.7)

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

Static force at end-effector

f3  =  
        fs[DH, 3, 3,  {fx,  fy,  0}];
f3  //  Nice

Static moment at end-effector

n3  =  
        ns[DH, 3, 3,  {fx,  fy,  0}];
n3  //  Nice

Static force at link 2

f2  =  
        fs[DH, 2, 2,  {fx,  fy,  0}];
f2  //  Nice

Static moment at link 2

n2  =  ns[DH, 2, 2];
n2  //  Nice

Static force at link 1

f1  =  fs[DH, 1, 1];
f1  //  Nice

Static moment at link 1

n1  =  ns[DH, 1, 1];
n1  //  Nice

Required torque at link 1

τ1  =  τs[DH,  1]; 
τ1  //  TraditionalForm

Required torque at link 2

τ2  =  τs[DH,  2]; 
τ2  //  TraditionalForm

Relationship to Jacobian

Jacobian

j3  = Take[Jacobian1[DH, 3, 3], {1, 2}];  
    j3  //  TraditionalForm

τ to F relationship in matrix form

mat  =  
        Outer[Coefficient[#1,  #2] &,  {τ1, τ2}, {fx,  
            fy}] ;
mat  //  TraditionalForm

Equal to (J^T)?

mat  ==  Transpose[j3]

Verify result for gravity

Force at end-effector due to gravity

fend  =  
        R[DH, 3, 0] . {0, g, 0}  //  Simplify;
fend  // Nice

Torque at joint 2

τs[DH, 2,  fend]  //  Simplify  //  
      TraditionalForm

Torque at joint 1

τs[DH, 1,  fend]  //  Simplify  //  
      TraditionalForm