Modeling and Code Generation for a Robot Arm - Maple Programming Help

Online Help

All Products    Maple    MapleSim


Home : Support : Online Help : Applications and Example Worksheets : Science and Engineering : applications/RobotArm

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:

 A1cosθsinθ00sinθcosθ0000100001:A210000100001d0001:B1A2.A1:A310000cosαsinα00sinαcosα00001:A4100a010000100001:B2A3.A4:HB1.B2;

_rtable18446746790769198014

(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.

H1evalH,θ=θ1,α=π/2,a=0,d=lengthArm1:H2evalH,θ=θ2,α=0,d=0,a=lengthArm2:H3evalH,θ=θ3,α=0,a=lengthArm3+lengthTip,d=0:H14H1·H2·H3:

Path for Robot Tip to Follow

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

path  x=35050sint+75 sin3 t,                    y=45080 cost+50 sin5 t,                    z=50030 sin2 t+60 cos5 t:

Deriving Joint Angles

First Angle

v0,0,0,1:wH14·v:eq1w1w2=xy:Θ1solveeq1,θ1

Θ1arctanyx

(5.1.1)

Second Angle

uw12+w22+w32A:vw3z:Wsimplifyu2 lengthArm1v:ΘsolveW,θ3:Θ2evalΘ,A=z2+x2+y2

Θ2arccosx2+y2+z22zlengthArm1+lengthArm12lengthArm22lengthArm322lengthArm3lengthTiplengthTip22lengthArm2lengthArm3+lengthTip

(5.2.1)

Third Angle

W1w3z:W2evalW1,θ3=Θ2:solsolveW2=0,θ2:

Θ3simplifysol1+π/2

Θ3arctanlengthArm2x2+y2x2+y2+z22zlengthArm1+lengthArm12+lengthArm22lengthArm322lengthArm3lengthTiplengthTip22lengthArm3+lengthTipx2+y2+z22zlengthArm1+lengthArm12lengthArm22+2lengthArm3+2lengthTiplengthArm2lengthArm322lengthArm3lengthTiplengthTip2x2+y2+z22zlengthArm1+lengthArm12lengthArm22+2lengthArm32lengthTiplengthArm2lengthArm322lengthArm3lengthTiplengthTip2lengthArm22lengthArm3+lengthTip2zlengthArm1x2+y2+z22zlengthArm1+lengthArm12+lengthArm22lengthArm322lengthArm3lengthTiplengthTip22x2+y2+z22zlengthArm1+lengthArm12lengthArm2x2+y2+z22zlengthArm1+lengthArm12+lengthArm22lengthArm322lengthArm3lengthTiplengthTip2,lengthArm2lengthArm3+lengthTipzlengthArm1x2+y2+z22zlengthArm1+lengthArm12lengthArm22+2lengthArm3+2lengthTiplengthArm2lengthArm322lengthArm3lengthTiplengthTip2x2+y2+z22zlengthArm1+lengthArm12lengthArm22+2lengthArm32lengthTiplengthArm2lengthArm322lengthArm3lengthTiplengthTip2lengthArm22lengthArm3+lengthTip2+x2+y2x2+y2+z22zlengthArm1+lengthArm12+lengthArm22lengthArm322lengthArm3lengthTiplengthTip22x2+y2+z22zlengthArm1+lengthArm12lengthArm2+π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 = 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

N200:

radiusBase150:radiusArm170:radiusJoint2radiusArm1+5:radiusArm260:radiusArm350:

 lengthArm1  500: lengthArm2 450: lengthArm3 350: lengthTip200:

forpfrom0toNdo   s2.0 π p/N;   pathAtTimeevalpath,t=s:  pxp,pyp,pzprhspathAtTime1,rhspathAtTime2,rhspathAtTime3;for i from 1 to 3 do      θp,ievalsubspath,Θi,t=s:   end doend do:

pathTracepointplot3dseqpxu,pyu,pzu,u=0..N,color=ColorRGB,0/255,79/255,121/255,connect=true:

optscolor=ColorRGB,108/255,122/255,137/255,grid=10,2:baseCylcylinder0,0,0,radiusBase,80,opts:arm1cylinder0,0,0,radiusArm1,lengthArm1,opts:joint2translaterotatecylinder0,0,0,radiusJoint2,2 radiusJoint2,opts, π/2,0,0,0,radiusJoint2,lengthArm1:arm2cylinder0,0,0,radiusArm2,lengthArm2,opts:joint3rotatecylinder0,0,0,radiusArm2,2 radiusArm2,opts, π/2,0,0:arm3cylinder0,0,0,radiusArm3,lengthArm3,opts:tipcone0,0,0,radiusArm3,lengthTip,opts:

robotAnimprocp    local Joint2, Arm2,Joint3,Arm3,Tip:    Joint2rotatejoint2,0,0,θp,1:    Arm2rotatetranslaterotatearm2,0,θp,3,0,0,0,lengthArm1,0,0,θp,1:    Joint3rotatetranslatejoint3,lengthArm2sinθp,3,radiusArm2,lengthArm1+lengthArm2cosθp,3,0,0,θp,1:    Arm3rotatetranslaterotatearm3,0,θp,3 θp,2,0,lengthArm2sinθp,3,0,lengthArm1+lengthArm2cosθp,3,0,0,θp,1:    Tiprotatetranslaterotatetip,0,π  θp,3  θp,2,0,lengthArm2sinθp,3+lengthArm3+lengthTipsinθp,3+θp,2,0,lengthArm1+lengthArm2cosθp,3+lengthArm3+lengthTipcosθ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..200