Modeling and Code Generation for a Robot Arm - Maple Help

Modeling and Code Generation for a Robot Arm

Introduction

This application models a robot arm with three degrees of freedom. To model the arm, the worksheet does the following:

 • 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

Reference:

Transformation Matrix for One Joint

 > $\mathrm{restart}:$$\mathrm{with}\left(\mathrm{plots}\right):$$\mathrm{with}\left(\mathrm{plottools}\right):\mathrm{with}\left(\mathrm{ColorTools}\right):$
 >
 ${{\mathrm{_rtable}}}_{{18446746790769198014}}$ (2.1)

Transformation Matrix of Tip wrt Base

Next, use parameters for a robot with a sequence of three arms and compute the transformation matrix for the tip of the robot with respect to its base.

 > $\mathrm{H1}≔\mathrm{eval}\left(\mathrm{H},\left[\mathrm{θ}=\mathrm{θ1},\mathrm{α}=-\mathrm{π}/2,\mathrm{a}=0,\mathrm{d}=\mathrm{lengthArm1}\right]\right):$$\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{H2}≔\mathrm{eval}\left(\mathrm{H},\left[\mathrm{θ}=\mathrm{θ2},\mathrm{\alpha }=0,\mathrm{d}=0,\mathrm{a}=\mathrm{lengthArm2}\right]\right):$$\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\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):$$\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{H14}≔\mathrm{H1}·\mathrm{H2}·\mathrm{H3}:$

Path for Robot Tip to Follow

This is the required path for the end effector, as a function of time.

 >

Deriving Joint Angles

First Angle

 > $\mathrm{v}≔⟨0,0,0,1⟩:$$\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{w}≔\mathrm{H14}·\mathrm{v}:$$\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{eq1}≔\frac{{\mathrm{w}}_{1}}{{\mathrm{w}}_{2}}=\frac{\mathrm{x}}{\mathrm{y}}:\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\mathrm{Θ1}≔\mathrm{solve}\left(\mathrm{eq1},\mathrm{θ1}\right)$
 ${\mathrm{Θ1}}{≔}{\mathrm{arctan}}{}\left(\frac{{y}}{{x}}\right)$ (5.1.1)

Second Angle

 > $\mathrm{u}≔{{\mathrm{w}}_{1}}^{2}+{{\mathrm{w}}_{2}}^{2}+{{\mathrm{w}}_{3}}^{2}-\mathrm{A}:\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{v}≔{\mathrm{w}}_{3}-\mathrm{z}:\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\mathrm{\Theta }≔\mathrm{solve}\left(\mathrm{W},\mathrm{θ3}\right):\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\mathrm{Θ2}≔\mathrm{eval}\left(\mathrm{\Theta },\mathrm{A}={\mathrm{z}}^{2}+{\mathrm{x}}^{2}+{\mathrm{y}}^{2}\right)$
 ${\mathrm{Θ2}}{≔}{\mathrm{arccos}}{}\left(\frac{{{x}}^{{2}}{+}{{y}}^{{2}}{+}{{z}}^{{2}}{-}{2}{}{z}{}{\mathrm{lengthArm1}}{+}{{\mathrm{lengthArm1}}}^{{2}}{-}{{\mathrm{lengthArm2}}}^{{2}}{-}{{\mathrm{lengthArm3}}}^{{2}}{-}{2}{}{\mathrm{lengthArm3}}{}{\mathrm{lengthTip}}{-}{{\mathrm{lengthTip}}}^{{2}}}{{2}{}{\mathrm{lengthArm2}}{}\left({\mathrm{lengthArm3}}{+}{\mathrm{lengthTip}}\right)}\right)$ (5.2.1)

