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