def compute_contact_span(link): X, Y = get_link_dimensions(link) # Face representation for individual forces (y) M_face_y = zeros((16, 12)) for i in range(4): M_face_y[4 * i, [3 * i, 3 * i + 1, 3 * i + 2]] = [1, 0, -mu] M_face_y[4 * i + 1, [3 * i, 3 * i + 1, 3 * i + 2]] = [-1, 0, -mu] M_face_y[4 * i + 2, [3 * i, 3 * i + 1, 3 * i + 2]] = [0, 1, -mu] M_face_y[4 * i + 3, [3 * i, 3 * i + 1, 3 * i + 2]] = [0, -1, -mu] # Span for individual forces S0 = span_of_face(M_face_y) # Transform from individual forces to contact wrench (x) M_x_to_y = zeros((6, 12)) M_x_to_y[0, :] = [1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0] M_x_to_y[1, :] = [0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0] M_x_to_y[2, :] = [0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1] M_x_to_y[3, :] = Y * array([0, 0, 1, 0, 0, -1, 0, 0, -1, 0, 0, 1]) M_x_to_y[4, :] = -X * array([0, 0, 1, 0, 0, 1, 0, 0, -1, 0, 0, -1]) M_x_to_y[5, :] = X * array([0, 1, 0, 0, 1, 0, 0, -1, 0, 0, -1, 0]) \ - Y * array([1, 0, 0, -1, 0, 0, -1, 0, 0, 1, 0, 0]) # Span for contact wrench S = dot(M_x_to_y, S0) print "Span shape:", S.shape return S
def compute_Coulomb_span(): M_face_y = zeros((4, 3)) M_face_y[0, :] = [+1, 0, -mu] M_face_y[1, :] = [-1, 0, -mu] M_face_y[2, :] = [0, +1, -mu] M_face_y[3, :] = [0, -1, -mu] return span_of_face(M_face_y)