def update(self, dt):
self.t += dt
if self.alive:
self.helm.update(dt)
self.sail.update(dt)
self.update_masts(self.sail.current)
# damp roll
self.roll *= pow(0.6, dt)
# Compute some bobbing motion
rollmoment = 0.05 * sin(self.t)
pitch = 0.02 * sin(0.31 * self.t)
# Compute the forward vector from the curent heading
q = self.get_quaternion()
forward = q * Vector3(0, 0, 1)
angular_velocity = self.helm.current * min(forward.dot(self.vel), 2) * 0.03
angle_to_wind = map_angle(self.world.wind_angle - self.angle)
sail_power = get_sail_power(angle_to_wind)
heeling_moment = get_heeling_moment(angle_to_wind)
rollmoment += angular_velocity * 0.5 # lean over from centrifugal force of turn
rollmoment -= heeling_moment * 0.05 * self.sail.current # heel from wind force
# Update ship angle and position
self.angle = map_angle(self.angle + angular_velocity * dt)
accel = forward * self.sail.current * sail_power * 0.5 * dt
self.vel += accel
self.vel *= pow(0.7, dt) # Drag
self.pos += self.vel * dt
# Float
if self.alive:
self.pos += Vector3(0, -0.5, 0) * self.pos.y * dt
else:
self.pos += Vector3(0, -1, 0) * dt
self.roll += rollmoment * dt
# Apply ship angle and position to model
self.rot = (
q *
Quaternion.new_rotate_axis(pitch, Vector3(1, 0, 0)) *
Quaternion.new_rotate_axis(self.roll, Vector3(0, 0, 1))
)
rotangle, (rotx, roty, rotz) = self.rot.get_angle_axis()
self.model.rotation = (degrees(rotangle), rotx, roty, rotz)
self.model.pos = self.pos
# Adjust sail angle to wind direction
if self.alive:
sail_angle = get_sail_setting(angle_to_wind)
for sail in self.model.nodes[1:]:
sail.rotation = degrees(sail_angle), 0, 1, 0
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 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 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 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 generate_track(chromosome_elements: List[ChromosomeElem]) -> List[TrackPoint]:
track_points: List[TrackPoint] = []
start_position = Vector3(x=49.7, y=0.5, z=50.0)
span = Vector3(x=0.0, y=0.0, z=0.0)
span_dist = 2
track_script = TrackScript(chromosome_elements=chromosome_elements)
if track_script.parse_chromosome():
dy = 0.0
s = start_position
s.y = 0.5
span.x = 0.0
span.y = 0.0
span.z = span_dist
turn_val = 10.0
for track_script_element in track_script.track:
if track_script_element.state == State.AngleDY:
turn_val = track_script_element.value
elif track_script_element.state == State.CurveY:
dy = track_script_element.value * turn_val
else:
dy = 0.0
for i in range(track_script_element.num_to_set):
turn = dy
rot = Quaternion.new_rotate_euler(math.radians(turn), 0, 0)
span = rot * span.normalized()
span *= span_dist
s = s + span
track_points.append(TrackPoint(x=s.x, y=s.z))
return track_points
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()
def transform_by_euler_angle(accel, euler):
"""
"""
heading, attitude, bank = euler[1], euler[2], euler[0]
return Quartclid.new_rotate_euler(heading, attitude,
bank) * Vector3(*accel)
from euclid import Quaternion, Vector3
import glob
import math
import os
import socket
QUART_PI=0.25*math.pi
HALF_PI=0.5*math.pi
PI2=2*math.pi
X_UNIT=Vector3(1.0, 0.0, 0.0)
Y_UNIT=Vector3(0.0, 1.0, 0.0)
NEG_Y_UNIT=-Y_UNIT
Z_UNIT=Vector3(0.0, 0.0, 1.0)
NULL_VEC=Vector3(0.0,0.0,0.0)
NULL_ROT=Quaternion.new_rotate_axis(0.0, Y_UNIT)
def testAllAngleForXY():
testAngleForXY(57735, 100000, 1.0/6)
testAngleForXY(1732, 1000, 1.0/3)
testAngleForXY(1732, -1000, 4.0/6)
testAngleForXY(57735, -100000, 5.0/6)
testAngleForXY(0,1,2.0)
testAngleForXY(1,0,1.0/2)
testAngleForXY(0,-1,1.0)
testAngleForXY(-57735, -100000, 7.0/6)
testAngleForXY(-1732, -1000, 4.0/3)
testAngleForXY(-1732, 1000, 5.0/3)
testAngleForXY(-57735, 100000, 11.0/6)
testAngleForXY(-1,0,3.0/2)
def get_quaternion(self):
"""Get the Quarternion of the ship's current heading."""
return Quaternion.new_rotate_axis(self.angle, Vector3(0, 1, 0))
from euclid import Quaternion, Vector3
import glob
import math
import os
import socket
QUART_PI = 0.25 * math.pi
HALF_PI = 0.5 * math.pi
PI2 = 2 * math.pi
X_UNIT = Vector3(1.0, 0.0, 0.0)
Y_UNIT = Vector3(0.0, 1.0, 0.0)
NEG_Y_UNIT = -Y_UNIT
Z_UNIT = Vector3(0.0, 0.0, 1.0)
NULL_VEC = Vector3(0.0, 0.0, 0.0)
NULL_ROT = Quaternion.new_rotate_axis(0.0, Y_UNIT)
def testAllAngleForXY():
testAngleForXY(57735, 100000, 1.0 / 6)
testAngleForXY(1732, 1000, 1.0 / 3)
testAngleForXY(1732, -1000, 4.0 / 6)
testAngleForXY(57735, -100000, 5.0 / 6)
testAngleForXY(0, 1, 2.0)
testAngleForXY(1, 0, 1.0 / 2)
testAngleForXY(0, -1, 1.0)
testAngleForXY(-57735, -100000, 7.0 / 6)
testAngleForXY(-1732, -1000, 4.0 / 3)
testAngleForXY(-1732, 1000, 5.0 / 3)
testAngleForXY(-57735, 100000, 11.0 / 6)
testAngleForXY(-1, 0, 3.0 / 2)
