Quadcopter automatically enters hover after API angle rate command

[praveenVnktsh]

Question

What's your question?

I am currently using the moveByAngleRatesZAsync() api command, and I notice that after each call completes with .join() , the drone automatically enters hover mode. Is there any way to bypass this?

This is the command I am using -

self.drone.moveByAngleRatesZAsync(self.dronePose[0], self.dronePose[1], self.dronePose[2], self.dronePose[3], 0.01).join()

Include context on what you are trying to achieve

I am trying to make the drone pass through gates using proximal policy optimization and a discrete action space. The actions are to increase the rates in either axis.

Context details

Settings.json -

{
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
  "SettingsVersion": 1.2,
  "SimMode": "Multirotor",
  "ClockSpeed": 1.0,
  "CameraDefaults": {
    "CaptureSettings": [
      {
        "ImageType": 0,
        "Width": 128,
        "Height": 128,
        "FOV_Degrees": 90
      },

      {
        "ImageType": 5,
        "Width": 128,
        "Height": 128,
        "FOV_Degrees": 90
      }
    ],
    "SubWindows": [
      {
        "WindowID": 0,
        "CameraName": "0",
        "ImageType": 3,
        "VehicleName": "",
        "Visible": true
      },
      {
        "WindowID": 1,
        "CameraName": "1",
        "ImageType": 5,
        "VehicleName": "",
        "Visible": true
      },
      {
        "WindowID": 2,
        "CameraName": "2",
        "ImageType": 0,
        "VehicleName": "",
        "Visible": true
      }
    ],
    "X": 0.0,
    "Y": 0,
    "Z": 0.0,
    "Pitch": 0,
    "Roll": 0,
    "Yaw": 0
  },
  "Vehicles": {
    "drone_0": {
      "Pitch": 0.0,
      "Roll": 0.0,
      "VehicleType": "SimpleFlight",
      "X": -0.47,
      "Y": -1.80,
      "Yaw": 90.0,
      "Z": 0.0,
      "DefaultVehicleState": "Armed",
      "EnableCollisionPassthrogh": false,
      "EnableCollisions": true,
      "AllowAPIAlways": true,
      "RC": {
        "RemoteControlID": 0,
        "AllowAPIWhenDisconnected": false
      }
    }
  }
}

I am using the neurips competition binaries. However, I am facing the same issue even with the latest version of airsim.

Here is the openai gym wrapper for reference:

