final

L-ED · Mar 6, 2024 · 6e7455b · 6e7455b
1 parent 48e5af4
commit 6e7455b
Show file tree

Hide file tree

Showing 6 changed files with 115 additions and 117 deletions.
diff --git a/gym_pybullet_drones/assets/custom.urdf b/gym_pybullet_drones/assets/custom.urdf
@@ -2,7 +2,7 @@
 
 <robot name="cf2">
 
-  <properties kf="1.9571e-08" km="2.415297e-10" thrust2weight="2.25" prop_radius="15.24e-2" battery_v="11.1" kv="2450"/>
+  <properties kf="1.9571e-08" km="2.415297e-10" thrust2weight="2.25" prop_radius="15.24e-2" battery_v="11.1" kv="2450" pitch="0.1143"/>
 
   <link name="base_link"> 
 

diff --git a/gym_pybullet_drones/devices/sensors/imu.py b/gym_pybullet_drones/devices/sensors/imu.py
@@ -92,7 +92,7 @@ def mpu6000(base=None): #spi(low) - 100kHZ, spi(high) - 1MHZ, SPI(readonly) - 20
     }
 
     return IMU(
-        frequency= int(1e4),
+        frequency= int(1e3),
         accel_params=accel,
         gyro_params=gyro,
         base=base

diff --git a/gym_pybullet_drones/envs/single_agent_rl/HoverIMU.py b/gym_pybullet_drones/envs/single_agent_rl/HoverIMU.py
@@ -25,6 +25,7 @@ def __init__(
 
         self.max_g = 5*9.8
         self.max_ang_vel = 10 #10 
+        self.max_radius = 2
 
 
         if client is None:
@@ -88,7 +89,7 @@ def check_termination(self):
         term = False
         pos = np.copy(self.drone.state.world.pos)
         pos[2] -= 1
-        is_out = sum(pos**2) > 4
+        is_out = sum(pos**2) > self.max_radius**2
         if is_out:
             term = True
         return term
@@ -113,32 +114,25 @@ def reward(self):
 
         disp = np.array([0, 0, 1]) - state.world.pos
         displ_dir = disp/np.linalg.norm(disp)
-
+
+        displ_normalized = np.sum(disp**2)/self.max_radius**2
+
         vel = state.world.vel
         flight_dir = vel/np.linalg.norm(vel)
 
+        vel_normalized = np.sum(vel**2)/self.drone.max_speed**2
+
+        closenes_reward = (1-displ_normalized)*(1-vel_normalized)
+
         # print('Position: ', pos, " Velocity: ", vel)
         # print('Displacement ', disp, "Disp_dir ", displ_dir)
         # print("Velocity dir ", flight_dir)
 
-        # p.addUserDebugLine(
-        #     pos, pos+pos*2000000000
-        # )
-
-        # p.addUserDebugLine(
-        #     np.array([0, 0, 1]), np.array([0, 0, 1])+np.array([0, 0, 2]), [0, 1, 0]
-        # )
-
-
         # print(flight_dir, displ_dir)
 
-        ang_cos = np.dot(flight_dir, displ_dir)
-        # print("Cos between dirs", ang_cos)
-        # print(ang_cos)
+        dir_reward = np.dot(flight_dir, displ_dir)
+        angles_reward = -np.linalg.norm(state.world.ang_vel) 
 
-        reward = 100*ang_cos - np.linalg.norm(np.abs(vel)) - \
-                10*np.sum(disp**2)- \
-                10*np.linalg.norm(np.abs(state.world.rpy)) - \
-                100*np.linalg.norm(np.abs(state.world.ang_vel))
+        reward = closenes_reward + dir_reward + angles_reward
 
         return reward
diff --git a/gym_pybullet_drones/examples/hover_rl.py b/gym_pybullet_drones/examples/hover_rl.py
@@ -8,9 +8,10 @@ def main(test=True):
     agent = PPO(
         'MlpPolicy', 
         env=env,
-        verbose=1
+        verbose=1,
+        n_steps=8000
     )
-    # agent.learn(30000)
+    agent.learn(1000000)
     env = HoverIMU(visualize=True)
 
     state, _=env.reset()
@@ -20,7 +21,7 @@ def main(test=True):
             deterministic=True
         )
         state, reward, terminated, truncated, info = env.step(action)
-        print(info['timestemp'])
+        print(info['timestemp'], reward)
 
         time.sleep(env.timestep)
         if terminated or truncated:

diff --git a/gym_pybullet_drones/model.ipynb b/gym_pybullet_drones/model.ipynb
diff --git a/gym_pybullet_drones/vehicles/quad.py b/gym_pybullet_drones/vehicles/quad.py
@@ -121,6 +121,7 @@ def _parseURDF(self):
         # self.COLLISION_Z_OFFSET = COLLISION_SHAPE_OFFSETS[2]
 
         self.prop_diam = float(urdf_tree[0].attrib['prop_radius'])*2
+        self.prop_pitch = float(urdf_tree[0].attrib['pitch'])
         # self.GND_EFF_COEFF = float(urdf_tree[0].attrib['gnd_eff_coeff'])
         # DRAG_COEFF_XY = float(urdf_tree[0].attrib['drag_coeff_xy'])
         # DRAG_COEFF_Z = float(urdf_tree[0].attrib['drag_coeff_z'])
@@ -136,6 +137,7 @@ def _parseURDF(self):
         vbat = float(urdf_tree[0].attrib['battery_v'])
         kv = float(urdf_tree[0].attrib['kv'])
         self.max_rpm = vbat*kv
+        self.max_speed = self.max_rpm*self.prop_pitch/60 # meters per second
 
     def load_model(self):
 
@@ -337,10 +339,10 @@ def update_state(self, new_state):
         lin_vel, ang_vel = pb.getBaseVelocity(self.ID, self.client)
         pos, qtr = pb.getBasePositionAndOrientation(self.ID, self.client)
 
-        self.state.world.vel = lin_vel
-        self.state.world.ang_vel = ang_vel
-        self.state.world.pos = pos
-        self.state.world.qtr = qtr
+        self.state.world.vel = np.array(lin_vel)
+        self.state.world.ang_vel = np.array(ang_vel)
+        self.state.world.pos = np.array(pos)
+        self.state.world.qtr = np.array(qtr)
 
         # local_lin_vel = np.dot(
         #     self.state.R.T,