def coriolismatrix(rbtdef, geom, ifunc=None):

    if not ifunc:
        ifunc = identity

    C = zeros(rbtdef.dof, rbtdef.dof)

    rbtdeftmp = deepcopy(rbtdef)
    rbtdeftmp.gravityacc = zeros(3, 1)
    rbtdeftmp.ddq = zeros(rbtdeftmp.dof, 1)
    rbtdeftmp.frictionmodel = None

    # # Coriolis vector c element k is given by
    # # c[k] = dq.T * A[k] * dq[k]
    # # where A[k] is a (non-unique) matrix

    # # Find upper triangular A[k]'s

    A = [zeros(rbtdef.dof, rbtdef.dof) for k in range(rbtdef.dof)]

    for i in range(rbtdef.dof):
        rbtdeftmp.dq = zeros(rbtdeftmp.dof, 1)
        rbtdeftmp.dq[i] = 1
        geomtmp = Geometry(rbtdeftmp)
        fw_results = rne_forward(rbtdeftmp, geomtmp, ifunc)
        c = rne_backward(rbtdeftmp, geomtmp, fw_results, ifunc)
        for k in range(rbtdef.dof):
            A[k][i, i] = c[k]

    for i in range(rbtdef.dof):
        for j in range(i+1, rbtdef.dof):
            rbtdeftmp.dq = zeros(rbtdeftmp.dof, 1)
            rbtdeftmp.dq[i] = rbtdeftmp.dq[j] = 1
            geomtmp = Geometry(rbtdeftmp)
            fw_results = rne_forward(rbtdeftmp, geomtmp, ifunc)
            c = rne_backward(rbtdeftmp, geomtmp, fw_results, ifunc)
            for k in range(rbtdef.dof):
                A[k][i, j] = ifunc(c[k] - A[k][i, i] - A[k][j, j])

    # # Coriolis vector c is
    # # c[k] = dq.T * A[k] * dq[k]
    # # thus, Coriolis matrix is
    # # C[k, :] = dq.T * A[k]
    # # so that
    # # c[k] = C[k, :] * dq

    for k in range(rbtdef.dof):
        C[k, :] = rbtdef.dq.T * A[k]

    C = ifunc(C)

    return C
File saved
dataset/ETHPy150Open cdsousa/SymPyBotics/sympybotics/dynamics/rne.py/coriolismatrix--crossover with--dataset/ETHPy150Open WebGLTools/GL-Shader-Validator/GLShaderValidator.py/GLShaderValidatorCommand.on_post_save