예제 #1
0
def compute_giwc_from_forces():
    A = calculate_local_forces_to_gi_matrix()
    CFC_all = block_diag(*[dot(CFC, block_diag(contact.R.T))
                           for contact in contacts for _ in xrange(4)])
    S = span_of_face(CFC_all)
    F = face_of_span(dot(A, S))
    return F
예제 #2
0
def compute_giwc_from_wrenches(full=True):
    """
    Compute Gravito-Inertial Wrench Cone (GIWC) from Contact Wrench Cones
    (CWCs).
    """
    global CWC_all
    A = calculate_local_wrench_to_gi_matrix()
    # right vector of CWC_all is the stacked vector w_all of
    # contact wrenches in the *world* frame
    CWC_all = block_diag(*[
        dot(CWC, block_diag(contact.R.T, contact.R.T))
        for contact in contacts])
    S = span_of_face(CWC_all)
    F = face_of_span(dot(A, S))
    return F