class Drone:
"""
Class for a single drone in the simulation.
"""
RIGID_BODY_MASS = .5 # Mass of physics object
COLLISION_SPHERE_RADIUS = .1 # Radius of the bullet collision sphere around the drone
FRICTION = 0 # No friction between drone and objects, not really necessary
TARGET_FORCE_MULTIPLIER = 1 # Allows to change influence of the force applied to get to the target
AVOIDANCE_FORCE_MULTIPLIER = 10 # Allows to change influence of the force applied to avoid collisions
TARGET_PROXIMITY_RADIUS = .5 # Drone reduces speed when in proximity of a target
AVOIDANCE_PROXIMITY_RADIUS = .6 # Distance when an avoidance manoeuvre has to be done
# TODO: Check if other values are better here (and find out what they really do as well..)
LINEAR_DAMPING = 0.95
LINEAR_SLEEP_THRESHOLD = 0
def __init__(self, manager, number):
"""
Initialises the drone as a bullet and panda object.
:param manager: The drone manager creating this very drone.
"""
self.manager = manager # Drone manager handling this drone
self.base = manager.base # Simulation
self.crazyflie = None # object of real drone, if connected to one
self.debug = False # If debugging info should be given
self.in_flight = False # If currently in flight
self.number = number # Number of drone in list
# Every drone has its own vector to follow if an avoidance manouver has to be done
self.avoidance_vector = LVector3f(random.uniform(-1, 1),
random.uniform(-1, 1),
random.uniform(-1, 1)).normalize()
# Create bullet rigid body for drone
drone_collision_shape = BulletSphereShape(self.COLLISION_SPHERE_RADIUS)
self.drone_node_bullet = BulletRigidBodyNode("RigidSphere")
self.drone_node_bullet.addShape(drone_collision_shape)
self.drone_node_bullet.setMass(self.RIGID_BODY_MASS)
# Set some values for the physics object
self.drone_node_bullet.setLinearSleepThreshold(
self.LINEAR_SLEEP_THRESHOLD)
self.drone_node_bullet.setFriction(self.FRICTION)
self.drone_node_bullet.setLinearDamping(self.LINEAR_DAMPING)
# Attach to the simulation
self.drone_node_panda = self.base.render.attachNewNode(
self.drone_node_bullet)
# ...and physics engine
self.base.world.attachRigidBody(self.drone_node_bullet)
# Add a model to the drone to be actually seen in the simulation
drone_model = self.base.loader.loadModel(
"models/drones/drone_florian.egg")
drone_model.setScale(0.2)
drone_model.reparentTo(self.drone_node_panda)
# Set the position and target position to their default (origin)
default_position = LPoint3f(0, 0, 0)
self.drone_node_panda.setPos(default_position)
self.target_position = default_position
# Create a line renderer to draw a line from center to target point
self.line_creator = LineSegs()
# Then draw a default line so that the update function works as expected (with the removal)
self.target_line_node = self.base.render.attachNewNode(
self.line_creator.create(False))
# Create node for text
self.drone_text_node_panda = None
def get_pos(self) -> LPoint3f:
"""
Get the position of the drone.
:return: Position of the drone as an LPoint3 object
"""
return self.drone_node_panda.getPos()
def set_pos(self, position: LPoint3f):
"""
Set position of the drone.
This directly sets the drone to that position, without any transition or flight.
:param position: The position the drone is supposed to be at.
"""
self.drone_node_panda.setPos(position)
def get_target(self) -> LPoint3f:
"""
Get the current target position of the drone.
:return: The target position as a LPoint3 object.
"""
return self.target_position
def set_target(self, position: LPoint3f):
"""
Set the target position of the drone.
:param position: The target position to be set.
"""
self.target_position = position
def set_debug(self, active):
"""
De-/activate debug information such as lines showing forces and such.
:param active: If debugging should be turned on or off.
"""
self.debug = active
if active:
# Create a line so the updater can update it
self.target_line_node = self.base.render.attachNewNode(
self.line_creator.create(False))
# Write address of drone above model
self._draw_cf_name(True)
else:
self.target_line_node.removeNode()
self._draw_cf_name(False)
def update(self):
"""
Update the drone and its forces.
"""
# Update the force needed to get to the target
self._update_target_force()
# Update the force needed to avoid other drones, if any
self._update_avoidance_force()
# Combine all acting forces and normalize them to one acting force
self._combine_forces()
# Update the line drawn to the current target
if self.debug:
self._draw_target_line()
# Update real drone if connected to one
if self.crazyflie is not None and self.in_flight:
current_pos = self.get_pos()
self.crazyflie.cf.commander.send_position_setpoint(
current_pos.y, -current_pos.x, current_pos.z, 0)
# print("Update!")
def _update_target_force(self):
"""
Calculates force needed to get to the target and applies it.
"""
distance = (self.get_target() - self.get_pos()
) # Calculate distance to fly
# Normalise distance to get an average force for all drones, unless in close proximity of target,
# then slowing down is encouraged and the normalisation is no longer needed
# If normalisation would always take place overshoots would occur
if distance > self.TARGET_PROXIMITY_RADIUS:
distance = distance.normalized()
# Apply to drone (with force multiplier in mind)
self.drone_node_bullet.applyCentralForce(distance *
self.TARGET_FORCE_MULTIPLIER)
def _update_avoidance_force(self):
"""
Applies a force to drones that are to close to each other and to avoid crashes.
"""
# Save all drones in near proximity with their distance
near = []
for drone in self.manager.drones:
distance = len(drone.get_pos() - self.get_pos())
if 0 < distance < self.AVOIDANCE_PROXIMITY_RADIUS: # Check dist > 0 to prevent drone from detecting itself
near.append(drone)
# Calculate and apply forces for every opponent
for opponent in near:
# Vector to other drone
opponent_vector = opponent.get_pos() - self.get_pos()
# Multiplier to maximise force when opponent gets closer
multiplier = self.AVOIDANCE_PROXIMITY_RADIUS - len(opponent_vector)
# Calculate a direction follow (multipliers found by testing)
avoidance_direction = self.avoidance_vector * 2 - opponent_vector.normalized(
) * 10
avoidance_direction.normalize()
# Apply avoidance force
self.drone_node_bullet.applyCentralForce(
avoidance_direction * multiplier *
self.AVOIDANCE_FORCE_MULTIPLIER)
def _combine_forces(self):
"""
Combine all acting forces to one normalised force.
"""
total_force = self.drone_node_bullet.getTotalForce()
# Only do so if force is sufficiently big to retain small movements
if total_force.length() > 2:
self.drone_node_bullet.clearForces()
self.drone_node_bullet.applyCentralForce(total_force.normalized())
def destroy(self):
"""
Detach drone from the simulation and physics engine, then destroy object.
"""
self.drone_node_panda.removeNode()
self.base.world.removeRigidBody(self.drone_node_bullet)
self.target_line_node.removeNode()
if self.debug:
self.drone_text_node_panda.removeNode()
def _draw_target_line(self):
"""
Draw a line from the center to the target position in the simulation.
"""
# Remove the former line
self.target_line_node.removeNode()
# Create a new one
self.line_creator.setColor(1.0, 0.0, 0.0, 1.0)
self.line_creator.moveTo(self.get_pos())
self.line_creator.drawTo(self.target_position)
line = self.line_creator.create(False)
# And attach it to the renderer
self.target_line_node = self.base.render.attachNewNode(line)
def _draw_cf_name(self, draw):
"""
Show the address of the connected Craziefly, if there is one, above the model.
"""
if draw:
address = 'Not connected'
if self.crazyflie is not None:
address = self.crazyflie.cf.link_uri
text = TextNode('droneInfo')
text.setText(str(self.number) + '\n' + address)
text.setAlign(TextNode.ACenter)
self.drone_text_node_panda = self.base.render.attachNewNode(text)
self.drone_text_node_panda.setScale(.1)
self.drone_text_node_panda.setPos(self.drone_node_panda, 0, 0, .3)
else:
self.drone_text_node_panda.removeNode()
class Drone:
# RIGIDBODYMASS = 1
# RIGIDBODYRADIUS = 0.1
# LINEARDAMPING = 0.97
# SENSORRANGE = 0.6
# TARGETFORCE = 3
# AVOIDANCEFORCE = 25
# FORCEFALLOFFDISTANCE = .5
RIGIDBODYMASS = 0.5
RIGIDBODYRADIUS = 0.1
LINEARDAMPING = 0.95
SENSORRANGE = 0.6
TARGETFORCE = 1
AVOIDANCEFORCE = 10
FORCEFALLOFFDISTANCE = .5
def __init__(self,
manager,
position: Vec3,
uri="-1",
printDebugInfo=False):
self.base = manager.base
self.manager = manager
# The position of the real drone this virtual drone is connected to.
# If a connection is active, this value is updated each frame.
self.realDronePosition = Vec3(0, 0, 0)
self.canConnect = False # true if the virtual drone has a uri to connect to a real drone
self.isConnected = False # true if the connection to a real drone is currently active
self.uri = uri
if self.uri != "-1":
self.canConnect = True
self.id = int(uri[-1])
self.randVec = Vec3(random.uniform(-1, 1), random.uniform(-1, 1),
random.uniform(-1, 1))
# add the rigidbody to the drone, which has a mass and linear damping
self.rigidBody = BulletRigidBodyNode(
"RigidSphere") # derived from PandaNode
self.rigidBody.setMass(self.RIGIDBODYMASS) # body is now dynamic
self.rigidBody.addShape(BulletSphereShape(self.RIGIDBODYRADIUS))
self.rigidBody.setLinearSleepThreshold(0)
self.rigidBody.setFriction(0)
self.rigidBody.setLinearDamping(self.LINEARDAMPING)
self.rigidBodyNP = self.base.render.attachNewNode(self.rigidBody)
self.rigidBodyNP.setPos(position)
# self.rigidBodyNP.setCollideMask(BitMask32.bit(1))
self.base.world.attach(self.rigidBody)
# add a 3d model to the drone to be able to see it in the 3d scene
model = self.base.loader.loadModel(self.base.modelDir +
"/drones/drone1.egg")
model.setScale(0.2)
model.reparentTo(self.rigidBodyNP)
self.target = position # the long term target that the virtual drones tries to reach
self.setpoint = position # the immediate target (setpoint) that the real drone tries to reach, usually updated each frame
self.waitingPosition = Vec3(position[0], position[1], 0.7)
self.lastSentPosition = self.waitingPosition # the position that this drone last sent around
self.printDebugInfo = printDebugInfo
if self.printDebugInfo: # put a second drone model on top of drone that outputs debug stuff
model = self.base.loader.loadModel(self.base.modelDir +
"/drones/drone1.egg")
model.setScale(0.4)
model.setPos(0, 0, .2)
model.reparentTo(self.rigidBodyNP)
# initialize line renderers
self.targetLineNP = self.base.render.attachNewNode(LineSegs().create())
self.velocityLineNP = self.base.render.attachNewNode(
LineSegs().create())
self.forceLineNP = self.base.render.attachNewNode(LineSegs().create())
self.actualDroneLineNP = self.base.render.attachNewNode(
LineSegs().create())
self.setpointNP = self.base.render.attachNewNode(LineSegs().create())
def connect(self):
"""Connects the virtual drone to a real one with the uri supplied at initialization."""
if not self.canConnect:
return
print(self.uri, "connecting")
self.isConnected = True
self.scf = SyncCrazyflie(self.uri, cf=Crazyflie(rw_cache='./cache'))
self.scf.open_link()
self._reset_estimator()
self.start_position_printing()
# MOVE THIS BACK TO SENDPOSITIONS() IF STUFF BREAKS
self.scf.cf.param.set_value('flightmode.posSet', '1')
def sendPosition(self):
"""Sends the position of the virtual drone to the real one."""
cf = self.scf.cf
self.setpoint = self.getPos()
# send the setpoint
cf.commander.send_position_setpoint(self.setpoint[0], self.setpoint[1],
self.setpoint[2], 0)
def disconnect(self):
"""Disconnects the real drone."""
print(self.uri, "disconnecting")
self.isConnected = False
cf = self.scf.cf
cf.commander.send_stop_setpoint()
time.sleep(0.1)
self.scf.close_link()
def update(self):
"""Update the virtual drone."""
# self.updateSentPositionBypass(0)
self._updateTargetForce()
self._updateAvoidanceForce()
self._clampForce()
if self.isConnected:
self.sendPosition()
# draw various lines to get a better idea of whats happening
self._drawTargetLine()
# self._drawVelocityLine()
self._drawForceLine()
# self._drawSetpointLine()
self._printDebugInfo()
def updateSentPosition(self, timeslot):
transmissionProbability = 1
if self.id == timeslot:
if random.uniform(0, 1) < transmissionProbability:
# print(f"drone {self.id} updated position")
self.lastSentPosition = self.getPos()
else:
pass
# print(f"drone {self.id} failed!")
def updateSentPositionBypass(self, timeslot):
self.lastSentPosition = self.getPos()
def getPos(self) -> Vec3:
return self.rigidBodyNP.getPos()
def getLastSentPos(self) -> Vec3:
return self.lastSentPosition
def _updateTargetForce(self):
"""Applies a force to the virtual drone which moves it closer to its target."""
dist = (self.target - self.getPos())
if (dist.length() > self.FORCEFALLOFFDISTANCE):
force = dist.normalized()
else:
force = (dist / self.FORCEFALLOFFDISTANCE)
self.addForce(force * self.TARGETFORCE)
def _updateAvoidanceForce(self):
"""Applies a force the the virtual drone which makes it avoid other drones."""
# get all drones within the sensors reach and put them in a list
nearbyDrones = []
for drone in self.manager.drones:
dist = (drone.getLastSentPos() - self.getPos()).length()
if drone.id == self.id: # prevent drone from detecting itself
continue
if dist < self.SENSORRANGE:
nearbyDrones.append(drone)
# calculate and apply forces
for nearbyDrone in nearbyDrones:
distVec = nearbyDrone.getLastSentPos() - self.getPos()
if distVec.length() < 0.2:
print("BONK")
distMult = self.SENSORRANGE - distVec.length()
avoidanceDirection = self.randVec.normalized(
) * 2 - distVec.normalized() * 10
avoidanceDirection.normalize()
self.addForce(avoidanceDirection * distMult * self.AVOIDANCEFORCE)
def _clampForce(self):
"""Clamps the total force acting in the drone."""
totalForce = self.rigidBody.getTotalForce()
if totalForce.length() > 2:
self.rigidBody.clearForces()
self.rigidBody.applyCentralForce(totalForce.normalized())
def targetVector(self) -> Vec3:
"""Returns the vector from the drone to its target."""
return self.target - self.getPos()
def _printDebugInfo(self):
if self.printDebugInfo:
pass
def setTarget(self, target: Vec3):
"""Sets a new target for the drone."""
self.target = target
def setRandomTarget(self):
"""Sets a new random target for the drone."""
self.target = self.manager.getRandomRoomCoordinate()
def setNewRandVec(self):
self.randVec = Vec3(random.uniform(-1, 1), random.uniform(-1, 1),
random.uniform(-1, 1))
def addForce(self, force: Vec3):
self.rigidBody.applyCentralForce(force)
def setPos(self, position: Vec3):
self.rigidBodyNP.setPos(position)
def getVel(self) -> Vec3:
return self.rigidBody.getLinearVelocity()
def setVel(self, velocity: Vec3):
return self.rigidBody.setLinearVelocity(velocity)
def _drawTargetLine(self):
self.targetLineNP.removeNode()
ls = LineSegs()
# ls.setThickness(1)
ls.setColor(1.0, 0.0, 0.0, 1.0)
ls.moveTo(self.getPos())
ls.drawTo(self.target)
node = ls.create()
self.targetLineNP = self.base.render.attachNewNode(node)
def _drawVelocityLine(self):
self.velocityLineNP.removeNode()
ls = LineSegs()
# ls.setThickness(1)
ls.setColor(0.0, 0.0, 1.0, 1.0)
ls.moveTo(self.getPos())
ls.drawTo(self.getPos() + self.getVel())
node = ls.create()
self.velocityLineNP = self.base.render.attachNewNode(node)
def _drawForceLine(self):
self.forceLineNP.removeNode()
ls = LineSegs()
# ls.setThickness(1)
ls.setColor(0.0, 1.0, 0.0, 1.0)
ls.moveTo(self.getPos())
ls.drawTo(self.getPos() + self.rigidBody.getTotalForce() * 0.2)
node = ls.create()
self.forceLineNP = self.base.render.attachNewNode(node)
def _drawSetpointLine(self):
self.setpointNP.removeNode()
ls = LineSegs()
# ls.setThickness(1)
ls.setColor(1.0, 1.0, 1.0, 1.0)
ls.moveTo(self.getPos())
ls.drawTo(self.setpoint)
node = ls.create()
self.setpointNP = self.base.render.attachNewNode(node)
def _wait_for_position_estimator(self):
"""Waits until the position estimator reports a consistent location after resetting."""
print('Waiting for estimator to find position...')
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
log_config.add_variable('kalman.varPX', 'float')
log_config.add_variable('kalman.varPY', 'float')
log_config.add_variable('kalman.varPZ', 'float')
var_y_history = [1000] * 10
var_x_history = [1000] * 10
var_z_history = [1000] * 10
threshold = 0.001
with SyncLogger(self.scf, log_config) as logger:
for log_entry in logger:
data = log_entry[1]
var_x_history.append(data['kalman.varPX'])
var_x_history.pop(0)
var_y_history.append(data['kalman.varPY'])
var_y_history.pop(0)
var_z_history.append(data['kalman.varPZ'])
var_z_history.pop(0)
min_x = min(var_x_history)
max_x = max(var_x_history)
min_y = min(var_y_history)
max_y = max(var_y_history)
min_z = min(var_z_history)
max_z = max(var_z_history)
if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (max_z -
min_z) < threshold:
break
def _reset_estimator(self):
"""Resets the position estimator, this should be run before flying the drones or they might report a wrong position."""
cf = self.scf.cf
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
self._wait_for_position_estimator()
def position_callback(self, timestamp, data, logconf):
"""Updates the variable holding the position of the actual drone. It is not called in the update method, but by the drone itself (I think)."""
x = data['kalman.stateX']
y = data['kalman.stateY']
z = data['kalman.stateZ']
self.realDronePosition = Vec3(x, y, z)
# print('pos: ({}, {}, {})'.format(x, y, z))
def start_position_printing(self):
"""Activate logging of the position of the real drone."""
log_conf = LogConfig(name='Position', period_in_ms=50)
log_conf.add_variable('kalman.stateX', 'float')
log_conf.add_variable('kalman.stateY', 'float')
log_conf.add_variable('kalman.stateZ', 'float')
self.scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(self.position_callback)
log_conf.start()
