def test_frame(self):
# Given
s = visual.sphere()
a = visual.arrow()
f = visual.frame(s, a)
# Then
axis = (1.0, 0, 0)
assert_allclose(s.axis, axis, rtol=0, atol=1e-15)
assert_allclose(a.axis, axis, rtol=0, atol=1e-15)
assert_allclose(f.axis, axis, rtol=0, atol=1e-15)
# When
axis = (0, 1.0, 0.0)
f.axis = axis
# Then
assert_allclose(s.axis, axis, rtol=0, atol=1e-15)
assert_allclose(a.axis, axis, rtol=0, atol=1e-15)
assert_allclose(f.axis, axis, rtol=0, atol=1e-15)
# When
pos = (1., 1.0, 1.0)
f.pos = pos
# Then
assert_allclose(s.pos, pos, rtol=0, atol=1e-15)
assert_allclose(a.pos, pos, rtol=0, atol=1e-15)
assert_allclose(f.pos, pos, rtol=0, atol=1e-15)
def test_frame(self):
# Given
s = visual.sphere()
a = visual.arrow()
f = visual.frame(s, a)
# Then
axis = (1.0, 0, 0)
assert_allclose(s.axis, axis, rtol=0, atol=1e-15)
assert_allclose(a.axis, axis, rtol=0, atol=1e-15)
assert_allclose(f.axis, axis, rtol=0, atol=1e-15)
# When
axis = (0, 1.0, 0.0)
f.axis = axis
# Then
assert_allclose(s.axis, axis, rtol=0, atol=1e-15)
assert_allclose(a.axis, axis, rtol=0, atol=1e-15)
assert_allclose(f.axis, axis, rtol=0, atol=1e-15)
# When
pos = (1., 1.0, 1.0)
f.pos = pos
# Then
assert_allclose(s.pos, pos, rtol=0, atol=1e-15)
assert_allclose(a.pos, pos, rtol=0, atol=1e-15)
assert_allclose(f.pos, pos, rtol=0, atol=1e-15)
def func_init_robot_shape(self, robot_pos):
# draw 4 propellers
d = 0.145 # diameter of propeller is 14.5 cm
w = 0.16 # wheelbase is 16 cm (shortest distance between centers of two adjacent propellers)
p1 = visual.ring(pos=(w/2.0, w/2.0, 0), axis = (0, 0, 1), \
radius = d/2.0, thickness = 0.01, color = (1.0, 0, 0)) # Red
p2 = visual.ring(pos=(w/2.0, -w/2.0, 0), axis = (0, 0, 1), \
radius = d/2.0, thickness = 0.01, color = (0, 0, 1.0)) # Blue
p3 = visual.ring(pos=(-w/2.0, -w/2.0, 0), axis = (0, 0, 1), \
radius = d/2.0, thickness = 0.01, color = (0, 0, 1.0)) # Blue
p4 = visual.ring(pos=(-w/2.0, w/2.0, 0), axis = (0, 0, 1), \
radius = d/2.0, thickness = 0.01, color = (1.0, 0, 0)) # Red
# robot shape is a frame composites different objects
self.robot_draw = visual.frame(p1, p2, p3, p4)
self.robot_draw.pos = (robot_pos[0], robot_pos[1], robot_pos[2])
offset = 2.*gap # from center of pedestal to center of U-shaped upper assembly
top = vector(0,0,0) # top of inner bar of U-shaped upper assembly
theta1 = 1.3*pi/2. # initial upper angle (from vertical)
theta1dot = 0 # initial rate of change of theta1
theta2 = 0 # initial lower angle (from vertical)
theta2dot = 0 # initial rate of change of theta2
pedestal = box(pos = (top - vector(0, hpedestal/2.0, offset)),size = (wpedestal, 1.1*hpedestal, wpedestal), color = (0.4,0.4,0.5))
base = box(pos = (top - vector(0,hpedestal + tbase/2.0, offset)),size=(wbase, tbase, wbase),color = (0.4,0.4,0.5))
bar1 = box(pos=(L1display/2.0 - d/2.0, 0, -(gap+d)/2.0), size=(L1display, d, d), color=(1,0,0))
bar1b = box(pos=(L1display/2.0 - d/2.0, 0, (gap+d)/2.0), size=(L1display, d, d), color=(1,0,0))
frame1 = frame(bar1, bar1b)
frame1.pos = (0.0, 0.0, 0.0)
frame1.axis = (0.0, -1.0, 0.0)
frame1.rotate(axis=(0,0,1), angle = 180.0*theta1/pi)
bar2 = box(pos = (L2display/2.0 - d/2.0, 0, 0), size = (L2display, d, d), color = (0,1,0))
frame2 = frame(bar2)
frame2.pos = (0.0, -1.0*L1, 0.0)
frame2.axis = (0.0, -1.0, 0.0)
frame2.rotate(axis = (0,0,1), angle = 180.0*theta2/pi)
dt = 0.001
def anim():
global theta1, theta2, theta1dot, theta2dot
height=tbase,
length=wbase,
width=wbase,
color=pedestal.color)
shaft = cylinder(axis=(Lshaft, 0, 0),
length=Lshaft,
radius=Rshaft,
color=(0, 1, 0))
rotor = cylinder(pos=(Lshaft / 2 - Drotor / 2, 0, 0),
axis=(Drotor, 0, 0),
length=Drotor,
radius=Rrotor,
color=(1, 0, 0))
gyro = frame(shaft, rotor)
gyro.axis = (sin(theta) * sin(phi), cos(theta), sin(theta) * cos(phi))
trail = curve(radius=Rshaft / 8., color=(1, 1, 0))
r = Lshaft / 2.
dt = 0.0001
t = 0.
Nsteps = 20 # number of calculational steps between graphics updates
def anim():
global theta, phidot, alphadot, M, g, r, thetadot, phi, alpha, t
for step in range(Nsteps): # multiple calculation steps for accuracy
# Calculate accelerations of the Lagrangian coordinates:
atheta = (phidot**2 * sin(theta) * cos(theta) - 2. *
pedestal = box(pos=(top - vector(0, hpedestal / 2.0, offset)),
size=(wpedestal, 1.1 * hpedestal, wpedestal),
color=(0.4, 0.4, 0.5))
base = box(pos=(top - vector(0, hpedestal + tbase / 2.0, offset)),
size=(wbase, tbase, wbase),
color=(0.4, 0.4, 0.5))
bar1 = box(pos=(L1display / 2.0 - d / 2.0, 0, -(gap + d) / 2.0),
size=(L1display, d, d),
color=(1, 0, 0))
bar1b = box(pos=(L1display / 2.0 - d / 2.0, 0, (gap + d) / 2.0),
size=(L1display, d, d),
color=(1, 0, 0))
frame1 = frame(bar1, bar1b)
frame1.pos = (0.0, 0.0, 0.0)
frame1.axis = (0.0, -1.0, 0.0)
frame1.rotate(axis=(0, 0, 1), angle=180.0 * theta1 / pi)
bar2 = box(pos=(L2display / 2.0 - d / 2.0, 0, 0),
size=(L2display, d, d),
color=(0, 1, 0))
frame2 = frame(bar2)
frame2.pos = (0.0, -1.0 * L1, 0.0)
frame2.axis = (0.0, -1.0, 0.0)
frame2.rotate(axis=(0, 0, 1), angle=180.0 * theta2 / pi)
dt = 0.001
##phidot = (-alphadot+sqrt(alphadot**2+2*M*g*r*cos(theta)/I))/cos(theta)
pedestal = box(pos=top-vector(0,hpedestal/2.,0),
height=hpedestal, length=wpedestal, width=wpedestal,
color=(0.4,0.4,0.5))
base = box(pos=top-vector(0,hpedestal+tbase/2.,0),
height=tbase, length=wbase, width=wbase,
color=pedestal.color)
shaft = cylinder(axis=(Lshaft,0,0), length = Lshaft,
radius=Rshaft, color=(0,1,0))
rotor = cylinder(pos=(Lshaft/2 - Drotor/2, 0, 0),
axis=(Drotor, 0, 0), length = Drotor,
radius=Rrotor, color=(1,0,0))
gyro = frame(shaft, rotor)
gyro.axis = (sin(theta)*sin(phi),cos(theta),sin(theta)*cos(phi))
trail = curve(radius=Rshaft/8., color=(1,1,0))
r = Lshaft/2.
dt = 0.0001
t = 0.
Nsteps = 20 # number of calculational steps between graphics updates
def anim():
global theta, phidot, alphadot, M, g, r, thetadot, phi, alpha, t
for step in range(Nsteps): # multiple calculation steps for accuracy
# Calculate accelerations of the Lagrangian coordinates:
atheta = (phidot**2*sin(theta)*cos(theta)
-2.*(alphadot+phidot*cos(theta))*phidot*sin(theta)
