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()
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()