(reset) (si) (add-gravity :direction -[nz])
(add-point pt1 :body n :coordinates (L3 0 h))(add-point pt2 :body n :coordinates (L4 0 0))
(add-point pt3 :body n :coordinates (L2 0 0))(add-point pt4 :body n :coordinates (L4 B1 0))
(add-point pt5 :body n :coordinates (L4 -B1 0))(add-point pt6 :body n :coordinates (L2 B1 0))
(add-point pt7 :body n :coordinates (L2 -B1 0))(add-point pt8 :body n :coordinates (L4 a -r))
(add-point pt9 :body n :coordinates (L4 -a -r))(add-point pt10 :body n :coordinates (L2 a -r))
(add-point pt11 :body n :coordinates (L2 -a -r))(add-point pt12 :body n :coordinates (L4 B2 0))
(add-point pt13 :body n :coordinates (L5 B1 0))(add-point pt14 :body n :coordinates (L4 B1 h))
(add-point pt15 :body n :coordinates (L4 -B2 0))(add-point pt16 :body n :coordinates (L5 -B1 0))
(add-point pt17 :body n :coordinates (L4 -B1 h))(add-point pt18 :body n :coordinates (L2 B2 0))
(add-point pt19 :body n :coordinates (L1 B1 0))(add-point pt20 :body n :coordinates (L2 B1 h))
(add-point pt21 :body n :coordinates (L2 -B2 0))(add-point pt22 :body n :coordinates (L1 -B1 0))
(add-point pt23 :body n :coordinates (L2 -B1 h))(add-point pt24 :body n :coordinates (L3 B2 h))
(add-point pt25 :body n :coordinates (L3 -B2 h))(add-point pt27 :body n :coordinates (L4 B2 h))
(add-point pt28 :body n :coordinates (L3 B3 h))(add-point pt29 :body n :coordinates (L3 B2 h1))
(add-point pt30 :body n :coordinates (L4 -B2 h))(add-point pt31 :body n :coordinates (L3 -B3 h))
(add-point pt32 :body n :coordinates (L3 -B2 h1))(add-point pt26 :body n :coordinates (0 0 hs))
(add-point spt1 :body n :coordinates (-L3 0 h))(add-point spt2 :body n :coordinates (-L2 0 0))
(add-point spt3 :body n :coordinates (-L4 0 0))(add-point spt4 :body n :coordinates (-L2 B1 0))
(add-point spt5 :body n :coordinates (-L2 -B1 0))(add-point spt6 :body n :coordinates (-L4 B1 0))
(add-point spt7 :body n :coordinates (-L4 -B1 0))(add-point spt8 :body n :coordinates (-L2 a -r))
(add-point spt9 :body n :coordinates (-L2 -a -r))(add-point spt10 :body n :coordinates (-L4 a -r))
(add-point spt11 :body n :coordinates (-L4 -a -r))(add-point spt12 :body n :coordinates (-L2 B2 0))
(add-point spt13 :body n :coordinates (-L1 B1 0))(add-point spt14 :body n :coordinates (-L2 B1 h))
(add-point spt15 :body n :coordinates (-L2 -B2 0))(add-point spt16 :body n :coordinates (-L1 -B1 0))
(add-point spt17 :body n :coordinates (-L2 -B1 h))(add-point spt18 :body n :coordinates (-L4 B2 0))
(add-point spt19 :body n :coordinates (-L5 B1 0))(add-point spt20 :body n :coordinates (-L4 B1 h))
(add-point spt21 :body n :coordinates (-L4 -B2 0))(add-point spt22 :body n :coordinates (-L5 -B1 0))
(add-point spt23 :body n :coordinates (-L4 -B1 h))(add-point spt24 :body n :coordinates (-L3 B2 h))
(add-point spt25 :body n :coordinates (-L3 -B2 h))(add-point spt27 :body n :coordinates (-L2 B2 h))
(add-point spt28 :body n :coordinates (-L3 B3 h))(add-point spt29 :body n :coordinates (-L3 B2 h1))
(add-point spt30 :body n :coordinates (-L2 -B2 h))(add-point spt31 :body n :coordinates (-L3 -B3 h))
(add-point spt32 :body n :coordinates (-L3 -B2 h1))
; add carbody
(add-body cmass :name "carbody mass" :joint-coordinates pt26 :cm-coordinates pt26
:translate (y z) :body-rotation-axes (x z) :mass "mc" :inertia-matrix ("Icx" 0 "Icz"))
(add-point cmass27 :body cmass :coordinates pt27)(add-point cmass28 :body cmass :coordinates pt28)
(add-point cmass29 :body cmass :coordinates pt29)(add-point cmass30 :body cmass :coordinates pt30)
(add-point cmass31 :body cmass :coordinates pt31)(add-point cmass32 :body cmass :coordinates pt32)
(add-point scmass27 :body cmass :coordinates spt27)(add-point scmass28 :body cmass :coordinates spt28)
(add-point scmass29 :body cmass :coordinates spt29)(add-point scmass30 :body cmass :coordinates spt30)
(add-point scmass31 :body cmass :coordinates spt31)(add-point scmass32 :body cmass :coordinates spt32)
; add bogies
(add-body bmass :name "bogie mass" :joint-coordinates pt1 :cm-coordinates pt1
:translate (y z) :body-rotation-axes (x z) :mass "mb" :inertia-matrix ("Ibx" 0 "Ibz"))
(add-point bmass12 :body bmass :coordinates pt12)(add-point bmass13 :body bmass :coordinates pt13)
(add-point bmass14 :body bmass :coordinates pt14)(add-point bmass15 :body bmass :coordinates pt15)
(add-point bmass16 :body bmass :coordinates pt16)(add-point bmass17 :body bmass :coordinates pt17)
(add-point bmass18 :body bmass :coordinates pt18)(add-point bmass19 :body bmass :coordinates pt19)
(add-point bmass20 :body bmass :coordinates pt20)(add-point bmass21 :body bmass :coordinates pt21)
(add-point bmass22 :body bmass :coordinates pt22)(add-point bmass23 :body bmass :coordinates pt23)
(add-point bmass24 :body bmass :coordinates pt24)(add-point bmass25 :body bmass :coordinates pt25)
(add-body sbmass :name "sbogie mass" :joint-coordinates spt1 :cm-coordinates spt1
:translate (y z) :body-rotation-axes (x z) :mass "mb" :inertia-matrix ("Ibx" 0 "Ibz"))
(add-point sbmass12 :body sbmass :coordinates spt12)
(add-point sbmass13 :body sbmass :coordinates spt13)
(add-point sbmass14 :body sbmass :coordinates spt14)
(add-point sbmass15 :body sbmass :coordinates spt15)
(add-point sbmass16 :body sbmass :coordinates spt16)
(add-point sbmass17 :body sbmass :coordinates spt17)
(add-point sbmass18 :body sbmass :coordinates spt18)
(add-point sbmass19 :body sbmass :coordinates spt19)
(add-point sbmass20 :body sbmass :coordinates spt20)
(add-point sbmass21 :body sbmass :coordinates spt21)
(add-point sbmass22 :body sbmass :coordinates spt22)
(add-point sbmass23 :body sbmass :coordinates spt23)
(add-point sbmass24 :body sbmass :coordinates spt24)
(add-point sbmass25 :body sbmass :coordinates spt25)
; add wheel-sets
(add-body fwmass :name "front wheel mass" :joint-coordinates pt2 :cm-coordinates pt2
:translate (y z) :body-rotation-axes (x z) :mass "mw" :inertia-matrix ("Iwx" 0 "Iwz"))
(add-point fwmass2 :body fwmass :coordinates pt2)(add-point fwmass4 :body fwmass :coordinates pt4)
(add-point fwmass5 :body fwmass :coordinates pt5)(add-point fwmass8 :body fwmass :coordinates pt8)
(add-point fwmass9 :body fwmass :coordinates pt9)
(add-body rwmass :name "rear wheel mass" :joint-coordinates pt3 :cm-coordinates pt3
:translate (y z) :body-rotation-axes (x z) :mass "mw" :inertia-matrix ("Iwx" 0 "Iwz"))
(add-point rwmass3 :body rwmass :coordinates pt3)(add-point rwmass6 :body rwmass :coordinates pt6)
(add-point rwmass7 :body rwmass :coordinates pt7)(add-point rwmass10 :body rwmass :coordinates pt10)
(add-point rwmass11 :body rwmass :coordinates pt11)
(add-body sfwmass :name "sfront wheel mass" :joint-coordinates spt2 :cm-coordinates spt2
:translate (y z) :body-rotation-axes (x z) :mass "mw" :inertia-matrix ("Iwx" 0 "Iwz"))
(add-point sfwmass2 :body sfwmass :coordinates spt2)
(add-point sfwmass4 :body sfwmass :coordinates spt4)
(add-point sfwmass5 :body sfwmass :coordinates spt5)
(add-point sfwmass8 :body sfwmass :coordinates spt8)
(add-point sfwmass9 :body sfwmass :coordinates spt9)
(add-body srwmass :name "srear wheel mass" :joint-coordinates spt3 :cm-coordinates spt3
:translate (y z) :body-rotation-axes (x z) :mass "mw" :inertia-matrix ("Iwx" 0 "Iwz"))
(add-point srwmass3 :body srwmass :coordinates spt3)
(add-point srwmass6 :body srwmass :coordinates spt6)
(add-point srwmass7 :body srwmass :coordinates spt7)
(add-point srwmass10 :body srwmass :coordinates spt10)
(add-point srwmass11 :body srwmass :coordinates spt11)
; add input
(add-input Zr L)
; feedback force
(add-input F4px F) (add-input F4py F) (add-input F4pz F) (add-input F5px F) (add-input F5py F) (add-input F5pz F) (add-input F6px F) (add-input F6py F) (add-input F6pz F) (add-input F7px F)
(add-input F7py F) (add-input F7pz F) (add-input F24sx F) (add-input F24sy F) (add-input F24sz F)
(add-input F25sx F) (add-input F25sy F) (add-input F25sz F) (add-input sF4px F) (add-input sF4py F)
(add-input sF4pz F) (add-input sF5px F) (add-input sF5py F) (add-input sF5pz F) (add-input sF6px F)
(add-input sF6py F) (add-input sF6pz F) (add-input sF7px F) (add-input sF7py F) (add-input sF7pz F)
(add-input sF24sx F) (add-input sF24sy F) (add-input sF24sz F) (add-input sF25sx F) (add-input sF25sy F)
(add-input sF25sz F)
; front bogie part
; add suspension force
(add-line-force u4x :direction [nx] :point2 fwmass4 :point1 bmass13
:magnitude "-kpx*(x-x0) -cpx*v -input_ext(F4px)")
(add-line-force u4y :direction [ny] :point2 fwmass4 :point1 bmass12
:magnitude "-kpy*(x-x0) -cpy*v -input_ext(F4py)")
(add-line-force u4z :direction [nz] :point2 fwmass4 :point1 bmass14
:magnitude "-kpz*(x-x0) -cpz*v +(2*mb+mc)*g/8 -input_ext(F4pz)")
(add-line-force u5x :direction [nx] :point2 fwmass5 :point1 bmass16
:magnitude "-kpx*(x-x0) -cpx*v -input_ext(F5px)")
(add-line-force u5y :direction [ny] :point2 fwmass5 :point1 bmass15
:magnitude "-kpy*(x-x0) -cpy*v -input_ext(F5py)")
(add-line-force u5z :direction [nz] :point2 fwmass5 :point1 bmass17
:magnitude "-kpz*(x-x0) -cpz*v +(2*mb+mc)*g/8 -input_ext(F5pz)")
(add-line-force u6x :direction [nx] :point2 rwmass6 :point1 bmass19
:magnitude "-kpx*(x-x0) -cpx*v -input_ext(F6px)")
(add-line-force u6y :direction [ny] :point2 rwmass6 :point1 bmass18
:magnitude "-kpy*(x-x0) -cpy*v -input_ext(F6py)")
(add-line-force u6z :direction [nz] :point2 rwmass6 :point1 bmass20
:magnitude "-kpz*(x-x0) -cpz*v +(2*mb+mc)*g/8 -input_ext(F6pz)")
(add-line-force u7x :direction [nx] :point2 rwmass7 :point1 bmass22
:magnitude "-kpx*(x-x0) -cpx*v -input_ext(F7px)")
(add-line-force u7y :direction [ny] :point2 rwmass7 :point1 bmass21
:magnitude "-kpy*(x-x0) -cpy*v -input_ext(F7py)")
(add-line-force u7z :direction [nz] :point2 rwmass7 :point1 bmass23
:magnitude "-kpz*(x-x0) -cpz*v +(2*mb+mc)*g/8 -input_ext(F7pz)")
(add-line-force u24x :direction [nx] :point2 bmass24 :point1 cmass27
:magnitude "-ksx*(x-x0) -csx*v -input_ext(F24sx)")
(add-line-force u24y :direction [ny] :point2 bmass24 :point1 cmass28
:magnitude "-ksy*(x-x0) -csy*v -input_ext(F24sy)")
(add-line-force u24z :direction [nz] :point2 bmass24 :point1 cmass29
:magnitude "-ksz*(x-x0) -csz*v +(mc)*g/4 -input_ext(F24sz)")
(add-line-force u25x :direction [nx] :point2 bmass25 :point1 cmass30
:magnitude "-ksx*(x-x0) -csx*v -input_ext(F25sx)")
(add-line-force u25y :direction [ny] :point2 bmass25 :point1 cmass31
:magnitude "-ksy*(x-x0) -csy*v -input_ext(F25sy)")
(add-line-force u25z :direction [nz] :point2 bmass25 :point1 cmass32
:magnitude "-ksz*(x-x0) -csz*v +(mc)*g/4 -input_ext(F25sz)")
; add creep force
(add-line-force u8x :point1 fwmass8 :direction [nx] :magnitude "(-f33/e)*(e*(-y*tq(fwmass,1))/r
-a*ru(fwmass,2)) +((f11/e)*(tu(fwmass,1)+(r+y*tq(fwmass,1))*ru(fwmass,1)-e*rq(fwmass,2))
+(f12/e)*(ru(fwmass,2)-e*y/r))*rq(fwmass,2)")
(add-line-force u8y :point1 fwmass8 :direction [ny] :magnitude "(-f33/e)*(e*(-y*tq(fwmass,1))/r
-a*ru(fwmass,1))*rq(fwmass,2)+((-f11/e)*(tu(fwmass,1)+(r+y*tq(fwmass,1))*ru(fwmass,1)
-e*rq(fwmass,2))+(-f12/e)*(ru(fwmass,2)-e*y/r))")
(add-line-force u8z :point1 fwmass8 :direction [nz] :magnitude "((-f11/e)*(tu(fwmass,1)
+(r+y*tq(fwmass,1))*ru(fwmass,1)-e*rq(fwmass,2))+(-f12/e)*(ru(fwmass,2)-e*y/r))*(y+rq(fwmass,1))")
(add-line-force u9x :point1 fwmass9 :direction [nx] :magnitude "(-f33/e)*(e*y*tq(fwmass,1)/r
+a*ru(fwmass,2))+((f11/e)*(tu(fwmass,1)+(r-y*tq(fwmass,1))*ru(fwmass,1)-e*rq(fwmass,2))
+(f12/e)*(ru(fwmass,2)+e*y/r))*rq(fwmass,2)")
(add-line-force u9y :point1 fwmass9 :direction [ny] :magnitude "(-f33/e)*(e*y*tq(fwmass,1)/r
+a*ru(fwmass,2))*rq(fwmass,2)+((-f11/e)*(tu(fwmass,1)+(r-y*tq(fwmass,1))*ru(fwmass,1)
-e*rq(fwmass,2))+(-f12/e)*(ru(fwmass,2)+e*y/r))")
(add-line-force u9z :point1 fwmass9 :direction [nz] :magnitude "((f11/e)*(tu(fwmass,1)
+(r-y*tq(fwmass,1))*ru(fwmass,1)-e*rq(fwmass,2))+(f12/e)*(ru(fwmass,2)+e*y/r))*(y-rq(fwmass,1))")
(add-line-force u10x :point1 rwmass10 :direction [nx] :magnitude "(-f33/e)*(e*(-y*tq(rwmass,1))/r
-a*ru(rwmass,2))+((f11/e)*(tu(rwmass,1)+(r+y*tq(rwmass,1))*ru(rwmass,1)-e*rq(rwmass,2))
+(f12/e)*(ru(rwmass,2)-e*y/r))*rq(rwmass,2)")
(add-line-force u10y :point1 rwmass10 :direction [ny] :magnitude "(-f33/e)*(e*(-y*tq(rwmass,1))/r
-a*ru(rwmass,1))*rq(rwmass,2)+((-f11/e)*(tu(rwmass,1)+(r+y*tq(rwmass,1))*ru(rwmass,1)
-e*rq(rwmass,2))+(-f12/e)*(ru(rwmass,2)-e*y/r))")
(add-line-force u10z :point1 rwmass10 :direction [nz] :magnitude "((-f11/e)*(tu(rwmass,1)
+(r+y*tq(rwmass,1))*ru(rwmass,1)-e*rq(rwmass,2))+(-f12/e)*(ru(rwmass,2)-e*y/r))*(y+rq(rwmass,1))")
(add-line-force u11x :point1 rwmass11 :direction [nx] :magnitude "(-f33/e)*(e*y*tq(rwmass,1)/r
+a*ru(rwmass,2))+((f11/e)*(tu(rwmass,1)+(r-y*tq(rwmass,1))*ru(rwmass,1)-e*rq(rwmass,2))
+(f12/e)*(ru(rwmass,2)+e*y/r))*rq(rwmass,2)")
(add-line-force u11y :point1 rwmass11 :direction [ny] :magnitude "(-f33/e)*(e*y*tq(rwmass,1)/r
+a*ru(rwmass,2))* rq(rwmass,2)+((-f11/e)*(tu(rwmass,1)+(r-y*tq(rwmass,1))*ru(rwmass,1)
-e*rq(rwmass,2))+(-f12/e)*(ru(rwmass,2)+e*y/r))")
(add-line-force u11z :point1 rwmass11 :direction [nz] :magnitude "((f11/e)*(tu(rwmass,1)
+(r-y*tq(rwmass,1))*ru(rwmass,1)-e*rq(rwmass,2))+(f12/e)*(ru(rwmass,2)+e*y/r))* (y-rq(rwmass,1))")
; add creep moment
(add-moment T8x :body1 fwmass :direction [nx] :magnitude "((f12/e)*(tu(fwmass,1)
+(r+y*tq(fwmass,1))*ru(fwmass,1)-e*rq(fwmass,2))
+(-f22/e)*(ru(fwmass,2)-e*y/r))*rq(fwmass,2)*(y+rq(fwmass,1))")
(add-moment T8z :body1 fwmass :direction [nz] :magnitude "((f12/e)*(tu(fwmass,1)
+(r+y*tq(fwmass,1))*ru(fwmass,1)-e*rq(fwmass,2))+(-f22/e)*(ru(fwmass,2)-e*y/r))")
(add-moment T9x :body1 fwmass :direction [nx] :magnitude "((-f12/e)*(tu(fwmass,1)
+(r-y*tq(fwmass,1))*ru(fwmass,1)-e*rq(fwmass,2))
+( f22/e)*(ru(fwmass,2)+e*y/r))*rq(fwmass,2)*(y-rq(fwmass,1))")
(add-moment T9z :body1 fwmass :direction [nz] :magnitude "((f12/e)*(tu(fwmass,1)
+(r-y*tq(fwmass,1))*ru(fwmass,1)-e*rq(fwmass,2))+(-f22/e)*(ru(fwmass,2)+e*y/r))")
(add-moment T10x :body1 rwmass :direction [nx] :magnitude "((f12/e)*(tu(rwmass,1)
+(r+y*tq(rwmass,1))*ru(rwmass,1)-e*rq(rwmass,2))
+(-f22/e)*(ru(rwmass,2)-e*y/r))*rq(rwmass,2)*(y+rq(rwmass,1))")
(add-moment T10z :body1 rwmass :direction [nz] :magnitude "((f12/e)*(tu(rwmass,1)
+(r+y*tq(rwmass,1))*ru(rwmass,1)-e*rq(rwmass,2))+(-f22/e)*(ru(rwmass,2)-e*y/r))")
(add-moment T11x :body1 rwmass :direction [nx] :magnitude "((-f12/e)*(tu(rwmass,1)
+(r-y*tq(rwmass,1))*ru(rwmass,1)-e*rq(rwmass,2))
+(f22/e)*(ru(rwmass,2)+e*y/r))*rq(rwmass,2)*(y-rq(rwmass,1))")
(add-moment T11z :body1 rwmass :direction [nz] :magnitude "((f12/e)*(tu(rwmass,1)
+(r-y*tq(rwmass,1))*ru(rwmass,1)-e*rq(rwmass,2))+(-f22/e)*(ru(rwmass,2)+e*y/r))")
; add normal force
(add-line-force n8y :point1 fwmass8 :direction [ny]
:magnitude "-((mc+2*mb+4*mw)*g/8)*(y+rq(fwmass,1))")
(add-line-force n9y :point1 fwmass9 :direction [ny]
:magnitude "((mc+2*mb+4*mw)*g/8)*(y-rq(fwmass,1))")
(add-line-force n10y :point1 rwmass10 :direction [ny]
:magnitude "-((mc+2*mb+4*mw)*g/8)*(y+rq(rwmass,1))")
(add-line-force n11y :point1 rwmass11 :direction [ny]
:magnitude "((mc+2*mb+4*mw)*g/8)*(y-rq(rwmass,1))")
(add-line-force n8z :point1 fwmass8 :direction [nz]
:magnitude "-kzz*(x-x0-input_ext(Zr))+((mc+2*mb+4*mw)*g/8)")
(add-line-force n9z :point1 fwmass9 :direction [nz] :magnitude "-kzz*(x-x0)+((mc+2*mb+4*mw)*g/8)")
(add-line-force n10z :point1 rwmass10 :direction [nz] :magnitude "-kzz*(x-x0)+((mc+2*mb+4*mw)*g/8)")
(add-line-force n11z :point1 rwmass11 :direction [nz] :magnitude "-kzz*(x-x0)+((mc+2*mb+4*mw)*g/8)")
; add external moment
(add-moment n2x :body1 fwmass :direction [nx] :magnitude "(Iwy*e*ru(fwmass,2)/r)")
(add-moment n2z :body1 fwmass :direction [nz] :magnitude "-Iwy*e*ru(fwmass,1)/r")
(add-moment n3x :body1 rwmass :direction [nx] :magnitude "(Iwy*e*ru(rwmass,2)/r)")
(add-moment n3z :body1 rwmass :direction [nz] :magnitude "-Iwy*e*ru(rwmass,1)/r")
; rear bogie part
; add wheel force
(add-line-force su4x :direction [nx] :point2 sfwmass4 :point1 sbmass13
:magnitude "-kpx*(x-x0) -cpx*v -input_ext(sF4px)")
(add-line-force su4y :direction [ny] :point2 sfwmass4 :point1 sbmass12
:magnitude "-kpy*(x-x0) -cpy*v -input_ext(sF4py)")
(add-line-force su4z :direction [nz] :point2 sfwmass4 :point1 sbmass14
:magnitude "-kpz*(x-x0) -cpz*v +(2*mb+mc)*g/8 -input_ext(sF4pz)")
(add-line-force su5x :direction [nx] :point2 sfwmass5 :point1 sbmass16
:magnitude "-kpx*(x-x0) -cpx*v -input_ext(sF5px)")
(add-line-force su5y :direction [ny] :point2 sfwmass5 :point1 sbmass15
:magnitude "-kpy*(x-x0) -cpy*v -input_ext(sF5py)")
(add-line-force su5z :direction [nz] :point2 sfwmass5 :point1 sbmass17
:magnitude "-kpz*(x-x0) -cpz*v +(2*mb+mc)*g/8 -input_ext(sF5pz)")
(add-line-force su6x :direction [nx] :point2 srwmass6 :point1 sbmass19
:magnitude "-kpx*(x-x0) -cpx*v -input_ext(sF6px)")
(add-line-force su6y :direction [ny] :point2 srwmass6 :point1 sbmass18
:magnitude "-kpy*(x-x0) -cpy*v -input_ext(sF6py)")
(add-line-force su6z :direction [nz] :point2 srwmass6 :point1 sbmass20
:magnitude "-kpz*(x-x0) -cpz*v +(2*mb+mc)*g/8 -input_ext(sF6pz)")
(add-line-force su7x :direction [nx] :point2 srwmass7 :point1 sbmass22
:magnitude "-kpx*(x-x0) -cpx*v -input_ext(sF7px)")
(add-line-force su7y :direction [ny] :point2 srwmass7 :point1 sbmass21
:magnitude "-kpy*(x-x0) -cpy*v -input_ext(sF7py)")
(add-line-force su7z :direction [nz] :point2 srwmass7 :point1 sbmass23
:magnitude "-kpz*(x-x0) -cpz*v +(2*mb+mc)*g/8 -input_ext(sF7pz)")
(add-line-force su24x :direction [nx] :point2 sbmass24 :point1 scmass27
:magnitude "-ksx*(x-x0) -csx*v -input_ext(sF24sx)")
(add-line-force su24y :direction [ny] :point2 sbmass24 :point1 scmass28
:magnitude "-ksy*(x-x0) -csy*v -input_ext(sF24sy)")
(add-line-force su24z :direction [nz] :point2 sbmass24 :point1 scmass29
:magnitude "-ksz*(x-x0) -csz*v +(mc)*g/4 -input_ext(sF24sz)")
(add-line-force su25x :direction [nx] :point2 sbmass25 :point1 scmass30
:magnitude "-ksx*(x-x0) -csx*v -input_ext(sF25sx)")
(add-line-force su25y :direction [ny] :point2 sbmass25 :point1 scmass31
:magnitude "-ksy*(x-x0) -csy*v -input_ext(sF25sy)")
(add-line-force su25z :direction [nz] :point2 sbmass25 :point1 scmass32
:magnitude "-ksz*(x-x0) -csz*v +(mc)*g/4 -input_ext(sF25sz)")
; add creep force
(add-line-force su8x :point1 sfwmass8 :direction [nx] :magnitude "(-f33/e)*(e*(-y*tq(sfwmass,1))/r
-a*ru(sfwmass,2)) +((f11/e)*(tu(sfwmass,1)+(r+y*tq(sfwmass,1))*ru(sfwmass,1)-e*rq(sfwmass,2))
+(f12/e)*(ru(sfwmass,2)-e*y/r))*rq(sfwmass,2)")
(add-line-force su8y :point1 sfwmass8 :direction [ny] :magnitude "(-f33/e)*(e*(-y*tq(sfwmass,1))/r
-a*ru(sfwmass,1))*rq(sfwmass,2)+((-f11/e)*(tu(sfwmass,1)+(r+y*tq(sfwmass,1))*ru(sfwmass,1)
-e*rq(sfwmass,2))+(-f12/e)*(ru(sfwmass,2)-e*y/r))")
(add-line-force su8z :point1 sfwmass8 :direction [nz] :magnitude "((-f11/e)*(tu(sfwmass,1)
+(r+y*tq(sfwmass,1))*ru(sfwmass,1)-e*rq(sfwmass,2))
+(-f12/e)*(ru(sfwmass,2)-e*y/r))*(y+rq(sfwmass,1))")
(add-line-force su9x :point1 sfwmass9 :direction [nx] :magnitude "(-f33/e)*(e*y*tq(sfwmass,1)/r
+a*ru(sfwmass,2))+((f11/e)*(tu(sfwmass,1)+(r-y*tq(sfwmass,1))*ru(sfwmass,1)-e*rq(sfwmass,2))
+(f12/e)*(ru(sfwmass,2)+e*y/r))*rq(sfwmass,2)")
(add-line-force su9y :point1 sfwmass9 :direction [ny] :magnitude "(-f33/e)*(e*y*tq(sfwmass,1)/r
+a*ru(sfwmass,2))*rq(sfwmass,2)+((-f11/e)*(tu(sfwmass,1)+(r-y*tq(sfwmass,1))*ru(sfwmass,1)
-e*rq(sfwmass,2))+(-f12/e)*(ru(sfwmass,2)+e*y/r))")
(add-line-force su9z :point1 sfwmass9 :direction [nz]
:magnitude "((f11/e)*(tu(sfwmass,1)+(r-y*tq(sfwmass,1))*ru(sfwmass,1)-e*rq(sfwmass,2))
+(f12/e)*(ru(sfwmass,2)+e*y/r))*(y-rq(sfwmass,1))")
(add-line-force su10x :point1 srwmass10 :direction [nx] :magnitude "(-f33/e)*(e*(-y*tq(srwmass,1))/r
-a*ru(srwmass,2))+((f11/e)*(tu(srwmass,1)+(r+y*tq(srwmass,1))*ru(srwmass,1)
-e*rq(srwmass,2))+(f12/e)*(ru(srwmass,2)-e*y/r))*rq(srwmass,2)")
(add-line-force su10y :point1 srwmass10 :direction [ny] :magnitude "(-f33/e)*(e*(-y*tq(srwmass,1))/r
-a*ru(srwmass,1))*rq(srwmass,2)+((-f11/e)*(tu(srwmass,1)+(r+y*tq(srwmass,1))*ru(srwmass,1)
-e*rq(srwmass,2))+(-f12/e)*(ru(srwmass,2)-e*y/r))")
(add-line-force su10z :point1 srwmass10 :direction [nz] :magnitude "((-f11/e)*(tu(srwmass,1)
+(r+y*tq(srwmass,1))*ru(srwmass,1)-e*rq(srwmass,2))
+(-f12/e)*(ru(srwmass,2)-e*y/r))*(y+rq(srwmass,1))")
(add-line-force su11x :point1 srwmass11 :direction [nx] :magnitude "(-f33/e)*(e*y*tq(srwmass,1)/r
+a*ru(srwmass,2))+((f11/e)*(tu(srwmass,1)+(r-y*tq(srwmass,1))*ru(srwmass,1)-e*rq(srwmass,2))
+(f12/e)*(ru(srwmass,2)+e*y/r))*rq(srwmass,2)")
(add-line-force su11y :point1 srwmass11 :direction [ny] :magnitude "(-f33/e)*(e*y*tq(srwmass,1)/r
+a*ru(srwmass,2))* rq(srwmass,2)+((-f11/e)*(tu(srwmass,1)+(r-y*tq(srwmass,1))*ru(srwmass,1)
-e*rq(srwmass,2))+(-f12/e)*(ru(srwmass,2)+e*y/r))")
(add-line-force su11z :point1 srwmass11 :direction [nz] :magnitude "((f11/e)*(tu(srwmass,1)
+(r-y*tq(srwmass,1))*ru(srwmass,1)-e*rq(srwmass,2))+(f12/e)*(ru(srwmass,2)
+e*y/r))* (y-rq(srwmass,1))")
; add creep moment
(add-moment sT8x :body1 sfwmass :direction [nx] :magnitude "((f12/e)*(tu(sfwmass,1)
+(r+y*tq(sfwmass,1))*ru(sfwmass,1-e*rq(sfwmass,2))
+(-f22/e)*(ru(sfwmass,2)-e*y/r))*rq(sfwmass,2)*(y+rq(sfwmass,1))")
(add-moment sT8z :body1 sfwmass :direction [nz] :magnitude "((f12/e)*(tu(sfwmass,1)
+(r+y*tq(sfwmass,1))*ru(sfwmass,1)-e*rq(sfwmass,2))+(-f22/e)*(ru(sfwmass,2)-e*y/r))")
(add-moment sT9x :body1 sfwmass :direction [nx] :magnitude "((-f12/e)*(tu(sfwmass,1)
+(r-y*tq(sfwmass,1))*ru(sfwmass,1)-e*rq(sfwmass,2))
+( f22/e)*(ru(sfwmass,2)+e*y/r))*rq(sfwmass,2)*(y-rq(sfwmass,1))")
(add-moment sT9z :body1 sfwmass :direction [nz] :magnitude "((f12/e)*(tu(sfwmass,1)
+(r-y*tq(sfwmass,1))*ru(sfwmass,1)-e*rq(sfwmass,2))+(-f22/e)*(ru(sfwmass,2)+e*y/r))")
(add-moment sT10x :body1 srwmass :direction [nx] :magnitude "((f12/e)*(tu(srwmass,1)
+(r+y*tq(srwmass,1))*ru(srwmass,1)-e*rq(srwmass,2))
+(-f22/e)*(ru(srwmass,2)-e*y/r))*rq(srwmass,2)*(y+rq(srwmass,1))")
(add-moment sT10z :body1 srwmass :direction [nz] :magnitude "((f12/e)*(tu(srwmass,1)
+(r+y*tq(srwmass,1))*ru(srwmass,1)-e*rq(srwmass,2))+(-f22/e)*(ru(srwmass,2)-e*y/r))")
(add-moment sT11x :body1 srwmass :direction [nx] :magnitude "((-f12/e)*(tu(srwmass,1)
+(r-y*tq(srwmass,1))*ru(srwmass,1)-e*rq(srwmass,2))
+(f22/e)*(ru(srwmass,2)+e*y/r))*rq(srwmass,2)*(y-rq(srwmass,1))")
(add-moment sT11z :body1 srwmass :direction [nz] :magnitude "((f12/e)*(tu(srwmass,1)
+(r-y*tq(srwmass,1))*ru(srwmass,1)-e*rq(srwmass,2))+(-f22/e)*(ru(srwmass,2)+e*y/r))")
; add normal force
(add-line-force sn8y :point1 sfwmass8 :direction [ny]
:magnitude "-((mc+2*mb+4*mw)*g/8)*(y+rq(sfwmass,1))")
(add-line-force sn9y :point1 sfwmass9 :direction [ny]
:magnitude "((mc+2*mb+4*mw)*g/8)*(y-rq(sfwmass,1))")
(add-line-force sn10y :point1 srwmass10 :direction [ny]
:magnitude "-((mc+2*mb+4*mw)*g/8)*(y+rq(srwmass,1))")
(add-line-force sn11y :point1 srwmass11 :direction [ny]
:magnitude "((mc+2*mb+4*mw)*g/8)*(y-rq(srwmass,1))")
(add-line-force sn8z :point1 sfwmass8 :direction [nz] :magnitude "-kzz*(x-x0)+((mc+2*mb+4*mw)*g/8)")
(add-line-force sn9z :point1 sfwmass9 :direction [nz] :magnitude "-kzz*(x-x0)+((mc+2*mb+4*mw)*g/8)")
(add-line-force sn10z :point1 srwmass10 :direction [nz] :magnitude "-kzz*(x-x0)+((mc+2*mb+4*mw)*g/8)")
(add-line-force sn11z :point1 srwmass11 :direction [nz] :magnitude "-kzz*(x-x0)+((mc+2*mb+4*mw)*g/8)")
;;;add external moment
(add-moment sn2x :body1 sfwmass :direction [nx] :magnitude "(Iwy*e*ru(sfwmass,2)/r)")
(add-moment sn2z :body1 sfwmass :direction [nz] :magnitude "-Iwy*e*ru(sfwmass,1)/r")
(add-moment sn3x :body1 srwmass :direction [nx] :magnitude "(Iwy*e*ru(srwmass,2)/r)")
(add-moment sn3z :body1 srwmass :direction [nz] :magnitude "-Iwy*e*ru(srwmass,1)/r")
; add output
(add-out "dot([nz],vel(cmasscm))" "Zs'_Carbody")
; feedback accelerations
(add-out "dot([nx],dxdt(vel(bmass13,fwmass4)))" "a4px")
(add-out "dot([ny],dxdt(vel(bmass12,fwmass4)))" "a4py")
(add-out "dot([nz],dxdt(vel(bmass14,fwmass4)))" "a4pz")
(add-out "dot([nx],dxdt(vel(bmass16,fwmass5)))" "a5px")
(add-out "dot([ny],dxdt(vel(bmass15,fwmass5)))" "a5py")
(add-out "dot([nz],dxdt(vel(bmass17,fwmass5)))" "a5pz")
(add-out "dot([nx],dxdt(vel(bmass19,rwmass6)))" "a6px")
(add-out "dot([ny],dxdt(vel(bmass18,rwmass6)))" "a6py")
(add-out "dot([nz],dxdt(vel(bmass20,rwmass6)))" "a6pz")
(add-out "dot([nx],dxdt(vel(bmass22,rwmass7)))" "a7px")
(add-out "dot([ny],dxdt(vel(bmass21,rwmass7)))" "a7py")
(add-out "dot([nz],dxdt(vel(bmass23,rwmass7)))" "a7pz")
(add-out "dot([nx],dxdt(vel(cmass27,bmass24)))" "a24sx")
(add-out "dot([ny],dxdt(vel(cmass28,bmass24)))" "a24sy")
(add-out "dot([nz],dxdt(vel(cmass29,bmass24)))" "a24sz")
(add-out "dot([nx],dxdt(vel(cmass30,bmass25)))" "a25sx")
(add-out "dot([ny],dxdt(vel(cmass31,bmass25)))" "a25sy")
(add-out "dot([nz],dxdt(vel(cmass32,bmass25)))" "a25sz")
(add-out "dot([nx],dxdt(vel(sbmass13,sfwmass4)))" "sa4px")
(add-out "dot([ny],dxdt(vel(sbmass12,sfwmass4)))" "sa4py")
(add-out "dot([nz],dxdt(vel(sbmass14,sfwmass4)))" "sa4pz")
(add-out "dot([nx],dxdt(vel(sbmass16,sfwmass5)))" "sa5px")
(add-out "dot([ny],dxdt(vel(sbmass15,sfwmass5)))" "sa5py")
(add-out "dot([nz],dxdt(vel(sbmass17,sfwmass5)))" "sa5pz")
(add-out "dot([nx],dxdt(vel(sbmass19,srwmass6)))" "sa6px")
(add-out "dot([ny],dxdt(vel(sbmass18,srwmass6)))" "sa6py")
(add-out "dot([nz],dxdt(vel(sbmass20,srwmass6)))" "sa6pz")
(add-out "dot([nx],dxdt(vel(sbmass22,srwmass7)))" "sa7px")
(add-out "dot([ny],dxdt(vel(sbmass21,srwmass7)))" "sa7py")
(add-out "dot([nz],dxdt(vel(sbmass23,srwmass7)))" "sa7pz")
(add-out "dot([nx],dxdt(vel(scmass27,sbmass24)))" "sa24sx")
(add-out "dot([ny],dxdt(vel(scmass28,sbmass24)))" "sa24sy")
(add-out "dot([nz],dxdt(vel(scmass29,sbmass24)))" "sa24sz")
(add-out "dot([nx],dxdt(vel(scmass30,sbmass25)))" "sa25sx")
(add-out "dot([ny],dxdt(vel(scmass31,sbmass25)))" "sa25sy")
(add-out "dot([nz],dxdt(vel(scmass32,sbmass25)))" "sa25sz")
(linear :u (Zr F4px F4py F4pz F5px F5py F5pz F6px F6py F6pz F7px F7py F7pz F24sx F24sy F24sz F25sx F25sy F25sz sF4px sF4py sF4pz sF5px sF5py sF5pz sF6px sF6py sF6pz sF7px sF7py sF7pz sF24sx sF24sy sF24sz sF25sx sF25sy sF25sz))
(set-defaults mc 8040 mb 350.26 mw 1117.9 Icx 28540 Icz 312367.4 Ibx 300 Ibz 105.2
Iwx 608.1 Iwy 72 Iwz 608.1 B1 1 B2 1.29 B3 2.29 L1 0.9 L2 1.9 L3 3.29
L4 4.68 L5 5.68 h 0.47 hs 1.67 h1 1 a 0.7175 r 0.43 y 0.05 kpx 9e5
kpy 3.9e5 kpz 4.32e5 cpx 1e4 cpy 1e4 cpz 3e4 ksx 4.5e4 ksy 4.5e4 ksz 4.5e5 csx 9e4 csy 1.8e3 csz 4000 f11 2.212e6 f12 3120 f22 16 f33 2.563e6 kzz 1e6 e 100/3.6 )
(write-to-file write-matlab "D:\\matlab\\dof28_S2.m")