PROGRAM TEST
        EXTERNAL RES
        INTEGER NEQ,INFO(15),IDID,LRW,LIW,IWORK(1000),IPAR
        DOUBLE PRECISION T, Y(20), YPRIME(20), TOUT, RTOL(20),ATOL(20),
     *           RWORK(1055), RPAR,H0
        OPEN(UNIT=11,FILE='DATA')
        NEQ=20
        IDID=0 
        DO 72 II=1,7
        RTOL(II)=0.005
   72     ATOL(II)=0.005
        DO 73 II=8,20
        RTOL(II)=1.D0
   73     ATOL(II)=1.D0
        T=0.00
        Y(1)=-0.0617138900
        Y(2)=0.0
        Y(3)=0.452279
        Y(4)=0.22266839
        Y(5)=0.4873649
        Y(6)=-0.222668
        Y(7)=1.230547
        DO 61 MY=8,20
   61      Y(MY)=0.00
        Y(15)=98.5668
        Y(16)=-6.1226883
        TOUT=0.03
        DO 15 I=1,15
   15     INFO(I)=0    
        INFO(2)=1
        INFO(3)=1
        LIW=1000
        LRW=1055 
   22      CALL DDASSL(RES,NEQ,T,Y,YPRIME,TOUT,INFO,RTOL,ATOL,
     *      IDID,RWORK,LRW,IWORK,LIW,RPAR,IPAR,JAC)
        WRITE(*,*) '***************'
        WRITE(*,*) 'Time,                              Positions     '
        WRITE(*,90) T,Y(1),Y(2),Y(3),Y(4),Y(5),Y(6),Y(7)
   90   FORMAT(8F13.8)
        IF(T.GE.TOUT) STOP
        IF(IDID .GE.1) GO TO 22
        STOP
        END
C
        SUBROUTINE RES(T,Y,YPRIME,DELTA,IRES,RPAR,IPAR) 
        DOUBLE PRECISION T, Y(20), YPRIME(20), TOUT, RTOL(20), ATOL(20),
     *           RWORK(15), RPAR,DELTA(20)
        DOUBLE PRECISION M(7,7),GP(6,7),D,EA,RR,SA,SD,TB,UB,
     *      DA,ZF,RA,SB,ZT,U,C0,FORCE,FX,FY,XD,YD,
     *    G(7),LANG,E,FA,SS,SC,TA,UA,L0,M1,M2,M3,M4,M5,M6,M7,I1,
     *      I2,I3,I4,I5,I6,I7,F(7),GPT(7,6),SIBE,SITH,
     *   SIGA,SIPH,SIDE,SIOM,SIEP,SIBETH,SIPHDE,SIOMEP,COBE,
     *     COTH,COPH,COGA,CODE,COOM,COEP,COBETH,COPHDE,COOMEP,
     *  BEP,PHP,OMP,THP,DEP,EPP
        SIBE=SIN(Y(1))
        SITH=SIN(Y(2))
        SIGA=SIN(Y(3))
        SIPH=SIN(Y(4))
        SIDE=SIN(Y(5))
        SIOM=SIN(Y(6))
        SIEP=SIN(Y(7))
        SIBETH=SIN(Y(1)+Y(2))
        SIPHDE=SIN(Y(4)+Y(5))
        SIOMEP=SIN(Y(6)+Y(7))
        COBE=COS(Y(1))
        COTH=COS(Y(2))
        COGA=COS(Y(3))
        COPH=COS(Y(4))
        CODE=COS(Y(5)) 
        COOM=COS(Y(6))
        COEP=COS(Y(7))
        COBETH=COS(Y(1)+Y(2))
        COPHDE=COS(Y(4)+Y(5))
        COOMEP=COS(Y(6)+Y(7))
        BEP=YPRIME(1)
        PHP=YPRIME(4)
        OMP=YPRIME(6)
        THP=YPRIME(2)
        DEP=YPRIME(5)
        EPP=YPRIME(7)
        IRES=0
C   Geometry
        D=0.028
        EA=0.01421
        RR=0.007
        SA=0.01874
        SD=0.02
        TB=0.00916
        UB=0.00449
        DA=0.0115
        ZF=0.02
        RA=0.00092
        SB=0.01043
        ZT=0.04
        U=0.04
        C0=4530
        E=0.02
        FA=0.01421
        SS=0.035
        SC=0.018
        TA=0.02308
        UA=0.01228
        L0=0.07785
C Mass
        M1=0.04325
        M2=0.00365
        M3=0.02373
        M4=0.00706
        M5=0.07050
        M6=0.00706
        M7=0.05498
C Inertia
        I1=2.194D-6
        I2=4.410D-7
        I3=5.255D-6
        I4=5.667D-7
        I5=1.169D-5
        I6=5.667D-7
        I7=1.912D-5
