Speed-Limit RL Reward from Trajectory Samples

Requirements You are given a batch of 2D trajectories stored in a tensor with shape [batch, num_waypoint, 2]. The trajectory is sampled at 10 Hz, so consecutive waypoints are separated by 0.1 seconds. Implement a speed-related penalty that discourages the trajectory from exceeding a permitted speed. Consider both of these penalty formulations: The total duration for which the trajectory is above the limit. A per-step penalty based on the amount by which the speed exceeds the…