AltRobot.nb 1. Alternate Robot

Transcription

1 AltRobot.nb Alternate Robot 2008 November 5 printed 2008 November 6 30 In which I formulate Hamilton's equations for a simple three link industrial robot with a payload, imposing holonomic constraints before setting up the equations. I have added the ability to find torques required for an arbitrary motion of the end effector, and a simulation of the motion of the system. CONTENTS. Formulation: set up the four link system: a. the general system b. specializations (equal arms with a point mass at the end) c. the constraints 2. The Lagrangian and Hamilton's equations 3. Inverse dynamics: torques required for a given motion of the end effector a. linear motion b. more sensible motion 4. Simulations a. falling down, a sanity check b. exercising the computed torques Off@General::spellD; Formulation Define rotation matrices in the sense of Goldstein (G) for rotation about local x, y and z axes. Rx denotes a rotation of an angle angle around the local x axis and Ry and Rz the same for the y and z axes. The sign convention is G's. These become actual rotations when I specify the angle. I can use these sequentially to map each angular velocity into the next element's body coordinates. HRx = 88, 0, 0<, 80, Cos@angleD, Sin@angleD<, 80, -Sin@angleD, Cos@angleD<<L; HRy = 88Cos@angleD, 0, -Sin@angleD<, 80,, 0<, 8Sin@angleD, 0, Cos@angleD<<L; HRz = 88Cos@angleD, Sin@angleD, 0<, 8-Sin@angleD, Cos@angleD, 0<, 80, 0, <<L; It is convenient to have the body ex, ey and ez axes (in the body frame) ex = 8, 0, 0<; ey = 80,, 0<; ez = 80, 0, <; Characteristics of the robot LINKS denotes the number of links (including the payload, even though that is a point mass), CONSTRAINTS the number of holonomic constraints and NQ the total number of degrees of freedom after applying the holonomic constraints. The number of holonomic constraints in this case comes to 2, and we'll identify them as we go along. (We can also do an ad hoc formulation for this problem, and that might well be easier.) LINKS = 4; CONSTRAINTS = 2; NQ = 6 LINKS - CONSTRAINTS;

2 AltRobot.nb 2 Each link has a mass, three semiaxes and three principle moments of inertia, which I write in general M = Array@m, LINKSD; A = Array@a, LINKSD; A2 = Array@b, LINKSD; A3 = Array@c, LINKSD; I = Array@Ix, LINKSD; I2 = Array@Iy, LINKSD; I3 = Array@Iz, LINKSD; I treat the fourth link as a point mass, which means that a@4d = b@4d = c@4d = Ix@4D = Iy@4D = Iz@4D = 0; Consider only equal arm robots c@3d = c@2d; Variables and rotations Define center of mass coordinates and their rates of change for each link. Define position vectors for each link. X = Array@x, LINKSD; Y = Array@y, LINKSD; Z = Array@z, LINKSD; Xt = Array@xt, LINKSD; Yt = Array@yt, LINKSD; Zt = Array@zt, LINKSD; Do@r@iD = 8x@iD, y@id, z@id<, 8i,, LINKS<D; Q = Array@q, NQD; Qt = Array@qt, NQD; Define the rotation matrices R, R2 and R3 and the body axis rotation vector, wb, for each link. Calculate the body axes and the inertial rotation vector w for each link. Print the general body coordinate angular velocity components.

