| 14 | import torch |
| 15 | |
| 16 | def foot_detect(positions, thres=0.002): |
| 17 | fid_r, fid_l = 12,6 |
| 18 | velfactor, heightfactor = np.array([thres]), np.array([0.08]) |
| 19 | feet_l_x = (positions[1:, fid_l, 0] - positions[:-1, fid_l, 0]) ** 2 |
| 20 | feet_l_y = (positions[1:, fid_l, 1] - positions[:-1, fid_l, 1]) ** 2 |
| 21 | feet_l_z = (positions[1:, fid_l, 2] - positions[:-1, fid_l, 2]) ** 2 |
| 22 | feet_l_h = positions[1:,fid_l,2] |
| 23 | feet_l = (((feet_l_x + feet_l_y + feet_l_z) < velfactor).astype(int) & (feet_l_h < heightfactor).astype(int)).astype(np.float32) |
| 24 | feet_l = np.expand_dims(feet_l,axis=1) |
| 25 | feet_l = np.concatenate([np.array([[1.]]),feet_l],axis=0) |
| 26 | |
| 27 | feet_r_x = (positions[1:, fid_r, 0] - positions[:-1, fid_r, 0]) ** 2 |
| 28 | feet_r_y = (positions[1:, fid_r, 1] - positions[:-1, fid_r, 1]) ** 2 |
| 29 | feet_r_z = (positions[1:, fid_r, 2] - positions[:-1, fid_r, 2]) ** 2 |
| 30 | feet_r_h = positions[1:,fid_r,2] |
| 31 | feet_r = (((feet_r_x + feet_r_y + feet_r_z) < velfactor).astype(int) & (feet_r_h < heightfactor).astype(int)).astype(np.float32) |
| 32 | feet_r = np.expand_dims(feet_r,axis=1) |
| 33 | feet_r = np.concatenate([np.array([[1.]]),feet_r],axis=0) |
| 34 | return feet_l, feet_r |
| 35 | |
| 36 | def cal_contact_mask(dof, root_trans, root_rot): |
| 37 | motion_dict = { |