def step_with_vehicle_commands(
bullet_client, vehicle, radius, omega, timestep_sec=time_step
):
prev_friction_sum = None
# Proceed till the end of half of the circle.
n_steps = int(0.5 * 3.14 / (omega * timestep_sec))
desired_trajectory = []
controller_state = TrajectoryTrackingControllerState()
for step_num in range(n_steps):
desired_trajectory = build_trajectory(radius, omega, step_num)
TrajectoryTrackingController.perform_trajectory_tracking_PD(
desired_trajectory,
vehicle,
controller_state,
dt_sec=timestep_sec,
)
bullet_client.stepSimulation()
final_error = math.sqrt(
(vehicle.position[0] - desired_trajectory[0][0]) ** 2
+ (vehicle.position[1] - desired_trajectory[1][0]) ** 2
)
return final_error
def run(client, vehicle, plane_body_id, sliders, n_steps=1e6):
prev_friction_sum = None
controller_state = TrajectoryTrackingControllerState()
for step_num in range(n_steps):
if not client.isConnected():
print("Client got disconnected")
return
# Simple circular trajectory with radius R and angular velocity Omega in rad/sec
num_trajectory_points = 15
R = 40
omega_1 = 0.1
omega_2 = 0.2
if step_num > 1.4 * 3.14 / (TIMESTEP_SEC * omega_1):
raise ValueError("Terminate and plot.")
if step_num > 3.14 / (TIMESTEP_SEC * omega_1):
Omega = omega_2
alph = ((omega_1 - omega_2) / omega_2) * 3.14 / (TIMESTEP_SEC *
omega_1)
else:
Omega = omega_1
alph = 0
desheadi = step_num * Omega * TIMESTEP_SEC
trajectory = [
[
-(R - R * math.cos(
(step_num + i + alph) * Omega * TIMESTEP_SEC))
for i in range(num_trajectory_points)
],
[
R * math.sin((step_num + i + alph) * Omega * TIMESTEP_SEC)
for i in range(num_trajectory_points)
],
[(step_num + i + alph) * Omega * TIMESTEP_SEC
for i in range(num_trajectory_points)],
[R * Omega for i in range(num_trajectory_points)],
]
TrajectoryTrackingController.perform_trajectory_tracking_PD(
trajectory,
vehicle,
controller_state,
dt_sec=TIMESTEP_SEC,
heading_gain=0.05,
lateral_gain=0.65,
velocity_gain=1.8,
traction_gain=2,
derivative_activation=False,
speed_reduction_activation=False,
)
client.stepSimulation()
vehicle.sync_chassis()
frictions_ = frictions(sliders)
if prev_friction_sum is not None and not math.isclose(
sum(frictions_.values()), prev_friction_sum):
print("Updating")
return # will reset and take us to the next episode
prev_friction_sum = sum(frictions_.values())
look_at(client, vehicle.position, top_down=True)
print("Speed: {:.2f} m/s".format(vehicle.speed), end="\r")
vel.append(vehicle.speed)
head.append(vehicle.heading)
des.append(desheadi)
tim.append(step_num * TIMESTEP_SEC)
xx.append(vehicle.position[0])
yy.append(vehicle.position[1])
xdes.append(trajectory[0][0])
ydes.append(trajectory[1][0])