Modeling and Code Generation for a Robot Arm - Maple Help
For the best experience, we recommend viewing online help using Google Chrome or Microsoft Edge.
Our website is currently undergoing maintenance, which may result in occasional errors while browsing. We apologize for any inconvenience this may cause and are working swiftly to restore full functionality. Thank you for your patience.

Online Help

All Products    Maple    MapleSim


Home : Support : Online Help : Applications and Example Worksheets : Science and Engineering : Modeling and Code Generation for a Robot Arm

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:

Adapted from http://www.maplesoft.com/applications/view.aspx?SID=6850

Transformation Matrix for One Joint

> 

restart:withplots:withplottools:withColorTools:

> 

 A1≔cos⁡θ−sin⁡θ00sin⁡θcos⁡θ0000100001:A2≔10000100001d0001:B1≔A2.A1:A3≔10000cos⁡α−sin⁡α00sin⁡αcos⁡α00001:A4≔100a010000100001:B2≔A3.A4:H≔B1.B2;

H≔cos⁡θ−sin⁡θ⁢cos⁡αsin⁡θ⁢sin⁡αcos⁡θ⁢asin⁡θcos⁡θ⁢cos⁡α−cos⁡θ⁢sin⁡αsin⁡θ⁢a0sin⁡αcos⁡αd0001

(2.1)

Transformation Matrix of Tip with Respect to 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.

> 

H1≔evalH,θ=θ1,α=−π/2,a=0,d=lengthArm1:H2≔evalH,θ=θ2,α=0,d=0,a=lengthArm2:H3≔evalH,θ=θ3,α=0,a=lengthArm3+lengthTip,d=0:H14≔H1·H2·H3:

Path for Robot Tip to Follow

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

> 

path ≔ x=350−50⋅sint+75⋅ sin3⋅ t,                    y=450−80⋅ cost+50⋅ sin5⋅ t,                    z=500−30⋅ sin2⋅ t+60⋅ cos5⋅ t:

Deriving Joint Angles

First Angle

> 

v≔0,0,0,1:w≔H14·v:eq1≔w1w2=xy:Θ1≔solveeq1,θ1

Θ1≔arctan⁡yx

(5.1.1)

Second Angle

> 

u≔w12+w22+w32−A:v≔w3−z:W≔simplifyu−2 lengthArm1⋅v:Θ≔solveW,θ3:Θ2≔evalΘ,A=z2+x2+y2

Θ2≔arccos⁡x2+y2+z2−2⁢z⁢lengthArm1+lengthArm12−lengthArm22−lengthArm32−2⁢lengthArm3⁢lengthTip−lengthTip22⁢lengthArm2⁢lengthArm3+lengthTip

(5.2.1)

Third Angle

> 

W1≔w3−z:W2≔evalW1,θ3=Θ2:sol≔solveW2=0,θ2:

> 

Θ3≔simplifysol1+π/2

Θ3≔arctan⁡−lengthArm2⁢lengthArm3+lengthTip⁢x2+y2⁢x2+y2+z2−2⁢z⁢lengthArm1+lengthArm12+lengthArm22−lengthArm32−2⁢lengthArm3⁢lengthTip−lengthTip22⁢−x2+y2+z2−2⁢z⁢lengthArm1+lengthArm12−lengthArm22+−2⁢lengthArm3−2⁢lengthTip⁢lengthArm2−lengthArm32−2⁢lengthArm3⁢lengthTip−lengthTip2⁢x2+y2+z2−2⁢z⁢lengthArm1+lengthArm12−lengthArm22+2⁢lengthArm3+2⁢lengthTip⁢lengthArm2−lengthArm32−2⁢lengthArm3⁢lengthTip−lengthTip2lengthArm22⁢lengthArm3+lengthTip2−z−lengthArm1⁢x2+y2+z2−2⁢z⁢lengthArm1+lengthArm12+lengthArm22−lengthArm32−2⁢lengthArm3⁢lengthTip−lengthTip22x2+y2+z2−2⁢z⁢lengthArm1+lengthArm12+lengthArm22−lengthArm32−2⁢lengthArm3⁢lengthTip−lengthTip2⁢x2+y2+z2−2⁢z⁢lengthArm1+lengthArm12⁢lengthArm2,−lengthArm2⁢lengthArm3+lengthTip⁢z−lengthArm1⁢−x2+y2+z2−2⁢z⁢lengthArm1+lengthArm12−lengthArm22+−2⁢lengthArm3−2⁢lengthTip⁢lengthArm2−lengthArm32−2⁢lengthArm3⁢lengthTip−lengthTip2⁢x2+y2+z2−2⁢z⁢lengthArm1+lengthArm12−lengthArm22+2⁢lengthArm3+2⁢lengthTip⁢lengthArm2−lengthArm32−2⁢lengthArm3⁢lengthTip−lengthTip2lengthArm22⁢lengthArm3+lengthTip2+x2+y2⁢x2+y2+z2−2⁢z⁢lengthArm1+lengthArm12+lengthArm22−lengthArm32−2⁢lengthArm3⁢lengthTip−lengthTip22x2+y2+z2−2⁢z⁢lengthArm1+lengthArm12⁢lengthArm2+π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.

