| 16 | |
| 17 | |
| 18 | class MyODE(dynamics.ODEAbstract): |
| 19 | def __init__(self, nu=0): |
| 20 | super().__init__(manifolds.R3() * manifolds.SO3(), nu) |
| 21 | |
| 22 | def forward(self, x, u, data: dynamics.ODEData): |
| 23 | assert isinstance(data, MyODEData) |
| 24 | _, R_quat = x[:3], x[3:] # R is quat |
| 25 | R_quat = Quaternion(R_quat) |
| 26 | R = R_quat.toRotationMatrix() |
| 27 | data.xdot[:3] = R @ data.v0 |
| 28 | data.xdot[3:] = 0 |
| 29 | |
| 30 | def dForward(self, x, u, data: dynamics.ODEData): |
| 31 | Jp = data.Jx[:, :3] |
| 32 | Jw = data.Jx[:, 3:] |
| 33 | Jp[::3] = 0.0 |
| 34 | |
| 35 | Jp[:, :3] = 0.0 |
| 36 | Jw[:, 3:] = 0.0 |
| 37 | |
| 38 | def createData(self): |
| 39 | return MyODEData(self) |
| 40 | |
| 41 | |
| 42 | class MyODEData(dynamics.ODEData): |
no outgoing calls