

def inertiamatrix(rbtdef, geom, ifunc=None):
    'Generate mass matrix.'
    if (not ifunc):
        ifunc = identity
    M = zeros(rbtdef.dof, rbtdef.dof)
    rbtdeftmp = deepcopy(rbtdef)
    rbtdeftmp.gravityacc = zeros(3, 1)
    rbtdeftmp.frictionmodel = None
    rbtdeftmp.dq = zeros(rbtdeftmp.dof, 1)
    for i in range(M.rows):
        rbtdeftmp.ddq = zeros(rbtdeftmp.dof, 1)
        rbtdeftmp.ddq[i] = 1
        geomtmp = Geometry(rbtdeftmp)
        fw_results = rne_forward(rbtdeftmp, geomtmp, ifunc)
        Mcoli = rne_backward(rbtdeftmp, geomtmp, fw_results, ifunc=ifunc)
        M[:, i] = M[i, :i].T.col_join(Mcoli[i:, :])
    return M

dataset/ETHPy150Open cdsousa/SymPyBotics/sympybotics/dynamics/rne.py inertiamatrix/original