Third Angle

 > $\mathrm{W1}≔{\mathrm{w}}_{3}-\mathrm{z}:$$\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{W2}≔\mathrm{eval}\left(\mathrm{W1},\mathrm{θ3}=\mathrm{Θ2}\right):\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\mathrm{sol}≔\mathrm{solve}\left(\mathrm{W2}=0,\mathrm{θ2}\right):\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$
 > $\mathrm{Θ3}≔\mathrm{simplify}\left({\mathrm{sol}}_{1}+\mathrm{π}/2\right)$
 ${\mathrm{Θ3}}{≔}{\mathrm{arctan}}{}\left(\frac{{-}{\mathrm{lengthArm2}}{}\sqrt{\left({{x}}^{{2}}{+}{{y}}^{{2}}\right){}{\left({{x}}^{{2}}{+}{{y}}^{{2}}{+}{{z}}^{{2}}{-}{2}{}{z}{}{\mathrm{lengthArm1}}{+}{{\mathrm{lengthArm1}}}^{{2}}{+}{{\mathrm{lengthArm2}}}^{{2}}{-}{{\mathrm{lengthArm3}}}^{{2}}{-}{2}{}{\mathrm{lengthArm3}}{}{\mathrm{lengthTip}}{-}{{\mathrm{lengthTip}}}^{{2}}\right)}^{{2}}}{}\left({\mathrm{lengthArm3}}{+}{\mathrm{lengthTip}}\right){}\sqrt{{-}\frac{\left({{x}}^{{2}}{+}{{y}}^{{2}}{+}{{z}}^{{2}}{-}{2}{}{z}{}{\mathrm{lengthArm1}}{+}{{\mathrm{lengthArm1}}}^{{2}}{-}{{\mathrm{lengthArm2}}}^{{2}}{+}\left({2}{}{\mathrm{lengthArm3}}{+}{2}{}{\mathrm{lengthTip}}\right){}{\mathrm{lengthArm2}}{-}{{\mathrm{lengthArm3}}}^{{2}}{-}{2}{}{\mathrm{lengthArm3}}{}{\mathrm{lengthTip}}{-}{{\mathrm{lengthTip}}}^{{2}}\right){}\left({{x}}^{{2}}{+}{{y}}^{{2}}{+}{{z}}^{{2}}{-}{2}{}{z}{}{\mathrm{lengthArm1}}{+}{{\mathrm{lengthArm1}}}^{{2}}{-}{{\mathrm{lengthArm2}}}^{{2}}{+}\left({-}{2}{}{\mathrm{lengthArm3}}{-}{2}{}{\mathrm{lengthTip}}\right){}{\mathrm{lengthArm2}}{-}{{\mathrm{lengthArm3}}}^{{2}}{-}{2}{}{\mathrm{lengthArm3}}{}{\mathrm{lengthTip}}{-}{{\mathrm{lengthTip}}}^{{2}}\right)}{{{\mathrm{lengthArm2}}}^{{2}}{}{\left({\mathrm{lengthArm3}}{+}{\mathrm{lengthTip}}\right)}^{{2}}}}{-}\left({z}{-}{\mathrm{lengthArm1}}\right){}{\left({{x}}^{{2}}{+}{{y}}^{{2}}{+}{{z}}^{{2}}{-}{2}{}{z}{}{\mathrm{lengthArm1}}{+}{{\mathrm{lengthArm1}}}^{{2}}{+}{{\mathrm{lengthArm2}}}^{{2}}{-}{{\mathrm{lengthArm3}}}^{{2}}{-}{2}{}{\mathrm{lengthArm3}}{}{\mathrm{lengthTip}}{-}{{\mathrm{lengthTip}}}^{{2}}\right)}^{{2}}}{\left({{x}}^{{2}}{+}{{y}}^{{2}}{+}{{z}}^{{2}}{-}{2}{}{z}{}{\mathrm{lengthArm1}}{+}{{\mathrm{lengthArm1}}}^{{2}}\right){}{\mathrm{lengthArm2}}{}\left({{x}}^{{2}}{+}{{y}}^{{2}}{+}{{z}}^{{2}}{-}{2}{}{z}{}{\mathrm{lengthArm1}}{+}{{\mathrm{lengthArm1}}}^{{2}}{+}{{\mathrm{lengthArm2}}}^{{2}}{-}{{\mathrm{lengthArm3}}}^{{2}}{-}{2}{}{\mathrm{lengthArm3}}{}{\mathrm{lengthTip}}{-}{{\mathrm{lengthTip}}}^{{2}}\right)}{,}\frac{{-}{\mathrm{lengthArm2}}{}\left({\mathrm{lengthArm3}}{+}{\mathrm{lengthTip}}\right){}\left({z}{-}{\mathrm{lengthArm1}}\right){}\sqrt{{-}\frac{\left({{x}}^{{2}}{+}{{y}}^{{2}}{+}{{z}}^{{2}}{-}{2}{}{z}{}{\mathrm{lengthArm1}}{+}{{\mathrm{lengthArm1}}}^{{2}}{-}{{\mathrm{lengthArm2}}}^{{2}}{+}\left({2}{}{\mathrm{lengthArm3}}{+}{2}{}{\mathrm{lengthTip}}\right){}{\mathrm{lengthArm2}}{-}{{\mathrm{lengthArm3}}}^{{2}}{-}{2}{}{\mathrm{lengthArm3}}{}{\mathrm{lengthTip}}{-}{{\mathrm{lengthTip}}}^{{2}}\right){}\left({{x}}^{{2}}{+}{{y}}^{{2}}{+}{{z}}^{{2}}{-}{2}{}{z}{}{\mathrm{lengthArm1}}{+}{{\mathrm{lengthArm1}}}^{{2}}{-}{{\mathrm{lengthArm2}}}^{{2}}{+}\left({-}{2}{}{\mathrm{lengthArm3}}{-}{2}{}{\mathrm{lengthTip}}\right){}{\mathrm{lengthArm2}}{-}{{\mathrm{lengthArm3}}}^{{2}}{-}{2}{}{\mathrm{lengthArm3}}{}{\mathrm{lengthTip}}{-}{{\mathrm{lengthTip}}}^{{2}}\right)}{{{\mathrm{lengthArm2}}}^{{2}}{}{\left({\mathrm{lengthArm3}}{+}{\mathrm{lengthTip}}\right)}^{{2}}}}{+}\sqrt{\left({{x}}^{{2}}{+}{{y}}^{{2}}\right){}{\left({{x}}^{{2}}{+}{{y}}^{{2}}{+}{{z}}^{{2}}{-}{2}{}{z}{}{\mathrm{lengthArm1}}{+}{{\mathrm{lengthArm1}}}^{{2}}{+}{{\mathrm{lengthArm2}}}^{{2}}{-}{{\mathrm{lengthArm3}}}^{{2}}{-}{2}{}{\mathrm{lengthArm3}}{}{\mathrm{lengthTip}}{-}{{\mathrm{lengthTip}}}^{{2}}\right)}^{{2}}}}{\left({{x}}^{{2}}{+}{{y}}^{{2}}{+}{{z}}^{{2}}{-}{2}{}{z}{}{\mathrm{lengthArm1}}{+}{{\mathrm{lengthArm1}}}^{{2}}\right){}{\mathrm{lengthArm2}}}\right){+}\frac{{\mathrm{\pi }}}{{2}}$ (5.3.1)

