From e79d8cda764a2c3c88f818e0a179b6f0075ff2ba Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Mon, 16 Jun 2025 11:34:09 +0200
Subject: [PATCH 01/21] (refactor) refactor `kineamatics.py`

---
 pyproject.toml                                |   1 -
 src/lerobot/model/kinematics.py               | 576 ++++--------------
 .../so100_follower/config_so100_follower.py   |   9 +
 .../so100_follower_end_effector.py            |  19 +-
 src/lerobot/scripts/rl/gym_manipulator.py     |   1 +
 5 files changed, 132 insertions(+), 474 deletions(-)

diff --git a/pyproject.toml b/pyproject.toml
index 3686532edf..40fd8eaeba 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -68,7 +68,6 @@ dependencies = [
     "pyserial>=3.5",
     "pyzmq>=26.2.1",
     "rerun-sdk>=0.21.0",
-    "scipy>=1.14.0",
     "termcolor>=2.4.0",
     "torch>=2.2.1",
     "torchcodec>=0.2.1; sys_platform != 'win32' and (sys_platform != 'linux' or (platform_machine != 'aarch64' and platform_machine != 'arm64' and platform_machine != 'armv7l')) and (sys_platform != 'darwin' or platform_machine != 'x86_64')",
diff --git a/src/lerobot/model/kinematics.py b/src/lerobot/model/kinematics.py
index 367b609e19..7b7e9f104c 100644
--- a/src/lerobot/model/kinematics.py
+++ b/src/lerobot/model/kinematics.py
@@ -1,483 +1,123 @@
-# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-
 import numpy as np
-from numpy.typing import NDArray
-from scipy.spatial.transform import Rotation
-
-
-def skew_symmetric(w: NDArray[np.float32]) -> NDArray[np.float32]:
-    """Creates the skew-symmetric matrix from a 3D vector."""
-    return np.array([[0, -w[2], w[1]], [w[2], 0, -w[0]], [-w[1], w[0], 0]])
-
-
-def rodrigues_rotation(w: NDArray[np.float32], theta: float) -> NDArray[np.float32]:
-    """Computes the rotation matrix using Rodrigues' formula."""
-    w_hat = skew_symmetric(w)
-    return np.eye(3) + np.sin(theta) * w_hat + (1 - np.cos(theta)) * w_hat @ w_hat
-
-
-def screw_axis_to_transform(s: NDArray[np.float32], theta: float) -> NDArray[np.float32]:
-    """Converts a screw axis to a 4x4 transformation matrix."""
-    screw_axis_rot = s[:3]
-    screw_axis_trans = s[3:]
-
-    # Pure translation
-    if np.allclose(screw_axis_rot, 0) and np.linalg.norm(screw_axis_trans) == 1:
-        transform = np.eye(4)
-        transform[:3, 3] = screw_axis_trans * theta
-
-    # Rotation (and potentially translation)
-    elif np.linalg.norm(screw_axis_rot) == 1:
-        w_hat = skew_symmetric(screw_axis_rot)
-        rot_mat = np.eye(3) + np.sin(theta) * w_hat + (1 - np.cos(theta)) * w_hat @ w_hat
-        t = (
-            np.eye(3) * theta + (1 - np.cos(theta)) * w_hat + (theta - np.sin(theta)) * w_hat @ w_hat
-        ) @ screw_axis_trans
-        transform = np.eye(4)
-        transform[:3, :3] = rot_mat
-        transform[:3, 3] = t
-    else:
-        raise ValueError("Invalid screw axis parameters")
-    return transform
-
-
-def pose_difference_se3(pose1: NDArray[np.float32], pose2: NDArray[np.float32]) -> NDArray[np.float32]:
-    """
-    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:
-    [R11 R12 R13 tx]
-    [R21 R22 R23 ty]
-    [R31 R32 R33 tz]
-    [ 0   0   0   1]
-
-    where R is the 3x3 rotation matrix and [tx,ty,tz] is the translation vector.
-
-    Args:
-        pose1: A 4x4 numpy array representing the first pose.
-        pose2: A 4x4 numpy array representing the second pose.
-
-    Returns:
-        A 6D numpy array concatenating translation and rotation differences.
-        First 3 elements are the translational difference (position).
-        Last 3 elements are the rotational difference in axis-angle representation.
-    """
-    rot1 = pose1[:3, :3]
-    rot2 = pose2[:3, :3]
-
-    translation_diff = pose1[:3, 3] - pose2[:3, 3]
-
-    # Calculate rotational difference using scipy's Rotation library
-    rot_diff = Rotation.from_matrix(rot1 @ rot2.T)
-    rotation_diff = rot_diff.as_rotvec()  # Axis-angle representation
-
-    return np.concatenate([translation_diff, rotation_diff])
-
 
-def se3_error(target_pose: NDArray[np.float32], current_pose: NDArray[np.float32]) -> NDArray[np.float32]:
-    pos_error = target_pose[:3, 3] - current_pose[:3, 3]
 
-    rot_target = target_pose[:3, :3]
-    rot_current = current_pose[:3, :3]
-    rot_error_mat = rot_target @ rot_current.T
-    rot_error = Rotation.from_matrix(rot_error_mat).as_rotvec()
-
-    return np.concatenate([pos_error, rot_error])
-
-
-class RobotKinematics:
-    """Robot kinematics class supporting multiple robot models."""
-
-    # Robot measurements dictionary
-    ROBOT_MEASUREMENTS = {
-        "koch": {
-            "gripper": [0.239, -0.001, 0.024],
-            "wrist": [0.209, 0, 0.024],
-            "forearm": [0.108, 0, 0.02],
-            "humerus": [0, 0, 0.036],
-            "shoulder": [0, 0, 0],
-            "base": [0, 0, 0.02],
-        },
-        "moss": {
-            "gripper": [0.246, 0.013, 0.111],
-            "wrist": [0.245, 0.002, 0.064],
-            "forearm": [0.122, 0, 0.064],
-            "humerus": [0.001, 0.001, 0.063],
-            "shoulder": [0, 0, 0],
-            "base": [0, 0, 0.02],
-        },
-        "so_old_calibration": {
-            "gripper": [0.320, 0, 0.050],
-            "wrist": [0.278, 0, 0.050],
-            "forearm": [0.143, 0, 0.044],
-            "humerus": [0.031, 0, 0.072],
-            "shoulder": [0, 0, 0],
-            "base": [0, 0, 0.02],
-        },
-        "so_new_calibration": {
-            "gripper": [0.33, 0.0, 0.285],
-            "wrist": [0.30, 0.0, 0.267],
-            "forearm": [0.25, 0.0, 0.266],
-            "humerus": [0.06, 0.0, 0.264],
-            "shoulder": [0.0, 0.0, 0.238],
-            "base": [0.0, 0.0, 0.12],
-        },
-    }
-
-    def __init__(self, robot_type: str = "so100"):
-        """Initialize kinematics for the specified robot type.
-
-        Args:
-            robot_type: String specifying the robot model ("koch", "so100", or "moss")
+class PlacoRobotKinematics:
+    """Robot kinematics using placo library for forward and inverse kinematics."""
+    
+    def __init__(self, urdf_path: str, ee_frame_name: str = "gripper_tip", joint_names: list[str] = None):
         """
-        if robot_type not in self.ROBOT_MEASUREMENTS:
-            raise ValueError(
-                f"Unknown robot type: {robot_type}. Available types: {list(self.ROBOT_MEASUREMENTS.keys())}"
-            )
-
-        self.robot_type = robot_type
-        self.measurements = self.ROBOT_MEASUREMENTS[robot_type]
-
-        # Initialize all transformation matrices and screw axes
-        self._setup_transforms()
-
-    def _create_translation_matrix(
-        self, x: float = 0.0, y: float = 0.0, z: float = 0.0
-    ) -> NDArray[np.float32]:
-        """Create a 4x4 translation matrix."""
-        return np.array([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]])
-
-    def _setup_transforms(self):
-        """Setup all transformation matrices and screw axes for the robot."""
-        # Set up rotation matrices (constant across robot types)
-
-        # Gripper orientation
-        self.gripper_X0 = np.array(
-            [
-                [1, 0, 0, 0],
-                [0, 0, 1, 0],
-                [0, -1, 0, 0],
-                [0, 0, 0, 1],
-            ],
-            dtype=np.float32,
-        )
-
-        # Wrist orientation
-        self.wrist_X0 = np.array(
-            [
-                [0, -1, 0, 0],
-                [1, 0, 0, 0],
-                [0, 0, 1, 0],
-                [0, 0, 0, 1],
-            ],
-            dtype=np.float32,
-        )
-
-        # Base orientation
-        self.base_X0 = np.array(
-            [
-                [0, 0, 1, 0],
-                [1, 0, 0, 0],
-                [0, 1, 0, 0],
-                [0, 0, 0, 1],
-            ],
-            dtype=np.float32,
-        )
-
-        # Gripper
-        # Screw axis of gripper frame wrt base frame
-        self.S_BG = np.array(
-            [
-                1,
-                0,
-                0,
-                0,
-                self.measurements["gripper"][2],
-                -self.measurements["gripper"][1],
-            ],
-            dtype=np.float32,
-        )
-
-        # Gripper origin to centroid transform
-        self.X_GoGc = self._create_translation_matrix(x=0.07)
-
-        # Gripper origin to tip transform
-        self.X_GoGt = self._create_translation_matrix(x=0.12)
-
-        # 0-position gripper frame pose wrt base
-        self.X_BoGo = self._create_translation_matrix(
-            x=self.measurements["gripper"][0],
-            y=self.measurements["gripper"][1],
-            z=self.measurements["gripper"][2],
-        )
-
-        # Wrist
-        # Screw axis of wrist frame wrt base frame
-        self.S_BR = np.array(
-            [0, 1, 0, -self.measurements["wrist"][2], 0, self.measurements["wrist"][0]], dtype=np.float32
-        )
-
-        # 0-position origin to centroid transform
-        self.X_RoRc = self._create_translation_matrix(x=0.0035, y=-0.002)
-
-        # 0-position wrist frame pose wrt base
-        self.X_BR = self._create_translation_matrix(
-            x=self.measurements["wrist"][0],
-            y=self.measurements["wrist"][1],
-            z=self.measurements["wrist"][2],
-        )
-
-        # Forearm
-        # Screw axis of forearm frame wrt base frame
-        self.S_BF = np.array(
-            [
-                0,
-                1,
-                0,
-                -self.measurements["forearm"][2],
-                0,
-                self.measurements["forearm"][0],
-            ],
-            dtype=np.float32,
-        )
-
-        # Forearm origin + centroid transform
-        self.X_ForearmFc = self._create_translation_matrix(x=0.036)
-
-        # 0-position forearm frame pose wrt base
-        self.X_BF = self._create_translation_matrix(
-            x=self.measurements["forearm"][0],
-            y=self.measurements["forearm"][1],
-            z=self.measurements["forearm"][2],
-        )
-
-        # Humerus
-        # Screw axis of humerus frame wrt base frame
-        self.S_BH = np.array(
-            [
-                0,
-                -1,
-                0,
-                self.measurements["humerus"][2],
-                0,
-                -self.measurements["humerus"][0],
-            ],
-            dtype=np.float32,
-        )
-
-        # Humerus origin to centroid transform
-        self.X_HoHc = self._create_translation_matrix(x=0.0475)
-
-        # 0-position humerus frame pose wrt base
-        self.X_BH = self._create_translation_matrix(
-            x=self.measurements["humerus"][0],
-            y=self.measurements["humerus"][1],
-            z=self.measurements["humerus"][2],
-        )
-
-        # Shoulder
-        # Screw axis of shoulder frame wrt Base frame
-        self.S_BS = np.array([0, 0, -1, 0, 0, 0], dtype=np.float32)
-
-        # Shoulder origin to centroid transform
-        self.X_SoSc = self._create_translation_matrix(x=-0.017, z=0.0235)
-
-        # 0-position shoulder frame pose wrt base
-        self.X_BS = self._create_translation_matrix(
-            x=self.measurements["shoulder"][0],
-            y=self.measurements["shoulder"][1],
-            z=self.measurements["shoulder"][2],
-        )
-
-        # Base
-        # Base origin to centroid transform
-        self.X_BoBc = self._create_translation_matrix(y=0.015)
-
-        # World to base transform
-        self.X_WoBo = self._create_translation_matrix(
-            x=self.measurements["base"][0],
-            y=self.measurements["base"][1],
-            z=self.measurements["base"][2],
-        )
-
-        # Pre-compute gripper post-multiplication matrix
-        self._fk_gripper_post = self.X_GoGc @ self.X_BoGo @ self.gripper_X0
-
-    def forward_kinematics(
-        self,
-        robot_pos_deg: NDArray[np.float32],
-        frame: str = "gripper_tip",
-    ) -> NDArray[np.float32]:
-        """Generic forward kinematics.
-
+        Initialize placo-based kinematics solver.
+        
         Args:
-            robot_pos_deg: Joint positions in degrees. Can be ``None`` when
-                computing the *base* frame as it does not depend on joint
-                angles.
-            frame: Target frame. One of
-                ``{"base", "shoulder", "humerus", "forearm", "wrist", "gripper", "gripper_tip"}``.
-
-        Returns
-        -------
-        NDArray[np.float32]
-            4×4 homogeneous transformation matrix of the requested frame
-            expressed in the world coordinate system.
+            urdf_path: Path to the robot URDF file
+            ee_frame_name: Name of the end-effector frame in the URDF
+            joint_names: List of joint names to control (if None, will use default naming)
         """
