Skip to content

[BUG] [imu.py] gravity in world frame was subtracted from lin acc in body frame #1959

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

[BUG] [imu.py] gravity in world frame was subtracted from lin acc in body frame #1959

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 9 additions & 2 deletions genesis/engine/sensors/imu.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,12 @@
MaybeMatrix3x3Type,
IMU as IMUOptions,
)
from genesis.utils.geom import inv_transform_by_trans_quat, transform_by_quat, transform_quat_by_quat
from genesis.utils.geom import (
inv_transform_by_trans_quat,
transform_by_quat,
transform_quat_by_quat,
inv_transform_by_quat,
)
from genesis.utils.misc import concat_with_tensor, make_tensor_field, tensor_to_array

from .base_sensor import (
Expand Down Expand Up @@ -211,7 +216,9 @@ def _update_shared_ground_truth_cache(
local_ang = inv_transform_by_trans_quat(ang, shared_metadata.offsets_pos, offset_quats)

*batch_size, n_imus, _ = local_acc.shape
local_acc = local_acc - gravity.unsqueeze(-2).expand((*batch_size, n_imus, -1))
local_acc = local_acc - inv_transform_by_quat(
gravity.unsqueeze(-2).expand((*batch_size, n_imus, -1)), offset_quats
)

# cache shape: (B, n_imus * 6)
strided_ground_truth_cache = shared_ground_truth_cache.reshape((*batch_size, n_imus, 2, 3))
Expand Down
Loading