I have another problem and I can't seem to wrap my head around what's causing these issues.
I'm trying to fly three sets of drones. One of these drones will float in place (around 0.4m above the ground) while the other two drones will find its way to the stationary drone and go around it. This action is given through an external learning model.
However, every attempt at running the code results in a different behavior. I've attached a Google Drive link to compilation of some videos of our tests here: https://drive.google.com/drive/folders/ ... sp=sharing
As you can see some of the videos, the drones sometimes just flip at take off or randomly drop in the middle of the process (while the propellers are turning) or just rush towards one direction.
Some of the problems I think might be related to it:
1. The LPS nodes. When I connect to a crazyflie using the client, the LPS nodes are not consistently turned on (whiches from red to green to red etc)
2. The Flow deck + LPS combination. This is a known issue at https://github.com/bitcraze/crazyflie-f ... issues/368 but I'm guessing this could be the reason why the drones are not floating (because of the kalman filter).
3. The drone itself. Maybe after multiple crashes it has been damaged but there is no way for me to know.
I've attached my code here just incase you may need some reference.
Code: Select all
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# Copyright (C) 2017-2018 Bitcraze AB
#
# Crazyflie Nano Quadcopter Client
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
# MA 02110-1301, USA.
"""
Version of the AutonomousSequence.py example connecting to 10 Crazyflies.
The Crazyflies go straight up, hover a while and land but the code is fairly
generic and each Crazyflie has its own sequence of setpoints that it files
to.
The layout of the positions:
x2 x1 x0
y3 10 4
^ Y
|
y2 9 6 3
|
+------> X
y1 8 5 2
y0 7 1
"""
import time
import math
#from vectors import Point, Vector
import vectormath as vmath
import numpy as np
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
# Import MotionCommander
from cflib.positioning.motion_commander import MotionCommander
# Bring in CSV
import csv
####################### start of changes by darrence ##################################
import importlib
mod = importlib.import_module('DSOInferenceModule-v1')
no_of_agents = 3
path_pb = "D:\DSO-AISwarm\crazyflie-lib-python\examples\swarm\DSOfrozen_graph_def.pb"
infer = mod.InferenceModule(path_pb)
observations = []
furtherest_allowable_distance = 0.7 #this is dm in slide 7
####################### end of changes ##################################
# Change uris and sequences according to your setup
URI1 = 'radio://0/40/1M'
URI2 = 'radio://0/78/2M/E7E7E7E7E7'
URI3 = 'radio://0/80/2M'
URI4 = 'radio://0/78/2M'
URI5 = 'radio://0/70/2M/E7E7E7E705'
URI6 = 'radio://0/70/2M/E7E7E7E706'
URI7 = 'radio://0/70/2M/E7E7E7E707'
URI8 = 'radio://0/70/2M/E7E7E7E708'
URI9 = 'radio://0/70/2M/E7E7E7E709'
URI10 = 'radio://0/70/2M/E7E7E7E70A'
# URI1 is the TARGET
uris = {
URI1,
URI2,
URI3,
# URI4,
# URI5,
# URI6,
# URI7,
# URI8,
# URI9,
# URI10
}
location = {} # Dictionary of URI and Location
rotation = {} # Dictionaty of URI and rotation
g_distance = {} # Nested Dicationary of Drone A to Drone B~X
g_bearing = {} #Nested Dictionary of Drone A to Drone B~X
# [dist_tgt, bearing_tgt, dist_agt, bearing_agt, orient_agt]
inputLM = [0,0,0,0,0]
for x in uris:
g_distance[x]={}
for x in uris:
g_bearing[x] = {}
def wait_for_position_estimator(scf):
print('Waiting for estimator to find position...')
log_config = LogConfig(name='Stabilizer', period_in_ms=10)
log_config.add_variable('stabilizer.roll', 'float')
log_config.add_variable('stabilizer.pitch', 'float')
log_config.add_variable('stabilizer.yaw', 'float')
var_y_history = [1000] * 10
var_x_history = [1000] * 10
var_z_history = [1000] * 10
threshold = 0.001
with SyncLogger(scf, log_config) as logger:
endTime = time.time() + 10
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)
print("{} {} {}".
format(max_x - min_x, max_y - min_y, max_z - min_z))
if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (
max_z - min_z) < threshold:
break
def wait_for_param_download(scf):
while not scf.cf.param.is_updated:
time.sleep(1.0)
print('Parameters downloaded for', scf.cf.link_uri)
def reset_estimator(scf):
cf = scf.cf
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
wait_for_position_estimator(cf)
def position_callback(scf,uri, timestamp, data, logconf):
x = data['kalman.stateX']
y = data['kalman.stateY']
z = data['kalman.stateZ']
# print("Uri: {}, Data: {}".format(uri, data))
point = (x,y,z) # (-39.528446197509766, -8.916569709777832, -0.5112752318382263)\
location[uri] = point # {'radio://0/33/1M': (-3.2653462886810303, -1.9112612009048462, 0.020919203758239746), 'radio://0/40/1M': (-4.532137870788574, -0.9649770855903625, 0.047263506799936295)
#rotation[uri] =
for k,v in location.items():
# Calculate distance for all drones
_d = calculate_distance(point,v)
g_distance[uri][k]= _d
g_distance[k][uri]= _d
FormLMArray(uri,g_distance,scf)
#print(location[uri])
#CalculateBearing(location[URI2],location[URI3])
#return point
def FormLMArray(uri,d_dict,scf):
cf = scf.cf
# Form list of keys and values
key_list = list(d_dict[uri].keys())
val_list = list(d_dict[uri].values())
# Find Distance to target
dist_tgt = d_dict[uri]['radio://0/40/1M']
# Find Bearing to Target
bearing_tgt = CalculateBearing(uri,location[uri], location[URI1])
# find shortest agents and its URI
dist_agt = min(filter(lambda x: x>0.0, val_list)) # Distance to nearest agent
dist_agt_uri = key_list[val_list.index(dist_agt)] # nearest agent URI
# Find bearing for shortest agent
####################### start of changes by Darrence ##################################
bearing_agt = CalculateBearing(uri,location[uri],location[dist_agt_uri])
orient_agt = CalculateBearing(dist_agt_uri,location[dist_agt_uri],location[uri]) # Orientation of the nearest neighboring agent
####################### end of changes ##################################
inputLM[0] = distance_tgt_transformation(dist_tgt) # Distance to the target
inputLM[1] = bearing_tgt # Bearing to the target
inputLM[2] = distance_agt_transformation(dist_agt) # Distance to the neareast neighboring agent
inputLM[3] = bearing_agt # Bearing to the neearest neighboring agent
# inputLM[4] = rotation[dist_agt_uri][2] #get YAW value
inputLM[4] = orient_agt # Orientation of the neareast neighboring agent
####################### start of changes by Darrence ##################################
observations.extend(inputLM)
ob = []
ob.append(inputLM)
actions = infer.inferenceMod(ob)
####################### end of changes ##################################
cf.commander.send_hover_setpoint(0,0,0,0.4)
if cf.link_uri == URI1:
pass
else:
#print(val_list)
print(uri, actions)
velocity_v = float(actions[0][0])*0.2
rotation_r = float(actions[0][1])*35
scf.cf.commander.send_hover_setpoint(vx=velocity_v, vy=0,yawrate=rotation_r, zdistance=0.4)
return(inputLM)
####################### start of changes by Darrence ##################################
def distance_tgt_transformation(act_distance):
dm=0.35 #0.35
dt=0.07
Dt=0.01 #0.25~0.5
return(min(dt*(act_distance/Dt),dm))
def distance_agt_transformation(act_distance):
dm=0.35 #0.35
dt=0.07
Dt=0.25 #0.25~0.5
return(min(dt*(act_distance/Dt),dt))
####################### end of changes ##################################
def stablizer_callback(uri, timestamp, data, logconf):
roll = data['stabilizer.roll']
pitch = data['stabilizer.pitch']
yaw = data['stabilizer.yaw']
#print("Uri: {}, Data: {}".format(uri, data))
stabilizer = (roll, pitch, yaw)
rotation[uri] = stabilizer
return(stabilizer)
#CalculateStabDiff(rotation[URI2],rotation[URI3])
# Get Position Data
def GetPosition(scf):
log_conf = LogConfig(name='Position', period_in_ms=10)
log_conf.add_variable('kalman.stateX', 'float')
log_conf.add_variable('kalman.stateY', 'float')
log_conf.add_variable('kalman.stateZ', 'float')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(lambda timestamp, data, logconf: position_callback(scf,scf.cf.link_uri, timestamp, data, logconf))
log_conf.start()
# Get Roll, Pitch, Yaw
def GetStabilizer(scf):
log_conf = LogConfig(name='Stablizer',period_in_ms=10)
log_conf.add_variable('stabilizer.roll', 'float')
log_conf.add_variable('stabilizer.pitch', 'float')
log_conf.add_variable('stabilizer.yaw', 'float')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(lambda timestamp, data, logconf: stablizer_callback(scf.cf.link_uri, timestamp, data, logconf))
log_conf.start()
# Calculate Distance Between Two Agents
def calculate_distance(p1, p2):
diff = [0,0,0]
diff[0] = p1[0]-p2[0]
diff[1] = p1[1]-p2[1]
diff[2] = p1[2]-p2[2]
result = (math.sqrt( (diff[0]*diff[0]) + (diff[1]*diff[1]) + (diff[2]*diff[2])))
return(result)
####################### start of changes by darrence ##################################
def CalculateBearing(uri, p1,p2):
# Finding Forward Vector of D1
#p1_l = location[p1]
#p2_l = location[p2]
#yaw is rotation along z-axis but measured using x-axis
nVector1 = vmath.Vector2(0,0)
yawinrad = np.radians(rotation[uri][2])
nVector1[0] = math.cos(yawinrad)
nVector1[1] = math.sin(yawinrad)
# Finding Directional Vector from D1 to D2
#Vector2 = vmath.Vector2(0,0)
#Vector2[0] = p2[0] - p1[0]
#Vector2[1] = p2[1] - p1[1]
#angle = nVector1.angle(Vector2,unit='deg')
#signed_angle = np.arctan2(Vector2[0],Vector2[1]) - np.arctan2(nVector1[0],nnVector1[1])
p1= np.asarray(p1)
p2= np.asarray(p2)
#p3 = np.asarray(p3)
v1 = nVector1
v2 = p2-p1
bearing =-1*np.rad2deg(math.atan2(v1[0]*v2[1]-v1[1]*v2[0],v1[0]*v2[0]+v1[1]*v2[1]))
return (bearing/180)
#return(np.degrees(signed_angle)/180)
####################### end of changes ##################################
####################### start of changes by darrence ##################################
def run_sequence(scf):
cf = scf.cf
#Take off
for y in range(10):
cf.commander.send_hover_setpoint(0, 0, 0, y / 25)
time.sleep(0.1)
#velocity_v = float(actions[0])*0.3
#rotation = float(actions[1])*75
#scf.cf.commander.send_hover_setpoint(vx=velocity_v,vy=0,yawrate=rotation, zdistance=0.4)
#observations.pop()
####################### end of changes ##################################
if __name__ == '__main__':
#logging.basicConfig(level=logging.DEBUG)
factory = CachedCfFactory(rw_cache='./cache')
cflib.crtp.init_drivers(enable_debug_driver=False)
x = 5
clock = -x
with Swarm(uris,factory=factory) as swarm:
swarm.parallel_safe(wait_for_param_download)
swarm.parallel(reset_estimator)
#swarm.parallel(run_sequence)
while True:
if time.perf_counter() >= clock + x:
print('Waiting for parameters to be downloaded...')
####################### start of changes by darrence ##################################
swarm.parallel_safe(GetPosition)
swarm.parallel_safe(GetStabilizer) #brough this line above of swarm.parallel_safe(GetPosition) because rotation{} needs to be populated first
#swarm.parallel_safe(calculate_distance)
#swarm.parallel(CalculateBearing)
####################### end of changes ##################################
Code: Select all
def FormArrayML:
Code: Select all
cf.commander.send_hover_setpoint(0,0,0,0.4)