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
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≔arctanyx
Second Angle
u≔w12+w22+w32−A:v≔w3−z:W≔simplifyu−2 lengthArm1⋅v:Θ≔solveW,θ3:Θ2≔evalΘ,A=z2+x2+y2
Θ2≔arccosx2+y2+z2−2zlengthArm1+lengthArm12−lengthArm22−lengthArm32−2lengthArm3lengthTip−lengthTip22lengthArm2lengthArm3+lengthTip
Third Angle
W1≔w3−z:W2≔evalW1,θ3=Θ2:sol≔solveW2=0,θ2:
Θ3≔simplifysol1+π/2
Θ3≔arctan−lengthArm2lengthArm3+lengthTipx2+y2x2+y2+z2−2zlengthArm1+lengthArm12+lengthArm22−lengthArm32−2lengthArm3lengthTip−lengthTip22−x2+y2+z2−2zlengthArm1+lengthArm12−lengthArm22+−2lengthArm3−2lengthTiplengthArm2−lengthArm32−2lengthArm3lengthTip−lengthTip2x2+y2+z2−2zlengthArm1+lengthArm12−lengthArm22+2lengthArm3+2lengthTiplengthArm2−lengthArm32−2lengthArm3lengthTip−lengthTip2lengthArm22lengthArm3+lengthTip2−z−lengthArm1x2+y2+z2−2zlengthArm1+lengthArm12+lengthArm22−lengthArm32−2lengthArm3lengthTip−lengthTip22x2+y2+z2−2zlengthArm1+lengthArm12+lengthArm22−lengthArm32−2lengthArm3lengthTip−lengthTip2x2+y2+z2−2zlengthArm1+lengthArm12lengthArm2,−lengthArm2lengthArm3+lengthTipz−lengthArm1−x2+y2+z2−2zlengthArm1+lengthArm12−lengthArm22+−2lengthArm3−2lengthTiplengthArm2−lengthArm32−2lengthArm3lengthTip−lengthTip2x2+y2+z2−2zlengthArm1+lengthArm12−lengthArm22+2lengthArm3+2lengthTiplengthArm2−lengthArm32−2lengthArm3lengthTip−lengthTip2lengthArm22lengthArm3+lengthTip2+x2+y2x2+y2+z2−2zlengthArm1+lengthArm12+lengthArm22−lengthArm32−2lengthArm3lengthTip−lengthTip22x2+y2+z2−2zlengthArm1+lengthArm12lengthArm2+π2
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≔pointplot3dseqpxu,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≔rotatetranslaterotatearm2,0,−θp,3,0,0,0,lengthArm1,0,0,−θp,1: Joint3≔rotatetranslatejoint3,lengthArm2⋅sinθp,3,−radiusArm2,lengthArm1+lengthArm2⋅cosθp,3,0,0,−θp,1: Arm3≔rotatetranslaterotatearm3,0,−θp,3 −θp,2,0,lengthArm2⋅sinθp,3,0,lengthArm1+lengthArm2⋅cosθp,3,0,0,−θp,1: Tip≔rotatetranslaterotatetip,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:
animaterobotAnim,t,t=$1..N
Download Help Document
