-
Notifications
You must be signed in to change notification settings - Fork 8
/
Copy pathexa_4_strange_pendulum.py
85 lines (54 loc) · 2.22 KB
/
exa_4_strange_pendulum.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
# -*- coding: utf-8 -*-
"""
Created on Tue Apr 14 20:41:09 2015
@author: oliver
"""
import numpy as np
from sympy import symbols
import mubosym as mbs
###############################################################
# general system setup example
myMBS = mbs.MBSworld('strange_pendulum', connect=False, force_db_setup=False)
b_n = []
m_n = []
b_n_max = 3
for ii in range(b_n_max):
b_n.append(str(ii))
m_n.append(str(ii)+"_M0")
I = [1.,1.,1.]
##################################
# cracy pendulum
myMBS.add_marker('world_M1', 'world',0.,0.,-np.pi/4.,0.,0.) #np.pi/4.,np.pi/4.)
#b_n[0] = myMBS.add_body_3d(999, 1, 1.0, I , 'rod-revolute', parameters = [0.,np.pi/2,2.0]) #[np.pi/2., 2.0])
#b_n[0] = myMBS.add_body_3d(999, 1, 1.0, I , 'rod-2-cardanic', parameters = [2.0]) #[np.pi/2., 2.0])
myMBS.add_body_3d(b_n[0], 'world_M1', 1.0, I , 'angle-rod', parameters = [-np.pi/4., 2.0])
myMBS.add_force_special(b_n[0], 'grav')
myMBS.add_marker(m_n[0], b_n[0], 0.,0.,0.)
myMBS.add_body_3d(b_n[1], m_n[0], 1.0, I, 'angle-rod', parameters = [-np.pi/4., 2.0])
myMBS.add_force_special(b_n[1], 'grav')
myMBS.add_marker(m_n[1], b_n[1], 0.,0.,0.)
myMBS.add_body_3d(b_n[2], m_n[1], 1.0, I, 'angle-rod', parameters = [-np.pi/4., 2.0])
myMBS.add_force_special(b_n[2], 'grav')
#myMBS.add_marker(b_n[2], 0.,0.,0.)
#b_n[3] = myMBS.add_body_3d(b_n[2], 0, 1.0, I, 'angle-rod', parameters = [np.pi/4., 2.0])
#myMBS.add_force(b_n[3], 'grav')
x0 = np.hstack(( 1. * np.ones(myMBS.dof), 1. * np.ones(myMBS.dof)))
#for b in myMBS.bodies.keys():
# myMBS.add_damping(b,0.1)
#################################################
# constants
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)
body_frames_in_graphics = [b_n[0],b_n[1],b_n[2]]
fixed_frames_in_graphics = []
myMBS.kaneify()
myMBS.prep_lambdas(body_frames_in_graphics, fixed_frames_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)
jac = myMBS.calc_lin_analysis_n(len(myMBS.x_t)-1)
myMBS.prepare(mbs.DATA_PATH, save=True)
myMBS.animate(t_max, dt, scale = 4, time_scale = 1, t_ani = 30.0)