Beispiel #1
0
    def __init__(self, world, space, pos=(0, 0, 0), size=1, motors_force=(0, 0, 0, 0)):
        self.world = world
        self.pos = pos
        x, y, z = pos
        self.size = size
        self.motors_force = motors_force
        self.motors_noise = [random()/100 for m in range(4)]

        # Create the quad inside the world

        # Motors
        motor_size = size / 10.
        motor_weight = 0.07
        arms_distance = size / 2.
        motor_front_color = (0, 0, 1) # color blue
        motor_roll_color = (0, 1, 0) # color green
        self.motors = [
            Sphere(world, space, (x, y, z-arms_distance), motor_size, motor_weight, motor_front_color),
            Sphere(world, space, (x, y, z+arms_distance), motor_size, motor_weight),
            Sphere(world, space, (x-arms_distance, y, z), motor_size, motor_weight, motor_roll_color),
            Sphere(world, space, (x+arms_distance, y, z), motor_size, motor_weight),
        ]

        # Center
        center_size = motor_size * 1.5
        center_weight = 0.5
        self.center = Sphere(world, space, (x, y, z), center_size, center_weight)

        # Stick parts together
        self.motors[0].joint(self.motors[1])
        self.motors[0].joint(self.center)
        self.motors[2].joint(self.motors[3])
        self.motors[2].joint(self.center)
        self.motors[0].joint(self.motors[2])
        self.motors[1].joint(self.motors[3])

        # set util vectors
        self._set_plane_vectors()
Beispiel #2
0
class Quad(object):
    def __init__(self, world, space, pos=(0, 0, 0), size=1, motors_force=(0, 0, 0, 0)):
        self.world = world
        self.pos = pos
        x, y, z = pos
        self.size = size
        self.motors_force = motors_force
        self.motors_noise = [random()/100 for m in range(4)]

        # Create the quad inside the world

        # Motors
        motor_size = size / 10.
        motor_weight = 0.07
        arms_distance = size / 2.
        motor_front_color = (0, 0, 1) # color blue
        motor_roll_color = (0, 1, 0) # color green
        self.motors = [
            Sphere(world, space, (x, y, z-arms_distance), motor_size, motor_weight, motor_front_color),
            Sphere(world, space, (x, y, z+arms_distance), motor_size, motor_weight),
            Sphere(world, space, (x-arms_distance, y, z), motor_size, motor_weight, motor_roll_color),
            Sphere(world, space, (x+arms_distance, y, z), motor_size, motor_weight),
        ]

        # Center
        center_size = motor_size * 1.5
        center_weight = 0.5
        self.center = Sphere(world, space, (x, y, z), center_size, center_weight)

        # Stick parts together
        self.motors[0].joint(self.motors[1])
        self.motors[0].joint(self.center)
        self.motors[2].joint(self.motors[3])
        self.motors[2].joint(self.center)
        self.motors[0].joint(self.motors[2])
        self.motors[1].joint(self.motors[3])

        # set util vectors
        self._set_plane_vectors()

    def draw(self):
        # draw motors
        [m.draw() for m in self.motors]
        self.center.draw()

    def step(self):
        # update force
        self.update_motors()
        # update pos
        self.pos = self.center.getPosition()
        # draw quad
        self.draw()

    def _set_plane_vectors(self):
        cx, cy, cz = self.center.getPosition()
        x, y, z = self.motors[0].getPosition()
        self.v1 = norm(vector(x-cx, y-cy, z-cz))
        x, y, z = self.motors[2].getPosition()
        self.v2 = norm(vector(x-cx, y-cy, z-cz))
        # Calculate pitch and roll
        self.pitch = 90 - degrees(self.v1.diff_angle(vector(0, 1, 0)))
        self.roll = degrees(self.v2.diff_angle(vector(0, 1, 0))) - 90
        # calculate yaw, (this works if the quad is close to a horizontal position)
        if self.v1.z <= 0:
            self.yaw = degrees(self.v1.diff_angle(vector(1, 0, 0)))
        else:
            self.yaw = 360 - degrees(self.v1.diff_angle(vector(1, 0, 0)))

    def update_motors(self):
        # apply noise to motors to emulate reality
        m_noise = self.motors_noise

        # force of motor should be orthogonal to the quad
        self._set_plane_vectors()
        v = self.v1.cross(self.v2)
        for i, f in enumerate(self.motors_force):
            self.motors[i].addForce(v*(f+m_noise[i]))

        # Add rotational fore to motors
        rotational_factor = 0.1
        rforce = self.v2*(self.motors_force[0]+m_noise[0])*rotational_factor
        self.motors[0].addForce(rforce)
        rforce = -self.v2*(self.motors_force[1]+m_noise[1])*rotational_factor
        self.motors[1].addForce(rforce)
        rforce = self.v1*(self.motors_force[2]+m_noise[2])*rotational_factor
        self.motors[2].addForce(rforce)
        rforce = -self.v1*(self.motors_force[3]+m_noise[3])*rotational_factor
        self.motors[3].addForce(rforce)


    @property
    def vel(self):
        return self.center.getLinearVel()