code-Psi0-yth-v21 / real /teleop /motion_utils.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
211ae03 verified
Raw History Blame Contribute Delete
276 Bytes
import numpy as np
def mat_update(prev_mat, mat):
if np.linalg.det(mat) < 1e-6:
return prev_mat
else:
return mat
def fast_mat_inv(mat):
ret = np.eye(4)
ret[:3, :3] = mat[:3, :3].T
ret[:3, 3] = -mat[:3, :3].T @ mat[:3, 3]
return ret