Set Vehicle Pose teleporting to random locations when vehicle collided with a wall

[SaundersJE97]

Bug report

• AirSim Version/#commit: v1.6.0
• UE/Unity version: Unreal Engine v4.27.2
• autopilot version:
• OS Version: Ubuntu 20.04

What's the issue you encountered?

I am using a reinforcement learning algorithm for motion planning and frequently when the drone collides with an object the drone teleports to the wrong start location. I have noticed this happens more often when the drone is closer to the obstacle. The behaviour has not been consistent, sometimes the drone will not take any movement commands, alternatively the drone will gain a huge speed boost and will consequently be thrusted in the forward direction.

Please see the following YouTube video for what I have been experiencing: https://www.youtube.com/watch?v=hGzm7fKZCTg

I am using the following client command to fly the drone through the environment. Where action is a variable in the range of [-1, 1].

quad_vel = Utils.getClient().getMultirotorState(self.droneName).kinematics_estimated.linear_velocity
Utils.getClient().simPause(False)
Utils.getClient().moveByVelocityZAsync(
    3.0,
    action,
    4.0,
    self.actionTime,
    vehicle_name=self.droneName
).join()
Utils.getClient().simPrintLogMessage('Action taken: ', str(action), severity=3)
Utils.getClient().simPause(True)

The teleportation code is as following.

# Up to this point, the simulator is paused to run the RL algorithm
Utils.getClient().simPause(False)
Utils.getClient().cancelLastTask(self.droneName)
time.sleep(1)

getClient().armDisarm(False, droneName) 
getClient().enableApiControl(False, droneName)
# Teleport to the start position on the ground
client.simSetVehiclePose(pose=airsim.Pose(airsim.Vector3r(position[0], position[1], position[2]), airsim.Quaternionr(orientation[0], orientation[1], orientation[2], orientation[3])),
                         ignore_collision=True,
                         vehicle_name=droneName)
time.sleep(1)
client.enableApiControl(True, droneName)
client.armDisarm(True, droneName)
client.moveByVelocityAsync(0, 0, 0, 1.0, vehicle_name=droneName).join()

Settings

{
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
  "SimMode": "Multirotor",
  "ViewMode": "",
  "SettingsVersion": 1.2,
  "ClockSpeed": 5.0,
  "LocalHostIp": "127.0.0.1",
  "ApiServerPort": 41451,
  "SubWindows": [
    {"WindowID": 0, "CameraName": "0", "ImageType": 0, "VehicleName": "Drone0", "Visible": false, "External": false},
    {"WindowID": 1, "CameraName": "0", "ImageType": 0, "VehicleName": "Drone0", "Visible": false, "External": false}
  ],
  "Vehicles": {
    "Drone0": {
      "VehicleType": "SimpleFlight",
      "AutoCreate": true,
      "DefaultVehicleState": "Armed",
      "Sensors": {
        "Distance_0": {
          "SensorType": 5,
          "Enabled" : true,
          "MinDistance": 0.2,
          "MaxDistance": 40,
          "X": 0, "Y": 0, "Z": 0,
          "Yaw": 0, "Pitch": -90, "Roll": 0,
          "DrawDebugPoints": false
        }
      },
      "Cameras": {
        "scene_cam": {
          "CaptureSettings": [
            {
              "FOV_Degrees": 90,
              "ImageType": 0,
              "Width": 256,
              "Height": 144
            }
          ],
          "Pitch": 0.0,
          "Roll": 0.0,
          "Yaw": 0.0,
          "X": 0.0,
          "Y": 0.0,
          "Z": -0.4
        },
        "depth_cam": {
          "CaptureSettings": [
            {
              "FOV_Degrees": 90,
              "ImageType": 1,
              "Width": 64,
              "Height": 64
            }
          ],
          "Pitch": 0.0,
          "Roll": 0.0,
          "Yaw": 0.0,
          "X": 0.0,
          "Y": 0.0,
          "Z": -0.6
        },
        "depth_dual_left": {
          "CaptureSettings": [
            {
              "FOV_Degrees": 90,
              "ImageType": 1,
              "Width": 16,
              "Height": 16
            }
          ],
          "Pitch": 0.0,
          "Roll": 0.0,
          "Yaw": 45.0,
          "X": 0.0,
          "Y": 0.0,
          "Z": -0.6
        },
        "depth_dual_right": {
          "CaptureSettings": [
            {
              "FOV_Degrees": 90,
              "ImageType": 1,
              "Width": 16,
              "Height": 16
            }
          ],
          "Pitch": 0.0,
          "Roll": 0.0,
          "Yaw": -45.0,
          "X": 0.0,
          "Y": 0.0,
          "Z": -0.6
        }
      }
    }
  }
}

