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
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