Kinematic redundancy examples

All the examples below illustrate kinematic redundancy for a three-link planar manipulator with consecutive link lengths of 3, 4 and 4. Note that the three-link manipulator is kinematically redundant when considering end-effector position (x, y) only (without orientation).

Definitions

Off[General::spell1];
Off[Solve::ifun];

norm[v_] := Sqrt[v . v];

\(\(redundantTrajectory[\ {l1_, l2_, l3_}, \ {xc_, \ yc_}, \ n_] := 
	
        Module[{x1, y1, eq1, eq2, x, y, \ sol, traj1}, 
		sol\  = 
            Table[{{x1, y1}\  = \ 
                  l1 {\ Cos[t], \ Sin[t]}\  // \ 
                    N, 
		eq1\  = \ \ \((x\  - \ x1)\)^2\  + \ \((y\  - \ 
                            y1)\)^2\  == \ 
                    l2^2; 
		eq2\  = \ \((x\  - \ xc)\)^2\  + \ \((y - \ 
                            yc)\)^2\  == \ l3^2; 
		Solve[{eq1, 
                    eq2}, \ {x, y}]}, \ {t, 0, 2  Pi - 2  Pi/n, \ 
                2  Pi/n}]; 
		traj1\  = \ \(If[#[\([2]\)] == {}, \ 
                  Null, 
				{{0, 
                      0}, #[\([1]\)], \ {x, y}\  /. \ #[\([2, 1]\)], {x, 
                        y}\  /. \ #[\([2, 2]\)], {xc, yc}}] &\)\  /@ \ 
              sol; 
		traj1\  = \ 
            DeleteCases[traj1, \ 
              Null]; 
		traj1\  = \ \(If[
                  Im[#[\([3]\)]]\  == \ {0, 0}, \ #, \ Null] &\)\  /@ \ 
              traj1; 
		traj1\  = \ 
            DeleteCases[traj1, Null]; 
		traj1
	];\)\)

\(\(createDiscontinuityTable[traj_] := 
	
        Module[{comp, s}, 
				comp\  = \ 
            Partition[traj, \ 2, \ 
              1]; 
				s = {\(norm[\((#[\([1, 3]\)]\  - \ #[\([2, 
                              3]\)])\)] &\)\  /@ \ comp, 
                Table[i, {i, 1, Length[comp]}]}\  // \ 
              Transpose; 
				s = \ 
            Sort[s]\  // \ Reverse];\)\)

\(\(noDiscontinuities[traj_, \ thresh_:  2. ] := 
	
        Module[{comp, s, \ sw, \ done, rp, \ curr, breaks}, 
		s = \ 
            createDiscontinuityTable[traj]; 
		sw\  = \ 
            Table[False, \ {Length[traj]}]; 
		done\  = \ 
            False; 
		breaks\  = \ {}; 
		While[\(! done\), 
			If[
              s[\([1, 1]\)]\  > \ thresh, 
				breaks\  = \ 
                Flatten[{breaks, s[\([1, 2]\)]}]; 
				s\  = \ 
                Drop[s, 1], 
				done\  = \ True]]; \ 
		breaks\  = \ 
            breaks\  // \ Sort; 
		sw\  = \ 
            Table[False, \ {Length[traj]}]; 
		curr\  = \ 
            True; 
		Do[
			rp\  = \ 
              Table[{i}, \ {i, \ breaks[\([j]\)] + 1, \ 
                  Length[traj]}]; 
			sw\  = \ 
              ReplacePart[sw, \ curr, \ 
                rp]; 
			curr\  = \ \(! curr\), \ {j, \ 1, \ 
              Length[breaks]}]; 
		Table[
            If[sw[\([i]\)]\  == \ True, \ {traj[\([i, 1]\)], 
                traj[\([i, 2]\)], traj[\([i, 4]\)], \ traj[\([i, 3]\)], 
                traj[\([i, 5]\)]}, 
						traj[\([i]\)]], {i, \ 1, \ 
              Length[traj]}]
];\)\)

\(\(noDiscontinuitiesTrajectory[l_, x_, n_:  99, \ thresh_:  2. ] := 
	
        noDiscontinuities[redundantTrajectory[l, x, n], thresh];\)\)

\(\(animateTrajectory[traj_, \ whi_:  0] := 
	
        Module[{g, p, plrng, 
            g2}, 
		g = \(Show[
                  Graphics[{Thickness[ .02], {RGBColor[1 - whi, 0, whi], 
                        Line\ [{#[\([1]\)], #[\([2]\)], #[\([whi + 
                                  3]\)], #[\([5]\)]}]}, {RGBColor[1, 0, 1], \ 
                        PointSize[ .06], \ 
                        Point[#[\([1]\)]]}, {RGBColor[0, 0, 0], \ 
                        PointSize[ .03], \ \ Point[#[\([2]\)]], \ 
                        Point[#[\([whi + 3]\)]], \ Point[#[\([5]\)]]}}], \ 
                  PlotRange -> All, DisplayFunction -> Identity, \ 
                  Frame -> True] &\)\  /@ \ 
              traj; 
		p\  = \({Min[#], \ Max[#]} &\)\  /@ \ 
              Transpose[
                Partition[
                  Flatten[\((\(FullOptions[#, \ PlotRange] &\)\  /@ \ g)\)], 
                  4]]; 
		plrng\  = \ {{p[\([1, 1]\)], 
                p[\([2, 2]\)]}, {p[\([3, 1]\)], \ 
                p[\([4, 2]\)]}}; 
		g2\  = \(Show[#, \ 
                  AspectRatio -> Automatic, \ PlotRange -> plrng, \ 
                  DisplayFunction -> $DisplayFunction, \ 
                  Axes -> {True, False}] &\)\  /@ \ g; \ g2];\)\)

\(\(\(centrode[traj_] := 
	
        Module[{c1, c2, c3, g1, 
            g2}, 
		c1\  = \ \(#[\([2]\)] &\)\  /@ \ 
              traj; 
		c2\  = \(#[\([3]\)] &\)\  /@ \ 
              traj; 
		c3\  = \ \(#[\([4]\)] &\)\  /@ \ 
              traj; \ 
		g1 = {RGBColor[0, 1, 0], \ 
              Line[c1]}; 
		g2\  = \ {RGBColor[1, 0, 0], \ 
              Line[c2]}; 
		g3\  = \ {RGBColor[0, 0, 1], \ 
              Line[c3]}; 
		Show[Graphics[{Thickness[ .01], g1, g2, g3}], 
            AspectRatio -> Automatic, 
            PlotRange -> All]];\)\(\ \)\)\)

\(\(drawOneConfig[traj_, n_, whi_:  0] := 
		Show[
          Graphics[{Thickness[ .02], {RGBColor[1 - whi, 0, whi], 
                Line\ [{traj[\([n, 1]\)], traj[\([n, 2]\)], 
                    traj[\([n, whi + 3]\)], 
                    traj[\([n, 5]\)]}]}, 
				{RGBColor[1, 0, 1], \ 
                PointSize[ .06], \ 
                Point[traj[\([n, 1]\)]]}, {RGBColor[0, 0, 0], \ 
                PointSize[ .03], \ Point[traj[\([n, 2]\)]], \ 
                Point[traj[\([n, whi + 3]\)]], \ 
                Point[traj[\([n, 5]\)]]}}], \ PlotRange -> All, 
          AspectRatio -> Automatic, 
          DisplayFunction -> Identity];\)\)

\(\(superimpose[traj_, \ n1_, n2_] := 
	
        Show[centrode[traj], drawOneConfig[traj, n1, 0], \ 
          drawOneConfig[traj, n2, 1], \ AspectRatio -> Automatic, 
          PlotRange -> All, DisplayFunction -> $DisplayFunction, 
          Axes -> {True, False}];\)\)

Example #1: Non-dextrous workspace: (x, y) = (3, 2)

First set of positions

Timing[tr = 
          noDiscontinuitiesTrajectory[ {3, 4, 4}, {3, 2}, 51, 
            4. ];]

animateTrajectory[tr, 0];

Second set of positions

animateTrajectory[tr, 1];

Aggregate set of positions

superimpose[tr, 25, 35];

Example #2: Dextrous workspace: (x, y) = (1.5, 1.5)

First set of positions

Timing[tr = 
          noDiscontinuitiesTrajectory[ {3, 4, 4}, {1.5, 1.5}, 51, 
            4. ];]

animateTrajectory[tr, 0];

Second set of positions

animateTrajectory[tr, 1];

Aggregate set of positions

superimpose[tr, 25, 35];

Example #3: End-effector position with infinite # of possible rotations

Timing[tr = 
          noDiscontinuitiesTrajectory[ {3, 4, 4}, {3/Sqrt[2], 3/Sqrt[2]}, 
            51, 4. ];]

Show[drawOneConfig[tr,  7, 1],  
        DisplayFunction -> $DisplayFunction,  
        Frame -> True];