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)
