-
Notifications
You must be signed in to change notification settings - Fork 11
/
dpendulum.py
executable file
·79 lines (63 loc) · 2.11 KB
/
dpendulum.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
#!/usr/bin/env python3
import numpy
import pyservoce
import zencad
import time
from zencad import *
import zencad.elibs.solver
import zencad.elibs.constraits as constraits
from zencad.elibs.rigid_body import rigid_body
#import zencad.libs.kinematic as kinematic
from zencad.libs.inertia import inertia
from zencad.libs.screw import screw
numpy.set_printoptions(suppress=True)
numpy.set_printoptions(precision=3, linewidth=160)
L=20
body = zencad.cylinder(r=5, h=L, center=True).rotateY(deg(90))
body2 = zencad.cylinder(r=5, h=L, center=True).rotateY(deg(90))
body3 = zencad.cylinder(r=5, h=L, center=True).rotateY(deg(90))
abody = zencad.disp(body)
bbody = zencad.disp(body2)
bbody3 = zencad.disp(body3)
a = rigid_body(inertia=inertia(radius = pyservoce.vector3(10,0,0)), pose=zencad.moveX(10))
b = rigid_body(inertia=inertia(radius = pyservoce.vector3(10,0,0)), pose=zencad.moveX(30))
b3 = rigid_body(inertia=inertia(radius = pyservoce.vector3(10,0,0)), pose=zencad.moveX(50))
a.add_view(abody)
b.add_view(bbody)
b3.add_view(bbody3)
#b.pose=zencad.transform.right(20) #* zencad.transform.rotateY(deg(20))
#a.set_speed(screw(lin=(0,0,0), ang=(0,0,0)))
#b.set_speed(screw(lin=(0,0,-6*10), ang=(0,6,0)))
c2 = constraits.spherical_rotator()
c2.attach_positive_connection(body=b3, pose=moveX(-10))
c2.attach_negative_connection(body=b, pose=moveX(10))
c = constraits.spherical_rotator()
c.attach_positive_connection(body=b, pose=moveX(-10))
c.attach_negative_connection(body=a, pose=moveX(10))
c1 = constraits.spherical_rotator()
c1.attach_reference(body=a, pose=moveX(-10))
solver = zencad.elibs.solver.matrix_solver(rigid_bodies=[a,b,b3], constraits=[c,c1,c2],
world_dempher=0.2,
gravity=(0,0,-100))
solver.update_views()
solver.update_globals()
#exit(0)
starttime = time.time()
lasttime = starttime
noinited = True
def animate(wdg):
global noinited
global lasttime
if noinited:
time.sleep(1)
noinited= False
maxdelta = 0.01
curtime = time.time()
delta = curtime - lasttime
lasttime = curtime
if delta > maxdelta:
delta = maxdelta
DELTA = delta
solver.solve()
solver.apply(DELTA)
show(animate=animate, animate_step = 0.00001)