lagrange.py 文件源码

def mass_matrix_full(self):
        """ Augments the coefficients of qdots to the mass_matrix. """

        n = len(self._q)
        if self.eom is None:
            raise ValueError('Need to compute the equations of motion first')
        #THE FIRST TWO ROWS OF THE MATRIX
        row1 = eye(n).row_join(zeros(n, n))
        row2 = zeros(n, n).row_join(self.mass_matrix)
        if self.coneqs is not None:
            m = len(self.coneqs)
            I = eye(n).row_join(zeros(n, n + m))
            below_eye = zeros(n + m, n)
            A = (self.mass_matrix).col_join((self._m_cd).row_join(zeros(m, m)))
            below_I = below_eye.row_join(A)
            return I.col_join(below_I)
        else:
            A = row1.col_join(row2)
            return A