add weighted moving filter, assets

src/lerobot/robots/unitree_g1/assets/g1/.gitignore

@@ -0,0 +1,2 @@
+*.gv
+*.pdf

src/lerobot/robots/unitree_g1/assets/g1/README.md

@@ -0,0 +1,33 @@
+# Unitree G1 Description (URDF & MJCF)
+
+## Overview
+
+This package includes a universal humanoid robot description (URDF & MJCF) for the [Unitree G1](https://www.unitree.com/g1/), developed by [Unitree Robotics](https://www.unitree.com/).
+
+MJCF/URDF for the G1 robot:
+
+| MJCF/URDF file name           | `mode_machine` | Hip roll reduction ratio | Update status | dof#leg | dof#waist | dof#arm | dof#hand |
+| ----------------------------- | :------------: | :----------------------: | ------------- | :-----: | :-------: | :-----: | :------: |
+| `g1_23dof`                    |       1        |           14.5           | Beta          |   6*2   |     1     |   5*2   |    0     |
+| `g1_29dof`                    |       2        |           14.5           | Beta          |   6*2   |     3     |   7*2   |    0     |
+| `g1_29dof_with_hand`          |       2        |           14.5           | Beta          |   6*2   |     3     |   7*2   |   7*2    |
+| `g1_29dof_lock_waist`         |       3        |           14.5           | Beta          |   6*2   |     1     |   7*2   |    0     |
+| `g1_23dof_rev_1_0`            |       4        |           22.5           | Up-to-date    |   6*2   |     1     |   5*2   |    0     |
+| `g1_29dof_rev_1_0`            |       5        |           22.5           | Up-to-date    |   6*2   |     3     |   7*2   |    0     |
+| `g1_29dof_with_hand_rev_1_0`  |       5        |           22.5           | Up-to-date    |   6*2   |     3     |   7*2   |   7*2    |
+| `g1_29dof_lock_waist_rev_1_0` |       6        |           22.5           | Up-to-date    |   6*2   |     1     |   7*2   |    0     |
+| `g1_dual_arm`                 |       9        |           null           | Up-to-date    |    0    |     0     |   7*2   |    0     |
+
+## Visulization with [MuJoCo](https://github.com/google-deepmind/mujoco)
+
+1. Open MuJoCo Viewer
+
+   ```bash
+   pip install mujoco
+   python -m mujoco.viewer
+   ```
+
+2. Drag and drop the MJCF/URDF model file (`g1_XXX.xml`/`g1_XXX.urdf`) to the MuJoCo Viewer.
+
+## Note for teleoperate
+g1_body29_hand14 is modified from [g1_29dof_with_hand_rev_1_0](https://github.com/unitreerobotics/unitree_ros/blob/master/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf)

src/lerobot/robots/unitree_g1/config_unitree_g1.py

@@ -26,7 +26,7 @@ from ..config import RobotConfig
 class UnitreeG1Config(RobotConfig):
    # id: str = "unitree_g1"
     motion_mode: bool = False
-    simulation_mode: bool = True
+    simulation_mode: bool = False
     kp_high = 40.0
     kd_high = 3.0
     kp_low = 80.0

src/lerobot/robots/unitree_g1/robot_kinematic_processor.py

@@ -7,7 +7,6 @@ from pinocchio import casadi as cpin
 from pinocchio.visualize import MeshcatVisualizer
 import os
 import sys
-from lerobot.robots.unitree_g1.eval_robot.utils.weighted_moving_filter import WeightedMovingFilter
 
 import logging_mp
 
@@ -16,6 +15,97 @@ parent2_dir = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__
 sys.path.append(parent2_dir)
 
 
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+class WeightedMovingFilter:
+    def __init__(self, weights, data_size=14):
+        self._window_size = len(weights)
+        self._weights = np.array(weights)
+        # assert np.isclose(np.sum(self._weights), 1.0), "[WeightedMovingFilter] the sum of weights list must be 1.0!"
+        self._data_size = data_size
+        self._filtered_data = np.zeros(self._data_size)
+        self._data_queue = []
+
+    def _apply_filter(self):
+        if len(self._data_queue) < self._window_size:
+            return self._data_queue[-1]
+
+        data_array = np.array(self._data_queue)
+        temp_filtered_data = np.zeros(self._data_size)
+        for i in range(self._data_size):
+            temp_filtered_data[i] = np.convolve(data_array[:, i], self._weights, mode="valid")[-1]
+
+        return temp_filtered_data
+
+    def add_data(self, new_data):
+        assert len(new_data) == self._data_size
+
+        if len(self._data_queue) > 0 and np.array_equal(new_data, self._data_queue[-1]):
+            return  # skip duplicate data
+
+        if len(self._data_queue) >= self._window_size:
+            self._data_queue.pop(0)
+
+        self._data_queue.append(new_data)
+        self._filtered_data = self._apply_filter()
+
+    @property
+    def filtered_data(self):
+        return self._filtered_data
+
+
+def visualize_filter_comparison(filter_params, steps):
+    import time
+
+    t = np.linspace(0, 4 * np.pi, steps)
+    original_data = np.array(
+        [np.sin(t + i) + np.random.normal(0, 0.2, len(t)) for i in range(35)]
+    ).T  # sin wave with noise, shape is [len(t), 35]
+
+    plt.figure(figsize=(14, 10))
+
+    for idx, weights in enumerate(filter_params):
+        filter = WeightedMovingFilter(weights, 14)
+        data_2b_filtered = original_data.copy()
+        filtered_data = []
+
+        time1 = time.time()
+
+        for i in range(steps):
+            filter.add_data(data_2b_filtered[i][13:27])  # step i, columns 13 to 26 (total:14)
+            data_2b_filtered[i][13:27] = filter.filtered_data
+            filtered_data.append(data_2b_filtered[i])
+
+        time2 = time.time()
+        print(f"filter_params:{filter_params[idx]}, time cosume:{time2 - time1}")
+
+        filtered_data = np.array(filtered_data)
+
+        # col0 should not 2b filtered
+        plt.subplot(len(filter_params), 2, idx * 2 + 1)
+        plt.plot(filtered_data[:, 0], label=f"Filtered (Window {filter._window_size})")
+        plt.plot(original_data[:, 0], "r--", label="Original", alpha=0.5)
+        plt.title("Joint 1 - Should not to be filtered.")
+        plt.xlabel("Step")
+        plt.ylabel("Value")
+        plt.legend()
+
+        # col13 should 2b filtered
+        plt.subplot(len(filter_params), 2, idx * 2 + 2)
+        plt.plot(filtered_data[:, 13], label=f"Filtered (Window {filter._window_size})")
+        plt.plot(original_data[:, 13], "r--", label="Original", alpha=0.5)
+        plt.title(f"Joint 13 - Window {filter._window_size}, Weights {weights}")
+        plt.xlabel("Step")
+        plt.ylabel("Value")
+        plt.legend()
+
+    plt.tight_layout()
+    plt.show()
+
+
+
 class G1_29_ArmIK:
     def __init__(self, Unit_Test=False, Visualization=False):
         np.set_printoptions(precision=5, suppress=True, linewidth=200)
@@ -25,11 +115,11 @@ class G1_29_ArmIK:
 
         if not self.Unit_Test:
             self.robot = pin.RobotWrapper.BuildFromURDF(
-                "src/lerobot/robots/unitree_g1/eval_robot/assets/g1/g1_body29_hand14.urdf", "src/lerobot/robots/unitree_g1/eval_robot/assets/g1/"
+                "src/lerobot/robots/unitree_g1/assets/g1/g1_body29_hand14.urdf", "src/lerobot/robots/unitree_g1/assets/g1/"
             )
         else:
             self.robot = pin.RobotWrapper.BuildFromURDF(
-                "src/lerobot/robots/unitree_g1/eval_robot/assets/g1/g1_body29_hand14.urdf", "src/lerobot/robots/unitree_g1/eval_robot/assets/g1/"
+                "src/lerobot/robots/unitree_g1eval_robot/assets/g1/g1_body29_hand14.urdf", "src/lerobot/robots/unitree_g1/assets/g1/"
             )  # for test
 
         self.mixed_jointsToLockIDs = [