From 335c92c961ffe09b23c0b03f43fcf47fb3f59866 Mon Sep 17 00:00:00 2001 From: Michel Aractingi Date: Mon, 12 May 2025 18:05:22 +0200 Subject: [PATCH] Added comment on SE(3) in kinematics and nits in `lerobot/envs/utils.py` --- lerobot/common/envs/utils.py | 4 +++- lerobot/scripts/server/kinematics.py | 9 +++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/lerobot/common/envs/utils.py b/lerobot/common/envs/utils.py index 78517abe..6003acc0 100644 --- a/lerobot/common/envs/utils.py +++ b/lerobot/common/envs/utils.py @@ -47,7 +47,9 @@ def preprocess_observation(observations: dict[str, np.ndarray]) -> dict[str, Ten # TODO(aliberts, rcadene): use transforms.ToTensor()? img = torch.from_numpy(img) - if img.dim() == 3: + # When preprocessing observations in a non-vectorized environment, we need to add a batch dimension. + # This is the case for human-in-the-loop RL where there is only one environment. + if img.ndim == 3: img = img.unsqueeze(0) # sanity check that images are channel last _, h, w, c = img.shape diff --git a/lerobot/scripts/server/kinematics.py b/lerobot/scripts/server/kinematics.py index d6b712c8..dc5baf54 100644 --- a/lerobot/scripts/server/kinematics.py +++ b/lerobot/scripts/server/kinematics.py @@ -51,6 +51,15 @@ def screw_axis_to_transform(S, theta): def pose_difference_se3(pose1, pose2): """ Calculates the SE(3) difference between two 4x4 homogeneous transformation matrices. + SE(3) (Special Euclidean Group) represents rigid body transformations in 3D space, combining rotation (SO(3)) and translation. + Each 4x4 matrix has the following structure, a 3x3 rotation matrix in the top-left and a 3x1 translation vector in the top-right: + + [R11 R12 R13 tx] + [R21 R22 R23 ty] + [R31 R32 R33 tz] + [ 0 0 0 1] + + where Rij is the 3x3 rotation matrix and [tx,ty,tz] is the translation vector. pose1 - pose2