-        frame = frame.lower()
-        if frame not in {
-            "base",
-            "shoulder",
-            "humerus",
-            "forearm",
-            "wrist",
-            "gripper",
-            "gripper_tip",
-        }:
-            raise ValueError(
-                f"Unknown frame '{frame}'. Valid options are base, shoulder, humerus, forearm, wrist, gripper, gripper_tip."
+        try:
+            import placo
+        except ImportError:
+            raise ImportError(
+                "placo is required for PlacoRobotKinematics. "
+                "Please install it with: pip install placo"
             )
-
-        # Base frame does not rely on joint angles.
-        if frame == "base":
-            return self.X_WoBo @ self.X_BoBc @ self.base_X0
-
-        robot_pos_rad = robot_pos_deg / 180 * np.pi
-
-        # Extract joint angles (note the sign convention for shoulder lift).
-        theta_shoulder_pan = robot_pos_rad[0]
-        theta_shoulder_lift = -robot_pos_rad[1]
-        theta_elbow_flex = robot_pos_rad[2]
-        theta_wrist_flex = robot_pos_rad[3]
-        theta_wrist_roll = robot_pos_rad[4]
-
-        # Start with the world-to-base transform; incrementally add successive links.
-        transformation_matrix = self.X_WoBo @ screw_axis_to_transform(self.S_BS, theta_shoulder_pan)
-        if frame == "shoulder":
-            return transformation_matrix @ self.X_SoSc @ self.X_BS
-
-        transformation_matrix = transformation_matrix @ screw_axis_to_transform(
-            self.S_BH, theta_shoulder_lift
-        )
-        if frame == "humerus":
-            return transformation_matrix @ self.X_HoHc @ self.X_BH
-
-        transformation_matrix = transformation_matrix @ screw_axis_to_transform(self.S_BF, theta_elbow_flex)
-        if frame == "forearm":
-            return transformation_matrix @ self.X_ForearmFc @ self.X_BF
-
-        transformation_matrix = transformation_matrix @ screw_axis_to_transform(self.S_BR, theta_wrist_flex)
-        if frame == "wrist":
-            return transformation_matrix @ self.X_RoRc @ self.X_BR @ self.wrist_X0
-
-        transformation_matrix = transformation_matrix @ screw_axis_to_transform(self.S_BG, theta_wrist_roll)
-        if frame == "gripper":
-            return transformation_matrix @ self._fk_gripper_post
-        else:  # frame == "gripper_tip"
-            return transformation_matrix @ self.X_GoGt @ self.X_BoGo @ self.gripper_X0
-
-    def compute_jacobian(
-        self, robot_pos_deg: NDArray[np.float32], frame: str = "gripper_tip"
-    ) -> NDArray[np.float32]:
-        """Finite differences to compute the Jacobian.
-        J(i, j) represents how the ith component of the end-effector's velocity changes wrt a small change
-        in the jth joint's velocity.
-
-        Args:
-            robot_pos_deg: Current joint positions in degrees
-            fk_func: Forward kinematics function to use (defaults to fk_gripper)
+        
+        self.robot = placo.RobotWrapper(urdf_path)
+        self.solver = placo.KinematicsSolver(self.robot)
+        self.solver.mask_fbase(True)  # Fix the base
+        
+        self.ee_frame_name = ee_frame_name
+        
+        # Set joint names
+        if joint_names is None:
+            # Default joint names for SO-ARM100
+            self.joint_names = ["1", "2", "3", "4", "5"]
+        else:
+            self.joint_names = joint_names
+        
+        # Initialize frame task for IK
+        self.tip_starting_pose = np.eye(4)
+        self.tip_frame = self.solver.add_frame_task(self.ee_frame_name, self.tip_starting_pose)
+        self.tip_frame.configure(self.ee_frame_name, "soft", 1.0, 1.0)
+    
+    def forward_kinematics(self, robot_pos_deg, frame=None):
         """
-
-        eps = 1e-8
-        jac = np.zeros(shape=(6, 5))
-        delta = np.zeros(len(robot_pos_deg[:-1]), dtype=np.float64)
-        for el_ix in range(len(robot_pos_deg[:-1])):
-            delta *= 0
-            delta[el_ix] = eps / 2
-            sdot = (
-                pose_difference_se3(
-                    self.forward_kinematics(robot_pos_deg[:-1] + delta, frame),
-                    self.forward_kinematics(robot_pos_deg[:-1] - delta, frame),
-                )
-                / eps
-            )
-            jac[:, el_ix] = sdot
-        return jac
-
-    def compute_positional_jacobian(
-        self, robot_pos_deg: NDArray[np.float32], frame: str = "gripper_tip"
-    ) -> NDArray[np.float32]:
-        """Finite differences to compute the positional Jacobian.
-        J(i, j) represents how the ith component of the end-effector's position changes wrt a small change
-        in the jth joint's velocity.
-
+        Compute forward kinematics for given joint configuration.
+        
         Args:
-            robot_pos_deg: Current joint positions in degrees
-            fk_func: Forward kinematics function to use (defaults to fk_gripper)
+            robot_pos_deg: Joint positions in degrees (numpy array)
+            frame: Target frame name (if None, uses ee_frame_name)
+        
+        Returns:
+            4x4 transformation matrix of the end-effector pose
         """
-        eps = 1e-8
-        jac = np.zeros(shape=(3, 5))
-        delta = np.zeros(len(robot_pos_deg[:-1]), dtype=np.float64)
-        for el_ix in range(len(robot_pos_deg[:-1])):
-            delta *= 0
-            delta[el_ix] = eps / 2
-            sdot = (
-                self.forward_kinematics(robot_pos_deg[:-1] + delta, frame)[:3, 3]
-                - self.forward_kinematics(robot_pos_deg[:-1] - delta, frame)[:3, 3]
-            ) / eps
-            jac[:, el_ix] = sdot
-        return jac
-
-    def ik(
-        self,
-        current_joint_pos: NDArray[np.float32],
-        desired_ee_pose: NDArray[np.float32],
-        position_only: bool = True,
-        frame: str = "gripper_tip",
-        max_iterations: int = 5,
-        learning_rate: float = 1,
-    ) -> NDArray[np.float32]:
-        """Inverse kinematics using gradient descent.
-
+        if frame is None:
+            frame = self.ee_frame_name
+        
+        # Convert degrees to radians
+        robot_pos_rad = np.deg2rad(robot_pos_deg[:len(self.joint_names)])
+        
+        # Update joint positions in placo robot
+        for i, joint_name in enumerate(self.joint_names):
+            self.robot.set_joint(joint_name, robot_pos_rad[i])
+        
+        # Update kinematics
+        self.robot.update_kinematics()
+        
+        # Get the transformation matrix
+        return self.robot.get_T_world_frame(frame)
+    
+    def ik(self, current_joint_pos, desired_ee_pose, position_only=True, frame=None):
+        """
+        Compute inverse kinematics using placo solver.
+        
         Args:
-            current_joint_state: Initial joint positions in degrees
+            current_joint_pos: Current joint positions in degrees (used as initial guess)
             desired_ee_pose: Target end-effector pose as a 4x4 transformation matrix
-            position_only: If True, only match end-effector position, not orientation
-            frame: Target frame. One of
-                ``{"base", "shoulder", "humerus", "forearm", "wrist", "gripper", "gripper_tip"}``.
-            max_iterations: Maximum number of iterations to run
-            learning_rate: Learning rate for gradient descent
-
+            position_only: If True, only match position (not orientation)
+            frame: Target frame name (if None, uses ee_frame_name)
+        
         Returns:
             Joint positions in degrees that achieve the desired end-effector pose
         """
-        # Do gradient descent.
-        current_joint_state = current_joint_pos.copy()
-        for _ in range(max_iterations):
-            current_ee_pose = self.forward_kinematics(current_joint_state, frame)
-            if not position_only:
-                error = se3_error(desired_ee_pose, current_ee_pose)
-                jac = self.compute_jacobian(current_joint_state, frame)
-            else:
-                error = desired_ee_pose[:3, 3] - current_ee_pose[:3, 3]
-                jac = self.compute_positional_jacobian(current_joint_state, frame)
-            delta_angles = np.linalg.pinv(jac) @ error
-            current_joint_state[:-1] += learning_rate * delta_angles
-
-            if np.linalg.norm(error) < 5e-3:
-                return current_joint_state
-        return current_joint_state
+        if frame is None:
+            frame = self.ee_frame_name
+        
+        # Convert current joint positions to radians for initial guess
+        current_joint_rad = np.deg2rad(current_joint_pos[:len(self.joint_names)])
+        
+        # Set current joint positions as initial guess
+        for i, joint_name in enumerate(self.joint_names):
+            self.robot.set_joint(joint_name, current_joint_rad[i])
+        
+        # Update the target pose for the frame task
+        self.tip_frame.T_world_frame = desired_ee_pose
+        
+        # Configure the task based on position_only flag
+        if position_only:
+            # Only constrain position, not orientation
+            self.tip_frame.configure(self.ee_frame_name, "soft", 1.0, 0.0)
+        else:
+            # Constrain both position and orientation
+            self.tip_frame.configure(self.ee_frame_name, "soft", 1.0, 1.0)
+        
+        # Solve IK
+        self.solver.solve(True)
+        self.robot.update_kinematics()
+        
+        # Extract joint positions
+        joint_positions_rad = []
+        for joint_name in self.joint_names:
+            joint = self.robot.get_joint(joint_name)
+            joint_positions_rad.append(joint)
+        
+        # Convert back to degrees
+        joint_positions_deg = np.rad2deg(joint_positions_rad)
+        
+        # Preserve gripper position if present in current_joint_pos
+        if len(current_joint_pos) > len(self.joint_names):
+            result = np.zeros_like(current_joint_pos)
+            result[:len(self.joint_names)] = joint_positions_deg
+            result[len(self.joint_names):] = current_joint_pos[len(self.joint_names):]
+            return result
+        else:
+            return joint_positions_deg
\ No newline at end of file
diff --git a/src/lerobot/robots/so100_follower/config_so100_follower.py b/src/lerobot/robots/so100_follower/config_so100_follower.py
index 249b44960c..355bd3ff5c 100644
--- a/src/lerobot/robots/so100_follower/config_so100_follower.py
+++ b/src/lerobot/robots/so100_follower/config_so100_follower.py
@@ -44,6 +44,15 @@ class SO100FollowerConfig(RobotConfig):
 class SO100FollowerEndEffectorConfig(SO100FollowerConfig):
     """Configuration for the SO100FollowerEndEffector robot."""
 
+    # Path to URDF file for kinematics
+    urdf_path: str | None = None
+    
+    # End-effector frame name in the URDF
+    ee_frame_name: str = "gripperframe"
+    
+    # Joint names for kinematics (if None, uses default naming)
+    joint_names: list[str] | None = None
+
     # Default bounds for the end-effector position (in meters)
     end_effector_bounds: dict[str, list[float]] = field(
         default_factory=lambda: {
diff --git a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
index d01b60e932..4faa3685ba 100644
--- a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
+++ b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
@@ -22,7 +22,7 @@
 
 from lerobot.cameras import make_cameras_from_configs
 from lerobot.errors import DeviceNotConnectedError
-from lerobot.model.kinematics import RobotKinematics
+from lerobot.model.kinematics_utils import PlacoRobotKinematics
 from lerobot.motors import Motor, MotorNormMode
 from lerobot.motors.feetech import FeetechMotorsBus
 
@@ -30,7 +30,6 @@
 from .config_so100_follower import SO100FollowerEndEffectorConfig
 
 logger = logging.getLogger(__name__)
-EE_FRAME = "gripper_tip"
 
 
 class SO100FollowerEndEffector(SO100Follower):
@@ -64,7 +63,17 @@ def __init__(self, config: SO100FollowerEndEffectorConfig):
         self.config = config
 
         # Initialize the kinematics module for the so100 robot
-        self.kinematics = RobotKinematics(robot_type="so_new_calibration")
+        if self.config.urdf_path is None:
+            raise ValueError(
+                "urdf_path must be provided in the configuration for end-effector control. "
+                "Please set urdf_path in your SO100FollowerEndEffectorConfig."
+            )
+        
+        self.kinematics = PlacoRobotKinematics(
+            urdf_path=self.config.urdf_path,
+            ee_frame_name=self.config.ee_frame_name,
+            joint_names=self.config.joint_names
+        )
 
         # Store the bounds for end-effector position
         self.end_effector_bounds = self.config.end_effector_bounds
@@ -126,7 +135,7 @@ def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
 
         # Calculate current end-effector position using forward kinematics
         if self.current_ee_pos is None:
-            self.current_ee_pos = self.kinematics.forward_kinematics(self.current_joint_pos, frame=EE_FRAME)
+            self.current_ee_pos = self.kinematics.forward_kinematics(self.current_joint_pos, frame=self.config.ee_frame_name)
 
         # Set desired end-effector position by adding delta
         desired_ee_pos = np.eye(4)
@@ -143,7 +152,7 @@ def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
 
         # Compute inverse kinematics to get joint positions
         target_joint_values_in_degrees = self.kinematics.ik(
-            self.current_joint_pos, desired_ee_pos, position_only=True, frame=EE_FRAME
+            self.current_joint_pos, desired_ee_pos, position_only=True, frame=self.config.ee_frame_name
         )
 
         target_joint_values_in_degrees = np.clip(target_joint_values_in_degrees, -180.0, 180.0)
diff --git a/src/lerobot/scripts/rl/gym_manipulator.py b/src/lerobot/scripts/rl/gym_manipulator.py
index 76a136084d..390ceea67f 100644
--- a/src/lerobot/scripts/rl/gym_manipulator.py
+++ b/src/lerobot/scripts/rl/gym_manipulator.py
@@ -1293,6 +1293,7 @@ def _handle_intervention(self, action):
                 gripper_action = 1
 
             action = np.append(action, gripper_action)
+            action = torch.from_numpy(action).float()
 
         return action
 

From 52273f75c732d283c4936f185a5be780f2ebdb4f Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Mon, 16 Jun 2025 13:25:19 +0200
Subject: [PATCH 02/21] (refactor) refactor `gym_manipulator.py` to use the new
 kinematics from placo

---
 src/lerobot/model/kinematics.py               | 68 +++++++++----------
 .../so100_follower/config_so100_follower.py   |  4 +-
 .../so100_follower_end_effector.py            | 12 ++--
 src/lerobot/scripts/rl/gym_manipulator.py     | 39 ++++++-----
 4 files changed, 58 insertions(+), 65 deletions(-)

diff --git a/src/lerobot/model/kinematics.py b/src/lerobot/model/kinematics.py
index 7b7e9f104c..bc151abe3a 100644
--- a/src/lerobot/model/kinematics.py
+++ b/src/lerobot/model/kinematics.py
@@ -1,97 +1,91 @@
 import numpy as np
 
 
-class PlacoRobotKinematics:
+class RobotKinematics:
     """Robot kinematics using placo library for forward and inverse kinematics."""
-    
+
     def __init__(self, urdf_path: str, ee_frame_name: str = "gripper_tip", joint_names: list[str] = None):
         """
         Initialize placo-based kinematics solver.
-        
+
         Args:
             urdf_path: Path to the robot URDF file
             ee_frame_name: Name of the end-effector frame in the URDF
             joint_names: List of joint names to control (if None, will use default naming)
         """
-        try:
-            import placo
-        except ImportError:
-            raise ImportError(
-                "placo is required for PlacoRobotKinematics. "
-                "Please install it with: pip install placo"
-            )
-        
+        import placo
+
         self.robot = placo.RobotWrapper(urdf_path)
         self.solver = placo.KinematicsSolver(self.robot)
         self.solver.mask_fbase(True)  # Fix the base
-        
+
         self.ee_frame_name = ee_frame_name
-        
+
         # Set joint names
         if joint_names is None:
             # Default joint names for SO-ARM100
             self.joint_names = ["1", "2", "3", "4", "5"]
         else:
             self.joint_names = joint_names
-        
+
         # Initialize frame task for IK
         self.tip_starting_pose = np.eye(4)
         self.tip_frame = self.solver.add_frame_task(self.ee_frame_name, self.tip_starting_pose)
         self.tip_frame.configure(self.ee_frame_name, "soft", 1.0, 1.0)
-    
+
     def forward_kinematics(self, robot_pos_deg, frame=None):
         """
         Compute forward kinematics for given joint configuration.
-        
+
         Args:
             robot_pos_deg: Joint positions in degrees (numpy array)
             frame: Target frame name (if None, uses ee_frame_name)
-        
+
         Returns:
             4x4 transformation matrix of the end-effector pose
         """
         if frame is None:
             frame = self.ee_frame_name
-        
+
         # Convert degrees to radians
-        robot_pos_rad = np.deg2rad(robot_pos_deg[:len(self.joint_names)])
-        
+        robot_pos_rad = np.deg2rad(robot_pos_deg[: len(self.joint_names)])
+
         # Update joint positions in placo robot
         for i, joint_name in enumerate(self.joint_names):
             self.robot.set_joint(joint_name, robot_pos_rad[i])
-        
+
         # Update kinematics
         self.robot.update_kinematics()
-        
+
         # Get the transformation matrix
         return self.robot.get_T_world_frame(frame)
-    
+
     def ik(self, current_joint_pos, desired_ee_pose, position_only=True, frame=None):
         """
         Compute inverse kinematics using placo solver.
-        
+
         Args:
             current_joint_pos: Current joint positions in degrees (used as initial guess)
             desired_ee_pose: Target end-effector pose as a 4x4 transformation matrix
             position_only: If True, only match position (not orientation)
             frame: Target frame name (if None, uses ee_frame_name)
-        
+
         Returns:
             Joint positions in degrees that achieve the desired end-effector pose
         """
         if frame is None:
             frame = self.ee_frame_name
-        
+
         # Convert current joint positions to radians for initial guess
-        current_joint_rad = np.deg2rad(current_joint_pos[:len(self.joint_names)])
-        
+        current_joint_rad = np.deg2rad(current_joint_pos[: len(self.joint_names)])
+
         # Set current joint positions as initial guess
         for i, joint_name in enumerate(self.joint_names):
             self.robot.set_joint(joint_name, current_joint_rad[i])
-        
+
         # Update the target pose for the frame task
         self.tip_frame.T_world_frame = desired_ee_pose
-        
+
         # Configure the task based on position_only flag
         if position_only:
             # Only constrain position, not orientation
@@ -99,25 +93,25 @@ def ik(self, current_joint_pos, desired_ee_pose, position_only=True, frame=None)
         else:
             # Constrain both position and orientation
             self.tip_frame.configure(self.ee_frame_name, "soft", 1.0, 1.0)
-        
+
         # Solve IK
         self.solver.solve(True)
         self.robot.update_kinematics()
-        
+
         # Extract joint positions
         joint_positions_rad = []
         for joint_name in self.joint_names:
             joint = self.robot.get_joint(joint_name)
             joint_positions_rad.append(joint)
-        
+
         # Convert back to degrees
         joint_positions_deg = np.rad2deg(joint_positions_rad)
-        
+
         # Preserve gripper position if present in current_joint_pos
         if len(current_joint_pos) > len(self.joint_names):
             result = np.zeros_like(current_joint_pos)
-            result[:len(self.joint_names)] = joint_positions_deg
-            result[len(self.joint_names):] = current_joint_pos[len(self.joint_names):]
+            result[: len(self.joint_names)] = joint_positions_deg
+            result[len(self.joint_names) :] = current_joint_pos[len(self.joint_names) :]
             return result
         else:
-            return joint_positions_deg
\ No newline at end of file
+            return joint_positions_deg
diff --git a/src/lerobot/robots/so100_follower/config_so100_follower.py b/src/lerobot/robots/so100_follower/config_so100_follower.py
index 355bd3ff5c..e9ad0acebf 100644
--- a/src/lerobot/robots/so100_follower/config_so100_follower.py
+++ b/src/lerobot/robots/so100_follower/config_so100_follower.py
@@ -46,10 +46,10 @@ class SO100FollowerEndEffectorConfig(SO100FollowerConfig):
 
     # Path to URDF file for kinematics
     urdf_path: str | None = None
-    
+
     # End-effector frame name in the URDF
     ee_frame_name: str = "gripperframe"
-    
+
     # Joint names for kinematics (if None, uses default naming)
     joint_names: list[str] | None = None
 
diff --git a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
index 4faa3685ba..65e8092d48 100644
--- a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
+++ b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
@@ -22,7 +22,7 @@
 
 from lerobot.cameras import make_cameras_from_configs
 from lerobot.errors import DeviceNotConnectedError
-from lerobot.model.kinematics_utils import PlacoRobotKinematics
+from lerobot.model.kinematics import RobotKinematics
 from lerobot.motors import Motor, MotorNormMode
 from lerobot.motors.feetech import FeetechMotorsBus
 
@@ -68,11 +68,11 @@ def __init__(self, config: SO100FollowerEndEffectorConfig):
                 "urdf_path must be provided in the configuration for end-effector control. "
                 "Please set urdf_path in your SO100FollowerEndEffectorConfig."
             )
-        
-        self.kinematics = PlacoRobotKinematics(
+
+        self.kinematics = RobotKinematics(
             urdf_path=self.config.urdf_path,
             ee_frame_name=self.config.ee_frame_name,
-            joint_names=self.config.joint_names
+            joint_names=self.config.joint_names,
         )
 
         # Store the bounds for end-effector position
@@ -135,7 +135,7 @@ def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
 
         # Calculate current end-effector position using forward kinematics
         if self.current_ee_pos is None:
-            self.current_ee_pos = self.kinematics.forward_kinematics(self.current_joint_pos, frame=self.config.ee_frame_name)
+            self.current_ee_pos = self.kinematics.forward_kinematics(self.current_joint_pos)
 
         # Set desired end-effector position by adding delta
         desired_ee_pos = np.eye(4)
@@ -152,7 +152,7 @@ def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
 
         # Compute inverse kinematics to get joint positions
         target_joint_values_in_degrees = self.kinematics.ik(
-            self.current_joint_pos, desired_ee_pos, position_only=True, frame=self.config.ee_frame_name
+            self.current_joint_pos, desired_ee_pos, position_only=True
         )
 
         target_joint_values_in_degrees = np.clip(target_joint_values_in_degrees, -180.0, 180.0)
diff --git a/src/lerobot/scripts/rl/gym_manipulator.py b/src/lerobot/scripts/rl/gym_manipulator.py
index 390ceea67f..1f3741d48c 100644
--- a/src/lerobot/scripts/rl/gym_manipulator.py
+++ b/src/lerobot/scripts/rl/gym_manipulator.py
@@ -264,7 +264,7 @@ def __init__(
     def _get_observation(self) -> np.ndarray:
         """Helper to convert a dictionary from bus.sync_read to an ordered numpy array."""
         obs_dict = self.robot.get_observation()
-        joint_positions = np.array([obs_dict[name] for name in self._joint_names], dtype=np.float32)
+        joint_positions = np.array([obs_dict[name] for name in self._joint_names])
 
         images = {key: obs_dict[key] for key in self._image_keys}
         return {"agent_pos": joint_positions, "pixels": images}
@@ -1090,13 +1090,11 @@ def __init__(self, env, ee_pose_limits):
             dtype=np.float32,
         )
 
-        # Initialize kinematics instance for the appropriate robot type
-        robot_type = getattr(env.unwrapped.robot.config, "robot_type", "so101")
-        if "so100" in robot_type or "so101" in robot_type:
-            # Note to be compatible with the rest of the codebase,
-            # we are using the new calibration method for so101 and so100
-            robot_type = "so_new_calibration"
-        self.kinematics = RobotKinematics(robot_type)
+        self.kinematics = RobotKinematics(
+            urdf_path=env.unwrapped.robot.config.urdf_path,
+            ee_frame_name=env.unwrapped.robot.config.ee_frame_name,
+            joint_names=env.unwrapped.robot.config.joint_names,
+        )
 
     def observation(self, observation):
         """
@@ -1110,7 +1108,7 @@ def observation(self, observation):
         """
         current_joint_pos = self.unwrapped._get_observation()["agent_pos"]
 
-        current_ee_pos = self.kinematics.forward_kinematics(current_joint_pos, frame="gripper_tip")[:3, 3]
+        current_ee_pos = self.kinematics.forward_kinematics(current_joint_pos)[:3, 3]
         observation["agent_pos"] = np.concatenate([observation["agent_pos"], current_ee_pos], -1)
         return observation
 
@@ -1157,12 +1155,11 @@ def __init__(
         self.event_lock = Lock()  # Thread-safe access to events
 
         # Initialize robot control
-        robot_type = getattr(env.unwrapped.robot.config, "robot_type", "so101")
-        if "so100" in robot_type or "so101" in robot_type:
-            # Note to be compatible with the rest of the codebase,
-            # we are using the new calibration method for so101 and so100
-            robot_type = "so_new_calibration"
-        self.kinematics = RobotKinematics(robot_type)
+        self.kinematics = RobotKinematics(
+            urdf_path=env.unwrapped.robot.config.urdf_path,
+            ee_frame_name=env.unwrapped.robot.config.ee_frame_name,
+            joint_names=env.unwrapped.robot.config.joint_names,
+        )
         self.leader_torque_enabled = True
         self.prev_leader_gripper = None
 
@@ -1260,14 +1257,14 @@ def _handle_intervention(self, action):
         leader_pos_dict = self.robot_leader.bus.sync_read("Present_Position")
         follower_pos_dict = self.robot_follower.bus.sync_read("Present_Position")
 
-        leader_pos = np.array([leader_pos_dict[name] for name in leader_pos_dict], dtype=np.float32)
-        follower_pos = np.array([follower_pos_dict[name] for name in follower_pos_dict], dtype=np.float32)
+        leader_pos = np.array([leader_pos_dict[name] for name in leader_pos_dict])
+        follower_pos = np.array([follower_pos_dict[name] for name in follower_pos_dict])
 
         self.leader_tracking_error_queue.append(np.linalg.norm(follower_pos[:-1] - leader_pos[:-1]))
 
         # [:3, 3] Last column of the transformation matrix corresponds to the xyz translation
-        leader_ee = self.kinematics.forward_kinematics(leader_pos, frame="gripper_tip")[:3, 3]
-        follower_ee = self.kinematics.forward_kinematics(follower_pos, frame="gripper_tip")[:3, 3]
+        leader_ee = self.kinematics.forward_kinematics(leader_pos)[:3, 3]
+        follower_ee = self.kinematics.forward_kinematics(follower_pos)[:3, 3]
 
         action = np.clip(leader_ee - follower_ee, -self.end_effector_step_sizes, self.end_effector_step_sizes)
         # Normalize the action to the range [-1, 1]
@@ -1293,7 +1290,6 @@ def _handle_intervention(self, action):
                 gripper_action = 1
 
             action = np.append(action, gripper_action)
-            action = torch.from_numpy(action).float()
 
         return action
 
@@ -1342,6 +1338,9 @@ def step(self, action):
         # NOTE:
         obs, reward, terminated, truncated, info = self.env.step(action)
 
+        if isinstance(action, np.ndarray):
+            action = torch.from_numpy(action)
+
         # Add intervention info
         info["is_intervention"] = is_intervention
         info["action_intervention"] = action

From 1d6dabbf13b3517f9316afbf30031d54c3531b8c Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Mon, 16 Jun 2025 14:28:42 +0200
Subject: [PATCH 03/21] (pyproject) added `kinematics` optional dependency to
 install placo

---
 pyproject.toml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/pyproject.toml b/pyproject.toml
index 40fd8eaeba..b763bb5b28 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -86,6 +86,7 @@ dora = [
 dynamixel = ["dynamixel-sdk>=3.7.31"]
 feetech = ["feetech-servo-sdk>=1.0.0"]
 gamepad = ["pygame>=2.5.1", "hidapi>=0.14.0"]
+kinematics = ["placo>=0.9.6"]
 intelrealsense = [
     "pyrealsense2>=2.55.1.6486 ; sys_platform != 'darwin'",
     "pyrealsense2-macosx>=2.54 ; sys_platform == 'darwin'",

From a6b99af2909a19b59638715e21073a4290950383 Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Mon, 16 Jun 2025 18:44:14 +0200
Subject: [PATCH 04/21] (pyproject) bump gym-hil version

---
 pyproject.toml                  |  2 +-
 src/lerobot/model/kinematics.py | 14 ++++++++++++++
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/pyproject.toml b/pyproject.toml
index b763bb5b28..a98ff2ca97 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -100,7 +100,7 @@ stretch = [
     "pyrealsense2>=2.55.1.6486 ; sys_platform != 'darwin'"
 ]
 test = ["pytest>=8.1.0", "pytest-timeout>=2.4.0", "pytest-cov>=5.0.0", "pyserial>=3.5", "mock-serial>=0.0.1 ; sys_platform != 'win32'"]
-hilserl = ["transformers>=4.50.3", "gym-hil>=0.1.8", "protobuf>=5.29.3", "grpcio==1.71.0"]
+hilserl = ["transformers>=4.50.3", "gym-hil>=0.1.9", "protobuf>=5.29.3", "grpcio==1.71.0"]
 umi = ["imagecodecs>=2024.1.1"]
 video_benchmark = ["scikit-image>=0.23.2", "pandas>=2.2.2"]
 xarm = ["gym-xarm>=0.1.1 ; python_version < '4.0'"]
diff --git a/src/lerobot/model/kinematics.py b/src/lerobot/model/kinematics.py
index bc151abe3a..440cdb8dc1 100644
--- a/src/lerobot/model/kinematics.py
+++ b/src/lerobot/model/kinematics.py
@@ -1,3 +1,17 @@
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
 import numpy as np
 
 

From 5e47eb39da30881389c8cbdcb3d42831d255e05b Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Mon, 16 Jun 2025 18:45:46 +0200
Subject: [PATCH 05/21] (pyproject) add placo dependency to hilserl

---
 pyproject.toml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/pyproject.toml b/pyproject.toml
index a98ff2ca97..182907efce 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -100,7 +100,7 @@ stretch = [
     "pyrealsense2>=2.55.1.6486 ; sys_platform != 'darwin'"
 ]
 test = ["pytest>=8.1.0", "pytest-timeout>=2.4.0", "pytest-cov>=5.0.0", "pyserial>=3.5", "mock-serial>=0.0.1 ; sys_platform != 'win32'"]
-hilserl = ["transformers>=4.50.3", "gym-hil>=0.1.9", "protobuf>=5.29.3", "grpcio==1.71.0"]
+hilserl = ["transformers>=4.50.3", "gym-hil>=0.1.9", "protobuf>=5.29.3", "grpcio==1.71.0", "placo>=0.9.6"]
 umi = ["imagecodecs>=2024.1.1"]
 video_benchmark = ["scikit-image>=0.23.2", "pandas>=2.2.2"]
 xarm = ["gym-xarm>=0.1.1 ; python_version < '4.0'"]

From 6b252cdf1a92876821c3456f79efa87730d1ffe0 Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Tue, 17 Jun 2025 00:22:30 +0200
Subject: [PATCH 06/21] (docs) updated docs to convey the need for urdf added
 message to raise an error and ask the user the install the optional
 dependency

---
 docs/source/hilserl.mdx         | 1 +
 src/lerobot/model/kinematics.py | 8 +++++++-
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/docs/source/hilserl.mdx b/docs/source/hilserl.mdx
index 79b49c9141..b3ab40c898 100644
--- a/docs/source/hilserl.mdx
+++ b/docs/source/hilserl.mdx
@@ -24,6 +24,7 @@ This guide provides step-by-step instructions for training a robot policy using
 - A gamepad (recommended) or keyboard to control the robot
 - A Nvidia GPU
 - A real robot with a follower and leader arm (optional if you use the keyboard or the gamepad)
+- A URDF file for the robot for the kinematics package (check `lerobot/common/model/kinematics.py`)
 
 ## What kind of tasks can I train?
 
diff --git a/src/lerobot/model/kinematics.py b/src/lerobot/model/kinematics.py
index 440cdb8dc1..f1eb6d79e9 100644
--- a/src/lerobot/model/kinematics.py
+++ b/src/lerobot/model/kinematics.py
@@ -27,7 +27,13 @@ def __init__(self, urdf_path: str, ee_frame_name: str = "gripper_tip", joint_nam
             ee_frame_name: Name of the end-effector frame in the URDF
             joint_names: List of joint names to control (if None, will use default naming)
         """
-        import placo
+        try:
+            import placo
+        except ImportError as e:
+            raise ImportError(
+                "placo is required for RobotKinematics. "
+                "Please install the optional dependencies of `kinematics` in the package."
+            ) from e
 
         self.robot = placo.RobotWrapper(urdf_path)
         self.solver = placo.KinematicsSolver(self.robot)

From 54fc265f3f2cba46c34acb48a1a6cc63116fccb1 Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Wed, 18 Jun 2025 18:12:01 +0200
Subject: [PATCH 07/21] Remove joint names from
 config_so100_follower_end_effector

---
 src/lerobot/model/kinematics.py                          | 9 ++-------
 .../robots/so100_follower/config_so100_follower.py       | 3 ---
 .../robots/so100_follower/so100_follower_end_effector.py | 1 -
 src/lerobot/scripts/rl/gym_manipulator.py                | 2 --
 src/lerobot/teleoperators/gamepad/gamepad_utils.py       | 8 ++++----
 5 files changed, 6 insertions(+), 17 deletions(-)

diff --git a/src/lerobot/model/kinematics.py b/src/lerobot/model/kinematics.py
index f1eb6d79e9..4ff4c25bcc 100644
--- a/src/lerobot/model/kinematics.py
+++ b/src/lerobot/model/kinematics.py
@@ -18,14 +18,13 @@
 class RobotKinematics:
     """Robot kinematics using placo library for forward and inverse kinematics."""
 
-    def __init__(self, urdf_path: str, ee_frame_name: str = "gripper_tip", joint_names: list[str] = None):
+    def __init__(self, urdf_path: str, ee_frame_name: str = "gripperframe"):
         """
         Initialize placo-based kinematics solver.
 
         Args:
             urdf_path: Path to the robot URDF file
             ee_frame_name: Name of the end-effector frame in the URDF
-            joint_names: List of joint names to control (if None, will use default naming)
         """
         try:
             import placo
@@ -42,11 +41,7 @@ def __init__(self, urdf_path: str, ee_frame_name: str = "gripper_tip", joint_nam
         self.ee_frame_name = ee_frame_name
 
         # Set joint names
-        if joint_names is None:
-            # Default joint names for SO-ARM100
-            self.joint_names = ["1", "2", "3", "4", "5"]
-        else:
-            self.joint_names = joint_names
+        self.joint_names = list(self.robot.joint_names())
 
         # Initialize frame task for IK
         self.tip_starting_pose = np.eye(4)
diff --git a/src/lerobot/robots/so100_follower/config_so100_follower.py b/src/lerobot/robots/so100_follower/config_so100_follower.py
index e9ad0acebf..e943c8e180 100644
--- a/src/lerobot/robots/so100_follower/config_so100_follower.py
+++ b/src/lerobot/robots/so100_follower/config_so100_follower.py
@@ -50,9 +50,6 @@ class SO100FollowerEndEffectorConfig(SO100FollowerConfig):
     # End-effector frame name in the URDF
     ee_frame_name: str = "gripperframe"
 
-    # Joint names for kinematics (if None, uses default naming)
-    joint_names: list[str] | None = None
-
     # Default bounds for the end-effector position (in meters)
     end_effector_bounds: dict[str, list[float]] = field(
         default_factory=lambda: {
diff --git a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
index 65e8092d48..fc74cef3f7 100644
--- a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
+++ b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
@@ -72,7 +72,6 @@ def __init__(self, config: SO100FollowerEndEffectorConfig):
         self.kinematics = RobotKinematics(
             urdf_path=self.config.urdf_path,
             ee_frame_name=self.config.ee_frame_name,
-            joint_names=self.config.joint_names,
         )
 
         # Store the bounds for end-effector position
diff --git a/src/lerobot/scripts/rl/gym_manipulator.py b/src/lerobot/scripts/rl/gym_manipulator.py
index 1f3741d48c..49e93c3b56 100644
--- a/src/lerobot/scripts/rl/gym_manipulator.py
+++ b/src/lerobot/scripts/rl/gym_manipulator.py
@@ -1093,7 +1093,6 @@ def __init__(self, env, ee_pose_limits):
         self.kinematics = RobotKinematics(
             urdf_path=env.unwrapped.robot.config.urdf_path,
             ee_frame_name=env.unwrapped.robot.config.ee_frame_name,
-            joint_names=env.unwrapped.robot.config.joint_names,
         )
 
     def observation(self, observation):
@@ -1158,7 +1157,6 @@ def __init__(
         self.kinematics = RobotKinematics(
             urdf_path=env.unwrapped.robot.config.urdf_path,
             ee_frame_name=env.unwrapped.robot.config.ee_frame_name,
-            joint_names=env.unwrapped.robot.config.joint_names,
         )
         self.leader_torque_enabled = True
         self.prev_leader_gripper = None
diff --git a/src/lerobot/teleoperators/gamepad/gamepad_utils.py b/src/lerobot/teleoperators/gamepad/gamepad_utils.py
index 21a293c771..63e1e42bbf 100644
--- a/src/lerobot/teleoperators/gamepad/gamepad_utils.py
+++ b/src/lerobot/teleoperators/gamepad/gamepad_utils.py
@@ -307,8 +307,8 @@ def get_deltas(self):
             z_input = 0 if abs(z_input) < self.deadzone else z_input
 
             # Calculate deltas (note: may need to invert axes depending on controller)
-            delta_x = -y_input * self.y_step_size  # Forward/backward
-            delta_y = -x_input * self.x_step_size  # Left/right
+            delta_x = -x_input * self.x_step_size  # Forward/backward
+            delta_y = -y_input * self.y_step_size  # Left/right
             delta_z = -z_input * self.z_step_size  # Up/down
 
             return delta_x, delta_y, delta_z
@@ -465,8 +465,8 @@ def _update(self):
     def get_deltas(self):
         """Get the current movement deltas from gamepad state."""
         # Calculate deltas - invert as needed based on controller orientation
-        delta_x = -self.left_y * self.x_step_size  # Forward/backward
-        delta_y = -self.left_x * self.y_step_size  # Left/right
+        delta_x = -self.left_x * self.x_step_size  # Forward/backward
+        delta_y = self.left_y * self.y_step_size  # Left/right
         delta_z = -self.right_y * self.z_step_size  # Up/down
 
         return delta_x, delta_y, delta_z

From fe32addb4e64865d40154ea96667abb4d4f547b1 Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Thu, 19 Jun 2025 10:26:03 +0200
Subject: [PATCH 08/21] (fix) bug in gym_manipulator where get_observation was
 called at different levels which was causing a delay in the frame rate as the
 motors were queried several times

---
 src/lerobot/scripts/rl/gym_manipulator.py | 42 +++++++++++------------
 1 file changed, 20 insertions(+), 22 deletions(-)

diff --git a/src/lerobot/scripts/rl/gym_manipulator.py b/src/lerobot/scripts/rl/gym_manipulator.py
index 49e93c3b56..204ce5f654 100644
--- a/src/lerobot/scripts/rl/gym_manipulator.py
+++ b/src/lerobot/scripts/rl/gym_manipulator.py
@@ -254,20 +254,19 @@ def __init__(
         self._joint_names = [f"{key}.pos" for key in self.robot.bus.motors]
         self._image_keys = self.robot.cameras.keys()
 
-        # Read initial joint positions using the bus
-        self.current_joint_positions = self._get_observation()["agent_pos"]
+        self.current_observation = None
 
         self.use_gripper = use_gripper
 
         self._setup_spaces()
 
-    def _get_observation(self) -> np.ndarray:
+    def _get_observation(self) -> dict[str, np.ndarray]:
         """Helper to convert a dictionary from bus.sync_read to an ordered numpy array."""
         obs_dict = self.robot.get_observation()
         joint_positions = np.array([obs_dict[name] for name in self._joint_names])
 
         images = {key: obs_dict[key] for key in self._image_keys}
-        return {"agent_pos": joint_positions, "pixels": images}
+        self.current_observation = {"agent_pos": joint_positions, "pixels": images}
 
     def _setup_spaces(self):
         """
@@ -281,24 +280,24 @@ def _setup_spaces(self):
             - The action space is defined as a Box space representing joint position commands. It is defined as relative (delta)
               or absolute, based on the configuration.
         """
-        example_obs = self._get_observation()
+        self._get_observation()
 
         observation_spaces = {}
 
         # Define observation spaces for images and other states.
-        if "pixels" in example_obs:
-            prefix = "observation.images" if len(example_obs["pixels"]) > 1 else "observation.image"
+        if "pixels" in self.current_observation:
+            prefix = "observation.images" if len(self.current_observation["pixels"]) > 1 else "observation.image"
             observation_spaces = {
                 f"{prefix}.{key}": gym.spaces.Box(
-                    low=0, high=255, shape=example_obs["pixels"][key].shape, dtype=np.uint8
+                    low=0, high=255, shape=self.current_observation["pixels"][key].shape, dtype=np.uint8
                 )
-                for key in example_obs["pixels"]
+                for key in self.current_observation["pixels"]
             }
 
         observation_spaces["observation.state"] = gym.spaces.Box(
             low=0,
             high=10,
-            shape=example_obs["agent_pos"].shape,
+            shape=self.current_observation["agent_pos"].shape,
             dtype=np.float32,
         )
 
@@ -340,14 +339,13 @@ def reset(self, seed=None, options=None) -> tuple[dict[str, np.ndarray], dict[st
 
         self.robot.reset()
 
-        # Capture the initial observation.
-        observation = self._get_observation()
-
         # Reset episode tracking variables.
         self.current_step = 0
         self.episode_data = None
+        self.current_observation = None
+        self._get_observation()
 
-        return observation, {"is_intervention": False}
+        return self.current_observation, {"is_intervention": False}
 
     def step(self, action) -> tuple[dict[str, np.ndarray], float, bool, bool, dict[str, Any]]:
         """
@@ -367,8 +365,6 @@ def step(self, action) -> tuple[dict[str, np.ndarray], float, bool, bool, dict[s
                 - truncated (bool): True if the episode was truncated (e.g., time constraints).
                 - info (dict): Additional debugging information including intervention status.
         """
-        self.current_joint_positions = self._get_observation()["agent_pos"]
-
         action_dict = {"delta_x": action[0], "delta_y": action[1], "delta_z": action[2]}
 
         # 1.0 action corresponds to no-op action
@@ -376,6 +372,8 @@ def step(self, action) -> tuple[dict[str, np.ndarray], float, bool, bool, dict[s
 
         self.robot.send_action(action_dict)
 
+        self._get_observation()
+
         if self.display_cameras:
             self.render()
 
@@ -386,7 +384,7 @@ def step(self, action) -> tuple[dict[str, np.ndarray], float, bool, bool, dict[s
         truncated = False
 
         return (
-            self._get_observation(),
+            self.current_observation,
             reward,
             terminated,
             truncated,
@@ -399,11 +397,10 @@ def render(self):
         """
         import cv2
 
-        observation = self._get_observation()
-        image_keys = [key for key in observation if "image" in key]
+        image_keys = [key for key in self.current_observation if "image" in key]
 
         for key in image_keys:
-            cv2.imshow(key, cv2.cvtColor(observation[key].numpy(), cv2.COLOR_RGB2BGR))
+            cv2.imshow(key, cv2.cvtColor(self.current_observation[key].numpy(), cv2.COLOR_RGB2BGR))
             cv2.waitKey(1)
 
     def close(self):
@@ -520,7 +517,8 @@ def observation(self, observation):
         Returns:
             The modified observation with current values.
         """
-        present_current_observation = self.unwrapped._get_observation()["agent_pos"]
+        present_current_dict = self.env.unwrapped.robot.bus.sync_read("Present_Current")
+        present_current_observation = np.array([present_current_dict[name] for name in self.env.unwrapped.robot.bus.motors])
         observation["agent_pos"] = np.concatenate(
             [observation["agent_pos"], present_current_observation], axis=-1
         )
@@ -1105,7 +1103,7 @@ def observation(self, observation):
         Returns:
             Enhanced observation with end-effector pose information.
         """
-        current_joint_pos = self.unwrapped._get_observation()["agent_pos"]
+        current_joint_pos = self.unwrapped.current_observation["agent_pos"]
 
         current_ee_pos = self.kinematics.forward_kinematics(current_joint_pos)[:3, 3]
         observation["agent_pos"] = np.concatenate([observation["agent_pos"], current_ee_pos], -1)

From 76d5fe36e5aa15710223333ea43168ce41b3af81 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Thu, 19 Jun 2025 08:26:25 +0000
Subject: [PATCH 09/21] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 src/lerobot/scripts/rl/gym_manipulator.py | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/lerobot/scripts/rl/gym_manipulator.py b/src/lerobot/scripts/rl/gym_manipulator.py
index 204ce5f654..313c1a5df6 100644
--- a/src/lerobot/scripts/rl/gym_manipulator.py
+++ b/src/lerobot/scripts/rl/gym_manipulator.py
@@ -286,7 +286,9 @@ def _setup_spaces(self):
 
         # Define observation spaces for images and other states.
         if "pixels" in self.current_observation:
-            prefix = "observation.images" if len(self.current_observation["pixels"]) > 1 else "observation.image"
+            prefix = (
+                "observation.images" if len(self.current_observation["pixels"]) > 1 else "observation.image"
+            )
             observation_spaces = {
                 f"{prefix}.{key}": gym.spaces.Box(
                     low=0, high=255, shape=self.current_observation["pixels"][key].shape, dtype=np.uint8
@@ -518,7 +520,9 @@ def observation(self, observation):
             The modified observation with current values.
         """
         present_current_dict = self.env.unwrapped.robot.bus.sync_read("Present_Current")
-        present_current_observation = np.array([present_current_dict[name] for name in self.env.unwrapped.robot.bus.motors])
+        present_current_observation = np.array(
+            [present_current_dict[name] for name in self.env.unwrapped.robot.bus.motors]
+        )
         observation["agent_pos"] = np.concatenate(
             [observation["agent_pos"], present_current_observation], axis=-1
         )

From 8aabbd3309f4d268cdb30de143ab738d25c8d5f5 Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Fri, 20 Jun 2025 14:01:03 +0200
Subject: [PATCH 10/21] change ik to inverse_kinematics and target_frame_name

---
 src/lerobot/model/kinematics.py                        | 10 +++++-----
 .../so100_follower/so100_follower_end_effector.py      |  3 +--
 src/lerobot/teleoperators/gamepad/gamepad_utils.py     |  2 +-
 3 files changed, 7 insertions(+), 8 deletions(-)

diff --git a/src/lerobot/model/kinematics.py b/src/lerobot/model/kinematics.py
index 4ff4c25bcc..88b53b0409 100644
--- a/src/lerobot/model/kinematics.py
+++ b/src/lerobot/model/kinematics.py
@@ -75,7 +75,7 @@ def forward_kinematics(self, robot_pos_deg, frame=None):
         # Get the transformation matrix
         return self.robot.get_T_world_frame(frame)
 
-    def ik(self, current_joint_pos, desired_ee_pose, position_only=True, frame=None):
+    def inverse_kinematics(self, current_joint_pos, desired_ee_pose, position_only=True, target_frame=None):
         """
         Compute inverse kinematics using placo solver.
 
@@ -88,8 +88,8 @@ def ik(self, current_joint_pos, desired_ee_pose, position_only=True, frame=None)
         Returns:
             Joint positions in degrees that achieve the desired end-effector pose
         """
-        if frame is None:
-            frame = self.ee_frame_name
+        if target_frame is None:
+            target_frame = self.ee_frame_name
 
         # Convert current joint positions to radians for initial guess
         current_joint_rad = np.deg2rad(current_joint_pos[: len(self.joint_names)])
@@ -104,10 +104,10 @@ def ik(self, current_joint_pos, desired_ee_pose, position_only=True, frame=None)
         # Configure the task based on position_only flag
         if position_only:
             # Only constrain position, not orientation
-            self.tip_frame.configure(self.ee_frame_name, "soft", 1.0, 0.0)
+            self.tip_frame.configure(target_frame, "soft", 1.0, 0.0)
         else:
             # Constrain both position and orientation
-            self.tip_frame.configure(self.ee_frame_name, "soft", 1.0, 1.0)
+            self.tip_frame.configure(target_frame, "soft", 1.0, 1.0)
 
         # Solve IK
         self.solver.solve(True)
diff --git a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
index fc74cef3f7..d10179d063 100644
--- a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
+++ b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
@@ -150,11 +150,10 @@ def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
             )
 
         # Compute inverse kinematics to get joint positions
-        target_joint_values_in_degrees = self.kinematics.ik(
+        target_joint_values_in_degrees = self.kinematics.inverse_kinematics(
             self.current_joint_pos, desired_ee_pos, position_only=True
         )
 
-        target_joint_values_in_degrees = np.clip(target_joint_values_in_degrees, -180.0, 180.0)
         # Create joint space action dictionary
         joint_action = {
             f"{key}.pos": target_joint_values_in_degrees[i] for i, key in enumerate(self.bus.motors.keys())
diff --git a/src/lerobot/teleoperators/gamepad/gamepad_utils.py b/src/lerobot/teleoperators/gamepad/gamepad_utils.py
index 63e1e42bbf..51c6deb7b7 100644
--- a/src/lerobot/teleoperators/gamepad/gamepad_utils.py
+++ b/src/lerobot/teleoperators/gamepad/gamepad_utils.py
@@ -308,7 +308,7 @@ def get_deltas(self):
 
             # Calculate deltas (note: may need to invert axes depending on controller)
             delta_x = -x_input * self.x_step_size  # Forward/backward
-            delta_y = -y_input * self.y_step_size  # Left/right
+            delta_y = y_input * self.y_step_size  # Left/right
             delta_z = -z_input * self.z_step_size  # Up/down
 
             return delta_x, delta_y, delta_z

From 806a0b7ff204ca71713ef1828b0e043354bb3c47 Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Fri, 20 Jun 2025 14:26:39 +0200
Subject: [PATCH 11/21] Remove target frame from ik and fk and keep it in the
 constructor

---
 src/lerobot/model/kinematics.py | 33 ++++++++++++++-------------------
 1 file changed, 14 insertions(+), 19 deletions(-)

diff --git a/src/lerobot/model/kinematics.py b/src/lerobot/model/kinematics.py
index 88b53b0409..57e1f53992 100644
--- a/src/lerobot/model/kinematics.py
+++ b/src/lerobot/model/kinematics.py
@@ -18,13 +18,15 @@
 class RobotKinematics:
     """Robot kinematics using placo library for forward and inverse kinematics."""
 
-    def __init__(self, urdf_path: str, ee_frame_name: str = "gripperframe"):
+    def __init__(
+        self, urdf_path: str, target_frame_name: str = "gripperframe", joint_names: list[str] = None
+    ):
         """
         Initialize placo-based kinematics solver.
 
         Args:
             urdf_path: Path to the robot URDF file
-            ee_frame_name: Name of the end-effector frame in the URDF
+            target_frame_name: Name of the end-effector frame in the URDF
         """
         try:
             import placo
@@ -38,29 +40,24 @@ def __init__(self, urdf_path: str, ee_frame_name: str = "gripperframe"):
         self.solver = placo.KinematicsSolver(self.robot)
         self.solver.mask_fbase(True)  # Fix the base
 
-        self.ee_frame_name = ee_frame_name
+        self.target_frame_name = target_frame_name
 
         # Set joint names
-        self.joint_names = list(self.robot.joint_names())
+        self.joint_names = list(self.robot.joint_names()) if joint_names is None else joint_names
 
         # Initialize frame task for IK
-        self.tip_starting_pose = np.eye(4)
-        self.tip_frame = self.solver.add_frame_task(self.ee_frame_name, self.tip_starting_pose)
-        self.tip_frame.configure(self.ee_frame_name, "soft", 1.0, 1.0)
+        self.tip_frame = self.solver.add_frame_task(frame=self.target_frame_name, T_world_frame=np.eye(4))
 
-    def forward_kinematics(self, robot_pos_deg, frame=None):
+    def forward_kinematics(self, robot_pos_deg):
         """
-        Compute forward kinematics for given joint configuration.
+        Compute forward kinematics for given joint configuration given the target frame name in the constructor.
 
         Args:
             robot_pos_deg: Joint positions in degrees (numpy array)
-            frame: Target frame name (if None, uses ee_frame_name)
 
         Returns:
             4x4 transformation matrix of the end-effector pose
         """
-        if frame is None:
-            frame = self.ee_frame_name
 
         # Convert degrees to radians
         robot_pos_rad = np.deg2rad(robot_pos_deg[: len(self.joint_names)])
@@ -73,9 +70,9 @@ def forward_kinematics(self, robot_pos_deg, frame=None):
         self.robot.update_kinematics()
 
         # Get the transformation matrix
-        return self.robot.get_T_world_frame(frame)
+        return self.robot.get_T_world_frame(self.target_frame_name)
 
-    def inverse_kinematics(self, current_joint_pos, desired_ee_pose, position_only=True, target_frame=None):
+    def inverse_kinematics(self, current_joint_pos, desired_ee_pose, position_only=True):
         """
         Compute inverse kinematics using placo solver.
 
@@ -83,13 +80,10 @@ def inverse_kinematics(self, current_joint_pos, desired_ee_pose, position_only=T
             current_joint_pos: Current joint positions in degrees (used as initial guess)
             desired_ee_pose: Target end-effector pose as a 4x4 transformation matrix
             position_only: If True, only match position (not orientation)
-            frame: Target frame name (if None, uses ee_frame_name)
 
         Returns:
             Joint positions in degrees that achieve the desired end-effector pose
         """
-        if target_frame is None:
-            target_frame = self.ee_frame_name
 
         # Convert current joint positions to radians for initial guess
         current_joint_rad = np.deg2rad(current_joint_pos[: len(self.joint_names)])
@@ -104,10 +98,11 @@ def inverse_kinematics(self, current_joint_pos, desired_ee_pose, position_only=T
         # Configure the task based on position_only flag
         if position_only:
             # Only constrain position, not orientation
-            self.tip_frame.configure(target_frame, "soft", 1.0, 0.0)
+            self.tip_frame.configure(self.target_frame_name, "soft", 1.0, 0.0)
         else:
             # Constrain both position and orientation
-            self.tip_frame.configure(target_frame, "soft", 1.0, 1.0)
+            # TODO (maractingi-caroline): add variable weights for position and orientation
+            self.tip_frame.configure(self.target_frame_name, "soft", 1.0, 1.0)
 
         # Solve IK
         self.solver.solve(True)

From 7672dafc618a7266f2c37ddea12dab1825efdb69 Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Fri, 20 Jun 2025 15:58:12 +0200
Subject: [PATCH 12/21] (fix) general fixes in find joint limits and
 gym_manipulator

---
 src/lerobot/model/kinematics.py                          | 2 +-
 .../robots/so100_follower/config_so100_follower.py       | 2 +-
 .../robots/so100_follower/so100_follower_end_effector.py | 2 +-
 src/lerobot/scripts/find_joint_limits.py                 | 9 ++++++---
 src/lerobot/scripts/rl/gym_manipulator.py                | 4 ++--
 5 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/src/lerobot/model/kinematics.py b/src/lerobot/model/kinematics.py
index 57e1f53992..cac4a8b4bc 100644
--- a/src/lerobot/model/kinematics.py
+++ b/src/lerobot/model/kinematics.py
@@ -46,7 +46,7 @@ def __init__(
         self.joint_names = list(self.robot.joint_names()) if joint_names is None else joint_names
 
         # Initialize frame task for IK
-        self.tip_frame = self.solver.add_frame_task(frame=self.target_frame_name, T_world_frame=np.eye(4))
+        self.tip_frame = self.solver.add_frame_task(self.target_frame_name, np.eye(4))
 
     def forward_kinematics(self, robot_pos_deg):
         """
diff --git a/src/lerobot/robots/so100_follower/config_so100_follower.py b/src/lerobot/robots/so100_follower/config_so100_follower.py
index e943c8e180..9555b3e0e5 100644
--- a/src/lerobot/robots/so100_follower/config_so100_follower.py
+++ b/src/lerobot/robots/so100_follower/config_so100_follower.py
@@ -48,7 +48,7 @@ class SO100FollowerEndEffectorConfig(SO100FollowerConfig):
     urdf_path: str | None = None
 
     # End-effector frame name in the URDF
-    ee_frame_name: str = "gripperframe"
+    target_frame_name: str = "gripperframe"
 
     # Default bounds for the end-effector position (in meters)
     end_effector_bounds: dict[str, list[float]] = field(
diff --git a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
index d10179d063..60c31eaa7c 100644
--- a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
+++ b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
@@ -71,7 +71,7 @@ def __init__(self, config: SO100FollowerEndEffectorConfig):
 
         self.kinematics = RobotKinematics(
             urdf_path=self.config.urdf_path,
-            ee_frame_name=self.config.ee_frame_name,
+            target_frame_name=self.config.target_frame_name,
         )
 
         # Store the bounds for end-effector position
diff --git a/src/lerobot/scripts/find_joint_limits.py b/src/lerobot/scripts/find_joint_limits.py
index 346edf8acd..fa1ad87b98 100644
--- a/src/lerobot/scripts/find_joint_limits.py
+++ b/src/lerobot/scripts/find_joint_limits.py
@@ -50,6 +50,7 @@
     make_teleoperator_from_config,
     so100_leader,
 )
+from lerobot.common.utils.robot_utils import busy_wait
 
 
 @dataclass
@@ -76,12 +77,12 @@ def find_joint_and_ee_bounds(cfg: FindJointLimitsConfig):
         # Note to be compatible with the rest of the codebase,
         # we are using the new calibration method for so101 and so100
         robot_type = "so_new_calibration"
-    kinematics = RobotKinematics(robot_type=robot_type)
+    kinematics = RobotKinematics(cfg.robot.urdf_path, cfg.robot.target_frame_name)
 
     # Initialize min/max values
     observation = robot.get_observation()
     joint_positions = np.array([observation[f"{key}.pos"] for key in robot.bus.motors])
-    ee_pos = kinematics.forward_kinematics(joint_positions, frame="gripper_tip")[:3, 3]
+    ee_pos = kinematics.forward_kinematics(joint_positions)[:3, 3]
 
     max_pos = joint_positions.copy()
     min_pos = joint_positions.copy()
@@ -94,7 +95,7 @@ def find_joint_and_ee_bounds(cfg: FindJointLimitsConfig):
 
         observation = robot.get_observation()
         joint_positions = np.array([observation[f"{key}.pos"] for key in robot.bus.motors])
-        ee_pos = kinematics.forward_kinematics(joint_positions, frame="gripper_tip")[:3, 3]
+        ee_pos = kinematics.forward_kinematics(joint_positions)[:3, 3]
 
         # Skip initial warmup period
         if (time.perf_counter() - start_episode_t) < 5:
@@ -113,6 +114,8 @@ def find_joint_and_ee_bounds(cfg: FindJointLimitsConfig):
             print(f"Min joint pos position {np.round(min_pos, 4).tolist()}")
             break
 
+        busy_wait(0.01)
+
 
 if __name__ == "__main__":
     find_joint_and_ee_bounds()
diff --git a/src/lerobot/scripts/rl/gym_manipulator.py b/src/lerobot/scripts/rl/gym_manipulator.py
index 313c1a5df6..2a611b2a48 100644
--- a/src/lerobot/scripts/rl/gym_manipulator.py
+++ b/src/lerobot/scripts/rl/gym_manipulator.py
@@ -1094,7 +1094,7 @@ def __init__(self, env, ee_pose_limits):
 
         self.kinematics = RobotKinematics(
             urdf_path=env.unwrapped.robot.config.urdf_path,
-            ee_frame_name=env.unwrapped.robot.config.ee_frame_name,
+            target_frame_name=env.unwrapped.robot.config.target_frame_name,
         )
 
     def observation(self, observation):
@@ -1158,7 +1158,7 @@ def __init__(
         # Initialize robot control
         self.kinematics = RobotKinematics(
             urdf_path=env.unwrapped.robot.config.urdf_path,
-            ee_frame_name=env.unwrapped.robot.config.ee_frame_name,
+            target_frame_name=env.unwrapped.robot.config.target_frame_name,
         )
         self.leader_torque_enabled = True
         self.prev_leader_gripper = None

From 92749aa9cb48c22e97609a4ec5fe2dddc7328922 Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Fri, 20 Jun 2025 16:36:11 +0200
Subject: [PATCH 13/21] added orientation weight in config

---
 src/lerobot/model/kinematics.py               | 22 ++++++++++---------
 .../so100_follower/config_so100_follower.py   |  3 +++
 .../so100_follower_end_effector.py            |  3 ++-
 3 files changed, 17 insertions(+), 11 deletions(-)

diff --git a/src/lerobot/model/kinematics.py b/src/lerobot/model/kinematics.py
index cac4a8b4bc..5d6a4d3757 100644
--- a/src/lerobot/model/kinematics.py
+++ b/src/lerobot/model/kinematics.py
@@ -19,7 +19,11 @@ class RobotKinematics:
     """Robot kinematics using placo library for forward and inverse kinematics."""
 
     def __init__(
-        self, urdf_path: str, target_frame_name: str = "gripperframe", joint_names: list[str] = None
+        self,
+        urdf_path: str,
+        target_frame_name: str = "gripperframe",
+        joint_names: list[str] = None,
+        orientation_weight: float = 0.01,
     ):
         """
         Initialize placo-based kinematics solver.
@@ -27,6 +31,8 @@ def __init__(
         Args:
             urdf_path: Path to the robot URDF file
             target_frame_name: Name of the end-effector frame in the URDF
+            joint_names: List of joint names to use for the kinematics solver
+            orientation_weight: Weight for orientation constraint in IK, set to 0.0 to only constrain position
         """
         try:
             import placo
@@ -48,6 +54,9 @@ def __init__(
         # Initialize frame task for IK
         self.tip_frame = self.solver.add_frame_task(self.target_frame_name, np.eye(4))
 
+        # Set orientation weight
+        self.orientation_weight = orientation_weight
+
     def forward_kinematics(self, robot_pos_deg):
         """
         Compute forward kinematics for given joint configuration given the target frame name in the constructor.
@@ -72,14 +81,13 @@ def forward_kinematics(self, robot_pos_deg):
         # Get the transformation matrix
         return self.robot.get_T_world_frame(self.target_frame_name)
 
-    def inverse_kinematics(self, current_joint_pos, desired_ee_pose, position_only=True):
+    def inverse_kinematics(self, current_joint_pos, desired_ee_pose):
         """
         Compute inverse kinematics using placo solver.
 
         Args:
             current_joint_pos: Current joint positions in degrees (used as initial guess)
             desired_ee_pose: Target end-effector pose as a 4x4 transformation matrix
-            position_only: If True, only match position (not orientation)
 
         Returns:
             Joint positions in degrees that achieve the desired end-effector pose
@@ -96,13 +104,7 @@ def inverse_kinematics(self, current_joint_pos, desired_ee_pose, position_only=T
         self.tip_frame.T_world_frame = desired_ee_pose
 
         # Configure the task based on position_only flag
-        if position_only:
-            # Only constrain position, not orientation
-            self.tip_frame.configure(self.target_frame_name, "soft", 1.0, 0.0)
-        else:
-            # Constrain both position and orientation
-            # TODO (maractingi-caroline): add variable weights for position and orientation
-            self.tip_frame.configure(self.target_frame_name, "soft", 1.0, 1.0)
+        self.tip_frame.configure(self.target_frame_name, "soft", 1.0, self.orientation_weight)
 
         # Solve IK
         self.solver.solve(True)
diff --git a/src/lerobot/robots/so100_follower/config_so100_follower.py b/src/lerobot/robots/so100_follower/config_so100_follower.py
index 9555b3e0e5..defe9ce9ef 100644
--- a/src/lerobot/robots/so100_follower/config_so100_follower.py
+++ b/src/lerobot/robots/so100_follower/config_so100_follower.py
@@ -50,6 +50,9 @@ class SO100FollowerEndEffectorConfig(SO100FollowerConfig):
     # End-effector frame name in the URDF
     target_frame_name: str = "gripperframe"
 
+    # Weight for orientation constraint in IK
+    orientation_weight: float = 0.01
+
     # Default bounds for the end-effector position (in meters)
     end_effector_bounds: dict[str, list[float]] = field(
         default_factory=lambda: {
diff --git a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
index 60c31eaa7c..97db834d5a 100644
--- a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
+++ b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
@@ -72,6 +72,7 @@ def __init__(self, config: SO100FollowerEndEffectorConfig):
         self.kinematics = RobotKinematics(
             urdf_path=self.config.urdf_path,
             target_frame_name=self.config.target_frame_name,
+            orientation_weight=self.config.orientation_weight,
         )
 
         # Store the bounds for end-effector position
@@ -151,7 +152,7 @@ def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
 
         # Compute inverse kinematics to get joint positions
         target_joint_values_in_degrees = self.kinematics.inverse_kinematics(
-            self.current_joint_pos, desired_ee_pos, position_only=True
+            self.current_joint_pos, desired_ee_pos
         )
 
         # Create joint space action dictionary

From de44931c1987bbda17de826394a4e886e971ae52 Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Sat, 21 Jun 2025 15:27:15 +0200
Subject: [PATCH 14/21] fix directions in keyboard teleop

---
 src/lerobot/teleoperators/keyboard/teleop_keyboard.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/lerobot/teleoperators/keyboard/teleop_keyboard.py b/src/lerobot/teleoperators/keyboard/teleop_keyboard.py
index 41a4293cc5..de8e0288b8 100644
--- a/src/lerobot/teleoperators/keyboard/teleop_keyboard.py
+++ b/src/lerobot/teleoperators/keyboard/teleop_keyboard.py
@@ -200,13 +200,13 @@ def get_action(self) -> dict[str, Any]:
         # Generate action based on current key states
         for key, val in self.current_pressed.items():
             if key == keyboard.Key.up:
-                delta_x = int(val)
+                delta_y = -int(val)
             elif key == keyboard.Key.down:
-                delta_x = -int(val)
-            elif key == keyboard.Key.left:
                 delta_y = int(val)
+            elif key == keyboard.Key.left:
+                delta_x = int(val)
             elif key == keyboard.Key.right:
-                delta_y = -int(val)
+                delta_x = -int(val)
             elif key == keyboard.Key.shift:
                 delta_z = -int(val)
             elif key == keyboard.Key.shift_r:

From 713b53e292d18007673343dd3c8cb00a3c839de7 Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Mon, 30 Jun 2025 17:30:32 +0200
Subject: [PATCH 15/21] Removed orientation weight from configuration and added
 it as an argument in inverse kinematics Co-authored-by: CarolinePascal
 <caroline8.pascal@gmail.com>

---
 src/lerobot/model/kinematics.py                       | 11 ++++-------
 .../robots/so100_follower/config_so100_follower.py    |  3 ---
 .../so100_follower/so100_follower_end_effector.py     |  1 -
 3 files changed, 4 insertions(+), 11 deletions(-)

diff --git a/src/lerobot/model/kinematics.py b/src/lerobot/model/kinematics.py
index 5d6a4d3757..f3d22abf5e 100644
--- a/src/lerobot/model/kinematics.py
+++ b/src/lerobot/model/kinematics.py
@@ -23,7 +23,6 @@ def __init__(
         urdf_path: str,
         target_frame_name: str = "gripperframe",
         joint_names: list[str] = None,
-        orientation_weight: float = 0.01,
     ):
         """
         Initialize placo-based kinematics solver.
@@ -32,7 +31,6 @@ def __init__(
             urdf_path: Path to the robot URDF file
             target_frame_name: Name of the end-effector frame in the URDF
             joint_names: List of joint names to use for the kinematics solver
-            orientation_weight: Weight for orientation constraint in IK, set to 0.0 to only constrain position
         """
         try:
             import placo
@@ -54,9 +52,6 @@ def __init__(
         # Initialize frame task for IK
         self.tip_frame = self.solver.add_frame_task(self.target_frame_name, np.eye(4))
 
-        # Set orientation weight
-        self.orientation_weight = orientation_weight
-
     def forward_kinematics(self, robot_pos_deg):
         """
         Compute forward kinematics for given joint configuration given the target frame name in the constructor.
@@ -81,13 +76,15 @@ def forward_kinematics(self, robot_pos_deg):
         # Get the transformation matrix
         return self.robot.get_T_world_frame(self.target_frame_name)
 
-    def inverse_kinematics(self, current_joint_pos, desired_ee_pose):
+    def inverse_kinematics(self, current_joint_pos, desired_ee_pose, position_weight=1.0, orientation_weight=0.01):
         """
         Compute inverse kinematics using placo solver.
 
         Args:
             current_joint_pos: Current joint positions in degrees (used as initial guess)
             desired_ee_pose: Target end-effector pose as a 4x4 transformation matrix
+            position_weight: Weight for position constraint in IK
+            orientation_weight: Weight for orientation constraint in IK, set to 0.0 to only constrain position
 
         Returns:
             Joint positions in degrees that achieve the desired end-effector pose
@@ -104,7 +101,7 @@ def inverse_kinematics(self, current_joint_pos, desired_ee_pose):
         self.tip_frame.T_world_frame = desired_ee_pose
 
         # Configure the task based on position_only flag
-        self.tip_frame.configure(self.target_frame_name, "soft", 1.0, self.orientation_weight)
+        self.tip_frame.configure(self.target_frame_name, "soft", position_weight, orientation_weight)
 
         # Solve IK
         self.solver.solve(True)
diff --git a/src/lerobot/robots/so100_follower/config_so100_follower.py b/src/lerobot/robots/so100_follower/config_so100_follower.py
index defe9ce9ef..9555b3e0e5 100644
--- a/src/lerobot/robots/so100_follower/config_so100_follower.py
+++ b/src/lerobot/robots/so100_follower/config_so100_follower.py
@@ -50,9 +50,6 @@ class SO100FollowerEndEffectorConfig(SO100FollowerConfig):
     # End-effector frame name in the URDF
     target_frame_name: str = "gripperframe"
 
-    # Weight for orientation constraint in IK
-    orientation_weight: float = 0.01
-
     # Default bounds for the end-effector position (in meters)
     end_effector_bounds: dict[str, list[float]] = field(
         default_factory=lambda: {
diff --git a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
index 97db834d5a..5fe2993cb3 100644
--- a/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
+++ b/src/lerobot/robots/so100_follower/so100_follower_end_effector.py
@@ -72,7 +72,6 @@ def __init__(self, config: SO100FollowerEndEffectorConfig):
         self.kinematics = RobotKinematics(
             urdf_path=self.config.urdf_path,
             target_frame_name=self.config.target_frame_name,
-            orientation_weight=self.config.orientation_weight,
         )
 
         # Store the bounds for end-effector position

From 4c144a72b64ff500c0c5c9b53b46823563cfcde2 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Mon, 30 Jun 2025 15:30:59 +0000
Subject: [PATCH 16/21] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 src/lerobot/model/kinematics.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/lerobot/model/kinematics.py b/src/lerobot/model/kinematics.py
index f3d22abf5e..c26cafb0af 100644
--- a/src/lerobot/model/kinematics.py
+++ b/src/lerobot/model/kinematics.py
@@ -76,7 +76,9 @@ def forward_kinematics(self, robot_pos_deg):
         # Get the transformation matrix
         return self.robot.get_T_world_frame(self.target_frame_name)
 
-    def inverse_kinematics(self, current_joint_pos, desired_ee_pose, position_weight=1.0, orientation_weight=0.01):
+    def inverse_kinematics(
+        self, current_joint_pos, desired_ee_pose, position_weight=1.0, orientation_weight=0.01
+    ):
         """
         Compute inverse kinematics using placo solver.
 

From cd0a5d6fca1ce6fcb5996aca09ea38a0abcde4e2 Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Mon, 30 Jun 2025 17:48:28 +0200
Subject: [PATCH 17/21] Unifiying names in `kinematics.py` for consistency

---
 src/lerobot/model/kinematics.py | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/lerobot/model/kinematics.py b/src/lerobot/model/kinematics.py
index c26cafb0af..b2cd3fbae4 100644
--- a/src/lerobot/model/kinematics.py
+++ b/src/lerobot/model/kinematics.py
@@ -52,23 +52,23 @@ def __init__(
         # Initialize frame task for IK
         self.tip_frame = self.solver.add_frame_task(self.target_frame_name, np.eye(4))
 
-    def forward_kinematics(self, robot_pos_deg):
+    def forward_kinematics(self, joint_pos_deg):
         """
         Compute forward kinematics for given joint configuration given the target frame name in the constructor.
 
         Args:
-            robot_pos_deg: Joint positions in degrees (numpy array)
+            joint_pos_deg: Joint positions in degrees (numpy array)
 
         Returns:
             4x4 transformation matrix of the end-effector pose
         """
 
         # Convert degrees to radians
-        robot_pos_rad = np.deg2rad(robot_pos_deg[: len(self.joint_names)])
+        joint_pos_rad = np.deg2rad(joint_pos_deg[: len(self.joint_names)])
 
         # Update joint positions in placo robot
         for i, joint_name in enumerate(self.joint_names):
-            self.robot.set_joint(joint_name, robot_pos_rad[i])
+            self.robot.set_joint(joint_name, joint_pos_rad[i])
 
         # Update kinematics
         self.robot.update_kinematics()
@@ -110,19 +110,19 @@ def inverse_kinematics(
         self.robot.update_kinematics()
 
         # Extract joint positions
-        joint_positions_rad = []
+        joint_pos_rad = []
         for joint_name in self.joint_names:
             joint = self.robot.get_joint(joint_name)
-            joint_positions_rad.append(joint)
+            joint_pos_rad.append(joint)
 
         # Convert back to degrees
-        joint_positions_deg = np.rad2deg(joint_positions_rad)
+        joint_pos_deg = np.rad2deg(joint_pos_rad)
 
         # Preserve gripper position if present in current_joint_pos
         if len(current_joint_pos) > len(self.joint_names):
             result = np.zeros_like(current_joint_pos)
-            result[: len(self.joint_names)] = joint_positions_deg
+            result[: len(self.joint_names)] = joint_pos_deg
             result[len(self.joint_names) :] = current_joint_pos[len(self.joint_names) :]
             return result
         else:
-            return joint_positions_deg
+            return joint_pos_deg

From dd6687ff8adcf9d5b2a2313c4331dd05dd3e380f Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Wed, 2 Jul 2025 10:02:58 +0200
Subject: [PATCH 18/21] fix(keyboard teleop), update(end-effector-frame), added
 not for urdf path

---
 src/lerobot/model/kinematics.py                           | 2 +-
 .../robots/so100_follower/config_so100_follower.py        | 4 +++-
 src/lerobot/scripts/find_joint_limits.py                  | 2 +-
 src/lerobot/teleoperators/gamepad/gamepad_utils.py        | 8 ++++----
 src/lerobot/teleoperators/keyboard/teleop_keyboard.py     | 2 +-
 5 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/src/lerobot/model/kinematics.py b/src/lerobot/model/kinematics.py
index b2cd3fbae4..f059b97907 100644
--- a/src/lerobot/model/kinematics.py
+++ b/src/lerobot/model/kinematics.py
@@ -21,7 +21,7 @@ class RobotKinematics:
     def __init__(
         self,
         urdf_path: str,
-        target_frame_name: str = "gripperframe",
+        target_frame_name: str = "gripper_frame_link",
         joint_names: list[str] = None,
     ):
         """
diff --git a/src/lerobot/robots/so100_follower/config_so100_follower.py b/src/lerobot/robots/so100_follower/config_so100_follower.py
index 9555b3e0e5..7cd23d3409 100644
--- a/src/lerobot/robots/so100_follower/config_so100_follower.py
+++ b/src/lerobot/robots/so100_follower/config_so100_follower.py
@@ -45,10 +45,12 @@ class SO100FollowerEndEffectorConfig(SO100FollowerConfig):
     """Configuration for the SO100FollowerEndEffector robot."""
 
     # Path to URDF file for kinematics
+    # NOTE: It is highly recommended to use the urdf in the SO-ARM100 repo:
+    # https://github.com/TheRobotStudio/SO-ARM100/blob/main/Simulation/SO101/so101_new_calib.urdf
     urdf_path: str | None = None
 
     # End-effector frame name in the URDF
-    target_frame_name: str = "gripperframe"
+    target_frame_name: str = "gripper_frame_link"
 
     # Default bounds for the end-effector position (in meters)
     end_effector_bounds: dict[str, list[float]] = field(
diff --git a/src/lerobot/scripts/find_joint_limits.py b/src/lerobot/scripts/find_joint_limits.py
index fa1ad87b98..c61806d23d 100644
--- a/src/lerobot/scripts/find_joint_limits.py
+++ b/src/lerobot/scripts/find_joint_limits.py
@@ -36,6 +36,7 @@
 import draccus
 import numpy as np
 
+from lerobot.common.utils.robot_utils import busy_wait
 from lerobot.model.kinematics import RobotKinematics
 from lerobot.robots import (  # noqa: F401
     RobotConfig,
@@ -50,7 +51,6 @@
     make_teleoperator_from_config,
     so100_leader,
 )
-from lerobot.common.utils.robot_utils import busy_wait
 
 
 @dataclass
diff --git a/src/lerobot/teleoperators/gamepad/gamepad_utils.py b/src/lerobot/teleoperators/gamepad/gamepad_utils.py
index 51c6deb7b7..79c849a454 100644
--- a/src/lerobot/teleoperators/gamepad/gamepad_utils.py
+++ b/src/lerobot/teleoperators/gamepad/gamepad_utils.py
@@ -307,8 +307,8 @@ def get_deltas(self):
             z_input = 0 if abs(z_input) < self.deadzone else z_input
 
             # Calculate deltas (note: may need to invert axes depending on controller)
-            delta_x = -x_input * self.x_step_size  # Forward/backward
-            delta_y = y_input * self.y_step_size  # Left/right
+            delta_x = -y_input * self.y_step_size  # Forward/backward
+            delta_y = -x_input * self.x_step_size  # Left/right
             delta_z = -z_input * self.z_step_size  # Up/down
 
             return delta_x, delta_y, delta_z
@@ -465,8 +465,8 @@ def _update(self):
     def get_deltas(self):
         """Get the current movement deltas from gamepad state."""
         # Calculate deltas - invert as needed based on controller orientation
-        delta_x = -self.left_x * self.x_step_size  # Forward/backward
-        delta_y = self.left_y * self.y_step_size  # Left/right
+        delta_x = -self.left_y * self.y_step_size  # Forward/backward
+        delta_y = -self.left_x * self.x_step_size  # Left/right
         delta_z = -self.right_y * self.z_step_size  # Up/down
 
         return delta_x, delta_y, delta_z
diff --git a/src/lerobot/teleoperators/keyboard/teleop_keyboard.py b/src/lerobot/teleoperators/keyboard/teleop_keyboard.py
index de8e0288b8..d034982f12 100644
--- a/src/lerobot/teleoperators/keyboard/teleop_keyboard.py
+++ b/src/lerobot/teleoperators/keyboard/teleop_keyboard.py
@@ -196,6 +196,7 @@ def get_action(self) -> dict[str, Any]:
         delta_x = 0.0
         delta_y = 0.0
         delta_z = 0.0
+        gripper_action = 1.0
 
         # Generate action based on current key states
         for key, val in self.current_pressed.items():
@@ -230,7 +231,6 @@ def get_action(self) -> dict[str, Any]:
             "delta_z": delta_z,
         }
 
-        gripper_action = 1  # default gripper action is to stay
         if self.config.use_gripper:
             action_dict["gripper"] = gripper_action
 

From 6352236c6e0c705b87b1f9df50bc191e9b9cf206 Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Wed, 2 Jul 2025 14:37:26 +0200
Subject: [PATCH 19/21] reverted back key mappings for easier readability

---
 .../teleoperators/gamepad/gamepad_utils.py     | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/lerobot/teleoperators/gamepad/gamepad_utils.py b/src/lerobot/teleoperators/gamepad/gamepad_utils.py
index 79c849a454..9b62dc666e 100644
--- a/src/lerobot/teleoperators/gamepad/gamepad_utils.py
+++ b/src/lerobot/teleoperators/gamepad/gamepad_utils.py
@@ -295,8 +295,8 @@ def get_deltas(self):
         try:
             # Read joystick axes
             # Left stick X and Y (typically axes 0 and 1)
-            x_input = self.joystick.get_axis(0)  # Left/Right
-            y_input = self.joystick.get_axis(1)  # Up/Down (often inverted)
+            y_input = self.joystick.get_axis(0)  # Left/Right
+            x_input = self.joystick.get_axis(1)  # Up/Down (often inverted)
 
             # Right stick Y (typically axis 3 or 4)
             z_input = self.joystick.get_axis(3)  # Up/Down for Z
@@ -307,8 +307,8 @@ def get_deltas(self):
             z_input = 0 if abs(z_input) < self.deadzone else z_input
 
             # Calculate deltas (note: may need to invert axes depending on controller)
-            delta_x = -y_input * self.y_step_size  # Forward/backward
-            delta_y = -x_input * self.x_step_size  # Left/right
+            delta_x = -x_input * self.x_step_size  # Forward/backward
+            delta_y = -y_input * self.y_step_size  # Left/right
             delta_z = -z_input * self.z_step_size  # Up/down
 
             return delta_x, delta_y, delta_z
@@ -424,14 +424,14 @@ def _update(self):
             # These offsets are for the Logitech RumblePad 2
             if data and len(data) >= 8:
                 # Normalize joystick values from 0-255 to -1.0-1.0
-                self.left_x = (data[1] - 128) / 128.0
-                self.left_y = (data[2] - 128) / 128.0
+                self.left_y = (data[1] - 128) / 128.0
+                self.left_x = (data[2] - 128) / 128.0
                 self.right_x = (data[3] - 128) / 128.0
                 self.right_y = (data[4] - 128) / 128.0
 
                 # Apply deadzone
-                self.left_x = 0 if abs(self.left_x) < self.deadzone else self.left_x
                 self.left_y = 0 if abs(self.left_y) < self.deadzone else self.left_y
+                self.left_x = 0 if abs(self.left_x) < self.deadzone else self.left_x
                 self.right_x = 0 if abs(self.right_x) < self.deadzone else self.right_x
                 self.right_y = 0 if abs(self.right_y) < self.deadzone else self.right_y
 
@@ -465,8 +465,8 @@ def _update(self):
     def get_deltas(self):
         """Get the current movement deltas from gamepad state."""
         # Calculate deltas - invert as needed based on controller orientation
-        delta_x = -self.left_y * self.y_step_size  # Forward/backward
-        delta_y = -self.left_x * self.x_step_size  # Left/right
+        delta_x = -self.left_x * self.x_step_size  # Forward/backward
+        delta_y = -self.left_y * self.y_step_size  # Left/right
         delta_z = -self.right_y * self.z_step_size  # Up/down
 
         return delta_x, delta_y, delta_z

From cec36bd6d413eb35e9b6358a3436e2c4dfb97923 Mon Sep 17 00:00:00 2001
From: Adil Zouitine <adilzouitinegm@gmail.com>
Date: Tue, 24 Jun 2025 17:07:22 +0200
Subject: [PATCH 20/21] fix(record and preprocess): update dataset cropping and
 gym environment observation handling

- Added missing import for torch in crop_dataset_roi.py.
- Ensured tensor values in complementary_info are unsqueezed for proper handling.
- Simplified prefix assignment in gym_manipulator.py for observation spaces.
- Removed unnecessary blank lines for cleaner code.
---
 src/lerobot/scripts/rl/crop_dataset_roi.py | 7 ++++---
 src/lerobot/scripts/rl/gym_manipulator.py  | 6 +-----
 2 files changed, 5 insertions(+), 8 deletions(-)

diff --git a/src/lerobot/scripts/rl/crop_dataset_roi.py b/src/lerobot/scripts/rl/crop_dataset_roi.py
index 4cb7a3e8a8..1b7f320cfd 100644
--- a/src/lerobot/scripts/rl/crop_dataset_roi.py
+++ b/src/lerobot/scripts/rl/crop_dataset_roi.py
@@ -23,6 +23,7 @@
 import cv2
 
 # import torch.nn.functional as F  # noqa: N812
+import torch
 import torchvision.transforms.functional as F  # type: ignore  # noqa: N812
 from tqdm import tqdm  # type: ignore
 
@@ -224,7 +225,8 @@ def convert_lerobot_dataset_to_cropper_lerobot_dataset(
                 cropped = F.crop(value, top, left, height, width)
                 value = F.resize(cropped, resize_size)
                 value = value.clamp(0, 1)
-
+            if key.startswith("complementary_info") and isinstance(value, torch.Tensor) and value.dim() == 0:
+                value = value.unsqueeze(0)
             new_frame[key] = value
 
         new_dataset.add_frame(new_frame, task=task)
@@ -265,8 +267,7 @@ def convert_lerobot_dataset_to_cropper_lerobot_dataset(
     )
     parser.add_argument(
         "--push-to-hub",
-        type=bool,
-        default=False,
+        action="store_true",
         help="Whether to push the new dataset to the hub.",
     )
     parser.add_argument(
diff --git a/src/lerobot/scripts/rl/gym_manipulator.py b/src/lerobot/scripts/rl/gym_manipulator.py
index 2a611b2a48..673043b6eb 100644
--- a/src/lerobot/scripts/rl/gym_manipulator.py
+++ b/src/lerobot/scripts/rl/gym_manipulator.py
@@ -286,9 +286,7 @@ def _setup_spaces(self):
 
         # Define observation spaces for images and other states.
         if "pixels" in self.current_observation:
-            prefix = (
-                "observation.images" if len(self.current_observation["pixels"]) > 1 else "observation.image"
-            )
+            prefix = "observation.images"
             observation_spaces = {
                 f"{prefix}.{key}": gym.spaces.Box(
                     low=0, high=255, shape=self.current_observation["pixels"][key].shape, dtype=np.uint8
@@ -346,7 +344,6 @@ def reset(self, seed=None, options=None) -> tuple[dict[str, np.ndarray], dict[st
         self.episode_data = None
         self.current_observation = None
         self._get_observation()
-
         return self.current_observation, {"is_intervention": False}
 
     def step(self, action) -> tuple[dict[str, np.ndarray], float, bool, bool, dict[str, Any]]:
@@ -1877,7 +1874,6 @@ def make_robot_env(cfg: EnvConfig) -> gym.Env:
     if cfg.robot is None:
         raise ValueError("RobotConfig (cfg.robot) must be provided for gym_manipulator environment.")
     robot = make_robot_from_config(cfg.robot)
-
     teleop_device = make_teleoperator_from_config(cfg.teleop)
     teleop_device.connect()
 

From 02186c930c4b544cb1b24dd19d96e84ae090ff05 Mon Sep 17 00:00:00 2001
From: Michel Aractingi <michel.aractingi@huggingface.co>
Date: Wed, 2 Jul 2025 15:05:35 +0200
Subject: [PATCH 21/21] nit removed dead line Co-authored-by: leo-berte
 <leonardo.bertelli96@gmail.com>

---
 src/lerobot/scripts/rl/crop_dataset_roi.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/lerobot/scripts/rl/crop_dataset_roi.py b/src/lerobot/scripts/rl/crop_dataset_roi.py
index 1b7f320cfd..0b71b53634 100644
--- a/src/lerobot/scripts/rl/crop_dataset_roi.py
+++ b/src/lerobot/scripts/rl/crop_dataset_roi.py
@@ -21,8 +21,6 @@
 from typing import Dict, Tuple
 
 import cv2
-
-# import torch.nn.functional as F  # noqa: N812
 import torch
 import torchvision.transforms.functional as F  # type: ignore  # noqa: N812
 from tqdm import tqdm  # type: ignore