3 AltRobot.nb 3 Do@8R@iD = Rz ê. angle Ø f@id; R2@iD = Rx ê. angle Ø q@id; R3@iD = Rz ê. angle Ø y@id;<, 8i,, LINKS<D Do@ROT@iD = R3@iD.R2@iD.R@iD, 8i,, LINKS<D; Do@8ibar@iD = Transpose@ROT@iDD.ex; jbar@id = Transpose@ROT@iDD.ey; kbar@id = Transpose@ROT@iDD.ez;<, 8i,, LINKS<D; wx = ft Sin@qD Sin@yD + qt Cos@yD; wy = ft Sin@qD Cos@yD - qt Sin@yD; wz = ft Cos@qD + yt; gw = 8wx, wy, wz<; Do@wB@iD = gw ê. 8q Ø q@id, qt Ø qt@id, f Ø f@id, ft Ø ft@id, y Ø y@id, yt Ø yt@id<, 8i,, LINKS<D Do@w@iD = Simplify@Transpose@ROT@iDD.wB@iDD, 8i,, LINKS<D gw êê MatrixForm i qt Cos@yD + ft Sin@qD Sin@yD y ft Cos@yD Sin@qD - qt Sin@yD j z k yt + ft Cos@qD { DumpSave@"Robotprep.mx"D; The Lagrangian and Hamilton's equations Calculate the unconstrained Lagrangian. I won't display it, because it will become immensely simplified, and you can write this out yourself anyway. T = 0; Do@T += ê 2 m@id Hxt@iD^ 2 + yt@id^2 + zt@id^ 2L, 8i,, LINKS<D Do@T += ê 2 Ix@iD wb@id@@dd^2 + ê 2 Iy@iD wb@id@@2dd^ 2 + ê 2 Iz@iD wb@id@@3dd^2, 8i,, LINKS<D V = 0; Do@V += m@id g z@id, 8i,, LINKS<D L = T - V; Constraints Suppose the links to be connected along their kbar axes. I can do that using the vector notation, and then assign the results to the individual components. The total number of constraints here is nine, defining r[2] - r[4] in terms of r[] Do@r@i + D = r@id + c@id kbar@id + c@i + D kbar@i + D, 8i,, LINKS - <D Do@8x@iD = r@id@@dd; y@id = r@id@@2dd; z@id = r@id@@3dd;<, 8i,, LINKS<D Select r[], which gives me three more constraints, for a total of twelve at this point.

4 AltRobot.nb 4 r@d = 80, 0, c@d< 80, 0, c@d< The f orientation of all the links is the same, because links 2-4 do not rotate with respect to the first link in the k direction. This provides three more constraints, for a total of fifteen. f@2d = f@3d = f@4d = f@d; None of the links is supposed to spin about its axis. (We have an ambiguity in the first link because of the definitions of the Euler angles.) This gives me four more constraints, for a total of 9. y@d = y@2d = y@3d = y@4d = 0; The last two constraints q@d = 0; q@4d = q@3d; I need to replace x[], y[] and z[] explicitly, which is a Mathematica (or personal) foible, and set a number of angle derivatives equal to zero. x@d = y@d = 0; z@d = c@d; yt@d = yt@2d = yt@3d = yt@4d = 0; qt@d = 0; ft@2d = ft@3d = ft@4d = ft@d; Calculate all the velocities by direct differentiation Do@8xt@iD = D@x@iD, f@dd ft@d + D@x@iD, q@2dd qt@2d + D@x@iD, q@3dd qt@3d; yt@id = D@y@iD, f@dd ft@d + D@y@iD, q@2dd qt@2d + D@y@iD, q@3dd qt@3d; zt@id = D@z@iD, f@dd ft@d + D@z@iD, q@2dd qt@2d + D@z@iD, q@3dd qt@3d;<, 8i,, LINKS<D Reduced Lagrangian and Hamilton's equations Now I finally have a nice expression for L in terms of three generalized coordinates (ones I could have recognized from the beginning had I wished to do that).

5 AltRobot.nb 5 L = Collect@L, 8ft@D, qt@2d, qt@3d, g<, SimplifyD g H-c@2D Cos@q@3DD Hm@3D + 2 m@4dl - c@2d Cos@q@2DD Hm@2D + 2 Hm@3D + m@4dll - c@d Hm@D + 2 Hm@2D + m@3d + m@4dlll + ÅÅÅÅ 2 HIx@2D + c@2d2 Hm@2D + 4 Hm@3D + m@4dlll qt@2d c@2d 2 Cos@q@2D - q@3dd Hm@3D + 2 m@4dl qt@2d qt@3d + ÅÅÅÅ 2 HIx@3D + c@2d2 Hm@3D + 4 m@4dll qt@3d 2 + ÅÅÅÅ 4 HIy@2D + Iy@3D + 2 Iz@D + Iz@2D + Iz@3D + c@2d2 m@2d + 5 c@2d 2 m@3d + 8 c@2d 2 m@4d - Cos@2 q@3dd HIy@3D - Iz@3D + c@2d 2 Hm@3D + 4 m@4dll - Cos@2 q@2dd HIy@2D - Iz@2D + c@2d 2 Hm@2D + 4 Hm@3D + m@4dlll + 8 c@2d 2 m@3d Sin@q@2DD Sin@q@3DD + 6 c@2d 2 m@4d Sin@q@2DD Sin@q@3DDL ft@d 2 Now I can assign the q and qt variables f@d = q@d; ft@d = qt@d; q@2d = q@2d; qt@2d = qt@2d; q@3d = q@3d; qt@3d = qt@3d; We'll need p whether we use it directly or not Clear@pD; P = Array@p, NQD; Do@Print@i, ": ", p@id = Simplify@D@L, qt@iddd, "\n"d, 8i,, NQ<D : ÅÅÅÅ 2 qt@d HIy@2D + Iy@3D + 2 Iz@D + Iz@2D + Iz@3D + c@2d2 m@2d + 5 c@2d 2 m@3d + 8 c@2d 2 m@4d - Cos@2 q@3dd HIy@3D - Iz@3D + c@2d 2 Hm@3D + 4 m@4dll - Cos@2 q@2dd HIy@2D - Iz@2D + c@2d 2 Hm@2D + 4 Hm@3D + m@4dlll + 8 c@2d 2 m@3d Sin@q@2DD Sin@q@3DD + 6 c@2d 2 m@4d Sin@q@2DD Sin@q@3DDL 2: HIx@2D + c@2d 2 Hm@2D + 4 Hm@3D + m@4dlll qt@2d + 2 c@2d 2 Cos@q@2D - q@3dd Hm@3D + 2 m@4dl qt@3d 3: 2 c@2d 2 Cos@q@2D - q@3dd Hm@3D + 2 m@4dl qt@2d + HIx@3D + c@2d 2 Hm@3D + 4 m@4dll qt@3d DumpSave@"RobotHL.mx"D; Calculate the rates of change of p (including the applied torques)

6 AltRobot.nb 6 Clear@ptD; Pt = Array@pt, NQD; Do@Print@i, ": ", pt@id = Simplify@D@L, q@iddd + gq@id, "\n"d, 8i,, NQ<D : gq@d 2: gq@2d + g c@2d Hm@2D + 2 Hm@3D + m@4dll Sin@q@2DD - 2 c@2d 2 Hm@3D + 2 m@4dl qt@2d qt@3d Sin@q@2D - q@3dd + Cos@q@2DD qt@d 2 HHIy@2D - Iz@2D + c@2d 2 Hm@2D + 4 Hm@3D + m@4dlll Sin@q@2DD + 2 c@2d 2 Hm@3D + 2 m@4dl Sin@q@3DDL 3: gq@3d + c@2d Hm@3D + 2 m@4dl H2 c@2d qt@2d qt@3d Sin@q@2D - q@3dd + g Sin@q@3DDL + Cos@q@3DD qt@d 2 H2 c@2d 2 Hm@3D + 2 m@4dl Sin@q@2DD + HIy@3D - Iz@3D + c@2d 2 Hm@3D + 4 m@4dll Sin@q@3DDL 3: c@3d Hm@3D + 2 m@4dl H2 c@2d qt@2d qt@3d Sin@q@2D - q@3dd + g Sin@q@3DDL + Cos@q@3DD qt@d 2 H2 c@2d c@3d Hm@3D + 2 m@4dl Sin@q@2DD + HIy@3D - Iz@3D + c@3d 2 Hm@3D + 4 m@4dll Sin@q@3DDL Identify the generalized forces with the imposed torques gq@d = t; gq@2d = t2 - t3; gq@3d = t3; DumpSave@"RobotHaL.mx"D; The q evolution equations (and I will now introduce explicit time dependence for p). I am also giving the new variables new names because I don't want to redefine the original names..

7 AltRobot.nb 7 In[279]:= Exit@D Solve@p@D ã p@td, qt@dd; nqt@d = Simplify@qt@D ê. %@@DDD Solve@8p@2D ã p2@td, p@3d ã p3@td<, 8qt@2D, qt@3d<d; 8nqt@2D, nqt@3d< = Simplify@8qt@2D, qt@3d< ê. %@@DDD H2 p@tdl ê HIy@2D + Iy@3D + 2 Iz@D + Iz@2D + Iz@3D + c@2d 2 m@2d + 5 c@2d 2 m@3d + 8 c@2d 2 m@4d - Cos@2 q@3dd HIy@3D - Iz@3D + c@2d 2 Hm@3D + 4 m@4dll - Cos@2 q@2dd HIy@2D - Iz@2D + c@2d 2 Hm@2D + 4 Hm@3D + m@4dlll + 8 c@2d 2 m@3d Sin@q@2DD Sin@q@3DD + 6 c@2d 2 m@4d Sin@q@2DD Sin@q@3DDL 8HHIx@3D + c@2d 2 Hm@3D + 4 m@4dll p2@td - 2 c@2d 2 Cos@q@2D - q@3dd Hm@3D + 2 m@4dl p3@tdl ê H-2 c@2d 4 Cos@2 Hq@2D - q@3dld Hm@3D + 2 m@4dl 2 + Ix@2D HIx@3D + c@2d 2 Hm@3D + 4 m@4dll + c@2d 2 HIx@3D Hm@2D + 4 Hm@3D + m@4dll + c@2d 2 Hm@2D Hm@3D + 4 m@4dl + 2 Hm@3D m@3d m@4d + 4 m@4d 2 LLLL, H-2 c@2d 2 Cos@q@2D - q@3dd Hm@3D + 2 m@4dl p2@td + HIx@2D + c@2d 2 Hm@2D + 4 Hm@3D + m@4dlll p3@tdl ê H-2 c@2d 4 Cos@2 Hq@2D - q@3dld Hm@3D + 2 m@4dl 2 + Ix@2D HIx@3D + c@2d 2 Hm@3D + 4 m@4dll + c@2d 2 HIx@3D Hm@2D + 4 Hm@3D + m@4dll + c@2d 2 Hm@2D Hm@3D + 4 m@4dl + 2 Hm@3D m@3d m@4d + 4 m@4d 2 LLLL< DumpSave@"RobotH2aL.mx"D; In[]:= << "RobotH2aL.mx"; Off@General::spellD; Inverse dynamics Torque in terms of q, p, and pt In[73]:= Solve@Pt@@DD ã D@p@tD, td, td; ft = t ê. %@@DD; Solve@Pt@@3DD ã D@p3@tD, td, t3d; ft3 = t3 ê. %@@DD; Solve@HPt@@2DD ê. t3 Ø ft3l ã D@p2@tD, td, t2d; ft2 = t2 ê. %@@DD; outtorque = 8ft, ft2, ft3<; General inverse kinematics Denote the position of the end effector by {x4, y4, z4}. Recall that c[3] = c[2] so that we can use the simpler formulation for q2and q3. Here we have the cylindrical radius cr, and spherical radius sr. The other symbols are as defined in class. The sanity check shows that I haven't made a mistake here.

8 AltRobot.nb 8 In[80]:= Clear@x4, y4, z4d; cr = Sqrt@x4^2 + y4^2d; sr = Sqrt@x4 ^2 + y4 ^2 + Hz4L^ 2D; a = ArcTan@Hz4L ê crd; b = ArcSin@sr ê 2 ê 2 ê c@2dd; g = Pi ê 2 - b; q2 = Pi ê 2 - g - a; q3 = Pi + q2-2 b; f = ArcTan@x4, y4d + Pi ê 2; Simplify@2 c@2d Cos@q2D + 2 c@2d Cos@q3D, Assumptions Ø x4 > 0 && y4 > 0 && c@2d > 0 && c@3d > 0 && z4 œ RealsD Simplify@2 c@2d Sin@q2D + 2 c@2d Sin@q3D, Assumptions Ø x4 > 0 && y4 > 0 && c@2d > 0 && c@3d > 0 && z4 œ RealsD Out[89]= z4 Out[90]= "################ x4 2 + y4 #### 2 Select the desired motion, here a linear path (I use Q for the time interval because I've already used T for kinetic energy.) In[9]:= x4 = x4 + Hx42 - x4l Ht ê QL; y4 = y4 + Hy42 - y4l Ht ê QL; z4 = z4 + Hz42 - z4l Ht ê QL; There follows a series of substitutions. First substitute for the angles and their derivatives in the torque expression. (The expressions generated in the next three blocks are lengthy, and so I do not display them.) In[94]:= outtorque ê. 8q@D Ø f, q@2d Ø q2, q@3d Ø q3<; OT = % ê. 8qt@D Ø D@f, td, qt@2d Ø D@q2, td, qt@3d Ø D@q3, td<; Make the same substitutions in the formal expressions for p In[96]:= P ê. 8q@D Ø f, q@2d Ø q2, q@3d Ø q3<; OP = % ê. 8qt@D Ø D@f, td, qt@2d Ø D@q2, td, qt@3d Ø D@q3, td<; Use the previous step to replace p' in the torque In[98]:= OT = OT ê. 8D@p@tD, td Ø D@OP@@DD, td, D@p2@tD, td Ø D@OP@@2DD, td, D@p3@tD, td Ø D@OP@@3DD, td<; Choose a robot Now let me simplify with a view to approaching some actual numerical exercises. (I suppose the links of the robot to be slender members so I can ignore a[i], b[i] and the transverse moments of inertia. The full system is not a big deal, but this makes things a little clearer, I think.) In[99]:= m@d = m@2d = m@3d = ; m@4d = ê 0; c@d = c@2d = ; g = ; Ix@D = Ix@2D = Ix@3D = Iy@D = Iy@2D = Iy@3D = 0; a@d = a@2d = a@3d = b@d = b@2d = b@3d = 0; Do@Iz@iD = ê 3 m@id c@id^2, 8i,, 3<D;

9 AltRobot.nb 9 Plot the torques for the motion

10 AltRobot.nb 0 In[05]:= x4 = 0; x42 = ; y4 = ; y42 = 0; z4 = 0; z42 = ; Q = ; Plot@8f, q2, q3<, 8t, 0, Q<, PlotStyle Ø 88RGBColor@0,, 0D<, 8RGBColor@, 0, 0D<, 8RGBColor@0, 0, D<<, PlotLabel Ø "Joint Angles for a Linear Motion\ny HgreenL, q2 HredL q3 HblueL"D Print@"Initial Angles: ", N@80 Hf ê. t Ø 0L ê PiD, ", ", N@80 Hq2 ê. t Ø 0L ê PiD, ", ", N@80 Hq3 ê. t Ø 0L ê PiD, " degrees"d Print@"Final Angles: ", N@80 Hf ê. t Ø QL ê PiD, ", ", N@80 Hq2 ê. t Ø QL ê PiD, ", ", N@80 Hq3 ê. t Ø QL ê PiD, " degrees"d Plot@8OT@@DD, OT@@2DD, OT@@3DD<, 8t, 0, Q<, PlotStyle Ø 88RGBColor@0,, 0D<, 8RGBColor@, 0, 0D<, 8RGBColor@0, 0, D<<, PlotLabel Ø "Required Torques for a Linear Motion\nt HgreenL, t2 HredL t3 HblueL"D Joint Angles for a Linear Motion y HgreenL, q2 HredL q3 HblueL -0.5 Out[06]= Ü Graphics Ü Initial Angles: 80., , degrees Final Angles: 90., , degrees Required Torques for a Linear Motion t HgreenL, t2 HredL t3 HblueL Out[09]= Ü Graphics Ü Save these for later In[0]:= nt = OT@@DD; nt2 = OT@@2DD; nt3 = OT@@3DD;

11 AltRobot.nb Plot the motion in space working from the angles

12 AltRobot.nb 2 In[3]:= tx4 = x@4d ê. 8q@D Ø f, q@2d Ø q2, q@3d Ø q3<; ty4 = y@4d ê. 8q@D Ø f, q@2d Ø q2, q@3d Ø q3<; tz4 = z@4d ê. 8q@D Ø f, q@2d Ø q2, q@3d Ø q3<; ParametricPlot3D@8tx4, ty4, tz4<, 8t, 0, Q<D Out[6]= Ü Graphics3D Ü Simulations I want to clear the masses before I set up the equations so I can look at how different payloads affect the performance of the robot. (You could clear everything if you want equations for a more general robot.) In[3]:= Clear@mD; Develop the differential equations nqt[i] denotes qt[i] in terms of p[i], found earlier in the notebook. In[32]:= Qt ê. 8qt@D Ø nqt@d, qt@2d Ø nqt@2d, qt@3d Ø nqt@3d<; Pt ê. 8qt@D Ø nqt@d, qt@2d Ø nqt@2d, qt@3d Ø nqt@3d<; nqt = %% ê. 8q@D Ø q@td, q@2d Ø q2@td, q@3d Ø q3@td<; npt = %% ê. 8q@D Ø q@td, q@2d Ø q2@td, q@3d Ø q3@td<; ode = D@q@tD, td ã nqt@@dd; ode2 = D@q2@tD, td ã nqt@@2dd; ode3 = D@q3@tD, td ã nqt@@3dd; ode4 = D@p@tD, td ã npt@@dd; ode5 = D@p2@tD, td ã npt@@2dd; ode6 = D@p3@tD, td ã npt@@3dd; Go through the usual packaging

13 AltRobot.nb 3 In[42]:= odes = 8ode, ode2, ode3, ode4, ode5, ode6<; Clear@f0, q20, q30, p0, p20, p30d; ics = 8q@0D ã f0, q2@0d ã q20, q3@0d ã q30, p@0d ã p0, p2@0d ã p20, p3@0d ã p30< answer = 8q@tD, q2@td, q3@td, p@td, p2@td, p3@td< Out[44]= 8q@0D ã f0, q2@0d ã q20, q3@0d ã q30, p@0d ã p0, p2@0d ã p20, p3@0d ã p30< Out[45]= 8q@tD, q2@td, q3@td, p@td, p2@td, p3@td< In[46]:= DumpSave@"AltRobotNumerical.mx"D; Numerical examples. Free fall from rest: t = t2 = t3 = 0. f0 = 0; q20 = Pi ê 4; q30 = 3 Pi ê 4; p0 = p20 = p30 = 0; tf = 25; t = t2 = t3 = 0; soln = NDSolve@8odes, ics<, answer, 8t, 0, tf<d; nf = soln@@,, 2DD; nq2 = soln@@, 2, 2DD; nq3 = soln@@, 3, 2DD; nx2 = x@2d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; ny2 = y@2d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; nz2 = z@2d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; nx3 = x@3d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; ny3 = y@3d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; nz3 = z@3d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; nx4 = x@4d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; ny4 = y@4d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; nz4 = z@4d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<;

14 AltRobot.nb 4 Plot@8nq2, nq3<, 8t, 0, tf<, PlotRange Ø All, PlotStyle Ø 88RGBColor@, 0, 0D<, 8RGBColor@0, 0, D<<, PlotLabel Ø "q2 HredL and q3 HblueL"D 5 q2 HredL and q3 HblueL Ü Graphics Ü Look at the motion of the centers of mass of the three links. Link 2 simply rotates about its pivot point. The others execute complicated paths. There's no dissipation in this analysis, so the results are messy. (The system is complex enough for the response to be chaotic, but I don't know whether it is.) ParametricPlot@8ny2, nz2<, 8t, 0, tf<, AspectRatio Ø Automatic, PlotLabel Ø "Link 2"D ParametricPlot@8ny3, nz3<, 8t, 0, tf<, AspectRatio Ø Automatic, PlotLabel Ø "Link 3"D ParametricPlot@8ny4, nz4<, 8t, 0, tf<, AspectRatio Ø Automatic, PlotLabel Ø "Link 4: the end effector"d Link Ü Graphics Ü

