| 30 | return self._K_P * error + self._K_I * integral + self._K_D * derivative |
| 31 | |
| 32 | class TCP(nn.Module): |
| 33 | |
| 34 | def __init__(self, config): |
| 35 | super().__init__() |
| 36 | self.config = config |
| 37 | |
| 38 | self.turn_controller = PIDController(K_P=config.turn_KP, K_I=config.turn_KI, K_D=config.turn_KD, n=config.turn_n) |
| 39 | self.speed_controller = PIDController(K_P=config.speed_KP, K_I=config.speed_KI, K_D=config.speed_KD, n=config.speed_n) |
| 40 | |
| 41 | self.perception = resnet34(pretrained=True) |
| 42 | |
| 43 | self.measurements = nn.Sequential( |
| 44 | nn.Linear(1+2+6, 128), |
| 45 | nn.ReLU(inplace=True), |
| 46 | nn.Linear(128, 128), |
| 47 | nn.ReLU(inplace=True), |
| 48 | ) |
| 49 | |
| 50 | self.join_traj = nn.Sequential( |
| 51 | nn.Linear(128+1000, 512), |
| 52 | nn.ReLU(inplace=True), |
| 53 | nn.Linear(512, 512), |
| 54 | nn.ReLU(inplace=True), |
| 55 | nn.Linear(512, 256), |
| 56 | nn.ReLU(inplace=True), |
| 57 | ) |
| 58 | |
| 59 | self.join_ctrl = nn.Sequential( |
| 60 | nn.Linear(128+512, 512), |
| 61 | nn.ReLU(inplace=True), |
| 62 | nn.Linear(512, 512), |
| 63 | nn.ReLU(inplace=True), |
| 64 | nn.Linear(512, 256), |
| 65 | nn.ReLU(inplace=True), |
| 66 | ) |
| 67 | |
| 68 | self.speed_branch = nn.Sequential( |
| 69 | nn.Linear(1000, 256), |
| 70 | nn.ReLU(inplace=True), |
| 71 | nn.Linear(256, 256), |
| 72 | nn.Dropout2d(p=0.5), |
| 73 | nn.ReLU(inplace=True), |
| 74 | nn.Linear(256, 1), |
| 75 | ) |
| 76 | |
| 77 | self.value_branch_traj = nn.Sequential( |
| 78 | nn.Linear(256, 256), |
| 79 | nn.ReLU(inplace=True), |
| 80 | nn.Linear(256, 256), |
| 81 | nn.Dropout2d(p=0.5), |
| 82 | nn.ReLU(inplace=True), |
| 83 | nn.Linear(256, 1), |
| 84 | ) |
| 85 | self.value_branch_ctrl = nn.Sequential( |
| 86 | nn.Linear(256, 256), |
| 87 | nn.ReLU(inplace=True), |
| 88 | nn.Linear(256, 256), |
| 89 | nn.Dropout2d(p=0.5), |