task_mh2.py

import numpy as np
from physics_sim import PhysicsSim

class Task_mh():
    """Task (environment) that defines the goal and provides feedback to the agent."""
    def __init__(self, init_pose=None, init_velocities=None, 
        init_angle_velocities=None, runtime=5., target_pos=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) 
        self.action_repeat = 3

        self.state_size = self.action_repeat * 6
        self.action_low = 0
        self.action_high = 900
        self.action_size = 4
        self.take_off = np.array([0., 0., 20.])
        
        # Goal
        self.target_pos = target_pos if target_pos is not None else np.array([0., 0., 10.]) 

    def get_reward(self):
        """Uses current pose of sim to return reward."""
        
        # ----- Approach #1 -----
        # reward = 1.-.3*(abs(self.sim.pose[:3] - self.target_pos)).sum()
        
        # ----- Approach #2 -----
            # the reward function should be normalized between -1 and 1 (except for colisions) in order to the         NN better learn the gradients. The hyperbolic tangent function np.tanh is used for this purpose. But that didn't produce anything efficient.
        
        # reward = np.tanh(1-0.03*(abs(self.sim.pose[:3] - self.target_pos)).sum())
        
        # ----- Approach #3 -----
        
        # reward = 0
        # approach_x = abs(self.sim.pose[0]-self.target_pos[0])**2
        # approach_y = abs(self.sim.pose[1]-self.target_pos[1])**2
        # approach_z = abs(self.sim.pose[2]-self.target_pos[2])**2
        
        
        # dist_quadtotar = np.sqrt((self.sim.pose[0]-self.target_pos[0])**2 + (self.sim.pose[1]-self.target_pos[1])**2 + (self.sim.pose[2]-self.target_pos[2])**2)
        
        # positive reward when take-off.
        # if self.sim.pose[2] > self.take_off[2]:
        #     reward += 1.1
            
        # positive reward when higher z than target_pos.
        # elif self.sim.pose[2] > self.target_pos[2]:
        #     reward += 1.3
            
        #     # positive reward when close to target_pos.
        #     if dist_quadtotar < 10:
        #        reward += 70 # 100 500

        #     # positive reward when close to target_pos.
        #     if approach_x < 10:
        #        reward += 50 # 100 500
                
        #     # positive reward when close to target_pos.
        #     elif approach_y < 10:
        #        reward += 50 # 100 500
                
        #     # positive reward when close to target_pos.
        #     elif approach_z < 10:
        #        reward += 50 # 100 500
                
                
        # # negative reward if flying but not reaching the Z of target_pos.
        # # elif self.target_pos[2] > self.sim.pose[2]:
        # #    reward += -1.3
            
        # # negative reward if flying but not reaching the target_pos.
        # elif self.target_pos[2] > self.sim.pose[2] and dist_quadtotar >= 10:
        #     reward += -1.3
        
        # negative reward if done before the runtime finished and stop
        # elif self.sim.done and self.sim.runtime > self.sim.time:
        #     reward += -2 # -1
        
        # ----- Approach #4 -----
        # reward = np.tanh(1-0.0046*(abs(self.sim.pose[:3] - self.target_pos)).sum())
        reward = np.tanh(1-0.005*(abs(self.sim.pose[:3] - self.target_pos)).sum())
        
        return reward

    def step(self, rotor_speeds):
        """Uses action to obtain next state, reward, done."""
        reward = 0
        pose_all = []
        for _ in range(self.action_repeat):
            done = self.sim.next_timestep(rotor_speeds) # update the sim pose and velocities
            reward += self.get_reward() 
            pose_all.append(self.sim.pose)
        next_state = np.concatenate(pose_all)
        return next_state, reward, done

    def reset(self):
        """Reset the sim to start a new episode."""
        self.sim.reset()
        state = np.concatenate([self.sim.pose] * self.action_repeat) 
        return state