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