Grasp Matrix and frame jacobian

[MaximilianDio]

Hi,

I am currently researching multi-robot manipulation. A very important quantity is the grasp matrix G.
It maps forces from the end-effectors to the COM of an object.

Also, the transpose of the grasp matrix defines the relation between the twist at the COM of the object and the twist at the grasp points (same rigid object).
So x_i = Gi^Txo, where x_i is the twist at grasp point i and xo is the twist at the COM of the object.
Additionally, one can write the acceleration as dx_i = Gi^Tdxo+b, where b is the local acceleration (b = dGi^T*xo = [wo \cross (wo \cross roi);0]) with roi distance between COM and grasp point

I am currently calculating the grasp matrix and the local acceleration via

def grasp_constraints(model, data, com_frame, grasp_point_frame_list, w):
I3 = np.eye(3)
Z3 = np.zeros([3, 3])

# get relevant frames of object
p0_fID = model.getFrameId(com_frame)
pin.updateFramePlacement(model, data, p0_fID)

# position of COM and skew_symmetric matrix of angular velocity
r0o = data.oMf[p0_fID].translation
wo_S = pin.skew(w)

# transposed grasp matrix
GT = np.zeros([0, 6])
# local accelerations
b = np.array([])

grasp_point_frame_ID_list = []
for grasp_point_frame_name in grasp_point_frame_list:
    # get frame ID and update frame placement
    pi_fID = model.getFrameId(grasp_point_frame_name)
    grasp_point_frame_ID_list.append(pi_fID)
    pin.updateFramePlacement(model, data, pi_fID)

    # get grasp location in inertial frame
    r0i = data.oMf[pi_fID].translation

    # calculate virtual stick (in inertial frame)
    roi = r0i-r0o
    roi_S = pin.skew(roi)
    GT = np.append(GT, np.block([[I3, -roi_S], [Z3, I3]]), axis=0)

    # calculate local acceleration (in inertial frame)
    boi = np.block([wo_S @ (wo_S @ roi), np.zeros(3)])
    b = np.append(b, boi)

return [GT, b, p0_fID, grasp_point_frame_ID_list]