(self, locs, yaws, spds, acts)
| 74 | self.throt_accel = 0.5633837 |
| 75 | |
| 76 | def forward(self, locs, yaws, spds, acts): |
| 77 | # Kinematic bicycle model. Numbers are the tuned parameters from World on Rails |
| 78 | steer = acts[..., 0:1].item() |
| 79 | throt = acts[..., 1:2].item() |
| 80 | brake = acts[..., 2:3].astype(np.uint8) |
| 81 | |
| 82 | if (brake): |
| 83 | accel = self.brake_accel |
| 84 | else: |
| 85 | accel = self.throt_accel * throt |
| 86 | |
| 87 | wheel = self.steer_gain * steer |
| 88 | |
| 89 | beta = math.atan(self.rear_wb / (self.front_wb + self.rear_wb) * math.tan(wheel)) |
| 90 | yaws = yaws.item() |
| 91 | spds = spds.item() |
| 92 | next_locs_0 = locs[0].item() + spds * math.cos(yaws + beta) * self.dt |
| 93 | next_locs_1 = locs[1].item() + spds * math.sin(yaws + beta) * self.dt |
| 94 | next_yaws = yaws + spds / self.rear_wb * math.sin(beta) * self.dt |
| 95 | next_spds = spds + accel * self.dt |
| 96 | next_spds = next_spds * (next_spds > 0.0) # Fast ReLU |
| 97 | |
| 98 | next_locs = np.array([next_locs_0, next_locs_1]) |
| 99 | next_yaws = np.array(next_yaws) |
| 100 | next_spds = np.array(next_spds) |
| 101 | |
| 102 | return next_locs, next_yaws, next_spds |
| 103 | |
| 104 | |
| 105 | def get_entry_point(): |
no outgoing calls
no test coverage detected