{VERSION 1 0 "X11/Motif" "1.0"}{GLOBALS 3 1}{FONT 0 "-adobe-helve
tica-bold-r-normal--14-*" "helvetica" "Helvetica-Bold" 8 12 0 "He
lvetica-Bold" 12}{FONT 1 "-adobe-new century schoolbook-medium-r-
normal--*-140-*-*-*-*-*-*" "new century schoolbook" "Times-Roman"
 4 14 64 "Times-Roman" 14}{FONT 2 "-*-Courier-medium-r-normal--*-
120-*-*-m-*-*-*" "Courier" "Courier" 4 12 192 "Courier" 12}{FONT 
3 "-adobe-new century schoolbook-medium-r-normal--*-140-*-*-*-*-*
-*" "new century schoolbook" "Times-Roman" 4 14 64 "Times-Roman" 
14}{SCP_R 1 0 31{COM_R 2 0{TEXT 3 1150 "ENGINEERING APPLICATIONS \+
OF MAPLE V RELEASE 2\015\015Robot Kinematics:  Trajectory plannin
g for a two link planar robot\015\015(c) Copyright  G. Pino Porci
ello, Thomas Lee 1992\015\015\015Objective:\015\015     Use Maple
 to solve a typical path planning problem. The tip position of a \+
two-link planar robot will be commanded to follow a straight line
 trajectory. The two link robot has rotary joints, thus constrain
ing it to move in a plane. Such a task and configuration are comm
on in assembly line operations.\015\015     Maple will also be us
ed to animate the motion of the robot and to illustrate the swept
 area of the motion. This will help the user determine if the rob
ot moves in an acceptable manner.\015\015Solution:\015\015     To
 track a trajectory the so-called inverse kinematics problem must
 be solved. Inverse kinematics deals with determining what angles
 and geometric states of the robot are required to force the robo
t tip to follow the line joining specified end points.\015\015   \+
  After solving the inverse kinematics equations, joint angles wi
ll be plotted, and new Maple functions are used to animate the mo
tion of the robot and to illustrate the swept area of the motion.
\015     "}}{COM_R 3 0{TEXT 1 26 "\015\015load in needed packages
\015"}}{INP_R 4 0 "> "{TEXT 0 26 "with(linalg):\012with(plots):"}
}{OUT_R 5 0 4{TEXT 2 71 "Warning: new definition for   norm\012Wa
rning: new definition for   trace\012"}}{COM_R 6 0{TEXT 1 66 "\01
5define the inverse kinematics equations using operator notation \+
\012"}}{INP_R 7 0 "> "{TEXT 0 215 "Theta[2] := arccos( (x^2 + y^2
 - L1^2 - L2^2)/(2*L1*L2) ):\012Theta[1] := arctan( y,x)  - arcta
n( L2*sin(Theta[2])/(L1+L2*cos(Theta[2]) )):\012Theta[1] := unapp
ly( Theta[1], x, y );\012Theta[2] := unapply( Theta[2], x, y );\0
12"}}{OUT_R 8 0 7{DAG :3b3n4\`Theta`,2j2x0001@7,3n3\`x`n3\`y`,1,3
n5\`operator`n4\`arrow`pE+5(3n4\`arctan`,3a2x0002a2x0001p6(3p1A,2
*7n3\`L2`p6+5p6p6*7+9*3p20j2x0002p6*3p1Ep31p6*3n3\`L1`p31i2x0001*
3p27p31p3Cp31p39i2x0002p27p44/3p3Cj2x0004/3p6p31+5p39p6*5p2Ep6p39
p3Cp4Cp3Cp3CpE}}{OUT_R 9 0 7{DAG :3b3n4\`Theta`,2j2x0002@7,3n3\`x
`n3\`y`,1,3n5\`operator`n4\`arrow`pE(3n4\`arccos`,2+3*7+9*3a2x000
1p6j2x0001*3a2x0002p6p24*3n3\`L1`p6i2x0001*3n3\`L2`p6p2Fp24p2Cp2F
p32p2F/3p24p6pE}}{COM_R 10 0{TEXT 1 387 "\012Define joint angle v
ariables.\015\015Define a line in parametric form that connects t
he initial point (x1, y1) to the final poit (x2, y2).\015\015Eval
uate theta1 and theta2 at chosen poits alog the trajectory. The p
arameter t controls the point spacing (t will be incremented by 0
.05).\015\015Set degrees to 1 to eliminate it from expressions re
sulting from the convert() function (radians to degrees).\012\012
"}}{INP_R 11 0 "> "{TEXT 0 378 "increment := .05:\012divisions :=
 round( 1/increment+1 ):\012q1 := array( 1..2*divisions ):\012q2 \+
:= array( 1..2*divisions ):\012i := 1:\012for t from 0 by increme
nt to 1 do\012  x := x1 + t*(x2-x1);\012  y := y1 + t*(y2-y1);\01
2  q1[i] := t;\012  q2[i] := t;\012  q1[i+1] := evalf( convert( T
heta[1](x,y), degrees ) );\012  q2[i+1] := evalf( convert( Theta[
2](x,y), degrees ) ); \012  i := i+2;\012od:\012degrees := 1:\012
"}}{COM_R 12 0{TEXT 1 209 "\015\015The equations are now in a gen
eral form.\015\012We can now define the robot link lengths, and t
he initial and final\015trajectory postions. These values can be \+
changed to see different robot\015responses.\012\015link lengths\
015"}}{INP_R 13 0 "> "{TEXT 0 18 "L1 := 2: L2 := 2:\012"}}{COM_R 
14 0{TEXT 1 49 "\012define initial position of the line trajector
y \015"}}{INP_R 15 0 "> "{TEXT 0 17 "x1 := 2: y1 := 3:"}}{COM_R 
16 0{TEXT 1 46 "\012define final position of the line trajectory\
012"}}{INP_R 17 0 "> "{TEXT 0 19 "x2 := -3: y2 := .5:"}}{COM_R 18
 0{TEXT 1 57 "\015plot robot angles that follow specified line tr
ajectory\012"}}{INP_R 19 0 "> "{TEXT 0 93 "plot( \{convert(q1,lis
t), convert(q2,list)\}, style=POINT, title=`robot angle trajector
ies`) ;\012"}}{COM_R 20 0{TEXT 1 51 "\015calculate joint position
s to animate robot motion\015"}}{INP_R 21 0 "> "{TEXT 0 267 "pl :
= array( 1..divisions):\012i := 1:\012for t from 0 by increment t
o 1 do\012  x := x1 + t*(x2-x1);\012  y := y1 + t*(y2-y1);\012 px
1 := [0, 0,L1*cos(Theta[1](x,y)), L1*sin(Theta[1](x,y)), x, y];\0
12 pl[i] := plot( px1, style=LINE, scaling=CONSTRAINED, color=blu
e ); \012  i := i+1;\012od:"}}{COM_R 22 0{TEXT 1 54 "\015plot (sh
ow swept area) and animate robot arm motion \015"}}{INP_R 23 0 ">
 "{TEXT 0 137 "display( convert(pl,list), title=`robot swept area
` );\012display( convert(pl,list), insequence=true , title=`robot
 arm motion`, color=red);"}}{COM_R 24 0{TEXT 1 55 "\015\012try an
other line trajectory by defing new end points\015"}}{INP_R 25 0 
"> "{TEXT 0 37 "x1 := 2:  y1 := 3: \012x2 := -2: y2:= 3:"}}{COM_R
 26 0{TEXT 1 59 "\015plot new robot angles that follow the new li
ne trajectory\012"}}{INP_R 27 0 "> "{TEXT 0 93 "plot( \{convert(q
1,list), convert(q2,list)\}, style=POINT, title=`robot angle traj
ectories`) ;\012"}}{COM_R 28 0{TEXT 1 55 "\015calculate new joint
 positions to animate robot motion\015"}}{INP_R 29 0 "> "{TEXT 0 
267 "pl := array( 1..divisions):\012i := 1:\012for t from 0 by in
crement to 1 do\012  x := x1 + t*(x2-x1);\012  y := y1 + t*(y2-y1
);\012 px1 := [0, 0,L1*cos(Theta[1](x,y)), L1*sin(Theta[1](x,y)),
 x, y];\012 pl[i] := plot( px1, style=LINE, scaling=CONSTRAINED, \+
color=blue ); \012  i := i+1;\012od:"}}{COM_R 30 0{TEXT 1 58 "\01
5plot and animate robot arm motion of the new trajectory \015"}}
{INP_R 31 0 "> "{TEXT 0 137 "display( convert(pl,list), title=`ro
bot swept area` );\012display( convert(pl,list), insequence=true \+
, title=`robot arm motion`, color=red);"}}{INP_R 32 0 "> "{TEXT 0
 0 ""}}}{END}
