Improve velocity scaling (#11)

* Add LeRobot integration

* fix scaling

* increase accuracy

* version

* add joint dict to forward kinematics

Author: lukicdarkoo

README.md

@@ -20,7 +20,7 @@ The web application leverages the WebXR API, which combines your phone’s senso
 The package is available on [PyPI](https://pypi.org/project/teleop/). You can install it using pip:
 
 ```bash
-pip3 install teleop
+pip install teleop
 ```
 
 ## Usage
@@ -32,7 +32,7 @@ We provide some ready-to-use robot arm interfaces, but you can also create your
 A simple interface that prints the teleop responses. You can use it as a reference to build your own interface.
 
 ```bash
-python3 -m teleop.basic
+python -m teleop.basic
 ```
 
 ### xArm
@@ -41,7 +41,7 @@ Interface to teleoperate the [uFactory Lite 6](https://www.ufactory.cc/lite-6-co
 Minor changes are probably necessary to support other xArm robots.
 
 ```bash
-python3 -m teleop.xarm
+python -m teleop.xarm
 ```
 
 Note that the interface is very simple, it doesn't implement any kind of filtering.
@@ -53,7 +53,7 @@ Smart phones are typically 30fps while VR joysticks 90fps which is much more pre
 The ROS 2 interface is designed primarily for use with the [cartesian_controllers](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers) package, but it can also be adapted for [MoveIt Servo](https://moveit.picknik.ai/main/doc/examples/realtime_servo/realtime_servo_tutorial.html) or other packages.
 
 ```bash
-python3 -m teleop.ros2
+python -m teleop.ros2
 ```
 
 **Published topics:**
@@ -66,7 +66,7 @@ python3 -m teleop.ros2
 You can override the default topic names using standard ROS 2 arguments:
 
 ```bash
-python3 -m teleop.ros2 --ros-args -r target_frame:=/some_other_topic_name
+python -m teleop.ros2 --ros-args -r target_frame:=/some_other_topic_name
 ```
 
 ### ROS 2 Interface with IK
@@ -76,15 +76,15 @@ No servoing support, no problem.
 
 Panda arm usage example:
 ```bash
-python3 -m teleop.ros2_ik \
+python -m teleop.ros2_ik \
   --joint-names panda_joint1 panda_joint2 panda_joint3 panda_joint4 panda_joint5 panda_joint6 panda_joint7 \
   --ee-link panda_hand \
   --ros-args -r /joint_trajectory:=/panda_arm_controller/joint_trajectory
 ```
 
 xArm usage example:
 ```bash
-python3 -m teleop.ros2_ik \
+python -m teleop.ros2_ik \
   --joint-names joint1 joint2 joint3 joint4 joint5 joint6 \
   --ee-link link6 \
   --ros-args -r /joint_trajectory:=/joint_trajectory_controller/joint_trajectory
@@ -191,8 +191,8 @@ If you’d like to contribute, install the package in editable mode:
 # Install the package in editable mode
 git clone https://github.com/SpesRobotics/teleop.git
 cd teleop
-pip3 install -e .
+pip install -e .
 
 # Run the tests
-python3 -m pytest
+python -m pytest
 ```

setup.py

@@ -16,7 +16,7 @@
 
 setup(
     name="teleop",
-    version="0.1.1",
+    version="0.1.2",
     packages=["teleop", "teleop.basic", "teleop.ros2", "teleop.utils", "teleop.ros2_ik", "teleop.xarm"],
     long_description=long_description,
     long_description_content_type="text/markdown",

teleop/__init__.py

@@ -260,12 +260,16 @@ def __update(self, message):
         relative_orientation = received_pose[:3, :3] @ np.linalg.inv(
             self.__relative_pose_init[:3, :3]
         )
+
+        if scale > 1.0:
+            relative_position *= scale
+
         self.__pose = np.eye(4)
         self.__pose[:3, 3] = self.__absolute_pose_init[:3, 3] + relative_position
         self.__pose[:3, :3] = relative_orientation @ self.__absolute_pose_init[:3, :3]
 
         # Apply scale
-        if scale != 1.0:
+        if scale < 1.0:
             self.__pose = interpolate_transforms(
                 self.__absolute_pose_init, self.__pose, scale
             )

teleop/teleop-ui.js

@@ -186,6 +186,21 @@ class TeleopUI extends HTMLElement {
                     .info-box {
                         flex: 1;
                     }
+
+                    .auxilary-section {
+                        display: flex;
+                        flex-direction: row;
+                        justify-content: center;
+                        gap: 20px;
+                    }
+
+                    .scale-section {
+                        width: 300px;
+                    }
+
+                    .reserved-section {
+                        width: 200px;
+                    }
                 }
             </style>
             
@@ -207,16 +222,18 @@ class TeleopUI extends HTMLElement {
                     </div>
                 </div>
                 
+                <div class="auxilary-section">
                 <div class="scale-section">
                     <input type="range" class="scale-slider" id="scaleSlider" 
-                        min="0" max="3" step="1" value="3">
+                        min="0" max="5" step="1" value="3">
                     <div class="scale-value" id="scaleValue">scale 1.0</div>
                 </div>
 
                 <div class="reserved-section">
                     <div class="reserved-button" id="reservedButtonA">A</div>
                     <div class="reserved-button" id="reservedButtonB">B</div>
                 </div>
+                </div>
 
                 <div class="controls">
                     <button class="control-button gripper-button" id="gripperButton">
@@ -246,7 +263,7 @@ class TeleopUI extends HTMLElement {
 
         // Scale slider
         scaleSlider.addEventListener('input', (event) => {
-            const sliderValues = [0.1, 0.25, 0.5, 1.0];
+            const sliderValues = [0.1, 0.25, 0.5, 1.0, 2.0, 4.0];
             this.sliderValue = sliderValues[event.target.value];
             this.shadowRoot.getElementById('scaleValue').textContent = `scale scale ${this.sliderValue.toFixed(2)}`;
 

teleop/utils/jacobi_robot.py

@@ -55,6 +55,7 @@ def __init__(
         # Load robot model
         self.model = pin.buildModelFromUrdf(urdf_path)
         self.data = self.model.createData()
+        self.data_tmp = self.model.createData()
 
         # Get end-effector frame ID
         try:
@@ -87,10 +88,6 @@ def __init__(
         self.kp_pos = 1.0  # Position gain (reduced from 5.0)
         self.kp_ori = 0.5  # Orientation gain (reduced from 3.0)
         self.kd = 0.05  # Damping gain
-
-        # IK solver parameters
-        self.eps = 1e-4
-        self.max_iter = 100
         self.damping = 1e-4  # Increased damping for stability
 
         # IK regularization parameters
@@ -118,12 +115,27 @@ def __update_kinematics(self):
         pin.framesForwardKinematics(self.model, self.data, self.q)
         pin.computeJointJacobians(self.model, self.data, self.q)
 
-    def get_ee_pose(self) -> np.ndarray:
+    def get_ee_pose(self, joint_positions_dict: dict = None) -> np.ndarray:
         """Get current end-effector pose as 4x4 transformation matrix."""
+        if joint_positions_dict is not None:
+            return self._get_ee_pose_given_positions(joint_positions_dict)
         self.__update_kinematics()
         se3_pose = self.data.oMf[self.ee_frame_id]
         return se3_to_matrix(se3_pose)
 
+    def _get_ee_pose_given_positions(self, joint_positions_dict: dict) -> np.ndarray:
+        joint_positions = np.zeros(self.model.nq)
+        for joint_name, position in joint_positions_dict.items():
+            joint_index = self.__get_joint_index(joint_name)
+            if joint_index < 0 or joint_index >= self.model.nq:
+                raise ValueError(f"Joint '{joint_name}' not found in model.")
+            joint_positions[joint_index] = position
+
+        pin.forwardKinematics(self.model, self.data_tmp, joint_positions)
+        pin.framesForwardKinematics(self.model, self.data_tmp, joint_positions)
+        se3_pose = self.data_tmp.oMf[self.ee_frame_id]
+        return se3_to_matrix(se3_pose)
+
     def __get_ee_velocity(self) -> np.ndarray:
         """Get current end-effector velocity."""
         self.__update_kinematics()
@@ -175,8 +187,8 @@ def servo_to_pose(
         self,
         target_pose: np.ndarray,
         dt: float = 0.01,
-        linear_tol: float = 0.005,
-        angular_tol: float = 0.01,
+        linear_tol: float = 0.0005,
+        angular_tol: float = 0.005,
     ) -> bool:
         """
         Compute joint velocities for servoing to target pose with velocity/acceleration limits.
@@ -214,7 +226,7 @@ def servo_to_pose(
             linear_vel_norm = np.linalg.norm(desired_linear_vel)
             if linear_vel_norm > 0 and linear_vel_norm < self.min_linear_vel:
                 desired_linear_vel = desired_linear_vel * (
-                    self.min_angular_vel / linear_vel_norm
+                    self.min_linear_vel / linear_vel_norm
                 )
 
         if angular_error_norm > angular_tol:
@@ -572,6 +584,20 @@ def set_regularization_params(
             urdf_path, ee_link="link6", max_linear_vel=0.05, max_angular_vel=0.2
         )  # Reduced limits
 
+        print(
+            "EEF pose",
+            robot.get_ee_pose(
+                {
+                    "joint1": 0.0,
+                    "joint2": 0.0,
+                    "joint3": 0.0,
+                    "joint4": 0.0,
+                    "joint5": 0.0,
+                    "joint6": 0.0,
+                }
+            ),
+        )
+
         # Start visualization
         robot.start_visualization()