def draw(self,
screen,
q=Quaternion(1, 0, 0, 0),
centre_pos=Vector3(0, 0, 0)):
""" draw object at given rotation """
assert isinstance(screen, Screen)
self.origin()
#self.rotate(q)
self.rotate_move(q, centre_pos)
sides = self.sides()
edges = self.edges()
drawables = sides + edges
drawables.sort(key=lambda s: screen.depth(s.centroid()))
[s.draw(screen) for s in drawables]
def test_imu(self):
delay_sec = .05
imuc = IMUListener.IMUListener(delay_sec)
imuc.init()
while True:
control = imuc.get()
print(control)
if control is not None:
q = Quaternion(control["quatW"], control["quatX"],
control["quatY"], control["quatZ"]).normalized()
posi = Vector3(0, 0, 0)
if self.isGrid:
self.grid.draw(self.screen)
self.cube.draw(self.screen, q, posi)
event = pygame.event.poll()
if event.type == pygame.QUIT \
or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
break
pygame.display.flip()
pygame.time.delay(20)
self.cube.erase(self.screen)
def collect_anims2(self, joint, anims):
for (joint_name, joint_matrix, child_joints, vertex_refs,
actor) in joint:
#print "anims for %s" % joint_name
#get bone displacement
displacement = actor.LocalDisplacement()
origin = actor.Origin()
parentOrigin = actor.Parent().Origin()
if actor.Name() == self.figure.ParentActor().Name():
parentOrigin = origin
displacement = vec_add(vec_subtract(origin, parentOrigin),
displacement)
displacement = vec_subtract(origin, parentOrigin)
# get rotation
quat_tuple = actor.LocalQuaternion()
quat = Quaternion(quat_tuple[0], quat_tuple[1], quat_tuple[2],
quat_tuple[3])
hpr = radians_to_degrees(quat.get_euler())
#store displacement/rotation in anims data
if joint_name not in anims:
anims[joint_name] = []
anims[joint_name].append((displacement, hpr))
self.collect_anims2(child_joints, anims)
return anims
def deserialise(self, ser, estimated=False):
(px, py, pz)=ControlledSer.vAssign(ser, ControlledSer._POS)
(aw, ax, ay, az)=ControlledSer.qAssign(ser, ControlledSer._ATT)
(vx, vy, vz)=ControlledSer.vAssign(ser, ControlledSer._VEL)
return Mirrorable.deserialise(self, ser, estimated).setPos(Vector3(px,py,pz)).setAttitude(Quaternion(aw,ax,ay,az)).setVelocity(Vector3(vx,vy,vz))
def run(self):
tic = time.time()
if self.isGrid:
self.grid.draw(self.screen)
if self.control_method == 'test':
q = Quaternion(1,0,0,0) # Unit Quaternion
incr = Quaternion(0.96,0.01,0.01,0).normalized()
count = 0
while 1:
q = q*incr
if self.isGrid:
self.grid.draw(self.screen)
self.cube.draw(self.screen, q, Vector3(-count,count,count))
event = pygame.event.poll()
if event.type == pygame.QUIT \
or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
break
pygame.display.flip()
pygame.time.delay(100)
self.cube.erase(self.screen)
count-=1
# First Order Rotation/Translation Model
elif self.control_method == 'xbox':
sample_rate = .0005
#xbl = XboxListenerWindows.XBoxListener(delay_sec)
xbl = XboxListenerLinux.XBoxListener(sample_rate)
xbl.init()
rot_yaw_rate = 0.0
rot_pitch_rate = 0.0
rot_roll_rate = 0.0
vel_rate = 0.0
TOP_RATE = 2/3*np.pi
rpy = Vector3(0, 0, 0)
rpy_t = Vector3(0, 0, 0)
xyz = Vector3(self.cube.a, self.cube.b, self.cube.c)
xyz_t = Vector3(self.cube.a, self.cube.b, self.cube.c)
d_xyz = Vector3(0, 0, 0)
cur_pose = Quaternion(1, 0, 0, 0)
d_rpy = Vector3(0, 0, 0)
# Need To implement full quaternion dynamics
quat = np.matrix([1,0,0,0]).T
pos = np.matrix([0,0,0]).T
vel = np.matrix([0,0,0]).T
R = np.matrix(np.eye(4))
u = np.array([0,0,0,0]) # angular velocity and acceleration
# 1) With input form rotation matrix
# 2) Apply rotation matrix to position and velocity updates
# 3) Apply quaternion update formula for orientation
t = time.time()
while True:
dt = 0.05 # Desired
tnow = time.time()
if tnow - t < dt:
time.sleep(dt - (tnow - t))
else:
dt = tnow - t
t = time.time()
control = xbl.get()
if control is not None: # new update
rot_yaw_rate = float(control['leftX'])
rot_pitch_rate = float(control['rightY'])
rot_roll_rate = float(control['rightX'])
acc_rate = float(control['leftY'])
else:
rot_rate_yaw = 0
rot_pitch_rate = 0
rot_roll_rate = 0
acc_rate = 0
# Need To implement full quaternion dynamics
u[0] = TOP_RATE*rot_roll_rate
u[1] = TOP_RATE*rot_pitch_rate
u[2] = TOP_RATE*rot_yaw_rate
u[3] = 40*acc_rate
acc = np.matrix([0,0,u[3]]).T
# Apply Quaternion Update:
Omega = SO3.get_omega_matrix(u[0],u[1],u[2])
#quat = quat + 1/2*dt*Omega*quat # ZOH -- qdot = .5*Omega*quat
quat = la.expm(Omega*dt/2.0)*quat
R = SO3.quaternion_to_rotation_matrix(np.array(quat))
pos = pos + vel*dt + 1/2*dt**2*R*acc
vel = vel + dt*R*acc
xyz = Vector3(pos[0],pos[1],pos[2])
pose = Quaternion(quat[0],quat[1],quat[2],quat[3])
#print("Current Position", pos)
#print("Current Pose", quat)
if self.isGrid:
self.grid.draw(self.screen)
# Draw Updates
self.cube.draw(self.screen, pose, xyz)
event = pygame.event.poll()
if event.type == pygame.QUIT \
or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
break
pygame.display.flip()
pygame.time.delay(1) #Fast Update
self.cube.erase(self.screen)
#for c in cubes:
# c.erase(self.screen)
elif self.control_method == 'imu':
delay_sec = .05
imuc = IMUListener.IMUListener(delay_sec)
imuc.init()
while True:
control = imuc.get()
print(control)
if control is not None:
q = Quaternion(control["quatW"], control["quatX"], control["quatY"], control["quatZ"]).normalized()
posi = Vector3(0,0,0)
if self.isGrid:
self.grid.draw(self.screen)
self.cube.draw(self.screen,q,posi)
event = pygame.event.poll()
if event.type == pygame.QUIT \
or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
break
pygame.display.flip()
pygame.time.delay(20)
self.cube.erase(self.screen)
else:
print('invalid control input!')
def __init__(self, pos=None, rot=None, vel=None):
self.pos = pos if pos is not None else Point3()
self.rot = rot if rot is not None else Quaternion()
self.vel = vel if vel is not None else Vector3()