Analytically derives the Denavit & Hartenberg transformation matrix for each of the three joints

Generates optimized C code for the angle of the third joint, in terms of the position of the end effector

Lets the user specify a parametric path for the tip of the robot to follow

Animates the robot following the parametric path

$\mathrm{restart}\:$$\mathrm{with}\left(\mathrm{plots}\right)\:$$\mathrm{with}\left(\mathrm{plottools}\right)\:\mathrm{with}\left(\mathrm{ColorTools}\right)\:$

$$\begin{gathered} \mathrm{A1}≔\left[\begin{array}{cccc}\cos \left(\mathrm{\theta }\right)& -\sin \left(\mathrm{\theta }\right)& 0& 0\\ \sin \left(\mathrm{\theta }\right)& \cos \left(\mathrm{\theta }\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\: \\ \mathrm{A2}≔\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& \mathrm{d}\\ 0& 0& 0& 1\end{array}\right]\: \\ \mathrm{B1}≔\mathrm{A2}\.\mathrm{A1}\: \\ \mathrm{A3}≔\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \left(\mathrm{\alpha }\right)& -\sin \left(\mathrm{\alpha }\right)& 0\\ 0& \sin \left(\mathrm{\alpha }\right)& \cos \left(\mathrm{\alpha }\right)& 0\\ 0& 0& 0& 1\end{array}\right]\: \\ \mathrm{A4}≔\left[\begin{array}{cccc}1& 0& 0& \mathrm{a}\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\: \\ \mathrm{B2}≔\mathrm{A3}\.\mathrm{A4}\: \\ \mathrm{H}≔\mathrm{B1}\.\mathrm{B2}\; \end{gathered}$$

$H≔\left[\begin{array}{cccc}\cos \left(\mathrm{\theta }\right)& -\sin \left(\mathrm{\theta }\right)\cos \left(\mathrm{\alpha }\right)& \sin \left(\mathrm{\theta }\right)\sin \left(\mathrm{\alpha }\right)& \cos \left(\mathrm{\theta }\right)a\\ \sin \left(\mathrm{\theta }\right)& \cos \left(\mathrm{\theta }\right)\cos \left(\mathrm{\alpha }\right)& -\cos \left(\mathrm{\theta }\right)\sin \left(\mathrm{\alpha }\right)& \sin \left(\mathrm{\theta }\right)a\\ 0& \sin \left(\mathrm{\alpha }\right)& \cos \left(\mathrm{\alpha }\right)& d\\ 0& 0& 0& 1\end{array}\right]$

$\mathrm{H1}≔\mathrm{eval}\left(\mathrm{H}\,\left[\mathrm{\θ}=\mathrm{\θ1}\,\mathrm{\α}\=-\mathrm{\π}\/2\,\mathrm{a}\=0\,\mathrm{d}\=\mathrm{lengthArm1}\right]\right)\:$$\mathrm{H2}≔\mathrm{eval}\left(\mathrm{H}\,\left[\mathrm{\θ}=\mathrm{\θ2}\,\mathrm{\alpha }=0\,\mathrm{d}=0\,\mathrm{a}=\mathrm{lengthArm2}\right]\right)\:$$\mathrm{H3}≔\mathrm{eval}\left(\mathrm{H}\,\left[\mathrm{\θ}=\mathrm{\θ3}\,\mathrm{\alpha }=0\,\mathrm{a}=\mathrm{lengthArm3}+\mathrm{lengthTip}\,\mathrm{d}=0\right]\right)\:$$\mathrm{H14}≔\mathrm{H1}·\mathrm{H2}·\mathrm{H3}\:$

$$\begin{gathered} \mathrm{path} ≔ \left[\mathrm{x}\=350-50\cdot \sin \left(\mathrm{t}\right)\+75\cdot \sin \left(3\cdot \mathrm{t}\right)\, \\ \mathrm{y}\=450-80\cdot \cos \left(\mathrm{t}\right)\+50\cdot \sin \left(5\cdot \mathrm{t}\right)\, \\ \mathrm{z}\=500-30\cdot \sin \left(2\cdot \mathrm{t}\right)\+60\cdot \cos \left(5\cdot \mathrm{t}\right)\right]\: \end{gathered}$$

$\mathrm{CodeGeneration}\left[\mathrm{C}\right]\left(\mathrm{\Θ3}\,\mathrm{optimize}\,\mathrm{deducetypes}=\mathrm{false}\right)$

t1 = lengthArm3 + lengthTip;
t2 = t1 * lengthArm2;
t3 = x * x;
t4 = y * y;
t6 = z * z;
t8 = 0.2e1 * z * lengthArm1;
t9 = lengthArm1 * lengthArm1;
t10 = lengthArm2 * lengthArm2;
t11 = lengthArm3 * lengthArm3;
t13 = 0.2e1 * lengthArm3 * lengthTip;
t14 = lengthTip * lengthTip;
t15 = t3 + t4 + t6 - t8 + t9 + t10 - t11 - t13 - t14;
t16 = t15 * t15;
t18 = sqrt(t16 * (t3 + t4));
t25 = t1 * t1;
t29 = sqrt(-0.1e1 / t25 / t10 * (0.2e1 * t1 * lengthArm2 - t10 - t11 - t13 - t14 + t3 + t4 + t6 - t8 + t9) * (-0.2e1 * t1 * lengthArm2 - t10 - t11 - t13 - t14 + t3 + t4 + t6 - t8 + t9));
t32 = z - lengthArm1;
t38 = 0.1e1 / (t3 + t4 + t6 - t8 + t9);
t39 = 0.1e1 / lengthArm2;
t47 = atan2(t39 * t38 / t15 * (-t29 * t18 * t2 - t16 * t32), t39 * t38 * (-t29 * t32 * t2 + t18));
t49 = t47 + 0.3141592654e1 / 0.2e1;

$\mathrm{N}≔200\:$

$\mathrm{radiusBase}≔150\:$$\mathrm{radiusArm1}≔70\:$$\mathrm{radiusJoint2}≔\mathrm{radiusArm1}\+5\:$$\mathrm{radiusArm2}≔60\:$$\mathrm{radiusArm3}≔50\:$

$$\begin{gathered} \mathrm{lengthArm1} ≔ 500\: \\ \mathrm{lengthArm2} ≔450\: \\ \mathrm{lengthArm3} ≔350\: \\ \mathrm{lengthTip}≔200\: \end{gathered}$$

$$\begin{gathered} \mathbf{for}\mathrm{p}\mathbf{from}0\mathbf{to}\mathrm{N}\mathbf{do} \\ \mathrm{s}≔2.0 \mathrm{\π} \mathrm{p}\/\mathrm{N}\; \\ \mathrm{pathAtTime}≔\mathrm{eval}\left(\mathrm{path}\,\mathrm{t}\=\mathrm{s}\right)\: \\ \mathrm{px}\left[\mathrm{p}\right]\,\mathrm{py}\left[\mathrm{p}\right]\,\mathrm{pz}\left[\mathrm{p}\right]≔\mathrm{rhs}\left(\mathrm{pathAtTime}\left[1\right]\right)\,\mathrm{rhs}\left(\mathrm{pathAtTime}\left[2\right]\right)\,\mathrm{rhs}\left(\mathrm{pathAtTime}\left[3\right]\right)\; \\ \mathbf{for} \mathrm{i} \mathbf{from} 1 \mathbf{to} 3 \mathbf{do} \\ \mathrm{\θ}\left[\mathrm{p}\,\mathrm{i}\right]≔\mathrm{eval}\left(\mathrm{subs}\left(\mathrm{path}\,\mathrm{\Θ}\parallel \mathrm{i}\right)\,\mathrm{t}\=\mathrm{s}\right)\: \\ \mathbf{end} \mathbf{do} \\ \mathbf{end} \mathbf{do}\: \end{gathered}$$

$\mathrm{pathTrace}≔\mathrm{pointplot3d}\left(\left[\mathrm{seq}\left(\left[\mathrm{px}\left[\mathrm{u}\right]\,\mathrm{py}\left[\mathrm{u}\right]\,\mathrm{pz}\left[\mathrm{u}\right]\right]\,\mathrm{u}\=0..\mathrm{N}\right)\right]\,\mathrm{color}\=\mathrm{Color}\left(''RGB''\,\left[0\/255\,79\/255\,121\/255\right]\right)\,\mathrm{connect}\=\mathrm{true}\right)\:$

$$\begin{gathered} \mathrm{opts}≔\mathrm{color}\=\mathrm{Color}\left(''RGB''\,\left[108\/255\,122\/255\,137\/255\right]\right)\,\mathrm{grid}\=\left[10\,2\right]\: \\ \mathrm{baseCyl}≔\mathrm{cylinder}\left(\left[0\,0\,0\right]\,\mathrm{radiusBase}\,80\,\mathrm{opts}\right)\: \end{gathered}$$$\mathrm{arm1}≔\mathrm{cylinder}\left(\left[0\,0\,0\right]\,\mathrm{radiusArm1}\,\mathrm{lengthArm1}\,\mathrm{opts}\right)\:$$\mathrm{joint2}≔\mathrm{translate}\left(\mathrm{rotate}\left(\mathrm{cylinder}\left(\left[0\,0\,0\right]\,\mathrm{radiusJoint2}\,2 \mathrm{radiusJoint2}\,\mathrm{opts}\right)\, \mathrm{\π}\/2\,0\,0\right)\,0\,-\mathrm{radiusJoint2}\,\mathrm{lengthArm1}\right)\:$$\mathrm{arm2}≔\mathrm{cylinder}\left(\left[0\,0\,0\right]\,\mathrm{radiusArm2}\,\mathrm{lengthArm2}\,\mathrm{opts}\right)\:$$\mathrm{joint3}≔\mathrm{rotate}\left(\mathrm{cylinder}\left(\left[0\,0\,0\right]\,\mathrm{radiusArm2}\,2 \mathrm{radiusArm2}\,\mathrm{opts}\right)\, \mathrm{\π}\/2\,0\,0\right)\:$$\mathrm{arm3}≔\mathrm{cylinder}\left(\left[0\,0\,0\right]\,\mathrm{radiusArm3}\,\mathrm{lengthArm3}\,\mathrm{opts}\right)\:$$\mathrm{tip}≔\mathrm{cone}\left(\left[0\,0\,0\right]\,\mathrm{radiusArm3}\,\mathrm{lengthTip}\,\mathrm{opts}\right)\:$

$$\begin{gathered} \mathrm{robotAnim}≔\mathbf{proc}\left(\mathrm{p}\right) \\ \mathbf{local} \mathrm{Joint2}\, \mathrm{Arm2}\,\mathrm{Joint3}\,\mathrm{Arm3}\,\mathrm{Tip}\: \\ \mathrm{Joint2}≔\mathrm{rotate}\left(\mathrm{joint2}\,0\,0\,-\mathrm{\θ}\left[\mathrm{p}\,1\right]\right)\: \\ \mathrm{Arm2}≔\mathrm{rotate}\left(\mathrm{translate}\left(\mathrm{rotate}\left(\mathrm{arm2}\,0\,-\mathrm{\θ}\left[\mathrm{p}\,3\right]\,0\right)\,0\,0\,\mathrm{lengthArm1}\right)\,0\,0\,-\mathrm{\θ}\left[\mathrm{p}\,1\right]\right)\: \\ \mathrm{Joint3}≔\mathrm{rotate}\left(\mathrm{translate}\left(\mathrm{joint3}\,\mathrm{lengthArm2}\cdot \sin \left(\mathrm{\θ}\left[\mathrm{p}\,3\right]\right)\,-\mathrm{radiusArm2}\,\mathrm{lengthArm1}\+\mathrm{lengthArm2}\cdot \cos \left(\mathrm{\θ}\left[\mathrm{p}\,3\right]\right)\right)\,0\,0\,-\mathrm{\θ}\left[\mathrm{p}\,1\right]\right)\: \\ \mathrm{Arm3}≔\mathrm{rotate}\left(\mathrm{translate}\left(\mathrm{rotate}\left(\mathrm{arm3}\,0\,-\mathrm{\θ}\left[\mathrm{p}\,3\right] -\mathrm{\θ}\left[\mathrm{p}\,2\right]\,0\right)\,\mathrm{lengthArm2}\cdot \sin \left(\mathrm{\θ}\left[\mathrm{p}\,3\right]\right)\,0\,\mathrm{lengthArm1}\+\mathrm{lengthArm2}\cdot \cos \left(\mathrm{\θ}\left[\mathrm{p}\,3\right]\right)\right)\,0\,0\,-\mathrm{\θ}\left[\mathrm{p}\,1\right]\right)\: \\ \mathrm{Tip}≔\mathrm{rotate}\left(\mathrm{translate}\left(\mathrm{rotate}\left(\mathrm{tip}\,0\,\mathrm{\π} - \mathrm{\θ}\left[\mathrm{p}\,3\right] - \mathrm{\θ}\left[\mathrm{p}\,2\right]\,0\right)\,\mathrm{lengthArm2}\cdot \sin \left(\mathrm{\θ}\left[\mathrm{p}\,3\right]\right)\+\left(\mathrm{lengthArm3}\+\mathrm{lengthTip}\right)\cdot \sin \left(\mathrm{\θ}\left[\mathrm{p}\,3\right]\+\mathrm{\θ}\left[\mathrm{p}\,2\right]\right)\,0\,\mathrm{lengthArm1}\+\mathrm{lengthArm2}\cdot \cos \left(\mathrm{\θ}\left[\mathrm{p}\,3\right]\right)\+\left(\mathrm{lengthArm3}\+\mathrm{lengthTip}\right)\cdot \cos \left(\mathrm{\θ}\left[\mathrm{p}\,3\right]\+\mathrm{\θ}\left[\mathrm{p}\,2\right]\right)\right)\,0\,0\,-\mathrm{\θ}\left[\mathrm{p}\,1\right]\right)\: \\ \mathrm{display}\left(\mathrm{pathTrace}\,\mathrm{baseCyl}\,\mathrm{arm1}\,\mathrm{Joint2}\,\mathrm{Arm2}\,\mathrm{Joint3}\,\mathrm{Arm3}\,\mathrm{Tip}\,\mathrm{scaling}\=\mathrm{constrained}\,\mathrm{orientation}\=\left[80\,15\right]\,\mathrm{axes}\=\mathrm{none}\,\mathrm{style}\=\mathrm{patchnogrid}\right) \\ \mathbf{end} \mathbf{proc}\: \end{gathered}$$

$\mathrm{animate}\left(\mathrm{robotAnim}\,\left[\mathrm{t}\right]\,\mathrm{t}=\left[\$1..N\right]\right)$