> 

CodeGenerationC⁡Θ3,optimize,deducetypes=false

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;

Animation

> 

N≔200:

> 

radiusBase≔150:radiusArm1≔70:radiusJoint2≔radiusArm1+5:radiusArm2≔60:radiusArm3≔50:

> 

 lengthArm1 ≔ 500: lengthArm2 ≔450: lengthArm3 ≔350: lengthTip≔200:

> 

forpfrom0toNdo   s≔2.0 π p/N;   pathAtTime≔evalpath,t=s:  pxp,pyp,pzp≔rhspathAtTime1,rhspathAtTime2,rhspathAtTime3;for i from 1 to 3 do      θp,i≔evalsubspath,Θ∥i,t=s:   end doend do:

> 

pathTrace≔pointplot3dseq⁡pxu,pyu,pzu,u=0..N,color=ColorRGB,0/255,79/255,121/255,connect=true:

> 

opts≔color=ColorRGB,108/255,122/255,137/255,grid=10,2:baseCyl≔cylinder0,0,0,radiusBase,80,opts:arm1≔cylinder0,0,0,radiusArm1,lengthArm1,opts:joint2≔translaterotatecylinder0,0,0,radiusJoint2,2 radiusJoint2,opts, π/2,0,0,0,−radiusJoint2,lengthArm1:arm2≔cylinder0,0,0,radiusArm2,lengthArm2,opts:joint3≔rotatecylinder0,0,0,radiusArm2,2 radiusArm2,opts, π/2,0,0:arm3≔cylinder0,0,0,radiusArm3,lengthArm3,opts:tip≔cone0,0,0,radiusArm3,lengthTip,opts:

> 

robotAnim≔procp    local Joint2, Arm2,Joint3,Arm3,Tip:    Joint2≔rotatejoint2,0,0,−θp,1:    Arm2≔rotatetranslate⁡rotate⁡arm2,0,−θp,3,0,0,0,lengthArm1,0,0,−θp,1:    Joint3≔rotatetranslate⁡joint3,lengthArm2⋅sinθp,3,−radiusArm2,lengthArm1+lengthArm2⋅cosθp,3,0,0,−θp,1:    Arm3≔rotatetranslate⁡rotatearm3,0,−θp,3 −θp,2,0,lengthArm2⋅sinθp,3,0,lengthArm1+lengthArm2⋅cosθp,3,0,0,−θp,1:    Tip≔rotatetranslate⁡rotatetip,0,π − θp,3 − θp,2,0,lengthArm2⋅sinθp,3+lengthArm3+lengthTip⋅sinθp,3+θp,2,0,lengthArm1+lengthArm2⋅cosθp,3+lengthArm3+lengthTip⋅cosθp,3+θp,2,0,0,−θp,1:displaypathTrace,baseCyl,arm1,Joint2,Arm2,Joint3,Arm3,Tip,scaling=constrained,orientation=80,15,axes=none,style=patchnogridend proc:

> 

animate⁡robotAnim,t,t=$1..N

>