Example #1
0
def compute_GI_face(contacting_links, verbose=True):
    t0 = time.time()
    ncontacts = len(contacting_links)
    spans = [compute_contact_span(link) for link in contacting_links]
    n = sum([span.shape[1] for span in spans])

    # Span for w_all
    H = zeros((ncontacts * 6, n))
    curcol = 0
    for i in range(ncontacts):
        H[6 * i:6 * (i + 1), curcol:curcol + spans[i].shape[1]] = spans[i]
        curcol += spans[i].shape[1]
    print "H.shape:", H.shape

    # Transform from w_all to w_GI
    AGI = zeros((6, ncontacts * 6))
    for i in range(ncontacts):
        pi = contacting_links[i].p
        Ri = contacting_links[i].R
        AGI[:3, 6 * i:6 * i + 3] = -Ri
        AGI[3:, 6 * i:6 * i + 3] = -dot(crossmat(pi), Ri)
        AGI[3:, 6 * i + 3:6 * i + 6] = -Ri
    print "AGI.shape:", AGI.shape

    M = dot(AGI, H)        # span for w_GI
    CGI = face_of_span(M)  # face for w_GI
    if verbose:
        report("Compute CGI (%d contacts): %f ms" % (
            ncontacts, 1000 * (time.time() - t0)))
        report("CGI shape: %s" % str(CGI.shape))
    return CGI
Example #2
0
def compute_GI_face_forces(contacting_links):
    t0 = time.time()

    S0 = compute_Coulomb_span()
    print "Coulomb span shape:", S0.shape

    nb_links = len(contacting_links)
    nb_forces = 4 * nb_links
    H = block_diag(*([S0] * nb_forces))
    print "H.shape:", H.shape

    AGI = zeros((6, 3 * nb_forces))
    for i, link in enumerate(contacting_links):
        X, Y = get_link_dimensions(link)
        p, R = link.p, link.R
        a = [[+X, +Y, 0], [+X, -Y, 0], [-X, -Y, 0], [-X, +Y, 0]]
        for j in xrange(4):
            pi = p + dot(R, a[i])
            AGI[:3, 12 * i + 3 * j:12 * i + 3 * (j + 1)] = -R
            AGI[3:, 12 * i + 3 * j:12 * i + 3 * (j + 1)] = -dot(crossmat(pi), R)
    print "AGI.shape:", AGI.shape

    M = dot(AGI, H)        # span for w_GI
    CGI = face_of_span(M)  # face for w_GI
    report("Compute CGI (%d contacts): %f ms" % (
        nb_forces, 1000 * (time.time() - t0)))
    report("CGI shape: %s" % str(CGI.shape))
    return CGI