Calculate angle between two vectors.
(vector1, vector2)
| 45 | return common_intervals |
| 46 | |
| 47 | def calculate_angle(vector1, vector2): |
| 48 | """Calculate angle between two vectors.""" |
| 49 | vector1 = vector1 / (torch.norm(vector1, dim=-1).unsqueeze(-1) + 1e-6) |
| 50 | vector2 = vector2 / (torch.norm(vector2, dim=-1).unsqueeze(-1) + 1e-6) |
| 51 | dot_product = torch.sum(vector1 * vector2, dim=-1) |
| 52 | dot_product = torch.clamp(dot_product, -1.0, 1.0) |
| 53 | angle = torch.acos(dot_product) |
| 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.""" |
no outgoing calls
no test coverage detected