;;; Reset AutoSim data structures and erase all remnants of multibody system
(reset)
;;; Set the units system to SI
(si)
;;; Set up uniform gravitational field for multibody system
(add-gravity :direction -[nz])
;;; Define a point fixed in a body
(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 car body mass
(add-body cmass :name "carbody mass" :joint-coordinates pt26 :cm-coordinates pt26 :translate (y z)
:body-rotation-axes (x y z) :mass "mc" :inertia-matrix ("Icx" "Icy" "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 front bogie mass
(add-body bmass :name "bogie mass" :joint-coordinates pt1 :cm-coordinates pt1 :translate (y z)
:body-rotation-axes (x y z) :mass "mb" :inertia-matrix ("Ibx" "Iby" "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 rear bogie mass
(add-body sbmass :name "sbogie mass" :joint-coordinates spt1 :cm-coordinates spt1 :translate (y z)
:body-rotation-axes (x y z) :mass "mb" :inertia-matrix ("Ibx" "Iby" "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 the first wheel-set mass
(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 the second wheel-set mass
(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 the third wheel-set mass
(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 the fourth wheel-set mass
(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 wheel force
(add-line-force u4x :direction [nx] :point1 fwmass4 :point2 bmass13 :magnitude "-kpx*(x-x0) -cpx*v ")
(add-line-force u4y :direction [ny] :point1 fwmass4 :point2 bmass12 :magnitude "-kpy*(x-x0) -cpy*v ")
(add-line-force u4z :direction [nz] :point1 fwmass4 :point2 bmass14 :magnitude "-kpz*(x-x0) -cpz*v ")
(add-line-force u5x :direction [nx] :point1 fwmass5 :point2 bmass16 :magnitude "-kpx*(x-x0) -cpx*v ")
(add-line-force u5y :direction [ny] :point1 fwmass5 :point2 bmass15 :magnitude "-kpy*(x-x0) -cpy*v ")
(add-line-force u5z :direction [nz] :point1 fwmass5 :point2 bmass17 :magnitude "-kpz*(x-x0) -cpz*v ")
(add-line-force u6x :direction [nx] :point1 rwmass6 :point2 bmass19 :magnitude "-kpx*(x-x0) -cpx*v ")
(add-line-force u6y :direction [ny] :point1 rwmass6 :point2 bmass18 :magnitude "-kpy*(x-x0) -cpy*v ")
(add-line-force u6z :direction [nz] :point1 rwmass6 :point2 bmass20 :magnitude "-kpz*(x-x0) -cpz*v ")
(add-line-force u7x :direction [nx] :point1 rwmass7 :point2 bmass22 :magnitude "-kpx*(x-x0) -cpx*v ")
(add-line-force u7y :direction [ny] :point1 rwmass7 :point2 bmass21 :magnitude "-kpy*(x-x0) -cpy*v ")
(add-line-force u7z :direction [nz] :point1 rwmass7 :point2 bmass23 :magnitude "-kpz*(x-x0) -cpz*v ")
;;; Add bogie force
(add-line-force u24x :direction [nx] :point1 bmass24 :point2 cmass27 :magnitude "-ksx*(x-x0) -csx*v ")
(add-line-force u24y :direction [ny] :point1 bmass24 :point2 cmass28 :magnitude "-ksy*(x-x0) -csy*v ")
(add-line-force u24z :direction [nz] :point1 bmass24 :point2 cmass29 :magnitude "-ksz*(x-x0) -csz*v ")
(add-line-force u25x :direction [nx] :point1 bmass25 :point2 cmass30 :magnitude "-ksx*(x-x0) -csx*v ")
(add-line-force u25y :direction [ny] :point1 bmass25 :point2 cmass31 :magnitude "-ksy*(x-x0) -csy*v")
(add-line-force u25z :direction [nz] :point1 bmass25 :point2 cmass32 :magnitude "-ksz*(x-x0) -csz*v ")
;;; 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 "-(w/2)*(y+rq(fwmass,1))")
(add-line-force n9y :point1 fwmass9 :direction [ny] :magnitude "(w/2)*(y-rq(fwmass,1))")
(add-line-force n10y :point1 rwmass10 :direction [ny] :magnitude "-(w/2)*(y+rq(rwmass,1))")
(add-line-force n11y :point1 rwmass11 :direction [ny] :magnitude "(w/2)*(y-rq(rwmass,1))")
(add-line-force n8z :point1 fwmass8 :direction [nz] :magnitude "-kzz*(x-x0)+(w/2)")
(add-line-force n9z :point1 fwmass9 :direction [nz] :magnitude "-kzz*(x-x0)+(w/2)")
(add-line-force n10z :point1 rwmass10 :direction [nz] :magnitude "-kzz*(x-x0)+(w/2)")
(add-line-force n11z :point1 rwmass11 :direction [nz] :magnitude "-kzz*(x-x0)+(w/2)")
;;; Add external moment
(add-moment n2x :body1 fwmass :direction [nx] :magnitude "(Iwy*e*ru(fwmass,2)/r)+(2*f12*y*y/r-y*y*w)*tq(fwmass,1)")
(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)+(2*f12*y*y/r-y*y*w)*tq(rwmass,1)")
(add-moment n3z :body1 rwmass :direction [nz] :magnitude "-Iwy*e*ru(rwmass,1)/r")
;;; Add wheel force
(add-line-force su4x :direction [nx] :point1 sfwmass4 :point2 sbmass13 :magnitude "-kpx*(x-x0) -cpx*v ")
(add-line-force su4y :direction [ny] :point1 sfwmass4 :point2 sbmass12 :magnitude "-kpy*(x-x0) -cpy*v ")
(add-line-force su4z :direction [nz] :point1 sfwmass4 :point2 sbmass14 :magnitude "-kpz*(x-x0) -cpz*v ")
(add-line-force su5x :direction [nx] :point1 sfwmass5 :point2 sbmass16 :magnitude "-kpx*(x-x0) -cpx*v ")
(add-line-force su5y :direction [ny] :point1 sfwmass5 :point2 sbmass15 :magnitude "-kpy*(x-x0) -cpy*v ")
(add-line-force su5z :direction [nz] :point1 sfwmass5 :point2 sbmass17 :magnitude "-kpz*(x-x0) -cpz*v ")
(add-line-force su6x :direction [nx] :point1 srwmass6 :point2 sbmass19 :magnitude "-kpx*(x-x0) -cpx*v ")
(add-line-force su6y :direction [ny] :point1 srwmass6 :point2 sbmass18 :magnitude "-kpy*(x-x0) -cpy*v ")
(add-line-force su6z :direction [nz] :point1 srwmass6 :point2 sbmass20 :magnitude "-kpz*(x-x0) -cpz*v ")
(add-line-force su7x :direction [nx] :point1 srwmass7 :point2 sbmass22 :magnitude "-kpx*(x-x0) -cpx*v ")
(add-line-force su7y :direction [ny] :point1 srwmass7 :point2 sbmass21 :magnitude "-kpy*(x-x0) -cpy*v ")
(add-line-force su7z :direction [nz] :point1 srwmass7 :point2 sbmass23 :magnitude "-kpz*(x-x0) -cpz*v ")
;;; Add bogie force
(add-line-force su24x :direction [nx] :point1 sbmass24 :point2 scmass27 :magnitude "-ksx*(x-x0) -csx*v ")
(add-line-force su24y :direction [ny] :point1 sbmass24 :point2 scmass28 :magnitude "-ksy*(x-x0) -csy*v ")
(add-line-force su24z :direction [nz] :point1 sbmass24 :point2 scmass29 :magnitude "-ksz*(x-x0) -csz*v ")
(add-line-force su25x :direction [nx] :point1 sbmass25 :point2 scmass30 :magnitude "-ksx*(x-x0) -csx*v ")
(add-line-force su25y :direction [ny] :point1 sbmass25 :point2 scmass31 :magnitude "-ksy*(x-x0) -csy*v ")
(add-line-force su25z :direction [nz] :point1 sbmass25 :point2 scmass32 :magnitude "-ksz*(x-x0) -csz*v ")
;;; 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 "-(w/2)*(y+rq(sfwmass,1))")
(add-line-force sn9y :point1 sfwmass9 :direction [ny] :magnitude "(w/2)*(y-rq(sfwmass,1))")
(add-line-force sn10y :point1 srwmass10 :direction [ny] :magnitude "-(w/2)*(y+rq(srwmass,1))")
(add-line-force sn11y :point1 srwmass11 :direction [ny] :magnitude "(w/2)*(y-rq(srwmass,1))")
(add-line-force sn8z :point1 sfwmass8 :direction [nz] :magnitude "-kzz*(x-x0)+(w/2)")
(add-line-force sn9z :point1 sfwmass9 :direction [nz] :magnitude "-kzz*(x-x0)+(w/2)")
(add-line-force sn10z :point1 srwmass10 :direction [nz] :magnitude "-kzz*(x-x0)+(w/2)")
(add-line-force sn11z :point1 srwmass11 :direction [nz] :magnitude "-kzz*(x-x0)+(w/2)")
;;; Add external moment
(add-moment sn2x :body1 sfwmass :direction [nx] :magnitude "(Iwy*e*ru(sfwmass,2)/r)+(2*f12*y*y/r-y*y*w)*tq(sfwmass,1)")
(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)+(2*f12*y*y/r-y*y*w)*tq(srwmass,1)")
(add-moment sn3z :body1 srwmass :direction [nz] :magnitude "-Iwy*e*ru(srwmass,1)/r")
;;; Linearize the equations of motion for the multibody system
(linear :u )
;;; Assign default numerical values to parameters
(set-defaults mc 8041.3 mb 350.26 mw 1117.9 Icx 31930.66 Icz 357603.3 Icy 386009.2 Ibx 117.92
Ibz 783.66 Iby 668.07 Iwx 608.1 Iwy 72 Iwz 608.1 B1 1 B2 1.29 B3 1.45 L1 4.9
L2 5.9 L3 7.29 L4 8.68 L5 9.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 30000 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 )
;;; Generate complete analysis code in Matlab language
(write-to-file write-matlab "D:\\matlab\\DOF_31_Full_Train_Model.m")