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)