Code Generation

This is the C code for the angle of the third joint as a function of the position of the end effector, and the arm lengths.

 > $\mathrm{CodeGeneration}\left[\mathrm{C}\right]\left(\mathrm{Θ3},\mathrm{optimize},\mathrm{deducetypes}=\mathrm{false}\right)$
 t1 = x * x; t2 = y * y; t4 = z * z; t6 = 0.2e1 * z * lengthArm1; t7 = lengthArm1 * lengthArm1; t8 = lengthArm2 * lengthArm2; t9 = lengthArm3 * lengthArm3; t11 = 0.2e1 * lengthArm3 * lengthTip; t12 = lengthTip * lengthTip; t13 = t1 + t2 + t4 - t6 + t7 + t8 - t9 - t11 - t12; t14 = t13 * t13; t16 = sqrt(t14 * (t1 + t2)); t18 = lengthArm3 + lengthTip; t25 = t18 * t18; t29 = sqrt(-0.1e1 / t25 / t8 * (-0.2e1 * lengthArm2 * t18 + t1 - t11 - t12 + t2 + t4 - t6 + t7 - t8 - t9) * (0.2e1 * lengthArm2 * t18 + t1 - t11 - t12 + t2 + t4 - t6 + t7 - t8 - t9)); t32 = z - lengthArm1; t36 = 0.1e1 / (t1 + t2 + t4 - t6 + t7); t38 = 0.1e1 / lengthArm2; t48 = atan2(0.1e1 / t13 * t38 * t36 * (-t29 * t18 * t16 * lengthArm2 - t14 * t32), t38 * t36 * (-t29 * t32 * lengthArm2 * t18 + t16)); t50 = t48 + 0.3141592654e1 / 0.2e1;

Animation

 > $\mathrm{N}≔200:\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$
 > $\mathrm{radiusBase}≔150:\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\mathrm{radiusArm1}≔70:\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\mathrm{radiusJoint2}≔\mathrm{radiusArm1}+5:\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\mathrm{radiusArm2}≔60:\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\mathrm{radiusArm3}≔50:\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$
 >
 >
 > $\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):$$\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$
 > $\mathrm{opts}≔\mathrm{color}=\mathrm{Color}\left("RGB",\left[108/255,122/255,137/255\right]\right),\mathrm{grid}=\left[10,2\right]:\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\mathrm{baseCyl}≔\mathrm{cylinder}\left(\left[0,0,0\right],\mathrm{radiusBase},80,\mathrm{opts}\right):\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\mathrm{arm1}≔\mathrm{cylinder}\left(\left[0,0,0\right],\mathrm{radiusArm1},\mathrm{lengthArm1},\mathrm{opts}\right):\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\mathrm{arm2}≔\mathrm{cylinder}\left(\left[0,0,0\right],\mathrm{radiusArm2},\mathrm{lengthArm2},\mathrm{opts}\right):\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\mathrm{arm3}≔\mathrm{cylinder}\left(\left[0,0,0\right],\mathrm{radiusArm3},\mathrm{lengthArm3},\mathrm{opts}\right):\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$$\mathrm{tip}≔\mathrm{cone}\left(\left[0,0,0\right],\mathrm{radiusArm3},\mathrm{lengthTip},\mathrm{opts}\right):$$\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}\phantom{\rule[-0.0ex]{0.0em}{0.0ex}}$
 >
 > $\mathrm{animate}\left(\mathrm{robotAnim},\left[\mathrm{t}\right],\mathrm{t}=\left[\mathrm{}\left(1..200\right)\right]\right)$
 >