def close_step(self, i):
# Collide and write contacts to bodies.
self.collider.collide()
# Get variables.
link = self.hand.get_links()[i]
manifold = link.get_contacts()[0]
i_mask = np.array([False] * self.system.num_dofs())
i_mask[i] = True
if DEBUG:
print(manifold)
# Create contact frame g_oc.
g_wo = link.get_transform_world()
g_wc = manifold.frame_B()
g_oc = SE3.inverse(g_wo) * g_wc
# Create hand jacobian.
Ad_g_co = SE3.Ad(SE3.inverse(g_oc))
J_b = link.get_body_jacobian()
J_b[:,~i_mask] = 0
J_h = Ad_g_co @ J_b
B = np.array([0, 0, 1., 0, 0, 0]).reshape((6,1))
J_h = B.T @ J_h
if DEBUG:
print('g_oc')
print(g_oc)
print('J_b')
print(J_b)
print('Ad_g_co')
print(Ad_g_co)
print('J_h')
print(J_h)
# Take step.
alpha = 0.15
d = np.array([[manifold.dist]])
dq = np.linalg.lstsq(J_h, alpha*d)[0]
q = self.hand.get_state() + dq
return q
def get_body_jacobian(self):
J_s = self.get_spatial_jacobian()
J_b = SE3.Ad(SE3.inverse(self.get_transform_world())) @ J_s
return J_b