1c 2c SUBROUTINE ener 3c 4 subroutine ener(th,e) 5 parameter (n=10) 6 doubleprecision th(2*n),thd(n),et(1,1),r(n),j(n),m(n) 7 integer i 8 data g / 9.81/ 9 data r / n*1.0/ 10 data m / n*1.0/ 11 data j / n*0.3/ 12c 13 do 1000, i =1,n ,1 14 thd(i) = th(i+n) 15 1000 continue 16c 17 t1 = sin(th(6)) 18 t2 = r(6)*t1 19 t3 = t2*thd(6) 20 t4 = sin(th(3)) 21 t5 = r(3)*t4 22 t6 = t5*thd(3) 23 t7 = sin(th(1)) 24 t8 = r(1)*t7 25 t9 = t8*thd(1) 26 t10 = sin(th(2)) 27 t11 = r(2)*t10 28 t12 = t11*thd(2) 29 t13 = sin(th(4)) 30 t14 = r(4)*t13 31 t15 = t14*thd(4) 32 t16 = sin(th(5)) 33 t17 = r(5)*t16 34 t18 = t17*thd(5) 35 t19 = sin(th(7)) 36 t20 = r(7)*t19 37 t21 = t20*thd(7) 38 t22 = sin(th(8)) 39 t23 = r(8)*t22 40 t24 = t23*thd(8) 41 t26 = (-t3-t6-t9-t12-t15-t18-t21-t24)**2 42 t27 = cos(th(3)) 43 t29 = r(3)*t27*thd(3) 44 t30 = cos(th(1)) 45 t32 = r(1)*t30*thd(1) 46 t33 = cos(th(2)) 47 t35 = r(2)*t33*thd(2) 48 t36 = cos(th(4)) 49 t38 = r(4)*t36*thd(4) 50 t39 = cos(th(5)) 51 t41 = r(5)*t39*thd(5) 52 t42 = cos(th(6)) 53 t44 = r(6)*t42*thd(6) 54 t45 = cos(th(7)) 55 t47 = r(7)*t45*thd(7) 56 t48 = cos(th(8)) 57 t50 = r(8)*t48*thd(8) 58 t52 = (t29+t32+t35+t38+t41+t44+t47+t50)**2 59 t56 = sin(th(9)) 60 t57 = r(9)*t56 61 t58 = t57*thd(9) 62 t60 = (-t3-t6-t9-t12-t15-t18-t21-t24-t58)**2 63 t61 = cos(th(9)) 64 t63 = r(9)*t61*thd(9) 65 t65 = (t29+t32+t35+t38+t41+t44+t47+t50+t63)**2 66 t69 = sin(th(10)) 67 t70 = r(10)*t69 68 t73 = (-t3-t6-t9-t12-t15-t18-t21-t24-t58-t70*thd(10))**2 69 t74 = cos(th(10)) 70 t78 = (t29+t32+t35+t38+t41+t44+t47+t50+t63+r(10)*t74*thd(10))**2 71 t93 = thd(3)**2 72 t97 = (-t6-t9-t12-t15-t18)**2 73 t99 = (t29+t32+t35+t38+t41)**2 74 t103 = thd(1)**2 75 t106 = r(1)**2 76 t107 = t7**2 77 t110 = t30**2 78 t116 = thd(2)**2 79 t122 = thd(5)**2 80 t125 = thd(4)**2 81 t128 = m(8)*(t26+t52)/2+m(9)*(t60+t65)/2+m(10)*(t73+t78)/2+m(5)*g* 82 +(t8+t11+t5+t14+t17)+m(10)*g*(t8+t11+t5+t14+t17+t2+t20+t23+t57+t70) 83 ++m(9)*g*(t8+t11+t5+t14+t17+t2+t20+t23+t57)+m(1)*g*t8+J(3)*t93/2+m( 84 +5)*(t97+t99)/2+J(1)*t103/2+m(1)*(t106*t107*t103+t106*t110*t103)/2+ 85 +J(2)*t116/2+m(6)*g*(t8+t11+t5+t14+t17+t2)+J(5)*t122/2+J(4)*t125/2 86 t129 = thd(6)**2 87 t132 = thd(7)**2 88 t135 = thd(8)**2 89 t138 = thd(9)**2 90 t142 = (-t9-t12)**2 91 t144 = (t32+t35)**2 92 t151 = thd(10)**2 93 t155 = (-t6-t9-t12-t15)**2 94 t157 = (t29+t32+t35+t38)**2 95 t165 = (-t6-t9-t12)**2 96 t167 = (t29+t32+t35)**2 97 t172 = (-t3-t6-t9-t12-t15-t18-t21)**2 98 t174 = (t29+t32+t35+t38+t41+t44+t47)**2 99 t182 = (-t3-t6-t9-t12-t15-t18)**2 100 t184 = (t29+t32+t35+t38+t41+t44)**2 101 t194 = J(6)*t129/2+J(7)*t132/2+J(8)*t135/2+J(9)*t138/2+m(2)*(t142+ 102 +t144)/2+m(3)*g*(t8+t11+t5)+J(10)*t151/2+m(4)*(t155+t157)/2+m(2)*g* 103 +(t8+t11)+m(3)*(t165+t167)/2+m(7)*(t172+t174)/2+m(4)*g*(t8+t11+t5+t 104 +14)+m(6)*(t182+t184)/2+m(7)*g*(t8+t11+t5+t14+t17+t2+t20)+m(8)*g*(t 105 +8+t11+t5+t14+t17+t2+t20+t23) 106 et(1,1) = t128+t194 107 e = et(1,1) 108 return 109 end 110