Deformation Estimator¶

Implementation of a stateless kinematic deformation estimator block compatible with gym_jiminy reinforcement learning pipeline environment design.

gym_jiminy.common.blocks.deformation_estimator.flexibility_estimator(obs_imu_quats, obs_imu_indices, inv_obs_imu_quats, kin_imu_rots, kin_imu_quats, kin_flex_rots, kin_flex_quats, is_flex_flipped, is_chain_orphan, dev_imu_quats, inv_child_dev_imu_quats, parent_dev_imu_quats, deformation_flex_quats, ignore_twist)[source]¶

Compute the local deformation at an arbitrary set of flexibility points that are presumably responsible for most of the whole deformation of the mechanical structure.

Warning

The local deformation at each flexibility frame must be observable, ie the flexibility and IMU frames interleave with each others in a unique and contiguous sub-chain in theoretical kinematic tree of the robot.

Parameters:

obs_imu_quats (ndarray) – Orientation estimates of an unordered arbitrary set of IMUs as a 2D array whose first dimension gathers the 4 quaternion coordinates [qx, qy, qz, qw] while the second corresponds to N independent IMU data.
obs_imu_indices (Tuple[int, ...]) – M-tuple of ordered IMU indices of interest for which the total deviation will be computed.
inv_obs_imu_quats (ndarray) – Pre-allocated memory for storing the inverse of the orientation estimates for an ordered subset of the IMU data obs_imu_quats according to obs_imu_indices.
kin_imu_rots (Tuple[ndarray, ...]) – Tuple of M kinematic frame orientations corresponding to the ordered subset of IMUs obs_imu_indices, for the configuration of the theoretical robot model.
kin_imu_quats (ndarray) – Pre-allocated memory for storing the kinematic frame orientations of the ordered subset of IMUs of interest as a 2D array whose first dimension gathers the 4 quaternion coordinates while the second corresponds to the M independent IMUs.
kin_flex_rots (Tuple[ndarray, ...]) – Tuple of K kinematic frame orientations for all the flexibility points that interleaves with the ordered subset of IMUs of interest in the kinematic tree.
kin_flex_quats (ndarray) – Pre-allocated memory for storing the kinematic frame orientations of the flexibility points that interleaves with the ordered subset of IMUs of interest as a 2D array whose first dimension gathers the 4 quaternion coordinates while the second corresponds to the K independent flexibility points.
is_flex_flipped (ndarray) – Whether local deformation estimates for each flexibility point must be inverted to be consistent with standard URDF convention as 1D boolean array.
is_chain_orphan (Tuple[bool, bool]) – 2-Tuple stating whether first and last flexibility point is orphan respectively, ie only a single IMU is available for estimating its local deformation.
dev_imu_quats (ndarray) – Total deviation of th observed IMU data wrt to the theoretical model in world frame for the ordered subset of IMUs of interest, as a 2D array whose first dimension gathers the 4 quaternion coordinates while the second corresponds to the M independent IMUs.
inv_child_dev_imu_quats (ndarray) – Total deviation observed IMU data in child flexibility frame as a 2D array whose first dimension gathers the 4 quaternion coordinates while the second corresponds to the K independent flexibility frames.
parent_dev_imu_quats (ndarray) – Total deviation observed IMU data in parent flexibility frame as a 2D array whose first dimension gathers the 4 quaternion coordinates while the second corresponds to the K independent flexibility frames.
deformation_flex_quats (ndarray) – Pre-allocated memory for storing the local deformation estimates for each flexibility point flexibility points as a 2D array whose first dimension gathers the 4 quaternion coordinates while the second corresponds to the K independent flexibility points.
ignore_twist (bool) – Whether to ignore the twist of the orientation estimates of the ordered subset of IMUs of interest, and incidentally the twist of deformation at the flexibility points.

Return type:

None

gym_jiminy.common.blocks.deformation_estimator.get_flexibility_imu_frame_chains(pinocchio_model, flex_joint_names, imu_frame_names)[source]¶

Extract the minimal set of contiguous sub-chains in kinematic tree of a given model that goes through all the specified flexibility and IMU frames.

Parameters:

pinocchio_model (Model) – Model from which to extract sub-chains.
flex_joint_names (Sequence[str]) – Unordered sequence of joint names that must be considered as associated with flexibility frames.
imu_frame_names (Sequence[str]) – Unordered sequence of frame names that must be considered as associated with IMU frames.

Return type:

Sequence[Tuple[Sequence[str], Sequence[str | None], Sequence[bool]]]

class gym_jiminy.common.blocks.deformation_estimator.DeformationEstimator(name, env, *, imu_frame_names, flex_frame_names, ignore_twist=True, nested_imu_key=('features', 'mahony_filter'), update_ratio=1)[source]¶

Bases: BaseObserverBlock[ndarray, ndarray, BaseObsT, BaseActT]

Compute the local deformation at an arbitrary set of flexibility points that are presumably responsible for most of the whole deformation of the mechanical structure.

The number of IMU sensors and flexibility frames must be consistent:

If the robot has no freeflyer, there must be as many IMU sensors as flexibility frames (0), ie

*—o—x—o—x—o—x

x—o—x
Otherwise, it can either have one more IMU than flexibility frames (1), the same number (2), or up to one less IMU per branch in the kinematic tree (3).
1. x—o—x—o—x—o—x
  
  x—o—x
2. +—o—x—o—x—o—x
  
  x—o—x
3. +—o—x—o—x—o—+
  
  x—o—+

*: Fixed base, +: leaf frame, x: IMU frame, o: flexibility frame

(1): The pose of the freeflyer is ignored when estimating the deformation: at the flexibility frames in local frame. Mathematically, it is the same as (0) when considering a virtual IMU with fixed orientation to identity for the root joint.
(2): One has to compensate for the missing IMU by providing instead the: configuration of the freeflyer. More precisely, one should ensure that the orientation of the parent frame of the orphan flexibility frame matches the reality for the theoretical configuration. This usually requires making some assumptions to guess to pose of the frame that is not directly observable. Any discrepancy will be aggregated to the estimated deformation for the orphan flexibility frame specifically since both cannot be distinguished. This issue typically happens when there is no IMUs in the feet of a legged robot. In such a case, there is no better option than assuming that one of the active contact bodies remains flat on the ground. If the twist of the IMUs are ignored, then the twist of the contact body does not matter either, otherwise it must be set appropriately by the user to get a meaningless estimate for the twist of the deformation. If it cannot be observed by some exteroceptive sensor such as vision, then the most reasonable assumption is to suppose that it matches the twist of the IMU coming right after in the kinematic tree. This way, they will cancel out each other without adding bias to the twist of the orphan flexibility frame.
(3): This case is basically the same as (2), with the addition that only: the deformation of one of the orphan flexibility frames can be estimated at once, namely the one whose parent frame is declared as having known orientation. The other ones will be set to identity. For a legged robot, this corresponds to one of the contact bodies, usually the one holding most of the total weight.

Warning

(2) and (3) are not supported for now, as it requires using one additional observation layer responsible for estimating the theoretical configuration of the robot including its freeflyer, along with the name of the reference frame, ie the one having known orientation.