def jacobians()

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

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

    def jacobians(self) -> Tuple[List[torch.Tensor], torch.Tensor]:
        # Pre-allocate jacobian tensors
        batch_size = self.pose1.shape[0]
        dof = self.pose1.dof()
        dtype = self.pose1.dtype
        device = self.pose1.device
        Jerr_pose1 = torch.zeros(batch_size, 2 * dof, dof, dtype=dtype, device=device)
        Jerr_vel1 = torch.zeros_like(Jerr_pose1)
        Jerr_pose2 = torch.zeros_like(Jerr_pose1)
        Jerr_vel2 = torch.zeros_like(Jerr_pose1)

        Jlocal: List[torch.Tensor] = []
        error = self._error_from_pose_diff(self._new_pose_diff(Jlocal))
        Jerr_pose1[:, :dof, :] = Jlocal[0]
        identity = torch.eye(dof, dtype=dtype, device=device).repeat(batch_size, 1, 1)
        Jerr_vel1[:, :dof, :] = -self.dt.data.view(-1, 1, 1) * identity
        Jerr_vel1[:, dof:, :] = -identity
        Jerr_pose2[:, :dof, :] = Jlocal[1]
        Jerr_vel2[:, dof:, :] = identity
        return [Jerr_pose1, Jerr_vel1, Jerr_pose2, Jerr_vel2], error