mubosym/exa_5_rotating_pendulum.py at master · DocBO/mubosym · GitHub

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 Wed Jun  3 21:15:49 2015

@author: oliver
"""
import numpy as np
from sympy import symbols
import mubosym as mbs


###############################################################
# general system setup example
myMBS = mbs.MBSworld('rotating_pendulum', connect=False, force_db_setup=False)

I = [1.,1.,1.]

#############################################################
# rotating frame constraint
#
omega = 1.0
A = 5.5
def rotation_inp(t):
    return A*t #np.sin(omega*t)

def rotation_inp_diff(t):
    return A #*omega*np.cos(omega*t)

def rotation_inp_diff_2(t):
    return 0.# A*omega*omega*np.sin(omega*t)

#use of an external parameter (be carefull: you have to provide also the time derivatives up to second order)
myMBS.add_parameter('phi', rotation_inp, rotation_inp_diff, rotation_inp_diff_2)
myMBS.add_rotating_marker_para('rot_M0', 'world', 'phi', 0., 1.5, 0., 'Y')

#alternatively use of a steady rotating frame without extra parameter name, constant omega
#myMBS.add_rotating_marker('rot_M0', 'world',0., 0., 0., 2.5, 'Y')

myMBS.add_body_3d('mount', 'rot_M0', 1.0, I , 'rod-zero-X', parameters = [1.0]) #[np.pi/2., 2.0])
myMBS.add_force_special('mount', 'grav')


myMBS.add_marker('mount_M0', 'mount', 0.,0.,0.)
myMBS.add_body_3d('pendulum', 'mount_M0', 1.0, I , 'rod-1-cardanic', parameters = [-1.5,0.]) #[np.pi/2., 2.0])
myMBS.add_force_special('pendulum', 'grav')

x0 = np.hstack(( 0. * np.ones(myMBS.dof), 0. * np.zeros(myMBS.dof)))


body_frames_in_graphics = ['mount', 'pendulum']
fixed_frames_in_graphics = []


for b in myMBS.bodies.keys():
    myMBS.add_damping(b,0.5)


#################################################
# 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)


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)

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.animate(t_max, dt, scale = 4, time_scale = 1.0, t_ani = 20., labels = True)