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])