Get projected gravity
(sim_data)
| 573 | |
| 574 | |
| 575 | def get_projected_gravity(sim_data): |
| 576 | """Get projected gravity""" |
| 577 | quaternion = sim_data.qpos[3:7] # wxyz |
| 578 | |
| 579 | qw = quaternion[0] |
| 580 | qx = quaternion[1] |
| 581 | qy = quaternion[2] |
| 582 | qz = quaternion[3] |
| 583 | |
| 584 | gravity_orientation = np.zeros(3) |
| 585 | |
| 586 | gravity_orientation[0] = 2 * (-qz * qx + qw * qy) |
| 587 | gravity_orientation[1] = -2 * (qz * qy + qw * qx) |
| 588 | gravity_orientation[2] = 1 - 2 * (qw * qw + qz * qz) |
| 589 | |
| 590 | return gravity_orientation |
| 591 | |
| 592 | |
| 593 | # |
no outgoing calls
no test coverage detected