PYB_CLIENT = ENV.getPyBulletClient()
#### Initialize the LOGGER #################################
LOGGER = Logger(
logging_freq_hz=ENV.SIM_FREQ,
num_drones=3,
)
#### Initialize the CONTROLLERS ############################
CTRL_0 = HW2Control(env=ENV, control_type=0)
CTRL_1 = HW2Control(env=ENV, control_type=1)
CTRL_2 = HW2Control(env=ENV, control_type=2)
#### Initialize the ACTION #################################
ACTION = {}
OBS = ENV.reset()
STATE = OBS["0"]["state"]
ACTION["0"] = CTRL_0.compute_control(current_position=STATE[0:3],
current_velocity=STATE[10:13],
current_rpy=STATE[7:10],
target_position=STATE[0:3],
target_velocity=np.zeros(3),
target_acceleration=np.zeros(3))
STATE = OBS["1"]["state"]
ACTION["1"] = CTRL_1.compute_control(current_position=STATE[0:3],
current_velocity=STATE[10:13],
current_rpy=STATE[7:10],
target_position=STATE[0:3],
target_velocity=np.zeros(3),
target_acceleration=np.zeros(3))
STATE = OBS["2"]["state"]
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[5e-6,0.,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
#### Apply y-axis torque to the base ###############################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,5e-6,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,5e-6,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
#### Apply z-axis torque to the base ###############################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
#### To propeller 0 ################################################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
#### To the center of mass #########################################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=4, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
p.applyExternalTorque(DRONE_IDS[0], linkIndex=4, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
#### Step, sync the simulation, printout the state #################################################
p.stepSimulation(physicsClientId=PYB_CLIENT); sync(i, START, env.TIMESTEP)
if i%env.SIM_FREQ==0: env.render()
#### Reset #########################################################################################
if i>1 and i%((ARGS.duration_sec/(ARGS.num_resets+1))*env.SIM_FREQ)==0: env.reset(); p.setGravity(0, 0, 0, physicsClientId=PYB_CLIENT)
#### Close the environment #########################################################################
env.close()
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,5e-6,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,5e-6,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
#### Apply z-axis torque to the base ###############################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
#### To propeller 0 ################################################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
#### To the center of mass #########################################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=4, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
p.applyExternalTorque(DRONE_IDS[0],
linkIndex=4,
torqueObj=[0., 0., 5e-6],
flags=p.LINK_FRAME,
physicsClientId=PYB_CLIENT)
#### Step, sync the simulation, printout the state #################################################
p.stepSimulation(physicsClientId=PYB_CLIENT)
sync(i, START, env.TIMESTEP)
if i % env.SIM_FREQ == 0: env.render()
#### Reset #########################################################################################
if i > 1 and i % ((ARGS.duration_sec /
(ARGS.num_resets + 1)) * env.SIM_FREQ) == 0:
env.reset()
p.setGravity(0, 0, 0, physicsClientId=PYB_CLIENT)
#### Close the environment #########################################################################
env.close()
#### Compute trace's parameters ############################
DURATION_SEC = int(TRACE_TIMESTAMPS[-1])
SIMULATION_FREQ_HZ = int(len(TRACE_TIMESTAMPS)/TRACE_TIMESTAMPS[-1])
#### Initialize the simulation #############################
env = CtrlAviary(drone_model=DroneModel.CF2X,
num_drones=1,
initial_xyzs=np.array([0, 0, .1]).reshape(1, 3),
physics=ARGS.physics,
freq=SIMULATION_FREQ_HZ,
gui=ARGS.gui,
record=ARGS.record_video,
obstacles=False
)
INITIAL_STATE = env.reset()
action = {"0": np.zeros(4)}
pos_err = 9999.
#### TRACE_FILE starts at [0,0,0], sim starts at [0,0,INITIAL_STATE[2]]
TRACE_CTRL_REFERENCE[:, 2] = INITIAL_STATE["0"]["state"][2]
#### Initialize the logger #################################
logger = Logger(logging_freq_hz=SIMULATION_FREQ_HZ,
num_drones=2,
duration_sec=DURATION_SEC
)
#### Initialize the controller #############################
ctrl = DSLPIDControl(env)
