1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
|
c
c SUBROUTINE wheel
c
c Copyright INRIA
subroutine wheel(neq,t,z,zdot)
implicit double precision (t)
parameter (kn=3)
doubleprecision t,z(8),zdot(8),r,j(3),m
doubleprecision me3s(kn,kn)
doubleprecision const(kn,1),w(3*kn),rcond
integer i,k,neq,ierr
data g / 9.81/
data r / 1.0/
data m / 1.0/
data j / 0.3,0.4,1.0/
t1 = r**2
t2 = t1*m
t3 = t2+J(3)
t5 = cos(z(2))*t3
t9 = cos(2*z(2))
me3s(3,3) = t3
me3s(1,3) = t5
me3s(2,1) = 0
me3s(2,3) = 0
me3s(2,2) = J(1)+t2
me3s(3,1) = t5
me3s(1,2) = 0
me3s(3,2) = 0
me3s(1,1) = J(1)/2+t1*m*t9/2+t2/2+J(3)*t9/2+J(3)/2-J(1)*t9/2
t1 = r**2
t2 = z(4)**2
t4 = sin(2*z(2))
t5 = t2*t4
t11 = sin(z(2))
t12 = z(4)*z(6)
t34 = z(4)*t4
const(2,1) = -t1*m*t5/2+J(1)*t5/2-t1*t11*m*t12-r*cos(z(2))*m*g-
+J(3)*t11*t12-J(3)*t5/2
const(3,1) = t11*z(4)*z(5)*(2*t1*m+J(3))
const(1,1) = -z(5)*(-t1*m*t34-z(6)*t11*J(3)+J(1)*t34-J(3)*t34)
c
do 1000, i =1,kn ,1
zdot(i) = z(i+kn)
1000 continue
c
c we must solve M z =const
call dlslv(me3s,kn,kn,Const,kn,1,w, rcond,ierr,1)
if (ierr.ne.0) then
write(6,2000)
2000 format('Ill conditioned matrix!')
endif
c
do 1001, i =1,kn ,1
zdot(kn+i) = const(i,1)
1001 continue
c
t2 = sin(z(1))
zdot(7) = r*cos(z(2))*t2*z(4)+r*sin(z(2))*cos(z(1))*z(5)+r*t2*z(
+6)
t2 = cos(z(1))
zdot(8) = -r*cos(z(2))*t2*z(4)+r*sin(z(2))*sin(z(1))*z(5)-r*t2*z
+(6)
return
end
|
