def Multi_Point2Point():
# Set goals to go to
GOALS_1 = [(-1,-1,4),(1,1,2)]
GOALS_2 = [(1,-1,2),(-1,1,4)]
# Define the quadcopters
QUADCOPTERS={'q1':{'position':[1,0,4],'orientation':[0,0,0],'L':0.3,'r':0.1,'prop_size':[10,4.5],'weight':1.2},
'q2':{'position':[-1,0,4],'orientation':[0,0,0],'L':0.15,'r':0.05,'prop_size':[6,4.5],'weight':0.7}}
# Controller parameters
CONTROLLER_1_PARAMETERS = {'Motor_limits':[4000,9000],
'Tilt_limits':[-10,10],
'Yaw_Control_Limits':[-900,900],
'Z_XY_offset':500,
'Linear_PID':{'P':[300,300,7000],'I':[0.04,0.04,4.5],'D':[450,450,5000]},
'Linear_To_Angular_Scaler':[1,1,0],
'Yaw_Rate_Scaler':0.18,
'Angular_PID':{'P':[22000,22000,1500],'I':[0,0,1.2],'D':[12000,12000,0]},
}
CONTROLLER_2_PARAMETERS = {'Motor_limits':[4000,9000],
'Tilt_limits':[-10,10],
'Yaw_Control_Limits':[-900,900],
'Z_XY_offset':500,
'Linear_PID':{'P':[300,300,7000],'I':[0.04,0.04,4.5],'D':[450,450,5000]},
'Linear_To_Angular_Scaler':[1,1,0],
'Yaw_Rate_Scaler':0.18,
'Angular_PID':{'P':[22000,22000,1500],'I':[0,0,1.2],'D':[12000,12000,0]},
}
# Catch Ctrl+C to stop threads
signal.signal(signal.SIGINT, signal_handler)
# Make objects for quadcopter, gui and controllers
gui_object = gui.GUI(quads=QUADCOPTERS)
quad = quadcopter.Quadcopter(quads=QUADCOPTERS)
ctrl1 = controller.Controller_PID_Point2Point(quad.get_state,quad.get_time,quad.set_motor_speeds,params=CONTROLLER_1_PARAMETERS,quad_identifier='q1')
ctrl2 = controller.Controller_PID_Point2Point(quad.get_state,quad.get_time,quad.set_motor_speeds,params=CONTROLLER_2_PARAMETERS,quad_identifier='q2')
# Start the threads
quad.start_thread(dt=QUAD_DYNAMICS_UPDATE,time_scaling=TIME_SCALING)
ctrl1.start_thread(update_rate=CONTROLLER_DYNAMICS_UPDATE,time_scaling=TIME_SCALING)
ctrl2.start_thread(update_rate=CONTROLLER_DYNAMICS_UPDATE,time_scaling=TIME_SCALING)
# Update the GUI while switching between destination poitions
while(run==True):
for goal1,goal2 in zip(GOALS_1,GOALS_2):
ctrl1.update_target(goal1)
ctrl2.update_target(goal2)
for i in range(150):
for key in QUADCOPTERS:
gui_object.quads[key]['position'] = quad.get_position(key)
gui_object.quads[key]['orientation'] = quad.get_orientation(key)
gui_object.update()
quad.stop_thread()
ctrl1.stop_thread()
ctrl2.stop_thread()
def __init__(self, start, obstacles):
"""
initializer
start: 3d-vector with the starting coordinates of the drone
obestacles: list of 6d-vectors with the rectangular obstacles. format{pos_x, pos_y, pos_z, len_x, len_y, len_z}
"""
self.start = start
self.obs = obstacles
self.path = []
self.iteration_data = []
self.planReady = False
self.iterRunGo = False
self.pathIter = 0
self.goalIter = 0
self.QUADCOPTER = {
'q1': {
'position': start,
'orientation': [0, 0, 0],
'L': 0.175,
'r': 0.0665,
'prop_size': [8, 3.8],
'weight': 0.5,
'motorWeight': 0.035
}
}
# Controller parameters
self.CONTROLLER_PARAMETERS = {
'Motor_limits': [2000, 12000], #4000,12000
'Tilt_limits': [-10, 10],
'Yaw_Control_Limits': [-900, 900],
'Z_XY_offset': 500,
'Linear_PID': {
'P': [300, 300, 7000],
'I': [0.03, 0.03, 6],
'D': [450, 450, 4200]
},
'Linear_To_Angular_Scaler': [1, 1, 0],
'Yaw_Rate_Scaler': 0.18,
'Angular_PID': {
'P': [22000, 22000, 1500],
'I': [0, 0, 1.2],
'D': [12000, 12000, 0]
},
}
# Make objects for quadcopter, gui and controller
self.quad = quadcopter.Quadcopter(self.QUADCOPTER)
self.gui_object = gui.GUI(quads=self.QUADCOPTER, obs=obstacles)
self.ctrl = controller.Controller_PID_Point2Point(
self.quad.get_state,
self.quad.get_time,
self.quad.set_motor_speeds,
params=self.CONTROLLER_PARAMETERS,
quad_identifier='q1')
self.rrt = RRTStar(obstacle_list=self.obs)
def Single_Point2Point():
# Set goals to go to
GOALS = [(1, 1, 2), (1, -1, 4), (-1, -1, 2), (-1, 1, 4)]
# Define the quadcopters
QUADCOPTER = {
'q1': {
'position': [1, 0, 4],
'orientation': [0, 0, 0],
'L': 0.3,
'r': 0.1,
'prop_size': [10, 4.5],
'weight': 1.2
}
}
# Controller parameters
CONTROLLER_PARAMETERS = {
'Motor_limits': [4000, 9000],
'Tilt_limits': [-10, 10],
'Z_XY_offset': 500,
'Linear_PID': {
'P': [300, 300, 7000],
'I': [0.04, 0.04, 4.5],
'D': [450, 450, 5000]
},
'Linear_To_Angular_Scaler': [1, 1, 0],
'Angular_PID': {
'P': [22000, 22000, 1500],
'I': [0, 0, 0.01],
'D': [12000, 12000, 1800]
},
}
# Catch Ctrl+C to stop threads
signal.signal(signal.SIGINT, signal_handler)
# Make objects for quadcopter, gui and controller
quad = quadcopter.Quadcopter(QUADCOPTER)
gui_object = gui.GUI(quads=QUADCOPTER)
ctrl = controller.Controller_PID_Point2Point(quad.get_state,
quad.get_time,
quad.set_motor_speeds,
params=CONTROLLER_PARAMETERS,
quad_identifier='q1')
# Start the threads
quad.start_thread(dt=QUAD_DYNAMICS_UPDATE, time_scaling=TIME_SCALING)
ctrl.start_thread(update_rate=CONTROLLER_DYNAMICS_UPDATE,
time_scaling=TIME_SCALING)
# Update the GUI while switching between destination poitions
while (run == True):
for goal in GOALS:
ctrl.update_target(goal)
for i in range(150):
gui_object.quads['q1']['position'] = quad.get_position('q1')
gui_object.quads['q1']['orientation'] = quad.get_orientation(
'q1')
gui_object.update()
quad.stop_thread()
ctrl.stop_thread()
def Multi_Point2Point():
# Set goals to go to
FORMATIONS = json.load(open('quad_sim/formations.json'))
# Define the quadcopters
QUADCOPTERS = json.load(open('quad_sim/quadcopters.json'))
# Controller parameters
CONTROLLER_PARAMETERS = {
'Motor_limits': [4000, 9000],
'Tilt_limits': [-10, 10],
'Yaw_Control_Limits': [-900, 900],
'Z_XY_offset': 500,
'Linear_PID': {
'P': [300, 300, 7000],
'I': [0.04, 0.04, 4.5],
'D': [450, 450, 5000]
},
'Linear_To_Angular_Scaler': [1, 1, 0],
'Yaw_Rate_Scaler': 0.18,
'Angular_PID': {
'P': [22000, 22000, 1500],
'I': [0, 0, 1.2],
'D': [12000, 12000, 0]
},
}
# Create enough controller parameters
ctrl_params = [CONTROLLER_PARAMETERS.copy() for q in QUADCOPTERS]
# Catch Ctrl+C to stop threads
signal.signal(signal.SIGINT, signal_handler)
# Make objects for quadcopter, gui and controllers
gui_object = gui.GUI(quads=QUADCOPTERS)
quad = quadcopter.Quadcopter(quads=QUADCOPTERS)
controllers = [
controller.Controller_PID_Point2Point(quad.get_state,
quad.get_time,
quad.set_motor_speeds,
params=param,
quad_identifier=q)
for q, param in zip(QUADCOPTERS, ctrl_params)
]
# ctrl1 = controller.Controller_PID_Point2Point(quad.get_state,quad.get_time,quad.set_motor_speeds,params=CONTROLLER_1_PARAMETERS,quad_identifier='q1')
# ctrl2 = controller.Controller_PID_Point2Point(quad.get_state,quad.get_time,quad.set_motor_speeds,params=CONTROLLER_2_PARAMETERS,quad_identifier='q2')
# Start the threads
quad.start_thread(dt=QUAD_DYNAMICS_UPDATE, time_scaling=TIME_SCALING)
for ctrl in controllers:
ctrl.start_thread(update_rate=CONTROLLER_DYNAMICS_UPDATE,
time_scaling=TIME_SCALING)
# Update the GUI while switching between destination poitions
while (run == True):
key = input(
"Which formation would you like to make?\noptions: {}\n:".format(
[str(key) for key in FORMATIONS.keys()]))
while key not in FORMATIONS.keys():
key = input(
"Did not recognize input. Please select an option below...\noptions: {}\n:"
.format([str(key) for key in FORMATIONS.keys()]))
# TODO: Write a smart algorithm to set targets of nearest drones
for q, ctrl in zip(QUADCOPTERS, controllers):
ctrl.update_target(tuple(FORMATIONS[key][q]))
for i in range(300):
for key in QUADCOPTERS:
gui_object.quads[key]['position'] = quad.get_position(key)
gui_object.quads[key]['orientation'] = quad.get_orientation(
key)
gui_object.update()
quad.stop_thread()
for ctrl in controllers:
ctrl.stop_thread()
def __init__(self,
init_pose=None,
init_velocities=None,
init_angle_velocities=None,
runtime=5.,
target_pos=None,
factor=None):
"""Initialize a Task object.
Params
======
init_pose: initial position of the quadcopter in (x,y,z) dimensions and the Euler angles
init_velocities: initial velocity of the quadcopter in (x,y,z) dimensions
init_angle_velocities: initial radians/second for each of the three Euler angles
runtime: time limit for each episode
target_pos: target/goal (x,y,z) position for the agent
"""
# Simulation
#self.sim = PhysicsSim(init_pose, init_velocities, init_angle_velocities, runtime)
# Define the quadcopters
QUADCOPTER = {
'q1': {
'position': init_pose[:3],
'orientation': [0, 0, 0],
'L': 0.3,
'r': 0.1,
'prop_size': [10, 4.5],
'weight': 1.2
}
}
self.sim = QuadSim(QUADCOPTER)
self.action_repeat = 1
# Stat reporting
self.reward_arr = []
# Store Rotor behaviors
self.rotor_arr = []
# Noise tracking
self.noise_arr = []
# Action space
self.action_low = 4000
self.action_high = 9000 #5500 #9000
self.action_size = 4
# Variable factor for debug
self.factor = factor
self.randomize = False
# Goal
self.target_pos = target_pos if target_pos is not None else np.array(
[0., 0., 10.])
self.state_size = self.action_repeat * (self.get_state()).shape[0] #6
self.init_pose = init_pose
self.runtime = runtime
self.reached = False
self.dist = 0
self.agent_rotor_speeds = [0, 0, 0, 0]
self.pid_rotor_speeds = [0, 0, 0, 0]
# Controller parameters
CONTROLLER_PARAMETERS = {
'Motor_limits': [4000, 9000], #9000
'Tilt_limits': [-10, 10],
'Yaw_Control_Limits': [-900, 900],
'Z_XY_offset': 500,
'Linear_PID': {
'P': [300, 300, 7000],
'I': [0.04, 0.04, 4.5],
'D': [450, 450, 5000]
},
'Linear_To_Angular_Scaler': [1, 1, 0],
'Yaw_Rate_Scaler': 0.18,
'Angular_PID': {
'P': [22000, 22000, 1500],
'I': [0, 0, 1.2],
'D': [12000, 12000, 0]
},
}
self.ctr = controller.Controller_PID_Point2Point(
self.sim.get_state,
self.sim.get_time,
self.sim.set_motor_speeds,
params=CONTROLLER_PARAMETERS,
quad_identifier='q1')
def TwoQuadTest():
# Set goals to go to
GOALS_1 = [(-1, -1, 4), (1, 1, 2)]
GOALS_2 = [(1, -1, 2), (-1, 1, 4)]
# Define the quadcopters
# def __init__(self,position, orientation, L, r, prop_size, weight, gravity=9.81,b=0.0245):
quad_list = [
quadcopter.Quadcopter([1, 0, 4], [0, 0, 0], 0.3, 0.1, [10, 4.5], 1.2),
quadcopter.Quadcopter([-1, 0, 4], [0, 0, 0], 0.15, 0.05, [6, 4.5], 0.7)
]
# Controller parameters
# Catch Ctrl+C to stop threads
signal.signal(signal.SIGINT, signal_handler)
# Make objects for quadcopter, gui and controllers
gui_object = gui.GUI(quads=quad_list)
#quad = quadcopter.Quadcopter(quads=QUADCOPTERS)
quad_manager = quadcopter.QuadManager(quad_list)
ctrl1 = controller.Controller_PID_Point2Point(
quad_list[0].get_state, quad_manager.get_time,
quad_list[0].set_motor_speeds, [4000, 9000], [-10, 10], [-900, 900],
500, {
'P': [300, 300, 7000],
'I': [0.04, 0.04, 4.5],
'D': [450, 450, 5000]
}, [1, 1, 0], 0.18, {
'P': [22000, 22000, 1500],
'I': [0, 0, 1.2],
'D': [12000, 12000, 0]
})
ctrl2 = controller.Controller_PID_Point2Point(
quad_list[1].get_state, quad_manager.get_time,
quad_list[1].set_motor_speeds, [4000, 9000], [-10, 10], [-900, 900],
500, {
'P': [300, 300, 7000],
'I': [0.04, 0.04, 4.5],
'D': [450, 450, 5000]
}, [1, 1, 0], 0.18, {
'P': [22000, 22000, 1500],
'I': [0, 0, 1.2],
'D': [12000, 12000, 0]
})
# Start the threads
quad_manager.start_thread(dt=QUAD_DYNAMICS_UPDATE,
time_scaling=TIME_SCALING)
ctrl1.start_thread(update_rate=CONTROLLER_DYNAMICS_UPDATE,
time_scaling=TIME_SCALING)
ctrl2.start_thread(update_rate=CONTROLLER_DYNAMICS_UPDATE,
time_scaling=TIME_SCALING)
# Update the GUI while switching between destination poitions
while (run == True):
for goal1, goal2 in zip(GOALS_1, GOALS_2):
ctrl1.update_target(goal1)
ctrl2.update_target(goal2)
for i in range(150):
for q in gui_object.quads:
q.position = q.get_position()
q.orientation = q.get_orientation()
gui_object.update()
quad_manager.stop_thread()
ctrl1.stop_thread()
ctrl2.stop_thread()
