-
Notifications
You must be signed in to change notification settings - Fork 8
/
Copy pathexa_7_quarter_car.py
167 lines (126 loc) · 5.23 KB
/
exa_7_quarter_car.py
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
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
# -*- coding: utf-8 -*-
"""
Created on Tue Apr 14 20:41:09 2015
@author: oliver
"""
import numpy as np
from sympy import symbols, sin
import mubosym as mbs
from interp1d_interface import interp
###############################################################
# general system setup example
myMBS = mbs.MBSworld('quarter_car', connect=True, force_db_setup=False)
#prepare a standard
I_car = [500.,500.,500.]
I = [0.,0.,0.]
I_tire = [1.,1.,1.]
###################################
# quarter car model - revisited
myMBS.add_marker('world_M1', 'world', 0.,0.,0.)
###################################
# body of car
myMBS.add_body_3d('car_body', 'world_M1', 500.0, I_car , 'free-3-translate-z-rotate', parameters = [], graphics = False)
myMBS.add_marker('car_body_M0', 'car_body', 0.,0.,0.)
myMBS.add_force_special('car_body', 'grav')
myMBS.add_body_3d('tire_carrier', 'car_body_M0', 50.0, I , 'y-axes', parameters = [])
###################################
# body of tire
A = -0.15
omega = 0.8
def rotation_inp(t):
return A*np.sin(omega*t)
def rotation_inp_diff(t):
return A*omega*np.cos(omega*t)
def rotation_inp_diff_2(t):
return -A*omega*omega*np.sin(omega*t)
def rotation_inp_expr():
t, A = symbols('t A')
return A*sin(1.0*t)
k = interp(filename = mbs.DATA_PATH+'/vel_01.dat')
#high end definition of static variables...
@mbs.static_vars(t_p=0, diff_p=0)
def lateral_inp(t):
#return -20.
velocity = myMBS.get_control_signal(0)
v_soll = k.f_interp(t)
diff = (v_soll-velocity)/10.0
delt = (t-lateral_inp.t_p)
diff = (lateral_inp.diff_p *0.5 + diff* delt) / (delt + 0.5)
lateral_inp.diff_p = diff
lateral_inp.t_p = t
return -2000*diff
def lateral_inp_diff(t):
return 0.
def lateral_inp_diff_2(t):
return 0.
A = symbols('A')
myMBS.add_parameter_expr('phi', rotation_inp_expr(), {A: 0.05})
#myMBS.add_parameter('phi', rotation_inp, rotation_inp_diff, rotation_inp_diff_2)
myMBS.add_parameter('theta_lateral', lateral_inp, lateral_inp_diff, lateral_inp_diff_2)
myMBS.add_rotating_marker_para('tire_carrier_M0', 'tire_carrier', 'phi', 0.,-0.2,0., 'Y')
myMBS.add_body_3d('tire','tire_carrier_M0', 1.0, I_tire , 'revolute-Z', parameters = [])
#
####################################
## some tire forces
myMBS.add_force_special('tire_carrier', 'grav')
myMBS.add_force_special('tire', 'grav')
#myMBS.add_force_special('tire_carrier', 'spring-damper-axes', parameters = [20000., -0.9, 500.])
#myMBS.add_force('tire_carrier', 'car_body_M0', parameters = [20000., 0.8, 700.])
myMBS.add_force_spline_r('tire_carrier', 'car_body_M0', mbs.DATA_PATH+"/force_spring.dat", [0.8, 1.0])
myMBS.add_force_spline_v('tire_carrier', 'car_body_M0', mbs.DATA_PATH+"/force_damper.dat", [1.0])
myMBS.add_one_body_force_model('tiremodel', 'tire', 'tire_carrier_M0', 'tire')
myMBS.add_parameter_torque('tire', 'tire_carrier_M0', [0.,0.,1.], 'theta_lateral')
##################################
# create control signals:
vel_1 = myMBS.get_body('tire').get_vel_magnitude()
myMBS.add_control_signal(vel_1, "Geschw.", "m/s")
dist_z = myMBS.get_body('tire').z()
myMBS.add_control_signal(dist_z, "Abstand zur Spur", "m")
dist_x = myMBS.get_body('tire').x()
myMBS.add_control_signal(dist_x, "Fahrstrecke", "m")
##################################
# try to add into model
m1 = myMBS.get_model('tiremodel')
m1.add_signal(vel_1)
m1.add_signal(vel_1)
#x0 = np.hstack(( 0. * np.ones(myMBS.dof), 0. * np.ones(myMBS.dof)))
x0 = np.array([ -3.06217550e-02, -1.60406433e-08, 8.27725196e-01,
-3.21284978e-10, -6.54751512e-01, -1.59008129e+01,
-8.35888371e-04, -8.20890122e-10, 3.61938302e-05,
-2.51438728e-11, -3.30521499e-05, -2.50105938e-09])
#for b in myMBS.bodies.keys():
# myMBS.add_damping(b,0.1)
#################################################
# external force definitions
#def ext_sinus(t):
# omega = 0.5*t
# return 1000.0*np.sin(omega * t)
#
#myMBS.add_force_ext('tire', 'world_M0', 0.,1.,0., ext_sinus)
moving_frames_in_graphics = ['tire_carrier_M0']
fixed_frames_in_graphics = []#myMBS.mames[999][0], myMBS.marker_frames[999][1]]
forces_in_graphics = ['car_body']
bodies_in_graphics = {'tire': 'tire'}
##################################################################
# constant parameters
g = symbols('g')
constants = [ g ] # Parameter definitions
constants_vals = [9.81] # Numerical value
const_dict = dict(zip(constants, constants_vals))
myMBS.set_const_dict(const_dict)
myMBS.kaneify()
##################################################################
# now the equations are fully provided with parameters in ...
#################################################
myMBS.prep_lambdas(moving_frames_in_graphics, fixed_frames_in_graphics, forces_in_graphics, bodies_in_graphics)
dt = 0.01 # 10 ms for a nice animation result
t_max = 30.
myMBS.inte_grate_full(x0, t_max, dt, mode = 0, tolerance = 1e-0)
x_final = myMBS.x_t[-1]
################################################
# linear analysis of the last state (returns also the jacobian)
jac = myMBS.calc_lin_analysis_n(len(myMBS.x_t)-1)
myMBS.prepare(mbs.DATA_PATH, save=True)
myMBS.plotting(t_max, dt, plots='signals')
#myMBS.plotting(t_max, dt, plots='tire')
#x = myMBS.animate(t_max, dt, scale = 4, time_scale = 1.0, labels = True, t_ani = 30., center = 2)