From bc351a0134b09ac335842fc0b25f3807ec00690b Mon Sep 17 00:00:00 2001
From: yutang <yt.li2@siat.ac.cn>
Date: Thu, 19 Jun 2025 14:55:54 +0800
Subject: [PATCH] change pose control api to canfd

---
 .../common/robot_devices/robots/realman.py    | 48 +++++++++++++++++--
 1 file changed, 45 insertions(+), 3 deletions(-)

diff --git a/lerobot/common/robot_devices/robots/realman.py b/lerobot/common/robot_devices/robots/realman.py
index 72719505e..37b9466ac 100644
--- a/lerobot/common/robot_devices/robots/realman.py
+++ b/lerobot/common/robot_devices/robots/realman.py
@@ -13,6 +13,46 @@ from lerobot.common.robot_devices.cameras.utils import make_cameras_from_configs
 from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
 from lerobot.common.robot_devices.robots.configs import RealmanRobotConfig
 
+
+def smooth_movep_canfd(arm_controller, current_pose, target_pose, fps=100):
+    """
+    可变帧率版本
+    
+    Args:
+        fps: 目标帧率，决定每次移动的时长
+    """
+    total_duration = 1.0 / fps
+    control_dt = 0.002  # 2ms控制周期
+    num_points = max(1, int(total_duration / control_dt))
+    
+    print(f"{fps}fps移动: {total_duration*1000:.1f}ms, {num_points}个插值点")
+    
+    # 生成轨迹
+    trajectory = []
+    for i in range(num_points):
+        t = i / (num_points - 1) if num_points > 1 else 1.0
+        s = 6 * t**5 - 15 * t**4 + 10 * t**3  # S曲线
+        
+        pose = []
+        for axis in range(6):
+            value = current_pose[axis] + s * (target_pose[axis] - current_pose[axis])
+            pose.append(value)
+        trajectory.append(pose)
+    
+    # 执行
+    start_time = time.perf_counter()
+    for i, pose in enumerate(trajectory):
+        target_time = start_time + i * control_dt
+        current_time = time.perf_counter()
+        
+        sleep_time = target_time - current_time
+        # if sleep_time > 0:
+            # time.sleep(sleep_time)
+        
+        arm_controller.rmarm.rm_movep_canfd(pose, True)
+    
+    return True
+
 class RealmanRobot:
     def __init__(self, config: RealmanRobotConfig | None = None, **kwargs):
         if config is None:
@@ -189,10 +229,12 @@ class RealmanRobot:
             
             # result = self.arm.rmarm.rm_movej_p(target_pose, 100, 0, 0, 0)
             if self.last_endpose != target_pose:
-                self.arm.rmarm.rm_movep_follow(target_pose)
+                # self.arm.rmarm.rm_movep_follow(target_pose)
+                # self.arm.rmarm.rm_movep_canfd(target_pose, True)
+                self.arm.rmarm.rm_movep_canfd(target_pose, False)
+                self.last_endpose = target_pose
             self.arm.rmarm.rm_set_gripper_position(action['gripper'], False, 2)
-            self.last_endpose = target_pose
-
+            
             ret = self.arm.rmarm.rm_get_current_arm_state()
             target_joints = ret[1].get('joint', self.arm.init_joint_position)
             self.joint_queue.append(list(self.arm.read().values()))