15 AltRobot.nb 5 Link Ü Graphics Ü - Link 4: the end effector Ü Graphics Ü Response to the torque we derived above We need to impose the appropriate initial conditions, which is what the first block is doing. We need to note that the path chosen requires a nonzero initial rotation rates, and I need to put that in. (We cannot specify initial conditions and paths independently. This is a topic for another day, and probably another course.)

16 AltRobot.nb 6 In[47]:= f0 = Limit@f, t Ø 0D; N@80 f0 ê PiD q20 = Limit@q2, t Ø 0D; N@80 q20 ê PiD q30 = Limit@q3, t Ø 0D; N@80 q30 ê PiD p0 = OP@@DD ê. t Ø 0; p20 = OP@@2DD ê. t Ø 0; p30 = OP@@3DD ê. t Ø 0; Out[48]= 80. Out[50]= Out[52]= Redefine the arm masses and select the payload mass. (m[4] = /0 is the design mass.) Choose a final time, and let the torques be the design torques calculated earlier. m@d = m@2d = m@3d = ; m@4d = ê 20; tf = Q; t = nt; t2 = nt2; t3 = nt3; soln = NDSolve@8odes, ics<, answer, 8t, 0, tf<d; nf = soln@@,, 2DD; nq2 = soln@@, 2, 2DD; nq3 = soln@@, 3, 2DD; nx2 = x@2d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; ny2 = y@2d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; nz2 = z@2d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; nx3 = x@3d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; ny3 = y@3d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; nz3 = z@3d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; nx4 = x@4d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; ny4 = y@4d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; nz4 = z@4d ê. 8q@D Ø nf, q@2d Ø nq2, q@3d Ø nq3<; Plot the payload path in three dimensions

17 AltRobot.nb 7 In[90]:= ParametricPlot3D@8nx4, ny4, nz4-2 c@d<, 8t, 0, Q<D xone = Chop@nx4 ê. t Ø 0D; yone = Chop@ny4 ê. t Ø 0D; zone = Chop@-2 c@d + nz4 ê. t Ø 0D; xtwo = Chop@nx4 ê. t Ø QD; ytwo = Chop@ny4 ê. t Ø QD; ztwo = Chop@-2 c@d + nz4 ê. t Ø QD; Print@"Initial Position = 8", xone, ", ", yone, ", ", zone, "<"D Print@"Final Position = 8", xtwo, ", ", ytwo, ", ", ztwo, "<"D Out[90]= Ü Graphics3D Ü Initial Position = 80,., 0< Final Position = , ,.0368< Plot the angles for comparison with the desired angles.

18 AltRobot.nb 8 In[99]:= Plot@8nf, nq2, nq3, f, q2, q3<, 8t, 0, Q<, PlotStyle Ø 88RGBColor@0,, 0D<, 8RGBColor@, 0, 0D<, 8RGBColor@0, 0, D<<D Out[99]= Ü Graphics Ü