Solutions Manual - Force Control of Manipulators

Chapter 11 Force Contro] of Manipulators Exercises 11.1) Note this proЬlem asks for "Natural" constraints only. Natural Vx =0 v, =0 IVx =0 \Vy = О \Vz = О Constraints Fz = О � 11.2) Thc artificial constraints for the task above would Ье: Fx =0 Fr =0 Nx =0 Nr =0 Nz =0 where "а1" is the speed of insertion. Setting (11.9) and (11.14) equal: ёF + KvF;F + (KPF + м-1 К,)ц = м-•к,F,,,, Hence, to damp properly (i.e. to choosc KvF) onc needs to know к� (which is generally unkлown). 11.4) Use (5.l05) witl, fmmcs {А} a11d {В) reversed. First find :т, so invert �Т: � Т= [ 0.866 -og 0.5 о 0.866 О о 1 о О о Now 8 F=�R A F=[1 -8.66 5.0 ] -5.0 1 1.73 -3l т 8N= 8 P,0, ® 8 F+�R A N=[-6.3 -30.9 8 :.•.J= [ 1.0 1.73 -3 -6.3 -30.9 -15.8] -15.8)' r 11.5) Like 11.4. 0.866 -0.5 0.866 о о �т = [ g5 о = [41 :.8.J= (2.19 -11 о -8.66 о -5 ] 1 -5 о 1 -60.3) т +(4.3 2.5 O] r 8.19 О 44.3 -8.5 -60.3) т 11.6) When crowded а good strategy (assuming а fairly stiff Ьооk) is to put one comer of the book into [he crack and then t\vist the book towards the vcn.ical position while applying forcc to push it into the slot. Students should give some version of this suategy. 11.7) Good idea is to attach { С) to the door, so it moves as the task progresses. Then: Natural Vr = О Vz =0 Wx=0 Wr = 0 Fx=0 Nz =0 Artificial Vx = а1 Wz=� Fz = О Nx =0 Nr = О where "r" is radius of door (that is, distnnce from hinge to origin of {С)). "а1" is linear speed of manipulator hand. 11.8) Pretty easy: Natural Vx = О Vy =0 Wx =0 Wy =0 Wz =0 (с) Constraints Fz =0 х Here ]'ve assumed that because it is а very tight­ fitting cork, it is like а square peg in а square hole, and roration about Z is not possiЬle. Тhis assuшptioл is поt cruciaI, however. Then: Fx =0 Artificial consLraints Fr = О Nx =0 Nr = О Nz=0 11.9) Vsc control \aw: where Е = 80 -8, and thc plant has already been decoupled, that is, the p\ant is: r = В [В] Equate [А] and [В] 10 gct: Ё+KvE+J т K,xlE=0 Choose the Lyapunov equation: This is positive definite in bot\1 Е&Е (note J т К ,xJ is pos. dcf. 1.f К рх is pos. def. Since 1 1 K px l is а "congruence" transform which cannot change the sign of eigenvaJues- scc noыc·s book оп Iincar algebra, or others.) Oifferentiating: Неге we needed to i = -zt т KvE ( use aJJprox. that ) J ;;,о Which is negative definite iff К v is positive defi­ nite. А few more things would need to Ье said to make the proof rigorous, but Lhis is the gencral idea. Z 11.10) This proЬlcm is rcally bcyond thc scopc of the book, as it invotves multivariaЫe cont·rol theory. The sys1em sшdied is а nшlti-input, multi-output system which is couplcd, so thc first proЫem is "what does "critical damping" meai1 in this context. Let mc just mcntion а damping scheme which has given reasonaЫe results in lab experi­ ments: ln the same way that а scalar sys[em with stiffness kµ is dampcd \vith the value 2./f;. \УС have used J 7 2д;;.J to damp the system ,vith stiffncss of J T K,xJ. Note Д,,. Just has thc square roots of the diagonal elements or Крх on the diagonal. 11.11) №tural Vx =0 Vz =0 Wx =0 Wy =0 Wz=O Constraints Fr =0 Chapter 12 Robot Programming Languages and Systems Jn this chapter the exercises are of а programming nature. 1 have left the choice of which robot programmiпg language 10 use optional (with 8 or so suggestions). Thus it is hard to give solulions, and l have elected not to do so. lnstructors will рrоЬаЫу choo.sc to assign proЫems in thc languagc of somc availaЫc robot оп campus (if any) ancl lhe prohlem may use set.ups or fixtures local to that lab. Hence, 1 expect а lot of deviation from the suggested proЫems in the text. Chapter 13 Off-line Programming Systems Exercises 13.1) Collision detection (in this contcxt) rcfers to an algorithm capablc of dctcrmining if two or morc object (spatia1ly modelled) touch or inrersect each other. Collision avoidance is thc (difficult) proЬ!em of computing а path along which an object (or manipulator) is to move such that по collisions occur. Collision-free path planning is а more descriptive tcпn for collision avoidance. 13.2) А world model is а sct of data stored in compшer memory which describes somc aspccts of the maлipulator or its cnvironment. Path planning emulation is а term used to describe an algo­ rithm in а simulation system which attempts to model (or emulate) the path-planning algorithm of the robot controller. Dynamic emulation is а synonym for dynamic simulation-but imp1ies that U1e dynamic simulation is attempting to 111atch some physical system. 13.З) Automatic robot placemcnt is the resu1t of an algorithm which can determine the relative place­ ment between а robot and а workpiece so as to achieve certain objectives-kinematic reacha­ Ьility, singularity avoidance, collision avoidance, etc. Тime optimal paths are those which get from А to В in minimum tiшe. Error propagat'ion anal­ ysis js an algorithm to compute estimates of certainty of knowledge contained in а world model. 13.4) \Vireframe graphics depict polyhedra Ьу rendering al! of the edles of the polyhedra. ln shaded surface display, all the facels of polyhedra are projected onto the screen as 2-D polygons and then "filled..-i.e. all the pixels within those polygons are illuminated. Often, polygon sorting ог А 2-buffer technique is used to perform hidden line elimination. Also, hidden line eJiminaLion сап Ье performed with ed)e-based rendering Ьу the usc or some fancy algorithms. 13.5) RPL- RoЬot Programmiпg Languagcs arc morcor-less conventional computer languages with motion statements and other robot-spccific commands added. ТLP-Task Level Programming refers to laпguages or systems in which it is possiЫe to program at а high level, such as "pick up bolt # В-127". OLP- Off-Line Programming is defincd in the first sепtепсс or Chapter 13 ! 13.6) Calibrat·ion (in 1he context of OLP systems) refers to the task of bringing the simulated workcell into close corrcsJIOndcncc with tl1e actual cell. Coordinated motion is ,vhen two or morc dcvices must move together in kinematically syncluonizcd motion. Automatic scheduling is ал algorithm that сап automatically divide tasks iл ап automated setting up between multiple machi11es. 13.7) This is changing езсh year, so check the latest news. tn thc late 80'� and into the early 90's graphic powcr is рrоЬаЫу increasing Ьу а factor of 2 to 5 each year! 13.8) Placing components оп а ре board, spot-welding ап automobl1e, arc-welding з part, pзinting ап autornoЬile, deburring а casting. 13.9) The more "inteШgent" а prograrnming system is, the larger the world modcl required (roughly speaking!). However, the larger the ,vorld model, the more ways it сал Ье inaccuratc. Solutions to the Programming Exercises (Parts 2-7, 9-11) Programming Exercise (Рагt 2) PROGRAМ РР2 (INPUT,OUTPUT); CONST РI=З.14159; HALFPI =1.5707; DTOR=O. 017453; RTOD=57. 2957; ТУРЕ FRAME =ARRAY[ 1 .. 2, 1 "З] OF REAL; VЕСЗ=ААААУ [ 1 .. З) OF REAL; VAR UARELU, UAAELB, UURELC ,UCRELB :VЕСЗ; I.J :INTEGER; IARELU, IARELB, IURELC, IURELA: FААМЕ; IURELB, !CAELU, ICRELB: FRAМE; FUNCТION АТ2 (A,B:REAL): REAL; VAR ANS:REAL; BEGIN IF В= О.О THEN BEGIN IF А=О.О THEN ANS:=0.0 ELSE IF А >О.О THEN ANS:=HALFPI ELSE ANS:=-HALFPI; END ELSE BEGIN ANS: =ARCTAN (А/В); !F В<О.О THEN IF А<О.О THEN ANS:=ANS-PI ELSE ANS:=ANS+PI; END; AT2: =ANS; END; PROCEDURE TMULT (VAR BRELA,CRELB,CRELA:FRAМE); VAR I,J,K: INTEGER; BEGIN FOR !: = 1 ТО 2 00 BEG!N CRELA[l,З) : = BRELA[I,З); FOR J : =1 ТО 2 00 BEGIN CRELA[ I, З] : =CRELA[ I ,З] +BRELA[ I, J] *CRELB[J, З] ; CRELA[ I ,J] :=О. О; FOR К: = 1 ТО 2 00 CRELA[ I ,J) : =CAELA[ I ,J] +BRELA( I, К[ *CAELB ( K,J] ; END; END; END; PROCEDURE ТINVERT (VAR BRELA,ARELB: FRAME); VAR I,J:INTEGER; BEGIN ARELB( 1, 1] :-BRELA[ 1, 1]; ARELB[1,21 :=BAELA[2, 1]; ARELB(2, 1) : = BRELA[ 1 ,2]; ARELB[2,2] :=BRELA(2,2]; FOR I: =1 ТО 2 00 BEGIN ARELB(I,3] : = О.О; FOR J: =1 ТО 2 DO ARELB( I ,3] :=ARELB( 1 ,Э] ·BRELA(J, I] 'BRELA(J,3]; END; END; PROCEDURE UTOI (VAR UFORM: VЕСЭ; VAR IFORM: FRAME) : BEGIN IFORM(1,3] IFORM(2,3] IFORM(1, 1] IFORM(1,2] IFORM(2, 1] IFORM(2,2] END; :=UFORM(1]; : =UFORM(2]; :=COS (UFORM[З]*DTOR); :=-SIN (UFORM[Э]*DTOR); :~-IFORM[ 1 ,2]; :=IFORM[1, 1]; PROCEDURE IТOU ( VAR IFORM: FRAМE; VAR UFORU: VЕСЗ); BEGIN UFORM[ 1] : =IFORM[ 1,3]; UFORM[2) :=IFORM[2,З]; UFORM[З) :=АТ2 (IFORM[2, 1) ,IFORM( 1, 1) )'RTOD; END; BEGIN {MAINPGM.} WRIТE ( 'ENTER ELEMENTS OF ARELU ,ARELB, AND URELC' ) ; FOR I:=1 ТО 3 DO READ(UARELU[ 1]); FOR I:=1 ТО 3 DO READ (UARELB( 1]); FOR I: = 1 ТО 3 DO READ(UURELC[ I]); UTOI (UARELU, IARELU); UTOI (UARELB, IARELB); UTOI(UURELC,IURELC); ПNVERT (IARELU,IURELA); ПNVERT ( IURELC, ICRELU); TMULТ ( IARELB, IURELA, IURELB); TMULT (IURELB,ICRELU,ICRELB); WAIТELN ('С REL В, INТERNAL FOAM, ROW-WISE '); FOR 1: =1 ТО 2 DO BEGIN FOR J:=1 ТО З DO WRIТE (ICRELB[I ,JJ :В:З); WRIТELN; END; IТOU(ICRELB,UCRELB); WRIТELN( 'CAELB,USER FORM'); FOR I:=1 ТО З DO WRIТE(UCRELB[I] :8:3); END. Programming Exercise (Part З) program ррЗ (INPUT ,OUTPUT); CONST рi=З.14159; halfpi = 1 . 5707; dtor= O. 017453292; rtod=57. 29578; Conversion factors: rad/deg deg/ rad ТУРЕ frame=ARRAY[1"2,1"3) OF REAL; vec3=ARRAY [1 .. 3) OF REAL; { 2х 2 Rot. plus 2х 1 Pos. ) VAR i: integer; theta: vесЗ; wгelb: frame; 11, 12: REAL; t relw: frame; srelb,brels: framej trels: frame; t relwuser: vесЗ; srelbuser: vесЗ; trelsuser: vесз; ans: char i Joint angles for the 3-link arm ) Loc. of the wrist wrt the base ) Link lengths of the arm ) Defines the tool location Arm base location } PROCEOURE WRIТEFRAМE (VAR foo: frame); VAA i, j : integer; BEGIN FOR i:=1 ТО 2 00 BEGIN WRIТE (' [ '); FOR j:=1 ТО З 00 WRIТE (foo[i,j J: 10:З); WRIТELN(']'); ENO; END; PROCEDURE WRIТEVECT (VAR foo; vec3); i: integer; BEGIN FOR i: = 1 ТО 3 do WRIТELN(' [' ,foo)i) :10:Э, '] '); END; VAR FUNCТION АТ2 (а,ь: REAL): REAL; { 4 auadrant Arc Tangent ) VAR ans: REAL; BEGIN IF Ь= О.О THEN BEGIN IF а=О.О THEN ans:= О.О ELSE IF а>О,О THEN ans:=halfpi ELSE ans :=-halfpi; END ELSE BEGIN ans : = arctan {а/Ь) j { Pascal defined arctan IF Ь< О THEN IF а< о THEN ans:=ans-pi ELSE ans: = ans+pi; END; at2 :=ans; END; { atan2 ) PROCEDURE TMULT (VAR brela,crelb,crela: frame); VAR i, j ,k: INTEGER; BEGIN FOR i:=1 ТО 2 DO BEGIN crela(i,3) : = brela(i,3]; FOR j : = 1 ТО 2 00 BEGIN crela ( i ,3) : =crela ( i,3] tbrela ( i, j) •crelb[ j , 3) ; crela( i,j) :=О.О; FOR k:=1 ТО 2 00 crela[i, j] :=crela[ i, j] tbrela[ i, k) •crelb( k, j); ENO; ENO; ENO; { tmult } PROCEOURE ТINVERT (VAR brela,arelb: frame); VAR i, j: INTEGER; BEGIN arelb[1,1]:=brela(1,1J; { Transpose rotation part} arelb(1,21: = brela[2,11; arelb(2, 1) : = brela(1,2]; arelb[2,2] : = brela[2,2J; FOR i: :1 ТО 2 00 { Calculate position part } BEGIN arelb[i,ЗJ : = о.о; FOR j : =1 ТО 2 00 а relb[ i ,3] : =arelb [ i, З) -brela [ j, i) *brela I j , З) ; ENO; ENO; { tinvert } PAOCEOUAE К!N (VAA theta: vесз; VAA wrelb: frame); VAR beta: real; BEGIN wrelb(1,ЗJ:•11*cos (theta(1)) t 12*cos (theta[1) + theta[2)); v1relb[2,З):=11'sin (theta[1)) + 12'sin (theta[1) t theta[2]); { beta is the total wrist rotation } beta:=theta[1) + theta[2] + theta[З); wrelb[1,1]:=cos (beta); wrelb(1,2) :=-sin (beta); { note that 2х2 rotation matrices are anti­ wrelb(2, 1) :=-wrelb(1,2); { symmetric so that half of the matrix is f wrelb(2,2) :=wrelb[1, 1); { already computed } ENO: PROCEOURE UTOI(VAR uform: vесЗ; VAR iform: frame); BEGIN iform(1,3) :=uform(1]; iform[2,3J :=utorm[2J; iform[1,1]:=cos (uform[3)*dtor); iform [ 1, 2] : = -sin (uform (3] 'dtor) ; iform[2,1]:=-iform[1,2J; { Again, the 2х2 rotation matrix is anti·} iform[2,2] : = iform[1, 1 J; { symmetric, simplifying the calculations } ENO; PROCEOURE поu (VAR iform: frame; VAA uform: vесЗ}; BEGIN uform[1) :=iform[1,3); uform(2) : = iform[2,3); uform[3J: = at2(iform[2,1J, iform[1,1J)*rtod; { don't torget to use degrees} ENO; PROCEDURE WHERE (VAR theta: vесЗ; VAA trels: frame); { Find the tool wrt station } VAR trelb,wrelb: frame; BEGIN KIN(theta,wrelb); TMULT(wrelb,trelw,trelb); TMULT(brels,trelb,trels); ENO; Get the tool relative to the base } Get the tool relative to the station Set up the given data } PROCEDURE INIТDATA; BEGIN 11 :=о.5; 12:=О.5; trelwuser[1] ;=О.1; trelwuser[2] :=О.2; trelwuser[ЗJ ;=зо.о; initialize link lengths initialize tool location in user.coordinates } srelbuser [ 1]: =-О. 1; { ini tialize station location in user- coordinates } srelbuser[2J :=о.з; srelbuser[З] :=О.О; UTOI ( srelbuser, srelb) ; ТINVERT( srelb' ьrels) ; UTOI (trelwuser,trelw); END; { ini tdata } coпvert base description to internal form get the description in the proper direction Get the tool location in internal form ) BEGIN { Main Test Program !NJТDATA; VIRIТE( 'Care to do some robot kinematics?'); READLN ( ans) ; VIНI LE ans = 'у' 00 BEGIN WRПELN( • Enter theta1, theta2, thetaз: •) ; READLN(theta[1J,theta[2),theta[ЗJ); { All I/0 with user is in Degrees} FOR i:=1 ТО З DO theta[i] :=theta[i]*dtor; { But internally I'll use radians } WHERE(theta, trels); WRПELN( 'The frame description of the tool relative to the base is: '); WRIТEFRMtE(trels); WRIТELN; IТOU(trels, t relsuser); WRIТELN( 'The user-coordinates are: '); WRIТEVECT(trelsuser); WRIТELN: WRIТELN; WRIТE('Would you like to do some more?'); READLN(ans); END; END. Programming Exercise (Part 4) program рр4 ( INPUT, OUTPUT) ; CONST рi=З.14159; halfpi=1 . 5707; dtor•O. 0174532925; rtod=57. 29578; ТУРЕ vесЗ=ААААУ [ 1 .. З] OF REAL; frame=ARRAY[1"2,1"З) OF REAL; VAR i: !NTEGER; 11, 12: REAL; { Link Lengths ) current, goal: vесЗ; t relwuser: vесз; srelbuser: vесЗ; srelb,trelw,trels: frame; near, far: vесэ; neard, fard: vесЭ; sol: BOOLEAN; ans: char; { solutions in degrees for user } FUNCТION аt2(а,ь: REAL): REAL; { Atan2 } VAA ans: REAL; BEGIN IF Ь=О.О THEN BEGIN IF а = О.О THEN ans: = O.O ELSE IF а > О.О THEN ans:=halfpi ELSE ans: • -halfpi; END ELSE BEGIN ans:=arctan(a/b); { Pascal defined arctan IF Ь<О THEN I F а<О THEN ans: =ans- pi ELSE ans: =ans+pi; END; at2;=ans j END; { atan2 PROCEDURE WАПЕFААМЕ (VдА foo: frame); VAR i I j : intege r; BEGIN FOR i: = 1 ТО 2 DO BEGIN VIAIТE ( ' [ ' ) ; FOR j : =1 ТО 3 DO WRIТE(foo[i,j J: 10:3); WAIТELN ( '] ' ) ; END; END; PAOCEDURE WRIТEVECT (VAA foo: vесз); VAR i: integer; BEGIN FOR i: = 1 ТО З do VIAIТELN(' [' ,foo[i] :10:З, ']'); END; PROCEDURE IТOU (VAR iform: frame; VAR uform: vесз); BEGIN uform[1] :=iform[1,ЗJ; uform[2] :=iform[2,ЗJ; uform[ З] : =at2 (iform[2, 1 J , i form [ 1 , 1 J) •rtod; END; PROCEDURE UTOI (VAR uform: vесз; VAR iform: frame); BEGIN iform[1,ЗJ : = uform[ 1]; iform[2,3J :=uform[2]; iform[1,1]: = cos (uform[ЗJ*dtor); iform[1,2J: = -sin (uform[ЗJ*dtor); iform[2, 1 J: =-iform[ 1 ,21; iform[2,2J : = iform[1, 1); END; PROCEDUAE TMULT(VAA brela,crelb,crela: frame); VAR i,j ,k: INTEGER; BEGIN FOA i:=1 ТО 2 00 BEGIN crela[i,ЗJ : = brela[i,ЗJ; FOR j : =1 ТО 2 DO BEGIN crela[i,ЗJ: = crela(i,3) + brela[i,j]• crelb[j,ЗJ; crela[i,j] :=о.о; FOR k:=1 ТО 2 DO crela[i, j] :=crela[ i, j] +brela[i ,k] •crelb[k, j]; END; END· END; ' { tmult } PROCEDURE ТINVERT(VAR brela,arelb: frame); VAR i, j: INTEGER; BEGIN arelb(1, 1] : = brela[1, 1); arelbl1,2) :=brelal2, 1]; arelb[2, 1] : =brela[ 1,2]; arelb[2,2) : = brela[2,2]; FOR i: = 1 ТО 2 DO BEGIN arelb[i,3) :=О.О; FOR j : =1 ТО 2 DO arelb[i,3) : = arelb[i,3)-brela[j, i)* brela[ j ,З); END; END; { tinvert } PROCEDURE KIN(VAR theta: vесЗ; VAR wrelb: frame); VAR beta: real; BEG!N wrelb[1,ЗJ:=11* cos (theta[1))+12' cos (theta[1) + theta[2)); wrelb[2,ЗJ:=11* sin (theta[1])+12' sin (theta[1] + theta[2]); beta:•theta[ 1] + theta[2] + theta[З]; wrelb[1, 1] :=cos (beta); wrelb[1,2] : = -sin (beta); wrelb[2, 1]: •-wrelbl 1,21; wrelb[2,2): =wrelb[ 1, 1]; END; PROCEDURE WНERE(VAR theta: vесз; VAR trels: frame); VAR trelb ,wrelb Jbrels: frame; BEGIN KIN(theta,wrelb); TMULT(wrelb, trelw, trelb); tinvert { srelb I brels) j TMULт (brels, trelb, t rels); END; { where } Get the tool rel base } Get the tool rel station { Define а distance measure for two joint vectors } function dist (VAR а, Ь: vесз): real; VAR d,t: REAL; i: INTEGER; BEGIN d:=O.O; FOR i: =1 ТО 3 DO BEGIN t: =abs ( а [ i 1 - Ь ( i] ) ; d:=d+t; END; dist: =d; END; { dist PROCEDURE range(VAR а: REAL); { puts angle in (-180 180] } BEGIN WНILE a>pi DO а: = а-2* pi; WHILE а<� pi DO а:=а+ 2* pi; END; { range } PROCEDURE invkin (VAR wrelb: frame; VAR currentJnear,far:vecз; VAR sol:boolean); LABEL 5; VAR c2,s2,k1,k2,temp: REAL; i : INТEGER; goal, swap: vесз; solnear, solfar: BOOLEAN; BEGIN itou (wrelb, goal); { get х, у, and theta of goal point ) goal[Э) :=goal[Э]• dtor; { but put theta back into radians ) c2:=(goal[1]* goal[1J+goal[2]* goal[2]-11* 11- 12• 12)/(2.0* 11* 12); IF abs(c2)>1.0 THEN sol:=FALSE ELSE sol: =TRUE; IF NOT sol THEN GOTO 5; { No sol. - j ust exit wi th f lag set s2:=sqrt(1.0 . с2• с2); near[2] : = at2(s2,c2); { Assume Near - swap later if Far ) tar(2J : = -near[2]; k1: = 11 + 12* с2; k2: = 12* s2; temp:=at2(k2,k1); { temp, so we don't calc. it tv1ice) near[ 1 J : = at2(goal[2J ,goal( 1 J) -temp; far( 1]: =at2(goal(2] ,goal [ 1] )+temp; near[Э] : = goal[ЗJ-near[1]-near[2]; far(Э] :=goal[Э] - far[1] - far(21; FOR i: =1 ТО Э DO { Return all angles on range (-180,180] } BEGIN range(near[i]) range(far(i)); END; IF dist ( current, near )>dist ( current, f ar) THEN { swap } BEGIN swap:=near; near :=far; far:=swap; END; solnear: = TRUE; FOR i: 1 ТО З DO BEGIN IF (ABS(near(i]) >2.96) THEN solnear: = FALSE; END; solfar: =TRUE; FOR i:=1 ТО З DO BEGIN IF (ABS(far[i)) >2.96) THEN solfa r: =FALSE END; IF ( (NOT solnear) AND (NOT solfar)) THEN sol: =FALSE ; IF ( solnear AND (NOT solfar ) ) THEN far: = near; IF (solfar AND (NOT solnear)) THEN near:=far ; 5: END; { invkin PAOCEOURE Solve(VAR trels:frame; VAR current, near, far: vесЭ; VAR sol: boolean); VAR trelb,wrelt,wrelb: frame; BEGIN tmul t ( srelb, t rels, trelb); tinvert (trelw, wrelt); tmult (trelb ,wrelt, wrelb); invkin(wrelb, current, near, far, sol) ; END; { Solve } PROCEDURE INIТDATA; BEGIN 11 :=О.5; set up the given data } initialize link lengths 12:=О.5; trelwuser( 1) :=0.1; trelwuser[2] : = О.2; trelwuser(З] : =30.О; srelbuser[ 1] :=-0.1; srelbuser[2] : =0.3; srelbuser(З] : =О.О; UTOI ( srelbuser, srelb); UTOI(trelwuser, trelw); END; { initdata } Main test program } BEGIN initdata: { initialize TOOL and STAТION definitions FOR i:=1 то З DO current[i] :=О.О; V/RITE( 'Саге to da some robot inverse kinematics? '); READLN ( ans); VIНILE ans=' у' DO BEGIN WRIТELN('Enter GOAL: x,y,phi: '); READLN(goal[1],goal[2],goal[ЗJ); { Degrees utoi(goal, trels); Solve ( trels, current, near, far, sol); IF NOT sol THEN WRIТELN ( 'No solution ! '); current: = near; WRПELN( 'The Near solution is:' ) ; FOR i:=1 to 3 do neard[i] :=near[i]*rtod; WRПEVECT(neard); WRПELN; WRПELN( 'The Far solution is: ') ; FOR i: = 1 to 3 do fard(i]: =far[i]'rtod; WRПEVECT(fard); { cneck if inverse of 1vnere WRПELN; where(near, trels); itou(trels, goal); { into user representation WRПELN( 'Calling wnere (near) to cneck . ... ') ; WRIТELN( 'Goal is: '); writevect (goal); WRIТELN; where ( far, t rels); itou(trels, goal); { into user representation WRIТELN( 'Calling wnere(far) to check . ... ·); WRПELN( 'Goal is: '); writevect ( goal); WRIТELN; WRПE ( 'Would you like to do some more? •); READLN(ans); END; END. Programming Exercise (Part 5) program рр5 (INPUT,OUTPUT); CONST рi=Э. 14159; halfpi=1.5707; dtor=O. 0174532925; rtod=57. 29578; ТУРЕ frame = ARRAY[ 1 .. 2, 1 .. 3) OF REAL; vec3 = ARRAY[1 .. 3) OF REAL; mat33 = ARRAY[ 1 .. з, 1 .. З) OF REAL; VAR i: integer; theta: vесЗ; brelw,wrelb: frame; 11,12: REAL; wrelt,trelw: frame; srelb,brels: frame; srelt,trels: frame; trelwuser: vесЗ; srelbuser: vесз; trelsuser: vесЗ i current I near, far: vесЗ; thetadot: vесз; JAGnear,JACfar; mаtЗЭ; vrelt,vrelw: vесз; sol: boolean; ans: char; PROCEDURE WRIТEFRAME (VAR foo: frame); VAR i, j: integer; BEGIN FOR i:=1 ТО 2 DO BEGIN WRПE ('['); FOR j:=1 ТО З DO WRIТE [foo[i,j): 10 WRПELN (')' ) ; END; END; 3); PROCEDURE WRПEVECT (VAR foo: vесЗ); VAR i: integer; BEGIN FOA i:=1 ТО 3 do WRIТELN ('[',foo[i): 10 З,')'); END; FUNCТION АТ2 (а,Ь: REAL): REAL; { 4 Quadrant Аге Tangent } VAR ans: REAL; BEGIN IF Ь=О.О THEN BEG!N IF а=О.О THEN ans:=O.O ELSE IF а>О. О THEN ans: =hal fpi ELSE ans:=- halfpi; END ELSE BEGIN ans: =arctan (а/Ь); { Pascal defined arctan IF Ь<О THEN IF а<О THEN ans:=ans-pi ELSE ans:=ans+pi; END; at2:=ans; END; { atan2 PROCEDURE TMULT (VAR brela,crelb,crela: frame); VAR i,j ,k: INTEGER; BEGIN FOR i:=1 ТО 2 DO BEGIN crela[i,ЗJ : = ьrela[i,31; FOR j : = 1 ТО 2 DO BEGIN crela [ i, З] : =crela( i, З] +brela( i, j] •crelb[ j ,з] ; crela(i,j] := О.О; FOR k:=1 ТО 2 DO crela( i, j] :=crela[ i, j] +brela( i,k] •crelb(k, j]; END; END; END; { tmult } PROCEDURE ТINVERT (VAR brela,arelb: frame); VAR i, j: INTEGER; BEGIN arelb(1, 1] :=brela[1, 1]; arelb[ 1, 2] :=brela[2, 1]; arelb(2, 1] : = brela(1,2]; arelb[2,2J :=brela[2,2]; FOR i:=1 ТО 2 DO BEGIN arelb[i,3] : = О.О; FOR j: =1 ТО 2 DO arelb[ i,З] : =arelb[ i, З] -brela [ j , i] *brela[ j , З]; END; END; { tinvert } PROCEDURE KIN (VAR theta: vесз; VAR wrelb: frame); VAR beta: real; BEGIN wrelb [ 1, З) :=l Pcos (theta [ 1 J) + 12•cos (theta[ 1 J +theta( 2 J) ; wrelb (2, З] : =11 • sin (theta [ 1]) + l2*sin ( theta[ 1] +theta (2]); beta: =theta ( 1] +theta ( 2] + theta ( З] ; wrelb[1, 1] :=cos (beta); wrelb ( 1, 2] : =-sin (beta); wrelb(2, 1] :=-wrelb( 1, 2]; wrelb(2,2] :=wrelb[1, 1]; ENO; PROCEDURE UTOI (VAR uform: vесЗ; VAR iform: frame); BEGIN iform(1,3] :=uform( 1]; iform(2,З] : = uform(2]; iform[1, 1] : = cos (uform(З]•dtor); iform[1,2] :=-sin (uform[З] *dtar); iform(2, 1] : = -iform[ 1, 2]; iform[2, 2] :=iform( 1, 1]; ENO; PROCEDURE IТOU(VAR iform: frame; VAR uform: vесз); BEGIN uform[1] : =iform[1,3]; uform[2J :=iform[2,ЗJ; uform[ЗJ :=at2(iform[2, 1] ,iform[1, 1} )•rtod; ENO; function dist(VAR а,Ь: vесЗ): real; VAR d,t: REAL; i : INTEGER; BEGIN d: =O.O; FOR i: =1 ТО 3 00 BEGIN t: = abs(a[ i]-b[ i]); d:=d+t; END; dist : = d; END; { dist PROCEDURE range(VAR а: REAL); BEGIN WHILE a>pi 00 а :=a-2 "" pi; WНILE а< -pi DO a:=a+2*pi; END; { range } PROCEDURE invkin (VAR wrelb: f rame; VAA current, near, far vесЗ; VAR sol: boolean); LABEL 5; VAR c2,s2,k1,k2,temp: REAL; i: INTEGER; goal, swap: vесз; solnear, solfar : BOOLEAN; BEGIN itou(wrelb.goal); goal[ ЭJ : =goal{ э J *dtor; с2: = (goal [ 1] •goal [ 1] +goal [ 2] •goal[ 2] -11 *11-12*12) / (2. 0*11 *12); IF abs(c2)>1.0 THEN sol:=FALSE ELSE sol:=TRUE; IF NOT sol THEN GOTO 5; s2:=sqrt(1.0-c2*c2); near[2J : = at2 (s2,c2); far[2] :=-near[2); k1:=11+12*c2; k2 :=12•s2; temp:=at2 (k2,k1); near[ 1] : =at2(goal [ 2) , goal [ 1 J )-temp; far[ 1] : =at2 (goal [ 2), goal [ 1 ] ) +temp; near[Э) :=goal[ЭJ-near[ 1 J-near[2]; far[ э] :=goal[ э J -fa r[ 1 J -far[ 2) ; FOR i: = 1 ТО Э DO BEGIN range(near[il); range ( far [ i]); END; IF dist(current,near)>dist(current,far) THEN BEGIN swap:=near; near: =far; far:=swap; END; solnear : =TRUE; FOR i:=1 ТО Э DO BEGIN IF (ABS(near[i])>2.96) THEN solnear: =FALSE j END; solfar := TRUE; FOR i: = 1 ТО Э DO BEGIN IF ( ABS(far[i])>2.96 THEN sol far : =FALSE; END ; IF ( (NOT solnear AND (NOT solfar ) ) THEN sol: =FALSE ; IF ( solnear AND NOT sol far)) THEN far: = near; IF ( solfar AND ( NOT solnear ) ) THEN near :=far ; 5: END; ( invkin PROCEDURE SOLVE(VAR trels:frame; VAR cu rrent, near, far: vесЗ i VAR sol: boolean) j VAR trelb,wrelt,wrelb: frame; BEGIN tmult ( srelb, t rels, trelb); tinvert(trelw,wrelt); tmult (trelb, wrel t, wrelb); invkin (wrelb, current, near, far, sol) ; END; ( Solve } PROCEDURE WHERE (VAR theta: vесЗ; VAR trels: frame); VAR trelb,wrelb: frame; BEGIN КIN (theta,wrelb}; TMULт (wrelb, trelw. trelb); TMULT (brels,trelb,trels); END; PROCEDURE VELTRANS (VдR brela: frame; VAR vrela,vrelb: vесЗ); VAR w,px,py: real; BEGIN w:=vrela(З); { planar case,w is invariant } px:=brela(1,ЗJ; { рх and ру are the org. of в wrt А } py: = brela(2,3]; vrelb( 1) :=brela[ 1 , 1] • (vrela ( 1 J-py*w) +brela [ 2, 1] • (vrela [ 2] +px•w) ; vrelb( 2) : =brela( 1 , 2) • (vrela ( 1 J-py•w) +brela [ 2, 2] • (vrela ( 2) +px•w); vrelb( З] :=w; { equation implemented is: В А Т д V R* (V + w•(-py]) В В А (РХ] END; PROCEDURE JдCOBIAN (VдR theta: vесз; VAR JAC: mat33); VAR sЗ,сЗ 1 s23,с2З: real; BEGIN s3: = sin (theta(З]); сЗ :=cos (theta(З]); s23: = sin (theta[2] + theta(ЗJ); с2З:=соs (theta[2] + theta[ЗJ); JAC(1,1]:=12*s3 + 11*s23; JAC[1,2] =12*sЗ; JAC(1,3] = О.О JAC[2,1]:=12*c3 + 11 "' с2З; JAC(2,2J =12-сз; JАС[2,З] =О.О JАС[З,2] =1.0; JАС(З,3] =1.0 JАС(З,1]: = 1.0; END; PROCEDUAE VMULT (VAR М: mаtзз; VAR х,у: vесЗ); VдR i,j: integer; BEGIN FOR i:=1 ТО 3 DO BEGIN y[i] :=О.О; FOR j :=1 ТО З DO y[i]: = y(i] + M(i,j]'x(j]; END; END; PROCEDURE INIТDдTA; BEGIN 11 : = О.5; { Set up the given data } {у= м•х 12: = О.5; trelwuser[ 1] trel1vuser[2] trel1vuser[З] srelbuser[ 1] srelbuser[2] srelbuser[З] : = О.1; :=О.2; :=30.О; : = О.О; :=О.О; : =о.о; UTOI (srelbuser,srelb); ТINVERT (srelb,brels); uтor (trelwuser, trelw); ТINVERT (trelw,wrelt); END; { initdata } BEGIN { Main Test Program } INIТDATA; VIRIТE ( 'Care to do some robot kinematics? '); READLN ( ans); WHILE ans=' у• 00 BEGIN WRIТELN ( 'Enter cur pos (theta1, theta2,theta3):'); READLN (current[1 J ,current[2I ,current[З]); FOR i: = 1 to 3 do current ( i] : =current [ i] *dtor; WRIТELN ('Enter tool rel station (x,y,theta):'); READLN (trelsuser( 1], trelsuser(2] , trelsuser( 3]); UTOI (trelsuser,trels); SOLVE (trels,current,near,far,sol); IF sol THEN BEGIN WRIТELN ( 'The 2 solutions are (in radians): '); WRIТEVECT ( near) ; \VRIТELN ( 'and: • ) ; WAIТEVECT (far); WRIТELN ( 'Enter j oint vel (thetadot1 , 2, 3) : '); READLN (thetadot ( 1] , thetadot ( 2] , thetadot [ 3]); FOR i: = 1 ТО 3 00 thetadot ( i]: =thetadot [ i] *dtco; WRIТELN; JACOBIAN ( near, JACnear); { j acobian for the пеаr sol } VMULT (JACnear,thetadot,vrelw); VELTRANS (trelw,vrelw,vrelt); WRIТELN ( 'The tool vel for the near soln is: ') ; vrelt(З] :•vrelt[З]*rtod; WRIТEVECT (vrelt); IVRIТELN; JACOBIAN (far,JACfar); { jacobian for the far sol } VMULT (JACfar,thetadot,vrelw); VELTRANS (trelw, vrelw, vrelt); IVRIТELN ( 'The tool vel for the f ar soln is: '); vrelt[З] :=vrelt[З]*rtod; { convert to degrees/sec \VRIТEVECT [ vrelt) ; END ELSE WRIТELN ( 'Sorry, no sol for those coords. '); WRIТE ( 'Would you like to do some more?'); READLN ( ans) ; END; Programming Exercise (Part 6) PR0GRAM ррб ( INPUT, OUTPUT) ; C0NST dtor=0. 0174532925; rtod = 57. 29578; 11 12 m1 m2 тз о.5; О.5; 4.6; 2.3; 1.0; Izz = 0.1 i g = 9.8; { Mass of the links } { Moment of inertia for link З about Z } { Gravity } intstep = О.005; { Integration Step Size (seconds) } ТУРЕ vесз = ARRAY [ 1 .. 3) 0F REAL; mаtЗЗ = ARRAY( 1 .. 3, 1 .. 3) 0F REAL; VAR Ь: vесЗ; { viscous friction coefficients } theta, thetadot 1 thetadd : vесЗ j { pos. , vel. , асс. m: mat33; { manipulator mass matrix ) minv: mаtЗЗ; { inverse of mass matrix } v: vесЗ; tau: vесз; { vector of dynamic terms } { vector of j oint torques } s1,с1 1 s2,c2,s12,c12: REAL; i,j: INTEGER; len: REAL; { total length of time to simulate prnt: REAL; { how often to print {sec.) } time: REAL; ans: char; { time into simulation } Procedure inv33(VAR a,ainv: mаtЗЗ); { а is non-sing,sym } VAR det: real; BEGIN det:=a[1, 1 )*а(2,2)*а(З,З]+а[1,2]*а[2,З]'а[З, 1]; det : =det+a[ 1,З) •а[2, 1) •а [3,2); det :=det-a( 1,З) •а[2,2) *а[З, 1)-а ( 1,21 •а)2, 1 J *а[З ,ЗJ; det :=det-a[ 1, 1] •а[2,З] •а[З ,2); ainv( 1, 1) :=(а[2,2) 'а(З,3]-а(2,З] •а[3,2)) /det; ainv( 1,2) : = (а( 1,3) •а(З,2)-а[ 1 ,2) •а(З ,З)) /det; ainv[ 1,З] :=(а[ 1 ,21 •а[2,ЗJ-а[ 2, 2 [ •а[ 1 ,З]) /dot; ainv(2,2) :=(а(1, 1 J •а[З,3J-а(3, 11•а11,ЗJ) /det; ainv[2,ЗJ := (а(2, 1) 'а( 1,з )-а( 1, 1) •а[ 2,3)] /det; ainv[З ,З) := (а( 1, 1) •а[2,2)-а(1,2) *а(2, 1)) /det; ainv(2, 1 J :=ainv[ 1,2]; ainv[З,1) :•ainv(1,З]; ainv[З,2] :=ainv(2,3]; END; { invзз } PR0CEDURE trig; BEGIN s1: = sin (theta[1)); c1:=cos (theta(1)); s2: = sin (theta[2I); c2: = cos (theta(2J); s12:=sin (theta(1]+theta[2J); c12: = cos (theta(1 J+theta[2)); END; PROCEDURE calcm(VAR m: mаtЗЗ); VAR m2З: REAL; { Calculate the М Matrix ) BEGIN m23: =m2+m3 i m ( 1 , 1] : =12* 12*m23+2*11 * 12*m2З"с2+ 11•11 • (m1 +m23 )+ Izz; m [ 1 ,2] : =12* 12*m23+ 11 *12*m23*c2+Izz j m(1,З] : = Izz; m(2,1J:=m(1,2J; { М is symmetric} m(2,2] :=12*12*m23+Izz; m(2,З] : = Izz; m(З,1J: = m(1,ЗJ; m(З,2] : = m[2,З]; m(З,З] :=Izz; ENO; { м is symmetric { М is symmetric PROCEDURE calcsdt(VAR v: vесЗ); Calculate V } VAR m2З: REAL; { b(i] are viscous friction coefficients. BEGIN m2З: ::;m2+mз; v( 1 J :=-m2з•11 •12•s2•thetadot(2J *thetadot(2]; v ( 1] : =v [ 1 J-2*m23*11 '12'thetadot [ 1] *thetadot 12] ; v 11 J : =v 11] +m2з•12•g•c12+ (m1+m23) •11 •g•cl; v 1 1 J : =v 11 1 +Ь 1 1 ) *thetadot [ 1 J ; v [ 2] : =m23*11 •12•s2•thetadot [ 1) •thetadot ( 1) ; v ( 2) : =v ( 2] +m23*12'g*c12+Ь (2] *thetadot [ 1 J ; V[З] :=b(З]*thetadot[ЗJ; ENO; PROCEOURE VMULT (VAR м: mаtзз; VAR х,у: vесЗ); VAR i,j: integer; BEGIN FOR i: =1 ТО З 00 BEGIN y(i] :=О.О; FOR j: = 1 ТО З 00 Yli]:=y[i] + M(i,j]*x[j]; END; ENO; PROCEDURE update (VAR tau: vесз; VAR period: REAL; VAR theta, thetadot: vесЗ); VAR i, j, k: INTEGER; temp: vесЗ; BEGIN j :=round(period/instep); FOR i:=1 ТО j 00 BEGIN trig; calcm(m); invЗЗ(m,minv); calcsdt (v); do trig functions on current angles } calculate mass matrix } { invert mass matrix } { calculate velocity ,grav., frictian } FOR k:=1 то З DO temp[k) : =tau [k)-V[k]; vmult (minv, temp, thetadd); { compute j oint acceleration FOR k:=1 ТО 3 00 { do integration step } BEGIN theta [ k] : =theta [ k] +thetadot [ k] • intstep +о. s• intstep*intstep•thetadd [ kJ ; thetadot [ k): =thetadot [ k] +thetadd [ k] • intstep; ENO; END; END; { u pdate BEGJN { Main. } ans:='y'; WHJLE ans = 'y' 00 BEGIN WRIТELN( 'Want to simulate joint friction?'); REAOLN ( ans) ; IF ans= 'у' THEN FOR i:=1 ТО Э 00 b[i] :=5.0 friction } ELSE FOR i:=1 то э оо b[i] :-о.о; { по friction \VRПELN( 'How long to simulate?'); REAOLN ( len) ; WRIТELN( 'How often to print? (sec.)'); REAOLN ( prnt); WRIТELN( 'Enter initial joint positions (degrees):'); REAOLN (theta [ 1), theta[ 2], theta [Э] ) ; FOR i:=1 то э 00 theta[i] :=theta [i]*dtor; FOR i:=1 ТО Э 00 tau[i) :=О.О; FOR i:=1 то З 00 thetadot[i] : = О.О; time:=o.o; j :=round(len/prnt); WRIТELN(' Тime Joint 1 Joint 2 Joint Э'); FOR i:=1 ТО j 00 BEGIN update (tau, prnt, theta, thetadot); time: =time+prnt; writeln (time:9:2. theta [ 1) •rtod: 12 :Э, theta [2]*rtod:12:Э,theta [ЭJ•rtod:12:Э); ENO; WRIТELN( 'Want to Simulate the Robot?'); REAOLN ( ans) ; END; END. Progra111mi11g Exercise (Part 7) program рр7 ( INPUT, OUTPUT); CONST рi=Э.14159; halfpi=1 .5707; dtor=O. 0174532925; rtod=57 .29578; ТУРЕ frame = ARRAY(1 .. 2, 1 .. Э) OF REAL; vесЭ = ARRAY[1 .. Э] OF REAL; vec4 = ARRAY[1 .. 4] OF REAL; pntlist = ARRAY[1 .. 5) OF vесэ; seglist = ARRAY[1 .. 5, 1 ..З] of veC4; VAR i,j: integer; theta: vесэ; brelw, wrelb: frame; 11,12: REAL; wrelt, trelw: frame; srelb, brels; frame; srelt,trels: frame; t relwuse r: vесЭ; srelbuser: vесЭ; trelsuser: vесз; place, current, far: vесЗ; npnt: integer; viapnt, viavel: pntlist; path: seglist; trajectory: array[1 .. 300] of vесЗ; deltat.s: realj nticks: integer; sol: boolean; ans: char; PROCEDURE WRIТEFRAМE (VAR foo: frame); VAR i, j: integer; BEGIN FOR i: = 1 ТО 2 DO BEGIN WRIТE (' [ ') ; FOR j : = 1 ТО З DO IVRIТE (foo[ i, j]: 10 : З); WRIТELN ( '] ' ) ; END; END; PROCEDURE WRIТEVECT (VAR foo: vесЗ); VAR i: intege r; BEGIN FOR i:=1 ТО З do WRIТELN ('[',foo[i]: 10 END; 3,']'); FUNCТION АТ2 (а,ь: REAL): REAL; VAR ans: REAL; BEGIN IF Ь=О.О THEN BEGIN IF а =О.О THEN ans: = O.O ELSE IF а>О,0 THEN ans :=halfpi ELSE ans: = -halfpi END ELSE BEGIN ans: =arctan ( а/Ь); IF IF END; Ь<О THEN а<О THEN ans: ;;ans-pi ELSE ans: ;;ans+fpi; at2:=ans; END; { atan2 PROCEDURE TMULT (VAR brela,crelb,crela: frame); VAR i, j ,k: INTEGER; BEGIN FOR i : = 1 ТО 2 DO BEGIN crela[i,ЗJ :=brela[i,3]; FOA j:=1T02D0 BEGIN crela [ i ,3] : =crela [ i, з] +Ьrela ( i, j] •crelb( j , з]; crela[i,j]: = о.о; FOR k: =1 ТО 2 DO crela( i, j] :=crela[ i, j] +brela( i,k] •crelb[k, j]; END; END; END; { tmult PROCEDURE ТINVERT (VAR brela,arelb: frame); VAR i, j : INTEGER; BEGIN arelb[1, 1] : = brela[1,1]; arelb[1,2] :=brela[2, 1]; arelb[2, 1] :=brela[1,2); arelb[2,2] : = brela[2,2]; FOR i: = 1 ТО 2 00 BEGIN arelb[ i,З] : =О.О; FOR j: =1 ТО 2 00 arelb[ i, З] : =arelb[ i, 3) -brela[ j , i] *brela [ j ,з] ; END; END; { tinvert } PROCEDURE KIN (VAR theta: vесЗ; VAR wrelb: frame); VAR beta: real; BEGIN wrelb[1,3]:=11 •cos (theta [1))+12 •cos (theta [1]+theta [2]); wrelb[2,ЗJ:=11 *sin (theta [1))+12 *sin (theta [1]+theta [2)); beta: =theta [ 1) +theta [ 2) +theta [З] ; wrelb[1, 1 J : = cos (beta); wrelb[1,2] : = -sin (beta); wrelb[2, 1] :=-wrelb[1,2]; wrelb[2, 2] :=wrelb[ 1, 1]; END; PROCEDURE UTOI(VAR uform: vесз; VAR iform: frame); BEGIN iform[1,3] : = uform[1]; iform[2,ЗJ :=uform[2); iform[1, 1) :=cos (uform(ЗJ *dtor); iform[1,2] :=-sin (uform[З] *dtor); iform[2, 1) :=-iform[ 1,2); iform[2,2] :=iform[ 1, 1]; END; PROCEDURE IТOU(VAR iform: frame; VAR uform: vесз); BEGIN uform[ 1) :=iform( 1,З]; uform[2] :=iform(2,ЗJ; uform[З]: =at2 ( iform[2, 1] , iform( 1, 1 ) ] • rtod; END; PROCEDURE range ( VAR а: REAL); BEGIN WHILE a>pi 00 а:=а-2 *pi; WHILE а< -pi 00 а:=а+2 *pi; END; { range } PROCEDURE invk in (VAR wrelb: frame; VAR current,near,far:vecЗ; LABEL 5; VAR sol: boolean]; VAR c2,s2,k1,k2,temp: REAL; i: INТEGER; goal, swap: vесЗ; BEGIN itou(wrelb,goal); goal[ЗJ :=goal[ЗJ •dtor; c2:=(goal(1] •goal(1J+goal(2) •goal(2]-11 *11-12 *12)/(2.О *11 *12); IF abs (с2)>1.О THEN sol: = FALSE ELSE sol: = TRUE; IF NOT sol THEN GOTO 5; s2: =sqrt ( 1 . О-с2 *с2] ; near(2] :=at2(s2,c2); far(2] : = -near[2]; k1 :=11+12 *с2; k2: = 12 *s2; temp: = at2(k2,k1); near [ 1] : = at2( goal[ 2] ,goal [ 1 ] ) -temp; far[ 1] : = at2(goal[2) ,goal[ 1) )+temp; near[З) :=goal[З)-near[1 ]-near[2]; tar[ЗJ :=goal[ ЗJ -far( 1] -far( 2]; FOR i: = 1 ТО 3 00 BEGIN range(near[i]); range(far[i)); END; IF dist ( current, near )>dist (current, far) THEN BEGIN swap:=near; near: =fаг j far:=swap; END; 5: END; { invkin } PROCEDURE SOLVE ( VAR trels: frame; VAR current,near,far:vecз; VAR sol: boolean); VAR trelb,wrelt,wrelb: framej BEGIN tmult (srelb, trels, t relb} ; tinvert(trelw,wrelt) j tmult (trelb,wrel t, wrelb); invKin (wrelb, current I near I far, sol); END; { Solve } PROCEDURE WНERE(VAR theta: vесз; VAR trels: frame): VAR trelb,wrelb: frame; BEGIN KIN(theta,wrelb); TMULT(wrelb,trelw,trelb); TMULТ (brels, trelb, t rels) ; END; PROCEDURE INIТDATA; BEGIN s: "'О. 3333; deltat: = 0.2; Set up the given data set time scaling constant 11 : =О. 5; ini tialize link lengths 12: = o.s; trelwuser[1] :=О.1; trelwuser(2] : = О.2; trelwusor(З] :=30.0; srelbuser( 1] :=О.О; srelbuser[2] : = о.о; srelbuser(З] :=О.О; UTOI(srelbuser,srelb); ТINVERT(srelb,brels); uтor (trelwuser, trelw]; ТINVERT( trelw, wrelt); END; { initdata } { This is done differently than in the text. In this program а time scale factoг is used instead of giving the total time for а cubic segment. Its really simpler to do it j ust as in the text, Ьut this scaling business has some benefits too.} PROCEDURE CUBCOEFF (VAR thO' thf 'thdotO' thdotf: real; VAR се: vec4); BEGIN сс[11 :=thO; сс[2] :=thdotO/s; сс[З] :=З • (thf · thO) • 2 •thdotO/s • thdotf/s; сс[4] :=-2 • (thf • thO] + thdotf/s + thdotO/s; END; PROCEDURE JOINTVEL (VAR vjapnt: pntlist; VAR npnt: integer; VAR viavel: pntlist); { Compute the joint velocities at the via points } i,j: integer; VAR BEGIN for j: =1 to З do begin viave1[1] [ j] :=О.О; viavel[npnt][j] : =О.О; end; if npnt > 2 then begin for i :=2 to npnt - 1 do for j: =1 to З do viavel [ iJ[ j] :=О. 5 • ( (viapnt[ i] [ j] -viapnt[i-1 ][ j]} + (viapnt[i+1] [ j] -viapnt[i] [ j]}) •s; The velocity chosen for each via point is the average of the velocities for each path segment if all the via points were connected with straight lines. See chap. 7. } end; END; PROCEDURE RUNPATH (VAR path: seglist; VAR npnt: integer}; VAR k,i, j :integer; time, tprime: real; BEGIN time:=o.o; i:=1; nticks:=round((npnt • 1} *3/deltat}; writeln (' p ath in cartesian space (time,x,y,phi):'); for k: =1 to nticks do begin time:=time + deltat; tprime:=time •s; tor j :•1 to з do theta [j]:=path(i,j][1) + path (i,jJ[2) •tprime + path )i,j](Э) • sqr (tprime)+path)i,j)l4] • sqr (tprime) •tprime; WHEAE (theta,trels); поu (trels,trajectory [k)); write (time: 5 : 2); for j:=1 to Э do write (trajectory)k][j):1O:Э}; writeln; if time>З. о then Ьegin time: =О, о; { reset time each segment } i:=i+1; end; end; END; { here' s а hack to plot the path оп а ТТУ } PROCEDURE plotpath; var i,k,j: integer; xscale, yscale: real; ch: array[O .. Э] of char; grid: array[1 .. 60,1 .. 80] of char; begin xscale:=30.0; yscale:=22.5; ch[OJ: = '<'; ch [1]: = 'V'; ch[2]: = '>'; ch[ЗJ: = 't'; tor i: =1 to 60 do for j :=1 to 80 do grid[i,j] : = ' '; for j:=1 to 80 do grid [31,j]:='•'; for i:=1 to 60 do grid [i,40] : = '1 '; for k:=1 to nticks do begin i:=31-round (trajectory[k][2J • yscale); j :=round(trajectory[k][1] • xscale)+40; grid[i,j] :=ch[ (round(trajectory[k] [3]+225) div 90) mod 4]; end; for k: =1 to прпt do begin where (viapnt[KJ,trels); itou (trels,place); i:=31 -round (place[2] • yscale); j :=round (place[1] • xscale)+40; grid(i,j] := 1 end; for i:=1 to 57 do begin for j :=1 to 78 do write(grid[i,jJ); writeln; endj end; BEGJN { Main Test Program } INIТDATA; WRIТE ( • Care to plan some swell robot paths? ') ; READLN (ans); WHILE ans='y' DO BEGIN npnt: = 1; for i: = 1 to 3 do current[i] :=О.О; enter the path via points and convert to joint angles } WRIТELN ('Enter the initial position (x,y,phi): '); READLN (place [ 1], place [ 2], place [ 3]); UTOI (place,trels); SOLVE (trels I current, viapnt ( npnt J, far, sol); while sol do begin npnt: =npnt+1 ; writeln ('Enter the next via point (x,y,phi): '); writeln ( 1 (enter а point with по sol to terminate) '); readln (place [1],place[2],place[ЗJJ; UTOI ( place, trels); SOLVE (trels, viapnt [ npnt-1] J viapnt{ npnt] , fаг, sol); епd; npnt: =npnt -1 ; JOINTVEL (viapnt, прпt, viavel); for i:=1 to npnt-1 do for j : =1 to э do CUBCOEFF (viapnt [i] [ j], viapnt[ i+1][ j], viavel[ i] [ j], viavel[i+1] [ j] ,path[i, j]); RUNPATH(path,npnt); PLOTPATH; WRПE ( 'Would you like to do some more? '); REAOLN ( ans) ; END; END. Programmi11g Exercise (Part 9 a11d 10) PROGRAМ pp8([NPUT,OUTPUT); CONST dtor=O. 0174532925; rtod=57. 29578; 11 12 0.5; о. 5; m1 4. 6; mЗ 1.0j m2 Mass of the links 2.3; { Moment of inertia for link З about Z Izz = О .1; { Gravity } g = 9.8; intstep = 0.005; { Integration Step Size (seconds) ТУРЕ vec3 = ARRAY( 1. ,3] OF REAL; mat33 = ARRAY[1 .. 3,1"3] OF REAL; pathtype = ARRAY[1 .. 3, 1 .. 3, 1 .. 4] OF REAL; VAR { viscous friction coefficients Ь: vесз; theta, thetadot, thetadd : vесЗ; m: mat33; { manipulator mass matrix } minv: mаtзз; { inverse of mass matrix } { vector of state dependent dynamic terms } v: vесЗ; tau: vec3; { vector of j oint torques } s1, с1, s2, с2, s12, с12: REAL; i, j: INTEGER; len: REAL; { total length of time to simulate { how many ticks between printing nprnt: INTEGER; serper: REAL; { servo period } time: REAL; { time into simulation ans: char; pos, vel, асс: vесЭ; { desired path { gains kp, kv; vесЗ; stepans, dynans: char; tauprime, temp: vесз; { model of mass matrix used in control law mm: mаtЗЗ; vm: vесз; path: pathtype; { storage of сuЫс coefficients Normally, this routine would do the path planning based оп user input. Неге, for simplicity, I have the constants from the last homework precomputed. Not elegaпt, but i t was easiest. . . } PROCEDURE BEGIN path [1, 1, path[ 1, 1, path[ 1, 1, path[1, 1, 1] 2] 3) 4] := := := :• path[1,2, path[1,2, path[1 ,2, path(1,2, 11 2] З] 4] := -1.920; := О.ООО; : = 10.328; := -6.994; path[1,3, path[1,3, path[1,3, path(1,3, 1] 2] З] 4] := 0.349; := О.ООО; : = -0.204; : = 0.161; initpath; 1.047; О.ООО; -5,673; 3.167; path[2,1, 1] : = -1.459; -1.845 ·1.114 1.776 path(2, 1, 2) path(2,1, З] path[2, 1, 4) path(З, 1, path(З, 1, path(З, 1, path(З, 1, 1) := 1.415; 2) : = -0.324; З) := ·9.633; 4] := 5 .974; 11 := 0.306; 2] := 0.075; З) := 0.407; 4] :- -0.289; 11 : = -2.643; 2) : = 1 .253; 3) := 8 .563; 4) := -6.127; path[З,2, path(3,2, path[З,2, path(З,2, 11 2) 3) 4] раth(З,З, path(3,3, path(3,З, path(3,3, 11 : = 0.499; 2) := 0.022; 3) := -0.491; 4) : = 0.320; path[2,2, path[2,2, path(2,2, path(2,2, path(2,З, path(2,З, path(2,З, path(2,З, END; := -2.568; := • 1.667; : = 5.279; := -2 .964; { initpath PROCEDURE t r ig; BEGIN s 1 : =sin ( theta [ 1 J); с 1 : =cos ( theta [ 1 ] ) ; s2:•sin(theta[2J); c2:=cos(theta[2]); s12: = sin(theta(1 )+ theta[2J); c12:=cos(theta( 1)+ theta[2)); END; PROCEDURE calcm(VAR m: mаtЗЗ); VAR m2З: REAL; BEGIN m23: =m2+m3; m [ 1, 1] :=12* 12*m23+2* 11 •12•m2з•с2+11 •11 • (m1 +m2З )+Izz; m[ 1 ,2] ::::::12*12*m2З+11 *12*m23*c2+Izz; m(1,3J : = Izz; m12, 1 J : =m[ 1,21; m (2, 2) :=12' 12'm2З+Izz; m(2,3] :=Izz; m(3,1):=m[1,ЗJ; m(З,2] :=m[2,3J; m(3,3J : = Izz; END; М is symmetric М is symmetric М is symmet r ic PROCEDURE calcsdt(VAR v: vec3); VAR m23: REAL; BEGIN m2З: =m2+mз; v (1 J : =-m2з• 11 '12*thetadot [21 *thetadot [ 2) ; v( 1 J : =v(1 J -2*m23*11'12*s2*thetadotI1 J *thetadot (21; v[ 1) : =v[1) +m23' 12•g•c12+ (m1+m23) *11 •g•c1 ; v[ 1) :=v( 1)+Ь[ 1) *thetadot [ 1]; v[21 : =m23' 11 *12*s2*thetadot [ 11 *thetadot [ 1 J ; v[2) :=v[2J+m2э•12•g•c12+b[2)*thetadot[2); v[3) :=Ь[3) 'thetadot[3); END; PROCEDURE VMULT(VAR м: mаtзз; VAR х, у: vесЗ); VAR i,j: integer; BEGIN FOR i:;1 ТО З DO BEGIN y(i] ;;О.О; FOR j : ;1 ТО З DO y(iJ:;y[i] + M(i,j]*x[j]; END; END; PROCEDURE vadd(VAR а,Ь,с: vесЗ); { finds с VAR i: INTEGER; BEGIN FOR i: ; 1 то З DO c[i] ;;a[i]+b(i]; END; { vadd ) а+Ь ) PROCEDURE update(VAR tau: vесз; VAR period: REAL; VAR theta, thetadot: vесЭ) ; VAR i,j ,k: INTEGER; temp: vесз; BEGIN j : ;round ( period / instep); FOR i:;1 ТО j DO BEGIN trig; calcm(m); invЗЗ (m, minv); calcsdt ( v); FOR k:;1 ТО Э DO temp(k] ;;tau[k]-v[k]; vmult (minv, temp, thetadd); FOR k:;1 ТО Э DO BEGIN theta [ k] : ;theta[ k] +thetadot ( k] • instep +О. 5* intstep• intstep'thetadd [ k]; thetadot( k] : ;thetadot ( k] +thetadd ( k] • intstep; END; END; END; { update ) Procedure step: { Generate Step input at time BEGIN IF time >; 3.00 THEN pos[2J:;-50.0*dtor; END; PROCEDURE spline; { Generat:e Cubic Spline Path З .о ) VAR i, j: INTEGER; { scaled time ( always [О, 1]) tim: REAL; BEGIN IF time < 9.0 THEN BEGIN I F t ime>6. 00 THEN i; ;3 ELSE IF time>З.00 THEN i:;2 ELSE i:;1; t1m:;(time-(i-1 )'З.0)/З.о; FOR j :;1 ТО З DO BEGIN pos( j] ;;path( i, j, 1 J+(path [i, j, 2] + (path( i, j ,З] +path [ i, j ,4] *tim) *tim) *timj vel(j] :;path( i, j ,21 + (2. O*path( i, j ,З] +З. O*path [ i, j ,4] *tim) *tim; vel[j J :;о .ззззз•vеl [ j J; асс[j J :;2 .O*path[ i, j ,ЗJ +6.0'path[ i, j ,4] 'tim; асс [ j J : =О. зззз•о .ззззз•асс [ j J ; END; END; END; { spline } PROCEDURE servo; { tauprime based оп desired path and errors } BEGIN FOR i: = 1 ТО З DO BEGIN taupr ime [ i] : =асс [ i] +kv[ i] • ( vel [ i] -thetadot [ i]) +kp[ i] • (pos [ i] -theta[ i]); END; END; { servo } BEGIN initpath; ans:='y'; Main. plan the splined path } WНILE ans=' у. DO BEGIN WRIТELN ( 'How long to simulate?' ) ; READLN(len); WRIТELN ( 'Print position every N servo periods ... N=?'); READLN(nprnt); WRIТELN ( 'Spline path or Step? (У = Step).); readln ( stepans) ; writeln( 1 Use dynamic compensation?'); readln(dynans); WRIТELN ( 'Enter velocity gains: kv [ i] (З vals) : '); READLN(kv[ 1], kv[2], kv[З]); IF dynans = 'у' THEN BEGIN kp[1]: = 175.О; kp[2]:=11O.O; kр[З]:=2O.О; END ELSE BEGIN kp(1] :=1OO.О; kp[2] :=1OO.О; kр[З] :=1OO.О; END; theta[1] :=6O.О; theta[2] :=-11O.О; theta[З] :=2O.О; FOR i:=1 то з оо theta[i] :=theta[i] 'dtor; FOR i:=1 ТО 3 DO BEGIN pos[i] :=theta[i]; { initialize desired path } vel[i] : = О.О; acc[i] :=О.О; END; FOR i:=1 то 3 DO thetadot[i] :=О.О; { friction } FOR i:=1 то З DO b(i) :=s.o; time: •О, Oj serper: =О. 01; j : = round(len/serper); WRIТELN (' Тime Joint Joint 2 Joint 3') ; writeln (time:9:2. theta[ 1 J •rtod: 12:З, theta[2J •rtod: 12:з, theta[ЗJ •rtod: 12:З); FOR i:=1 ТО j DO BEGIN IF stepans=' у' THEN step ELSE spline; servo; { Uses gains and errors to compute tauprime } IF dynans=•y• THEN BEGIN calcm(mm); calcsdt(vm); vmult (mm, tauprime, temp); vadd{temp, vm, tau); END ELSE tau: =tauprime; { No dynamics used } update (tau, serper, theta, thetadot); time: =time+serper; IF (i MOD nprnt)=O THEN BEGIN writeln ( time: 9: 2, theta [ 1] * rtod: 12: З, theta(2) •rtod: 12:З, theta[З] •rtod: 12 :З); END; END; IVRIТELN( 'Want to Simulate the Robot?'); READLN ( ans) ; END; Programming Exercise (Part 11) PROGRAМ sm9 ( INPUT, OUTPUT) ; CONST dtor=O, 0174532925; rtod=57. 29578; 11 0.5; 12 0.5; m1 = 4. 6; 2.з; m2 mз 1 .о; Izz = 0.1; g = 9.8; intstep = о. 005; ТУРЕ vесЗ = ARRAY[ 1 ..З) OF REAL; mаtЗЗ = ARRAY[1 .. 3.1 .. З) OF REAL; frame = ARRAY[1"2.1"3] OF REAL; pathtype = АRRАУ[1"З,1"З,1"4] OF REAL; VAR Ь: vесз; theta, thetadot, thetadd: vесЗ; m: mаtзз; minv: mаtЗЗ; v: vесэ; tau: vесз; s1 ,с1 ,s2,c2,s12,c12: REALj i,j: INTEGER; len: REAL; nprnt: INТEGER; serper: REAL; servo period time: REALj time into simulation ans: char; pos, vel, асс: vесз; desired path kp,kv: vесЭ; stepans, dynans: char; tauprime, temp: vесз; fes: vесз; stored force acting оп hand fe: vесз; actual force acting оп hand eft: vесЗ; torque due to contact forces wrelb: frame: wrist rel base kpx: vесз; desired Cartesian sti ffness torques due to gravity gravtor: vесз; PROCEDURE tгig; BEGIN s1 :=sin(theta[1J); с1 :=cos(theta(1]); s2:=sin(theta[2)); c2:=cos(theta{2J); s12: =sin (theta { 1] +theta (2)); с12: =cos (theta { 1) +theta ( 2)); ENO; PROCEDURE calcm(. VAR m: mаtЗЗ); VAR m23: REAL; BEGIN m2З: =m2+mэ; m 11 , 11 : =12* 12•m23+2•11* 12*m23*c2+ 11 *11* (m1 +m2З )+Izz; m ( 1, 2] : =12*12*m2З+ 11 *12*m2Э*c2+Izz; m( 1,3] : = Izz; m(2,1]: = m{1,2J; m(2,2) :=12*12*m2З+Izz; m(2,З) :=Izz; m(З, 1 J : = m( 1,3); m(З,2) :=m(2,Э); m{З,З) :=Izz; EN0; PROCEDURE calcsdt(VAR v: vесЗ); VAR m2Э: REAL; BEGIN m23: =m2+m3; v { 1) : =-m23*l 1 *12*s2*thetadot{ 2) *thetadot { 2] ; v { 1) : =v ( 1J-2*m23*l 1 •12•s2•thetadot { 1) *thetadot { 2]; v { 1 J : =v { 1J+m2з•12•g•c12+ (m1+m23) • 11 •g•c1; v{ 1] :=v{1 )+Ь{1 J*thetadot{1]; v{2) :=m23*11*12*s2'thetadot(1 )*thetadot(1); v { 2): =v { 21+m2з•12•g•c12+Ь { 2) •thetadot{ 2 J ; v{Э): =Ь{3) 'thetadot {3); ENO; PROCEOURE gгav(VAR v: vесЭ); VAR m23: REAL; BEGIN m2З: •m2+mз; v { 1] : =m23* l2*g*c12+ (m1+m23) *11 •g•c1; v{21 :=m2з•12•g•c12; v{ЭJ :-о.о; EN0; PROCEDURE VMULT(VAR М: mаtЭЗ; VAR х,у: vесЭ); VAR i I j : integer j BEGIN FOR i:=1 ТО З 00 BEGIN y(i) : = О.О; FOR j:=1 ТО Э DO y(i] : = y(i) + M{i, j] 'x(j]; END; END; PROCEDURE vadd ( VAR а, Ь, с: vесЗ) ; VAR i: INTEGER; BEGIN FOR i : =1 ТО З 00 с { i] : =а { i] +Ь { i { ; END; vadd PROCEDURE JACOBIAN (VAR theta: vесз; VAR JAC: mаtЗЗ); VAR s3 J c3 J s2З,c23; real; BEGIN s3:=sin(theta[З]); cЗ:=cos(theta[З)); s2З:=sin(theta[2] + theta[З]); c2З: = cos(theta[2J+theta[З]); JAC[1, 1 J :=12•sз+11 •s2з; JAC[ 1,21 :-12•sз; JAC[ 1,ЗJ : =о.о; JAC[2,1] :=12'с3+11'с2З; JAC[2,2] :=12'сЗ; JАС[2,З]:=О.О; JАС[З, 1] :=1.0; JАС[З,2] :=1.0; JАС[З,З] :=1.0; END; PROCEDURE transpose (VAR х: mаtзз; VAR у: mаtЗЗ); VAR i, j : INTEGER; BEGIN FOR i:=1 ТО 3 DO FOR j : = 1 ТО З DO У [ i, j ] : =Х [ j, i] ; !=ND; transpose torque PROCEDURE calceft (VAR torque: vесЗ); VAR jac 1 jact: mаtЗЗ; i: INTEGER; BEGIN j acobian (theta, j ас) ; transpose(jac, jact); vmult(jact 1 fe,torque) i FOR i:=1 то з DO torque[i]:=-torque[i]; END; calceft J+т Fe PROCEDURE update (VAR tau: vесЗ; VAR period: REAL; VAR theta, thetadot: vесз); VAR i, j, k: INTEGER; temp: vесз; BEGJN j :=round(period/intstep); FOR i: =1 ТО j DO BEGIN tгig; calcm(m); invЗЗ{m,minv); calcsdt (v); calceft(eft); calc. torques due to hand forces FOR k:=1 то З 00 temp[k]:=tau[k]-V[k]-eft[k]; vmult (minv, temp, thetadd); FOR k:=1 ТО З 00 BEGIN thet:a ( k J: •theta ( k) +thetadot ( k] •intзtep +о. s• intstep• intstep*thetadd [ kJ ; thetadot [ k] : =thetadot[ k] +thetadd [ k] • intstep; END; END; END; update PROCEDURE KIN (VAR theta: vесз; VAR wrelb: frame); VAR beta: real; BEGIN wrelb[1,ЗJ:=11*cos(theta[1]) + 12*cos(theta[1) + theta(2]); wrelb[2,3): = 11'sin(theta[1J) + 12*sin(theta[1J + theta[2JJ; beta:=theta[1] + theta(2] + theta[З]; wrelb[1, 1) :=cos (beta); wrelb[1,2]: = -sin (beta); wrelb[2, 1) :=-wrelb[1,2]; wrelb[2, 2) :=Wrelb[1, 1 J; END; make forces appear at t=З secs PROGEOURE forcestep; VAR i: INTEGER; BEGIN IF timeo=З .о THEN BEGIN FOR i:=1 то 3 DO fe(i]: = fes(i); END; END; forcestep This control law is different tnan the опе suggested in the book in that the gravity model is used to feedforward some torque to cancel tne gravity effect. It will work without this, but then the arm sags due to gravity if made compliant in that direction. PROCEOURE se rvo; VAR е, edot: vесЗ; tpe: vесз; torque to fix position errors j ас, j act: mаtЗЗ; j асоыаn AND t ranspose kpxmat: mаtЗЗ; temp, temp2: vесЗ; BEGIN kpxmat (1, 1] : = kpx( 1 J; kpxmat ( 1 ,2) : =О.о; kpxmat( 1,3) : = О. о kpxmat[2, 1 J :=о.о; kpxmat[2,2] : = kpx[2]; kpxmat[2,ЗJ : = О.О kpxmat(З, 1) := о.о; kpxmat[З,2] : = о.о; kpxmat(З,ЗJ : = kрх[З] for present positian jacobian(theta, jac) j transpose ( jac, j act); FOR i:=1 ТО З DO e[i]: = pos[i] - theta(i]; vmult ( jac ,е, temp); vmult (kpxmat, temp, temp2) ; vmult ( jact, temp2, tpe); grav(gravtor); calculate gravity torques FOR i:=1 ТО З DO BEGIN edot( i] :=vel [ i] -thetadot ( i]; tauprime [ i] : =tpe ( i J +kv [ i J •edot[ i) +gravtor( i]; END; END; BEGIN servo ans:=•y•; Main. WHILE ans= 'у' 00 BEGIN fe[1] : = О.О; fe[2] ;=О.О; fе[З] : =О.О; по torques оп hand fes[ЗJ: = o.o; WRIТE( 'Enter Cartesian Force to apply ( Х and У values in Newtons' ) ; READLN(fes(1], fes(2]); WRIТELN( 'Forces appear at t=З s, How long to simulate?'); REAOLN ( len); WAITELN(' Print position every N servo periods READLN ( nprnt) ; WRIТELN('Enter Stiffnesses: kpx[i] (З vals):'); REAOLN(kpx[1] ,kpx[2] ,kрх(З]); kv[1] :=10.0; kv[2] : = 10.О; kv[З] :=10.О; theta[1] :=60.О; theta[2] : = -90.О; theta(З] : =30.О; FOR i:=1 ТО з 00 theta(i] :=theta[i]*dtor; FOR i: = 1 ТО З 00 BEGIN initialize desired path pos(i] :=theta(i]; vel(i] :=О.О; = acc(il : о.о; END; FOR i:=1 то з оо thetadot[il : = о.о; FOR i:=1 ТО 3 00 b(i]:=5.0; time: =О. о; serper; =О. 01; N=?•}; j : = round (len/serper); WRПELN (' Тime Х pos У pos' ) ; kin (theta, wrelb); writeln(time: 9: 2,wrelb( 1 ,3]: 12 :3 ,wrelb[2,3]: 12 :3); FOR i:=1 ТО j DO BEGIN Make forces appear at t=3 secs forcestep; servo; tau: =tauprime; No dynamics used update ( tau, serper, theta, thetadot) ; time: =time+serper; JF (i MOD nprnt)=O THEN BEGIN kin (theta, wrelb; writeln(time :9 :2,wrelb[ 1,3]: 12 :3,wrelb[ 2,3]: 12 :3); END; END; WRПELN( •want to Simulate the Robot?' J; READLN ( ans) ; END; END. Enter Cartesian Force to apply (Х and У values in Newtons 5 О Forces appear at t = 3 s, How long to simulate? 8 N=? Print position every N servo periods.. 50 Enter Stiffnesses: kpx [i] ( 3 vals): 10 200 200 У pos Х pos Time 0.183 0.00 0.683 0.181 0.50 0.685 0.183 1.00 0.684 0.183 1.50 0.684 0.183 2.00 0.684 0.183 2.50 0.684 0.183 3.00 0.684 0.180 3.50 0.698 0.179 4.00 о. 711 0.180 4.50 о. 723 о. 735 0.180 5.00 0.181 5.50 0,746 о. 756 0.182 6.00 6.50 0.182 0.765 0.183 7.00 0.774 7 .50 0.184 о. 783 0.185 8.00 о. 791 Want to Simulate the Robot? у Enter Cartesian Force to apply (Х and У values in Newtons О 5 Forces appear at t=3 s. How long to simulate? 8 Print position every N servo periods... N=? 50 Enter Stiffnesses: kpx[i] (3 vals): 10 200 200 Time 0.00 0.50 1.00 .50 2.00 Х pos 0.683 0.682 0.682 0.682 0.683 У pos о. 183 0.183 0.183 0.183 0.183 2. 50 3 .00 3. 50 4.00 4.50 5.00 5.50 6.00 6.50 7 .00 7 .50 8.00 0.683 0.683 0.679 0.678 0.678 0.678 0.678 0.679 0,679 0.679 0.679 0.679 0.183 0.183 0.204 0.207 0.208 0.208 0.208 0.208 0.208 0.208 0.208 0.208