Homework #2 Solutions

Definitions

Turn off annoying warnings

Off[General::spell];
Off[General::spell1];

Load in necessary packages

<< LinearAlgebra`MatrixManipulation`;
<< Graphics`Shapes`;

Elementary rotation and transform definitions

Definition of elementary rotation matrices

rX[
          a_] := {{1, 0, 0}, {0,  Cos[a],  -Sin[a]}, {0, Sin[a],  
            Cos[a]}};
rY[
          a_] := {{Cos[a], 0, Sin[a]}, {0, 1, 0},  {-Sin[a], 0, 
            Cos[a]}};
rZ[
          a_] := {{Cos[a],  -Sin[a], 0}, {Sin[a], Cos[a], 0}, {0, 0, 
            1}};

Convert a 3x3 rotation matrix and a 3x1 position vector to a 4x4 homogeneous operator

\(\(RAndPToT[r_, \ p_] := 
	
        Join[Transpose[
            Join[Transpose[r], \ {p}]], {{0, 0, 0, 1}}];\)\)

Convert a 3x3 rotation matrix to 4x4 homogeneous rotational operator

RToT[r_] := RAndPToT[r,  {0, 0, 0}];

Elementary homogeneous rotational operators

RX[a_] := RToT[rX[a]];
RY[a_] := RToT[rY[a]];
RZ[a_] := RToT[rZ[a]];

Angle-axis rotation

\(\(RK[θ_, \ k_] := 
	
        Module[{kx, \ ky, kz, \ v, c, s}, 
		{kx, \ ky, \ kz}\  = \ 
            k/Sqrt[k . k]; 
		{{kx^2\ v\  + \ c, kx\ ky\ v\  - \ kz\ s, \ 
                kx\ kz\ v\  + \ ky\ s}, 
			{kx\ ky\ v\  + \ kz\ s, \ 
                ky^2\ v + \ c, \ 
                ky\ kz\ v\  - \ kx\ s}, 
			{kx\ kz\ v\  - \ ky\ s, \ 
                ky\ kz\ v\  + \ kx\ s, \ 
                kz^2\ v\  + \ c}}\  /. \ {v -> \((1 - Cos[θ])\), \ 
              c -> Cos[θ], \ s -> Sin[θ]}];\)\)

Convert a 3x1 position vector to a 4x4 homogeneous translational operator

PToT[p_] := RAndPToT[IdentityMatrix[3],  p];

Elementary homogeneous translational operators

DX[x_] := PToT[{x, 0, 0}];
DY[y_] := PToT[{0, y, 0}];
DZ[z_] := PToT[{0, 0, z}];

Extract 3x3 rotation matrix from 4x4 homogeneous transform

RFromT[t_] := TakeMatrix[t, {1, 1}, {3, 3}];

Extract 3x1 position vector from 4x4 homogeneous transform

PFromT[t_] := 
        TakeMatrix[t, {1, 4}, {3, 4}]  //  Flatten;

Compute inverse of a 4x4 homogeneous transform

\(\(InverseT[t_] := 
	
        Module[{r, \ p}, 
		r\  = \ RFromT[t]; 
		p\  = \ 
            PFromT[t]; 
		RAndPToT[
            Transpose[r], \ \(-Transpose[r] . p\)]
	];\)\)

Static forward kinematic definitions

In this notebook, DH parameters should be speficied as table, with one entry per link in the form {(Subscript[α, i - 1]), (Subscript[a, i - 1]), (Subscript[d, i]), (Subscript[θ, i]), type}, where "type" is 0 for revolute joints and 1 for prismatic joints.

Degree to radian conversion (for DH parameter list)

\(\(ConvertFromDegreesOne[{alpha_, \ a_, \ d_, \ theta_, \ 
            type___}] := 
	{If[NumericQ[alpha], \ alpha\ Degree, \ 
              alpha], \ a, \ d, \ 
            If[NumericQ[theta], \ theta\ Degree, \ theta], \ type}\  // \ 
          FullSimplify;\)\)

ConvertFromDegrees[l_] := 
        ConvertFromDegreesOne  /@  l;

Basic one link homogeneous transform (based on DH parameters)

\(\(TLink[{α_, \ a_, \ \ d_, θ_, type___}] := 
	
        RX[α] . DX[a] . RZ[θ] . DZ[d];\)\)

Compound transform for multiple links (for DH parameter list)

TAll[l_] :=  Dot  @@ (TLink  /@  l);

Homogeneous transform for robot  from frame {j} to frame {i}

\(\(T[dh_, \ i_, \ 
          j_] := 
		Which[
			i\  == \ j, 
				IdentityMatrix[
            4], 
			i\  < \ j, 
				TAll[
            Take[dh, \ {i + 1, \ j}]], 
			i\  > \ j, 
				InverseT[
            TAll[Take[dh, {j + 1, i}]]]];\)\)

Rotation matrix for robot from frame {j} to frame {i}

R[dh_, i_, j_] := RFromT[T[dh,  i,  j]];

Position vector for robot from frame {j} to frame {i}

P[dh_, i_, j_] := PFromT[T[dh,  i,  j]];

Problem 1:

Part (a)

dh  =  {{0, 0, 0,  θ1,  0},  {π/2, L1, 0, θ2, 
            0},  {0, L2, 0, θ3, 0}};
dh  //  TraditionalForm

T01  =  T[dh, 0, 1];  T01  //  TraditionalForm

