Skip to content

Commit 89db91f

Browse files
tarzzztarzzz
committed
Added an illustrative example to demonstrate the workflow of
pydy_viz
1 parent e69d5cf commit 89db91f

File tree

3 files changed

+378
-7
lines changed

3 files changed

+378
-7
lines changed

‎illustrative_example_for_viz/illustrative_example.py

Lines changed: 221 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,221 @@
1+
#This program represents a hypothetical situation for a complete workflow
2+
#for simulating a three link pendulum with links as rigid bodies
3+
from sympy import symbols,sympify
4+
from sympy.physics.mechanics import *
5+
6+
#Number of links = 3
7+
N_links = 3
8+
9+
#Number of masses = 3
10+
N_bobs = 3
11+
12+
#Defining Dynamic Symbols ................
13+
14+
#Generalized coordinates(angular) ...
15+
16+
alpha = dynamicsymbols('alpha1 alpha2 alpha3')
17+
beta = dynamicsymbols('beta1 beta2 beta3')
18+
19+
#Generalized speeds(angular) ...
20+
alphad = dynamicsymbols('alpha1 alpha2 alpha3',1)
21+
betad = dynamicsymbols('beta1 beta2 beta3',1)
22+
23+
#Mass of each bob:
24+
m = symbols('m:'+str(N_bobs))
25+
26+
27+
#Length mass and radii of each link(assuming as rods) ..
28+
l = symbols('l:' + str(N_links))
29+
M = symbols('M:' + str(N_links))
30+
radii = symbols('radii:' + str(N_links))
31+
32+
#For storing Inertia for each link :
33+
Ixx = symbols('Ixx:'+str(N_links))
34+
Iyy = symbols('Iyy:'+str(N_links))
35+
36+
#gravity and time ....
37+
g, t = symbols('g t')
38+
39+
40+
#Now defining an Inertial ReferenceFrame First ....
41+
42+
I = ReferenceFrame('I')
43+
44+
#And some other frames ...
45+
46+
A = ReferenceFrame('A')
47+
A.orient(I,'Body',[alpha[0],beta[0],0],'ZXY')
48+
B = ReferenceFrame('B')
49+
B.orient(I,'Body',[alpha[1],beta[1],0],'ZXY')
50+
C = ReferenceFrame('C')
51+
C.orient(I,'Body',[alpha[2],beta[2],0],'ZXY')
52+
53+
54+
#Setting angular velocities of new frames ...
55+
A.set_ang_vel(I, alphad[0] * I.z + betad[0] * I.x)
56+
B.set_ang_vel(I, alphad[1] * I.z + betad[1] * I.x)
57+
C.set_ang_vel(I, alphad[2] * I.z + betad[2] * I.x)
58+
59+
60+
61+
# An Origin point, with velocity = 0
62+
O = Point('O')
63+
O.set_vel(I,0)
64+
65+
#Three more points, for masses ..
66+
P1 = O.locatenew('P1', l[0] * A.y)
67+
P2 = O.locatenew('P2', l[1] * B.y)
68+
P3 = O.locatenew('P3', l[2] * C.y)
69+
70+
#Setting velocities of points with v2pt theory ...
71+
P1.v2pt_theory(O, I, A)
72+
P2.v2pt_theory(P1, I, B)
73+
P3.v2pt_theory(P2, I, C)
74+
points = [P1,P2,P3]
75+
76+
Pa1 = Particle('Pa1', points[0], m[0])
77+
Pa2 = Particle('Pa2', points[1], m[1])
78+
Pa3 = Particle('Pa3', points[2], m[2])
79+
particles = [Pa1,Pa2,Pa3]
80+
81+
82+
83+
#defining points for links(RigidBodies)
84+
#Assuming CoM as l/2 ...
85+
P_link1 = O.locatenew('P_link1', l[0]/2 * A.y)
86+
P_link2 = O.locatenew('P_link1', l[1]/2 * B.y)
87+
P_link3 = O.locatenew('P_link1', l[2]/2 * C.y)
88+
89+
#setting velocities of these points with v2pt theory ...
90+
P_link1.v2pt_theory(O, I, A)
91+
P_link2.v2pt_theory(P_link1, I, B)
92+
P_link3.v2pt_theory(P_link2, I, C)
93+
94+
points_rigid_body = [P_link1,P_link2,P_link3]
95+
96+
97+
#defining inertia tensors for links
98+
99+
inertia_link1 = inertia(A,Ixx[0],Iyy[0],0)
100+
inertia_link2 = inertia(B,Ixx[1],Iyy[1],0)
101+
inertia_link3 = inertia(C,Ixx[2],Iyy[2],0)
102+
103+
#Defining links as Rigid bodies ...
104+
105+
link1 = RigidBody('link1', P_link1, A, M[0], (inertia_link1, O))
106+
link2 = RigidBody('link2', P_link2, B, M[1], (inertia_link2, P1))
107+
link3 = RigidBody('link3', P_link3, C, M[2], (inertia_link3, P2))
108+
links = [link1,link2,link3]
109+
110+
111+
#Defining a basic shape for links ..
112+
rod1 = Cylinder(length=l[0],radii=radii[0])
113+
rod2 = Cylinder(length=l[1],radii=radii[1])
114+
rod3 = Cylinder(length=l[2],radii=radii[2])
115+
116+
link1.shape(rod1)
117+
link2.shape(rod2)
118+
link3.shape(rod3)
119+
120+
#Applying forces on all particles , and adding all forces in a list..
121+
forces = []
122+
for particle in particles:
123+
124+
mass = particle.get_mass()
125+
point = particle.get_point()
126+
forces.append((point, -mass * g * I.y) )
127+
128+
#Applying forces on rigidbodies ..
129+
for link in links:
130+
mass = link.get_mass()
131+
point = link.get_masscenter()
132+
forces.append((point, -mass * g * I.y) )
133+
134+
kinetic_differentials = []
135+
for i in range(0,N_bobs):
136+
kinetic_differentials.append(alphad[i] - alpha[i])
137+
kinetic_differentials.append(betad[i] - beta[i])
138+
139+
#Adding particles and links in the same system ...
140+
total_system = []
141+
for particle in particles:
142+
total_system.append(particle)
143+
144+
for link in links:
145+
total_system.append(link)
146+
147+
q = []
148+
for angle in alpha:
149+
q.append(angle)
150+
for angle in beta:
151+
q.append(angle)
152+
print q
153+
u = []
154+
155+
for vel in alphad:
156+
u.append(vel)
157+
for vel in betad:
158+
u.append(vel)
159+
160+
print u
161+
kane = KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kinetic_differentials)
162+
fr, frstar = kane.kanes_equations(forces, total_system)
163+
164+
print fr
165+
166+
#Now we have symbolic equations of motion. ..
167+
# we integrate them numerically. ..
168+
169+
params = [g ,l1,l2,l3,m1,m2,m3,M1,M2,M3]
170+
171+
param_vals = [9.8 ,1.0,1.0,1.0,2,2,2,5,5,5]
172+
173+
right_hand_side = code_generator(kane,params)
174+
175+
#setting initial conditions ..
176+
init_conditions = [radians(45),radians(45),radians(30),\
177+
radians(30),radians(15),radians(15),\
178+
0, 0, 0,\
179+
0, 0, 0]
180+
t = [0,0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1.0]
181+
182+
numerical_vals = odeint(right_hand_side,init_conditions,t)
183+
184+
#Now for each t, we have numerical vals of coordinates ..
185+
#Now we set up a visualization frame,
186+
187+
frame1 = VisualizationFrame('frame1',I,O)
188+
189+
frame1.add_rigidbodies(links)
190+
191+
frame1.add_particles(particles)
192+
193+
param_vals_for_viz = {'g':9.8 ,'l1':1.0,'l2':1.0,'l3':1.0,'m1':2,'m2':2,'m3':2,'M1':5,'M1':5,'M1':5]
194+
195+
json = frame1.generate_json(initial_conditions,q)
196+
#Here we can replace initial_conditions with the conditions at any
197+
#specific time interval ....
198+
199+
##This line does following things ...
200+
##calls RigidBody.transform_matrix() on all rigidbodies,
201+
#so that we have info of rigidbodies(CoM,rotation,translation)
202+
# w.r.t VisualizationFrame('I' in this case)
203+
##Even if they are defined in any other frame ....
204+
##calls point.set_pos() for all particles with arg
205+
##as Point in VisualizationFrame(O here) ..
206+
##With these and init_conditions, we have a starting point of visualization ..
207+
208+
scene = Scene()
209+
Scene.view(json) # Just visualize/view the system at initial_conditions,
210+
#w.r.t VIsualizationFrame(I in this case)
211+
# No simulation here. ..
212+
213+
Scene.simulate(json,numerical_vals) # modify the input json,
214+
#Add the values at different times from numerical_vals,
215+
#To the json, which is then passed to
216+
#javascript
217+
#
218+
#(alpha1,alpha2,alpha3,beta1,beta2,beta3) at time=t
219+
220+
221+

