Detect contact from foot velocities and height.
(joints: torch.Tensor, foot_idx: list = FOOT_IDX, vel_ts: float = 0.01, height_ts: float = 0.02, device: str = 'cuda')
| 54 | return angle |
| 55 | |
| 56 | def get_contact(joints: torch.Tensor, foot_idx: list = FOOT_IDX, vel_ts: float = 0.01, height_ts: float = 0.02, device: str = 'cuda') -> torch.Tensor: |
| 57 | """Detect contact from foot velocities and height.""" |
| 58 | foot_pos = joints[:, foot_idx] # (frames, 2, 3) |
| 59 | # Calculate foot velocity |
| 60 | foot_vel = foot_pos[1:] - foot_pos[:-1] |
| 61 | foot_vel = torch.cat([foot_vel, foot_vel[-1:]], dim=0) |
| 62 | |
| 63 | vel_ts = torch.tensor(vel_ts, dtype=torch.float32).to(device) |
| 64 | height_ts = torch.tensor(height_ts, dtype=torch.float32).to(device) |
| 65 | |
| 66 | # Calculate the velocity magnitudes (norm) |
| 67 | delta = torch.norm(foot_vel, dim=-1) |
| 68 | # Detect foot contact based on velocity and height threshold |
| 69 | contact = (delta < vel_ts) | (foot_pos[:, :, 2] < height_ts).to(device) |
| 70 | return contact.int() |
| 71 | |
| 72 | def get_range(contact: torch.Tensor, contact_state: bool = True) -> list: |
| 73 | """Get contact range from contact info. |
no outgoing calls
no test coverage detected