| 15 | |
| 16 | |
| 17 | class TwistModelExplicit(aligator.dynamics.ExplicitDynamicsModel): |
| 18 | def __init__(self, dt: float): |
| 19 | self.B = np.eye(nu) |
| 20 | self.dt = dt |
| 21 | super().__init__(space, nu) |
| 22 | |
| 23 | def __getinitargs__(self): |
| 24 | return (self.dt,) |
| 25 | |
| 26 | def forward(self, x, u, data: aligator.dynamics.ExplicitDynamicsData): |
| 27 | assert data.good |
| 28 | space.integrate(x, self.dt * self.B @ u, data.xnext) |
| 29 | |
| 30 | def dForward(self, x, u, data: aligator.dynamics.ExplicitDynamicsData): |
| 31 | Jx = data.Jx |
| 32 | Ju = data.Ju |
| 33 | v_ = self.dt * self.B @ u |
| 34 | dv_du = self.dt * self.B |
| 35 | |
| 36 | space.Jintegrate(x, v_, Jx, 0) |
| 37 | Jxnext_dv = space.Jintegrate(x, v_, 1) |
| 38 | Ju[:, :] = Jxnext_dv @ dv_du |
| 39 | |
| 40 | def createData(self): |
| 41 | return TwistData(self) |
| 42 | |
| 43 | |
| 44 | class TwistData(aligator.dynamics.ExplicitDynamicsData): |