Body Orientation State Observation¶
Implementation of a stateless body orientation state estimator block compatible with gym_jiminy reinforcement learning pipeline environment design.
- gym_jiminy.common.blocks.body_orientation_observer.update_twist(q, twist, omega, time_constant_inv, dt)[source]¶
Update the twist estimate of the Twist-after-Swing decomposition of given orientations in quaternion representation using leaky integrator.
- Parameters:
q (ndarray) – Current swing estimate as a quaternion. It will be updated in-place to add the estimated twist.
twist (ndarray) – Current twist estimate to update in-place.
omega (ndarray) – Current angular velocity estimate in local frame.
time_constant_inv (float) – Inverse of the time constant of the leaky integrator used to update the twist estimate.
dt (float) – Time step, in seconds, between consecutive Quaternions.
- Return type:
None
- class gym_jiminy.common.blocks.body_orientation_observer.BodyObserver(name, env, *, nested_imu_quat_key=('features', 'mahony_filter', 'quat'), nested_imu_omega_key=('features', 'mahony_filter', 'omega'), twist_time_constant=None, compute_rpy=True, update_ratio=1)[source]¶
Bases:
BaseObserverBlock[Dict[str,ndarray],ndarray,BaseObs,BaseAct]Compute the orientation and angular velocity in local frame of the parent body associated with all the IMU sensors of the robot.
Note
The twist angle is not part of the internal state, even if being integrated over time, because it is uniquely determined from the orientation estimate.
- Parameters:
name (str) – Name of the block.
env (InterfaceJiminyEnv[BaseObs, BaseAct]) – Environment to connect with.
nested_imu_quat_key (Tuple[str | int, ...] | Tuple[Unpack[Tuple[str | int, ...]], Sequence[Tuple | int | Tuple[int | None, int | None]]]) – Nested key from environment observation mapping to the IMU quaternion estimates. Their ordering must be consistent with the true IMU sensors of the robot.
nested_imu_omega_key (Tuple[str | int, ...] | Tuple[Unpack[Tuple[str | int, ...]], Sequence[Tuple | int | Tuple[int | None, int | None]]]) – Nested key from environment observation mapping to the IMU angular velocity in local frame estimates. Their ordering must be consistent with the true IMU sensors of the robot.
twist_time_constant (float | None) – If specified, it corresponds to the time constant of the leaky integrator (Exponential Moving Average) used to estimate the twist part of twist-after-swing decomposition of the estimated orientation in place of the Mahony Filter. If 0.0, then its is kept constant equal to zero. None to kept the original estimate provided by Mahony Filter. See remove_twist_from_quat and update_twist documentations for details. Optional: 0.0 by default.
compute_rpy (bool) – Whether to compute the Yaw-Pitch-Roll Euler angles representation for the 3D orientation of the IMU, in addition to the quaternion representation. Optional: True by default.
update_ratio (int) – Ratio between the update period of the observer and the one of the subsequent observer. -1 to match the simulation timestep of the environment. Optional: 1 by default.
- property fieldnames: Dict[str, List[List[str]]]¶
Get mapping between each scalar element of the observation space of the observer block and the associated fieldname for logging.
It is expected to return an object with the same structure than the observation space, but having lists of string as leaves. Generic fieldnames are used by default.