class AirSimDroneEnv(AirSimEnv):
    def __init__(self, image_shape=(1, 10), device=torch.device('cuda')):
        self.image_shape = image_shape
        super().__init__(image_shape)
        self.stepLength = 0.05
        self.episodes = 0

        self.device = device

        self.state = {
            "position": np.zeros(3),
            "collision": False,
            "prev_position": np.zeros(3),
        }
        self.action_space = spaces.Discrete(9)
        # self.action_space = spaces.Box(np.array([-0.3, -0.3, -0.3, -0.03]), np.array([0.3, 0.3, 0.3, 0.03]), dtype = np.float64 )

        self.drone = airsim.MultirotorClient()
        self.drone.confirmConnection()
        self.drone.simLoadLevel('Soccer_Field_Easy')
        self.activeGates = sorted(self.drone.simListSceneObjects('Gate.*'))
        self.origActiveGates = sorted(self.drone.simListSceneObjects('Gate.*'))

        print(self.activeGates)

        cv2.namedWindow('recon', cv2.WINDOW_GUI_NORMAL)
        cv2.namedWindow('fpv', cv2.WINDOW_GUI_NORMAL)

        self.model = OverallGatePoseEstimator(10).to(device=device)
        self.model.load_state_dict(torch.load(
            'src/trainedModel/trial9/model_81.pth'))
        self.model.eval()

        self.transforms = transforms.Compose([
            transforms.ToPILImage(),
            transforms.ToTensor(),
        ])

        self.reset()

    def __del__(self):
        self.drone.reset()

    def _get_obs(self):
        responses = self.drone.simGetImages([
            airsim.ImageRequest("0", airsim.ImageType.Scene, False, False),
            airsim.ImageRequest(
                "1", airsim.ImageType.Segmentation, False, False)
        ])
        rgbImg = responses[0]
        segImg = responses[1]

        seg1d = np.fromstring(segImg.image_data_uint8, dtype=np.uint8)
        seg = seg1d.reshape(segImg.height, segImg.width, 3)

        rgb1d = np.fromstring(rgbImg.image_data_uint8, dtype=np.uint8)
        rgb = rgb1d.reshape(rgbImg.height, rgbImg.width, 3)
        truthArray = np.where(seg[:, :, 0] == 134, seg[:, :, 1] == 93,
                              seg[:, :, 2] == 210)
        if truthArray.any() == False:
            self.dead = True

        rgb[truthArray] = np.array([36, 20, 250])
        rgb = cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB)

        inp = self.transforms(rgb).float().to(
            device=self.device).view(1, 3, 128, 128)
        z, mu, logvar = self.model.vae.encodeAndReparametrize(inp)
        recon = self.model.vae.decode(z).view(
            3, 128, 128).cpu().detach().numpy()
        cv2.imshow('fpv', rgb)
        cv2.imshow('recon', recon.transpose((1, 2, 0)))
        cv2.waitKey(1)

        output = z.cpu().detach().numpy()

        self.drone_state = self.drone.getMultirotorState()
        # print(self.drone_state)

        self.state["position"] = self.drone_state.kinematics_estimated.position
        self.state["velocity"] = self.drone_state.kinematics_estimated.linear_velocity
        self.state['orientation'] = to_eularian_angles(
            self.drone_state.kinematics_estimated.orientation)
        collision = self.drone.simGetCollisionInfo().has_collided
        self.state["collision"] = collision

        return output

    def _do_action(self, action):
        # quad_vel = self.drone.getMultirotorState().kinematics_estimated.linear_velocity
        quad_offset = []
        yaw = 0
        if action == 0:
            quad_offset = [self.stepLength, 0, 0, 0]
        elif action == 1:
            quad_offset = [0, self.stepLength, 0, 0]
        elif action == 2:
            quad_offset = [0, 0, self.stepLength, 0]
        elif action == 3:
            quad_offset = [-self.stepLength, 0, 0, 0]
        elif action == 4:
            quad_offset = [0, -self.stepLength, 0, 0]
        elif action == 5:
            quad_offset = [0, 0, -self.stepLength, 0]
        elif action == 6:
            quad_offset = [0, 0, 0, self.stepLength]
            # yaw = 30
            # quad_offset = (0, 0, 0)
        elif action == 7:
            quad_offset = [0, 0, 0, -self.stepLength]
            # yaw = -30
            # quad_offset = (0, 0, 0)
        else:
            quad_offset = [0, 0, 0, 0]

        # currentState = self.drone.getMultirotorState()
        # p, r, y = to_eularian_angles(currentState.kinematics_estimated.orientation)
        # z = currentState.kinematics_estimated.position.z_val
        self.dronePose += np.array(quad_offset)
        # print(self.dronePose)
        self.drone.moveByAngleRatesZAsync(self.dronePose[0], self.dronePose[1], self.dronePose[2], self.dronePose[3], 0.001).join()
        # self.drone.moveByAngleRatesZAsync(0, 0.1, 0, 1, -1).join()
        # cv2.waitKey(1000)

    def _compute_reward(self):

        reward = 0

        quadPoint = np.array(
            [
                self.state["position"].x_val,
                self.state["position"].y_val,
                self.state["position"].z_val,
            ]
        )

        if self.state["collision"]:
            reward = -100
        else:
            self.state['targetGate'] = self.activeGates[0]
            self.state['targetGatePose'] = self.drone.simGetObjectPose(
                self.state['targetGate'])
            gatePoint = np.array(
                [

                    self.state['targetGatePose'].position.x_val + 0.47 ,
                    self.state['targetGatePose'].position.y_val + 1.80,
                    self.state['targetGatePose'].position.z_val,
                ]
            )
            dist = np.linalg.norm(gatePoint - quadPoint)

            distReward = 10 - dist
            # speedReward = (np.linalg.norm(
            #     [self.state["velocity"].x_val, self.state["velocity"].y_val, self.state["velocity"].z_val, ]) - 0.5)
            reward = distReward

            if dist < 1.5:
                print('Passed a gate! - ', self.activeGates[0])
                reward += 30
                self.activeGates.pop(0)

        if reward < 0:
            self.timeStatic += 1

        self.totReward += reward
        done = 0
        if reward <= -10 or len(self.activeGates) == 0 or self.timeStatic == 15 or self.dead:
            done = 1
            reward -= 100
            print('EPISODE', self.episodes, 'REWARD = ', self.totReward)
            self.episodes += 1

        return reward, done

    def step(self, action):
        self._do_action(action)
        obs = self._get_obs()
        reward, done = self._compute_reward()
        return obs, reward, done, self.state

    def reset(self):
        self.drone.reset()
        # self.drone.simSetObjectPose('StartBlock', Pose(Vector3r(100, 100, 100)))
        # self.drone.simStartRace(2)
        self.drone.enableApiControl()
        self.drone.simResetRace()
        # self.drone.simStartRace(2)
        self.drone.arm()

        self.drone.moveByVelocityZAsync(0,0,0,0.1).join()
        # self.drone.moveByRollPitchYawZAsync(0, 0, -1.5, 0.5, 2).join()
        # self.drone.moveByVelocityZAsync(0,0,0,1).join()
        self.timeStatic = 0
        self.totReward = 0
        self.dead = False
        self.dronePose = np.array([0.0, 0.0, 0.0, 1.0])
        self.activeGates = self.origActiveGates.copy()
        return self._get_obs()

[stale[bot]]

This issue has been automatically marked as stale because it has not had activity from the community in the last year. It will be closed if no further activity occurs within 20 days.

[praveenVnktsh]

For anyone reading this in the future: I think the issue occurred because the command is not prolonged over several timesteps. The next command was taking more time than the simulation step and hence the quad was hovering after angle rate command.

Try to reduce the time delay between successive API calls to solve the issue.