def step(self, q_dot):
q_dot = np.array(q_dot).reshape((-1, 1))
self.set_velocity(q_dot)
v_b = q_dot[self.mask]
g_prev = self.get_transform_world()
v_s = SE3.Ad(g_prev) @ v_b
g_next = SE3.exp(v_s) * g_prev
self.set_transform_world(g_next)
def get_spatial_jacobian(self):
J = np.zeros((6, len(self.xi)))
exp = SE3.identity()
for i in range(len(self.xi)):
if i - 1 >= 0:
exp = exp * SE3.exp(self.xi[i - 1] * self.q[i - 1, 0])
J[:, i, None] = SE3.Ad(exp) @ self.xi[i]
J_s = np.zeros((6, len(self.mask)))
J_s[:, self.mask] = J
return J_s
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
def get_spatial_jacobian(self):
return SE3.Ad(self.get_transform_world()) @ self.get_body_jacobian()