T12  =  T[dh, 1, 2];  T12  //  TraditionalForm

T23  =  T[dh, 2, 3];  T23  //  TraditionalForm

T03  =  
        T01 . (T12 . T23  //  FullSimplify)  //  
          FullSimplify;
T03  //  TraditionalForm

Part (b)

See solutions text.

Problem 2:

Part (a)

dh  =  {{0, 0, 0,  θ1,  0},  {π/2, 0, 0, θ2, 
            0},  {0, L3, 0, θ3, 0}};
dh  //  TraditionalForm

T01  =  T[dh, 0, 1];  T01  //  TraditionalForm

T12  =  T[dh, 1, 2];  T12  //  TraditionalForm

T23  =  T[dh, 2, 3];  T23  //  TraditionalForm

Part (b)

T03  =  T01 . T12 . T23  //  FullSimplify;
T03  //  TraditionalForm

For remainder of the inverse kinematics, see solutions text.

Problem 3:

See solutions text.

Problem 4:

See solutions text.

Problem 5:

Here, we generate the workspace of the two-link manipulator numerically, by sweeping the manipulator through all possible angles and plotting the corresponding location of the end-effector.

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

dh  =  dh  /.  {L1  -> 2,  L2 -> 1}

P2  =  Take[P[dh, 0, 3]  //  FullSimplify,  {1, 2}];
{#} &  /@  P2 //  TraditionalForm

tbl  =  
        Partition[
          Flatten[Table[
              P2  //  N,  {θ1, 
                0, π,  π/50},  {θ2,  -π/
                  2,  π,  π/50}]], 2];

ListPlot[tbl,  PlotJoined -> False,  
        PlotStyle -> {RGBColor[1, 0, 0]},  Frame -> True];

Problem 6:

We will solve this problem by trying to visualize the axes of rotations that can be achieved for certain values of ϕ. The complete set of rotation axes can be viewed as points on a unit sphere, and for a given value of ϕ, we can generate rotation matrices for random values of θ1, θ2 and θ3. These rotation matrices can then be converted to an equivalent angle-axis representation and plotted in 3d. Below, the function visualize[...] does just that, giving a 3d view of a random set of rotation axes for a given value of ϕ, and three separate planar views in the (kx, ky), (kx, kz) and the (ky, kz) planes, in red, green and blue, respectively.

Definitions

rr  :=  Random[Real, {-π, π}];

\(\(kv[rm_, θ_, \ ϕv_] := 
	
        Module[{vars, rn, \ 
            kdir}, 
		vars\  = {θ1, \ θ2, \ θ3}; 
		rn\  = \ N[rm]\  /. \ 
              Join[\(\((#[\([1]\)] -> #[\([2]\)])\) &\)\  /@ \ 
                  Transpose[{vars, θ}], \ {ϕ -> ϕv}]; 
		kdir = {rn[\([3, 2]\)]\  - \ rn[\([2, 3]\)], rn[\([1, 3]\)] - rn[\([3, 1]\)], 
              rn[\([2, 1]\)] - rn[\([1, 2]\)]}; 
		kdir/
            Sqrt[kdir . kdir]];\)\)

\(\(visualize[rm_, \ ϕv_, \ n_:  1000] := 
	
        Module[{tst, g1, g2, g3, g4}, 
		tst\  = \ 
            Table[kv[rm, {rr, rr, rr}, ϕv], {n}]; 
		g1\  = \ 
            Show[Graphics3D[\({PointSize[ .02], Point[#]} &\)\  /@ \ tst], \ 
              PlotRange -> {{\(-1.1\), 1.1}, {\(-1.1\), 1.1}, {\(-1.1\), 
                    1.1}}, \ AspectRatio -> 1]; 
		g2\  = \ 
            Show[Graphics[\({RGBColor[1, 0, 0], PointSize[ .02], 
                      Point[{#[\([1]\)], #[\([2]\)]}]} &\)\  /@ \ tst], \ 
              PlotRange -> {{\(-1.1\), 1.1}, {\(-1.1\), 1.1}}, \ 
              AspectRatio -> 1]; 
		g3 = 
            Show[Graphics[\({RGBColor[0, 1, 0], PointSize[ .02], 
                      Point[{#[\([1]\)], #[\([3]\)]}]} &\)\  /@ \ tst], \ 
              PlotRange -> {{\(-1.1\), 1.1}, {\(-1.1\), 1.1}}, \ 
              AspectRatio -> 1]; 
		g4 = 
            Show[Graphics[\({RGBColor[0, 0, 1], PointSize[ .02], 
                      Point[{#[\([1]\)], #[\([3]\)]}]} &\)\  /@ \ tst], \ 
              PlotRange -> {{\(-1.1\), 1.1}, {\(-1.1\), 1.1}}, \ 
              AspectRatio -> 1]; \ {g1, g2, g3, g4}];\)\)

Rotation matrix

dh  =  {{0, 0, 0, θ1, 0}, {ϕ, 0, 0, θ2, 
            0}, {π - ϕ, 0, 0, θ3, 0}};
dh  //  TraditionalForm

rm  =  R[dh, 0, 3]

Visualization of rotation axes for ϕ = π/2

visualize[rm,  Pi/2,  1000];

Visualization of rotation axes for ϕ --> 0

visualize[rm,  0.001,  200];

Visualization of rotation axes for ϕ = π/4

visualize[rm,  Pi/4,  1000];

Problem 7:

See solutions text.

Problem 8:

See solutions text.