C Mass Matrix
        DO 44 IM=1,7
        DO 44 JM=1,7
   44    M(IM,JM)=0.0
        M(1,1)=M1*RA**2.0+M2*(RR**2-2*DA*RR*COTH+DA**2)+I1+I2
        M(2,1)=M2*(DA**2-DA*RR*COTH)+I2
        M(2,2)=M2*DA**2+I2
        M(3,3)=M3*(SA**2+SB**2)+I3
        M(4,4)=M4*(E-EA)**2+I4
        M(5,4)=M4*((E-EA)**2+ZT*(E-EA)*SIPH)+I4
        M(5,5)=M4*(ZT**2+2*ZT*(E-EA)*SIPH+(E-EA)**2)+M5*(TA**2+TB**2)+I4+I5
        M(6,6)=M6*(ZF-FA)**2+I6
        M(7,6)=M6*((ZF-FA)**2-U*(ZF-FA)*SIOM)+I6
        M(7,7)=M6*((ZF-FA)**2-2*U*(ZF-FA)*SIOM+U**2)+M7*(UA**2+UB**2)+I6+I7
        M(1,2)=M(2,1)
        M(4,5)=M(5,4)
        M(6,7)=M(7,6)
C Forces
        XD=SD*COGA+SC*SIGA+XB
        YD=SD*SIGA-SC*COGA+YB
        LANG=SQRT((XD-XC)**2+(YD-YC)**2)
        FORCE= -C0*(LANG-L0)/LANG
        FX=FORCE*(XD-XC)
        FY=FORCE*(YD-YC)
        F(1)=0.033-M2*DA*RR*THP*(THP+2*BEP)*SITH
        F(2)=M2*DA*RR*BEP**2*SITH
        F(3)=FX*(SC*COGA-SD*SIGA)+FY*(SD*COGA+SC*SIGA)
        F(4)=M4*ZT*(E-EA)*DEP**2*COPH
        F(5)=-M4*ZT*(E-EA)*PHP*(PHP-2*DEP)*COPH
        F(6)=-M6*U*(ZF-FA)*EPP**2*COOM
        F(7)=M6*U*(ZF-FA)*OMP*(OMP+2*EPP)*COOM
C Constraints
        G(1)=RR*COBE-D*COBETH-SS*SIGA-XB
        G(2)=RR*SIBE-D*SIBETH+SS*COGA-YB
        G(3)=RR*COBE-D*COBETH-E*SIPHDE-ZT*CODE-XA
        G(4)=RR*SIBE-D*SIBETH+E*COPHDE-ZT*SIDE-YA
        G(5)=RR*COBE-D*COBETH-ZF*COOMEP-U*SIEP-XA
        G(6)=RR*SIBE-D*SIBETH-ZF*SIOMEP+U*COEP-YA
C Jacobian of Constriant
        DO 45 IM=1,6
        DO 45 JM=1,7
   45    GP(IM,JM)=0.0
        GP(1,1)=-RR*SIBE+D*SIBETH
        GP(1,2)=D*SIBETH
        GP(1,3)=-SS*COGA
        GP(2,1)=RR*COBE-D*COBETH
        GP(2,2)=-D*COBETH
        GP(2,3)=-SS*SIGA
        GP(3,1)=-RR*SIBE+D*SIBETH
        GP(3,2)=D*SIBETH
        GP(3,4)=-E*COPHDE
        GP(3,5)=-E*COPHDE+ZT*SIDE
        GP(4,1)=RR*COBE-D*COBETH
        GP(4,2)=-D*COBETH
        GP(4,4)=-E*SIPHDE
        GP(4,5)=-E*SIPHDE-ZT*CODE
        GP(5,1)=-RR*SIBE+D*SIBETH
        GP(5,2)=D*SIBETH
        GP(5,6)=ZF*SIOMEP
        GP(5,7)=ZF*SIOMEP-U*COEP
        GP(6,1)=RR*COBE-D*COBETH
        GP(6,2)=-D*COBETH
        GP(6,6)=-ZF*COOMEP
        GP(6,7)=-ZF*COOMEP-U*SIEP
        GAP=YPRIME(3)
C Equation
        DELTA(1)=Y(8)-YPRIME(1)
        DELTA(2)=Y(9)-YPRIME(2)
        DELTA(3)=Y(10)-YPRIME(3)
        DELTA(4)=Y(11)-YPRIME(4)
        DELTA(5)=Y(12)-YPRIME(5)
        DELTA(6)=Y(13)-YPRIME(6)
        DELTA(7)=Y(14)-YPRIME(7)
        DO 55 NROW=1,7
           DELTA(NROW+7)=-F(NROW)
        DO 55 NCOL=1,7
   55       DELTA(7+NROW)=DELTA(7+NROW)+M(NROW,NCOL)*YPRIME(7+NCOL)
        DO 57 IRR=1,6
        DO 57 ICL=1,7
   57      GPT(ICL,IRR)=GP(IRR,ICL)
        DO 56 NROW=1,7
        DO 56 NCOL=1,6
   56       DELTA(7+NROW)=DELTA(7+NROW)+GPT(NROW,NCOL)*Y(14+NCOL)
        DO 58 NN=1,6
         DELTA(NN+14)=0.0
        DO 58 NC=1,7
   58     DELTA(NN+14)=GP(NN,NC)*Y(NC+7)+DELTA(NN+14) 
        RETURN
        END