| 69 | |
| 70 | |
| 71 | class MyQuadCost(aligator.CostAbstract): |
| 72 | def __init__(self, W: np.ndarray, x_ref: np.ndarray): |
| 73 | self.x_ref = x_ref |
| 74 | self.W = W |
| 75 | super().__init__(space, nu) |
| 76 | |
| 77 | def __getinitargs__(self): |
| 78 | return (self.W, self.x_ref) |
| 79 | |
| 80 | def __getstate__(self): |
| 81 | return dict() |
| 82 | |
| 83 | def evaluate(self, x, u, data): |
| 84 | assert isinstance(data, MyCostData) |
| 85 | data.value = _call(space, self.x_ref, x, self.W) |
| 86 | |
| 87 | def computeGradients(self, x, u, data): |
| 88 | assert isinstance(data, MyCostData) |
| 89 | data.Lx[:] = _jac(space, self.x_ref, x, self.W) |
| 90 | data.Lu[:] = 0.0 |
| 91 | |
| 92 | def computeHessians(self, x, u, data): |
| 93 | assert isinstance(data, MyCostData) |
| 94 | data.hess[:, :] = 0.0 |
| 95 | data.Lxx[:, :] = _hess(space, self.x_ref, x, self.W) |
| 96 | |
| 97 | def createData(self): |
| 98 | return MyCostData() |
| 99 | |
| 100 | |
| 101 | dt = 0.1 |