Camera moving away from drone

[defrag-bambino]

Hey,

I am seeing some strange behavior where after a few seconds, the onboard camera of the drone moves away, such that the drone sees itself. Here is a minimal example, only flying straight up. Also, the drone seems to be of slow?

import numpy as np
import tqdm
import matplotlib.pyplot as plt

from PyFlyt.core import Aviary

# the starting position and orientation
start_pos_quadx = np.array([0.0, 0.0, 1.0])
start_poss = np.array([start_pos_quadx])
start_orn = np.array([np.zeros(3)])

# individual spawn options
quadx_options = dict(use_camera=True, drone_model="cf2x", camera_angle_degrees=-90, camera_FOV_degrees=130)

# environment setup
env = Aviary(
    start_pos=start_poss,
    start_orn=start_orn,
    render=True,
    drone_type=["quadx"],
    drone_options=[quadx_options],
)

# set quadx to angular velocity control
env.set_mode([0])

# prepare a window for the camera
fig, ax = plt.subplots(1, 1, figsize=(10, 5))
ax.set_title("QUADX")
plt.ioff()
fig.tight_layout()

# Initialize the image
img_quadx = ax.imshow(env.drones[0].rgbaImg, animated=True)
img_w, img_h = env.drones[0].rgbaImg.shape[0], env.drones[0].rgbaImg.shape[0] # 128, 128

# simulate for 1000 steps (1000/120 ~= 8 seconds)
for i in tqdm.tqdm(range(250)):
    env.step()
    env.set_setpoint(0, np.array([0.0, 0.0, 0.0, 1.]))
    img_quadx.set_data(env.drones[0].rgbaImg)
    fig.canvas.draw_idle()
    plt.pause(0.001)

del env

[jjshoots]

Interesting, I've definitely never seen this before. I will look at it soon, thanks for the report!

[jjshoots]

@defrag-bambino I have looked at your issue and have identified the problem:

1. The drone seems slow because the RTF is way less than 1 (0.09 on my setup with i9-10980HK). In reality, the drone is reaching about 45 m/s upwards velocity with flight mode 0 and constant full throttle. You can query this using env.state(0)[-2]. The relevant documentation for what each component of the states means is here.
2. 45 m/s vertical speed is about 162 km/h, which is extremely fast for the simulator (and a drone of this size). There is probably something we can do with adjusting aerodynamics effects but at the simulation step resolution, that would incur quite a bit more computational overhead. However, with regards to your issue, the reason the camera is seemingly looking upwards at the drone is because of these reasons: a) your camera angle points upwards (downwards camera angle is positive) b) there is a slight delay between the camera's current capture location and the drone's actual location. This is an artifact of PyBullet's rendering capabilities and I don't think there's anything we can do. c) the drone is flying so fast upwards that the location where the camera image is taken is offset from where the drone actually is by about 0.5 meters, resulting in the camera looking at the drone itself.

There are many ways to fix the issue, but fundamentally I think that the simulation expectations are a big unrealistic. Perhaps using flight mode 6 and capping the maximum flight velocity?

[defrag-bambino]

Interesting, thanks for the info! Any idea why the sim is so slow? Because of rendering the images? Or the high physics update rate?

About the delay when rendering images: I've had a similar problem when using AirSim before. The solution there was to pause the simulation right before taking the image and continuing it right after. Would that also be possible with PyBullet?

Regarding your proposed solution of using flight mode 6, and even more generally, the documentation does not really state what the input limits or even its units (e.g. deg vs rad, m/s vs km/h?). I will definetly try mode 6 though, thanks.

[jjshoots]

All units are SI, so we're using m/s and rad/s for velocities. In terms of thrust, at full throttle, the cf2x produces a 2.25:1 T:W. It's motor specs are here with the weight defined here (thrust is in Newtons, mass is in kg).

The speed of the simulation being slow is interesting... Most likely plt and data copying is introducing a significant overhead. I will have another look at that in the morning.

[jjshoots]

@defrag-bambino, I finally got round to looking at this. It turns out that the main issue is related to #42. The solution is to set the camera_fps parameter for the quadx to something like 30 FPS. This can be achieved via:

# individual spawn options
quadx_options = dict(
    use_camera=True,
    drone_model="cf2x",
    camera_angle_degrees=-90,
    camera_FOV_degrees=130,
    camera_fps=30,
)

[defrag-bambino]

Thanks, this definitely answers the slow simulation. However, even without all the plotting stuff (and now RTF ~= 1.0) the drone still sees itself from a certain speed on. While I admit these speeds may be unreasonable to use, the problem still remains. For now, I will just reduce the speed of the drone. I am fine with closing this issue. If you want to keep it open for debugging later, feel free.

[jjshoots]

Yep, I think we can close the issue, the simulation is already paused between each step since it is a synchronous physics engine. I think this is something on Bullet's simulation side. Thanks for raising the issue!