def _compute_cost_weight()

in theseus/embodied/motionmodel/double_integrator.py [0:0]

• 21 lines of code
• 1 McCabe index (conditional complexity)

    def _compute_cost_weight(self) -> torch.Tensor:
        batch_size, dof, _ = self.Qc_inv.shape
        cost_weight = torch.empty(
            batch_size,
            2 * dof,
            2 * dof,
            dtype=self.Qc_inv.dtype,
            device=self.Qc_inv.device,
        )
        Q11 = 12.0 * self.dt.data.pow(-3.0) * self.Qc_inv.data
        Q12 = -6.0 * self.dt.data.pow(-2.0) * self.Qc_inv.data
        Q22 = 4.0 * self.dt.data.reciprocal() * self.Qc_inv.data
        cost_weight[:, :dof, :dof] = Q11
        cost_weight[:, :dof:, dof:] = Q12
        cost_weight[:, dof:, :dof] = Q12
        cost_weight[:, dof:, dof:] = Q22
        return (
            torch.linalg.cholesky(cost_weight.transpose(-2, -1).conj())
            .transpose(-2, -1)
            .conj()
        )