AirSim Added function to apply external forces (simple payload)

Fixes: #

No fixes, feature addition

About

added implementation of arbitrary external force
can be used as a simple payload forcing function
force vector is passed in world coordinates in units of Newtons
applied similarly to how wind has been implemented
external force gets added to next_wrench in the call to getNextKinematicsNoCollision() next_wrench = body_wrench + drag_wrench + ext_force_wrench;
rebased to latest airsim master
tested on UE4.26.0

How Has This Been Tested?

tested adding forces to hovering drone to see if it is pushed in the right direction

"""
Minimal example showing external force function. This example will apply
external forces in the following order:
- no additional forces for seconds 0-2
- 10N towards the ground for seconds 2-4
- no additional forces for seconds 4-8 to allow the drone to rise again
- 2N in the x direction for seconds 8-10
"""
import airsim
import time
from airsim.types import Pose, Quaternionr, Vector3r
import numpy as np

dt = 0.005
sim_time = 10
# ~ 6800 rad/s which is around the hover point for the drone
pwm = np.array([0.61, 0.61, 0.61, 0.61])
forces = [
    [0, 0, 0],
    [0, 0, 0],
    [0, 0, 10],
    [0, 0, 10],
    [0, 0, 0],
    [0, 0, 0],
    [0, 0, 0],
    [0, 0, 0],
    [2, 0, 0],
    [2, 0, 0],
]
# for printing
last_index = -1

# connect to airsim client
client = airsim.MultirotorClient()
client.confirmConnection()
client.reset()
client.enableApiControl(True)
client.armDisarm(True)

for ii in range(0, int(sim_time / dt)):
    # Send simple hover command without any control
    client.simPause(False)
    client.moveByMotorPWMsAsync(pwm[0], pwm[1], pwm[2], pwm[3], dt)
    f = forces[int(ii*dt)]
    if int(ii*dt) != last_index:
        last_index += 1
        print(f'applying external force: {f} Newtons')
    # in NED coordinates, so gravity would be +z
    client.simSetExtForce(Vector3r(f[0], f[1], f[2]))


    # run for dt seconds and pause to run in lockstep
    time.sleep(dt)
    client.simPause(True)

# disconnect
client.simPause(False)
client.reset()
client.enableApiControl(False)
client.armDisarm(False)