‎illustrative_example_for_viz/three_link_pendulum.py

Lines changed: 157 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,157 @@
1+
#For this one we assume RigidBody links
2+
from sympy import symbols,sympify
3+
from sympy.physics.mechanics import *
4+
5+
#Number of links = 3
6+
N_links = 3
7+
8+
#Number of masses = 3
9+
N_bobs = 3
10+
11+
#Defining Dynamic Symbols ................
12+
13+
#Generalized coordinates(angular) ...
14+
15+
alpha = dynamicsymbols('alpha1 alpha2 alpha3')
16+
beta = dynamicsymbols('beta1 beta2 beta3')
17+
18+
#Generalized speeds(angular) ...
19+
alphad = dynamicsymbols('alpha1 alpha2 alpha3',1)
20+
betad = dynamicsymbols('beta1 beta2 beta3',1)
21+
22+
#Mass of each bob:
23+
m = symbols('m:'+str(N_bobs))
24+
25+
26+
#Length and mass of each link ..
27+
l = symbols('l:' + str(N_links))
28+
M = symbols('M:' + str(N_links))
29+
#For storing Inertia for each link :
30+
Ixx = symbols('Ixx:'+str(N_links))
31+
Iyy = symbols('Iyy:'+str(N_links))
32+
33+
#gravity and time ....
34+
g, t = symbols('g t')
35+
36+
37+
#Now defining an Inertial ReferenceFrame First ....
38+
39+
I = ReferenceFrame('I')
40+
41+
#And some other frames ...
42+
43+
A = ReferenceFrame('A')
44+
A.orient(I,'Body',[alpha[0],beta[0],0],'ZXY')
45+
B = ReferenceFrame('B')
46+
B.orient(I,'Body',[alpha[1],beta[1],0],'ZXY')
47+
C = ReferenceFrame('C')
48+
C.orient(I,'Body',[alpha[2],beta[2],0],'ZXY')
49+
50+
51+
#Setting angular velocities of new frames ...
52+
A.set_ang_vel(I, alphad[0] * I.z + betad[0] * I.x)
53+
B.set_ang_vel(I, alphad[1] * I.z + betad[1] * I.x)
54+
C.set_ang_vel(I, alphad[2] * I.z + betad[2] * I.x)
55+
56+
57+
58+
# An Origin point, with velocity = 0
59+
O = Point('O')
60+
O.set_vel(I,0)
61+
62+
#Three more points, for masses ..
63+
P1 = O.locatenew('P1', l[0] * A.y)
64+
P2 = O.locatenew('P2', l[1] * B.y)
65+
P3 = O.locatenew('P3', l[2] * C.y)
66+
67+
#Setting velocities of points with v2pt theory ...
68+
P1.v2pt_theory(O, I, A)
69+
P2.v2pt_theory(P1, I, B)
70+
P3.v2pt_theory(P2, I, C)
71+
points = [P1,P2,P3]
72+
73+
Pa1 = Particle('Pa1', points[0], m[0])
74+
Pa2 = Particle('Pa2', points[1], m[1])
75+
Pa3 = Particle('Pa3', points[2], m[2])
76+
particles = [Pa1,Pa2,Pa3]
77+
78+
79+
80+
#defining points for links(RigidBodies)
81+
#Assuming CoM as l/2 ...
82+
P_link1 = O.locatenew('P_link1', l[0]/2 * A.y)
83+
P_link2 = O.locatenew('P_link1', l[1]/2 * B.y)
84+
P_link3 = O.locatenew('P_link1', l[2]/2 * C.y)
85+
#setting velocities of these points with v2pt theory ...
86+
P_link1.v2pt_theory(O, I, A)
87+
P_link2.v2pt_theory(P_link1, I, B)
88+
P_link3.v2pt_theory(P_link2, I, C)
89+
90+
points_rigid_body = [P_link1,P_link2,P_link3]
91+
92+
93+
#defining inertia tensors for links
94+
95+
inertia_link1 = inertia(A,Ixx[0],Iyy[0],0)
96+
inertia_link2 = inertia(B,Ixx[1],Iyy[1],0)
97+
inertia_link3 = inertia(C,Ixx[2],Iyy[2],0)
98+
99+
#Defining links as Rigid bodies ...
100+
101+
link1 = RigidBody('link1', P_link1, A, M[0], (inertia_link1, O))
102+
link2 = RigidBody('link2', P_link2, B, M[1], (inertia_link2, P1))
103+
link3 = RigidBody('link3', P_link3, C, M[2], (inertia_link3, P2))
104+
links = [link1,link2,link3]
105+
106+
107+
#Applying forces on all particles , and adding all forces in a list..
108+
forces = []
109+
for particle in particles:
110+
111+
mass = particle.get_mass()
112+
point = particle.get_point()
113+
forces.append((point, -mass * g * I.y) )
114+
115+
#Applying forces on rigidbodies ..
116+
for link in links:
117+
mass = link.get_mass()
118+
point = link.get_masscenter()
119+
forces.append((point, -mass * g * I.y) )
120+
kinetic_differentials = []
121+
for i in range(0,N_bobs):
122+
kinetic_differentials.append(alphad[i] - alpha[i])
123+
kinetic_differentials.append(betad[i] - beta[i])
124+
125+
#Adding particles and links in the same system ...
126+
total_system = []
127+
for particle in particles:
128+
total_system.append(particle)
129+
130+
for link in links:
131+
total_system.append(link)
132+
133+
q = []
134+
for angle in alpha:
135+
q.append(angle)
136+
for angle in beta:
137+
q.append(angle)
138+
print q
139+
u = []
140+
141+
for vel in alphad:
142+
u.append(vel)
143+
for vel in betad:
144+
u.append(vel)
145+
146+
print u
147+
kane = KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kinetic_differentials)
148+
fr, frstar = kane.kanes_equations(forces, total_system)
149+
150+
print fr
151+
152+
153+
154+
155+
156+
157+

‎three_link_pendulum/three_link_pendulum.py

Lines changed: 0 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -148,10 +148,3 @@
148148
fr, frstar = kane.kanes_equations(forces, total_system)
149149

150150
print fr
151-
152-
153-
154-
155-
156-
157-

0 commit comments

Comments
 (0)