diff --git a/gym_pybullet_drones/envs/CFAviary.py b/gym_pybullet_drones/envs/CFAviary.py index f1ceaa1a1..e3895597c 100644 --- a/gym_pybullet_drones/envs/CFAviary.py +++ b/gym_pybullet_drones/envs/CFAviary.py @@ -212,7 +212,6 @@ def step(self, i): while self.tick / self.firmware_freq < t + self.ctrl_dt: # Step the environment and print all returned information. - print("Action", self.action, self.setpoint.position.z) obs, reward, terminated, truncated, info = super().step(self.action) # Get state values from pybullet @@ -255,7 +254,6 @@ def step(self, i): # Get action new_action = self.PWM2RPM_SCALE * np.clip(np.array(self.pwms), self.MIN_PWM, self.MAX_PWM) + self.PWM2RPM_CONST - new_action = new_action[[3, 2, 1, 0]] # FL, BL, BR, FR if self.ACTION_DELAY: # Delays action commands to mimic real life hardware response delay diff --git a/gym_pybullet_drones/examples/cf.py b/gym_pybullet_drones/examples/cf.py index c5c25342a..b4defad79 100644 --- a/gym_pybullet_drones/examples/cf.py +++ b/gym_pybullet_drones/examples/cf.py @@ -45,7 +45,7 @@ from gym_pybullet_drones.utils.Logger import Logger from gym_pybullet_drones.utils.utils import sync, str2bool -DEFAULT_DRONES = DroneModel("racer") +DEFAULT_DRONES = DroneModel("cf2x") DEFAULT_PHYSICS = Physics("pyb") DEFAULT_GUI = True DEFAULT_PLOT = True @@ -94,43 +94,34 @@ def run( ) #### Run the simulation #################################### - with open("../assets/beta.csv", mode='r') as csv_file: - csv_reader = csv.DictReader(csv_file) - trajectory = iter([{ - "pos": np.array([ - float(row["p_x"]), - float(row["p_y"]), - float(row["p_z"]), - ]), - "vel": np.array([ - float(row["v_x"]), - float(row["v_y"]), - float(row["v_z"]), - ]), - } for row in csv_reader]) + delta = 75 # 3s + trajectory = [[0, 0, 0] for i in range(delta)] + \ + [[0, 0, i/delta] for i in range(delta)] + \ + [[i/delta, 0, 1] for i in range(delta)] + \ + [[1, i/delta, 1] for i in range(delta)] + \ + [[1-i/delta, 1, 1] for i in range(delta)] + \ + [[0, 1-i/delta, 1] for i in range(delta)] + \ + [[0, 0, 1-i/delta] for i in range(delta)] + action = np.zeros((NUM_DRONES,4)) ARM_TIME = 1. - TRAJ_TIME = 0.1 START = time.time() - for i in range(0, int(duration_sec*env.firmware_freq)): - t = i/env.firmware_freq + for i in range(0, len(trajectory)): + t = i/env.ctrl_freq #### Step the simulation ################################### obs, reward, terminated, truncated, info = env.step(i) - print(t) - if t > TRAJ_TIME: - for j in range(NUM_DRONES): - # try: - target = next(trajectory) - pos = target["pos"]+[INIT_XYZ[j][0], INIT_XYZ[j][1], 0] - vel = target['vel'] + for j in range(NUM_DRONES): + try: + target = trajectory[i] + pos = target+[INIT_XYZ[j][0], INIT_XYZ[j][1], 0] + vel = np.zeros(3) acc = np.zeros(3) - yaw = 0 + yaw = i*np.pi/delta/2 rpy_rate = np.zeros(3) - print(pos) env.sendFullStateCmd(pos, vel, acc, yaw, rpy_rate, t) - # except: - # break + except: + break #### Log the simulation ####################################