Exemplo n.º 1
0
    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
Exemplo n.º 2
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]
Exemplo n.º 4
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 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
Exemplo n.º 7
0
 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!')
Exemplo n.º 9
0
 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()
Exemplo n.º 10
0
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)
Exemplo n.º 11
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)
Exemplo n.º 12
0
 def get_quaternion(self):
     """Get the Quarternion of the ship's current heading."""
     return Quaternion.new_rotate_axis(self.angle, Vector3(0, 1, 0))
Exemplo n.º 13
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)