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