How can the issue be reproduced?

(Please see code above)

Include full error message in text form

(No errors experienced from python on UE)

What's better than filing an issue? Filing a pull request :).

[jonyMarino]

@SaundersJE97 This is a known limitation for doing RL quickly with our flight controller. You are teleporting the drone but the state of the controller remains the same. We would need to have an API method for saving the state of the controller and another one for loading it. Another way that was asked is to increase the clock frequency to go fast again to that location. You can spawn a new drone on that point and set its kinematic state, but you are not going to have the same controller state that when it collided. For now the only solution is to re run the simulation till that moment.

[SaundersJE97]

Thank you for your reply, this clears it up. Ideally I would benefit from a vehicle specific 'reset(vehicle_name)' api call. Is there a function within the SimpleFlight Controller that would reset the controller state? For example would resetting the firmware under 'SimpleFlightApi.hpp' would provide this?

firmware_->reset();

Thanks, Jack

[jonyMarino]

@SaundersJE9, In that case, you can call client.reset() before getClient().armDisarm(False, ... I think you would not need to cancel the last task.

[SaundersJE97]

@jonyMarino That works for a single drone, however I was considering doing multi-agent reinforcement learning and, although still useful, using client.reset() resets all drones within the environment. I understand it is currently a feature request to add client.reset(vehicle_name), I would be happy to investigate how to add this to the api since I will need it in my project.

Taking a look at AirLib and more specifically RpcLibServerBase.cpp, the command either sim_world_api-reset(); or 'getVehicleApi("")->reset()` is called. I tried modifying this to add a specific vehicle reset function within the api:

pimpl_->server.bind("resetVehicle", [&](const std::string& vehicle_name) -> void {
    //Exit if already resetting.
    static bool resetInProgress;
    if (resetInProgress)
        return;

    //Reset
    resetInProgress = true;
    getVehicleSimApi(vehicle_name)->reset();
    resetInProgress = false;
});

From my understanding this only resets the 'simulation' drone and not the AirLib instance of the drone. I am missing the controller reset function. I had some issues with this and had to run it on the game thread. Unfortunately, this still didn't fix the issue I outlined in the beginning of this thread.

void PawnSimApi::resetImplementation()
{
    UAirBlueprintLib::RunCommandOnGameThread([this]() {
    ...
    }
}

Looking into getVehicleApi(vehicle_name)->reset(); the object VehicleApiBase inherits from UpdatableObject which when reset is called will revert back to it's initalized state. I added getVehicleSimApi(vehicle_name)->reset(); to the reset(vehicle_name) api call but I still get the rebounding effect when hitting. My current thought is I need to reset the controller. However, within the SimpleFlightApi.hpp the firmware is reset when calling getVehicleApi->reset();.

    virtual void resetImplementation() override
    {
        MultirotorApiBase::resetImplementation();

        firmware_->reset();
    }

So if the controller is also being reset, I am not sure what could be causing it.

[jonyMarino]

@SaundersJE97 I would need to dig into this. Another idea is to use simAddVehicle(), but we don't have a simDestroyVehicle(), so I don't know how your RAM usage will end. Can you give it a try?

[SaundersJE97]

I hadn't considered that function, I'll give it a try and report back.

[Kinggrass]

@SaundersJE97 Did you find a solution or tried the last suggestion? Currently I also need to set the Drone to different locations but if I do so the Drone is instable afterwards and starts to drift. I am seeking some hints for a solution.

[SaundersJE97]

I did find a solution, albeit a very hacky work around. I forked the repository and made the changes where you can resetVehcle('') and it will reset the individual drone at specific locations: check it out - https://github.com/SaundersJE97/PRL4AirSim