debug+新增两个获取位姿的接口(Flange&Tool)

Massage/MassageControl/MassageRobot_nova5.py

@@ -18,6 +18,7 @@ import ctypes
 libc = ctypes.CDLL("libc.so.6")
 import atexit
 from tools.Trajectory import TrajectoryInterpolator
+from scipy.spatial.transform import Slerp
 class MassageRobot:
     def __init__(self,arm_config_path,is_log=False):
         self.logger = CustomLogger(
@@ -217,16 +218,14 @@ class MassageRobot:
                     process_start_time = time.time()
                     control_cycle = self.control_rate.to_sec() 
                     t_now = time.time() - self.traj_start_time
-                    i = int(t_now/self.dt)
-                    if i >= len(self.ts) - 1:
-                        i = len(self.ts) - 2
-                    alpha = (t_now - self.ts[i])/self.dt
+                    i = min(int(t_now / self.dt), len(self.ts) - 2)
+                    alpha = max(0.0, min(1.0, (t_now - self.ts[i]) / self.dt))
 
                     pos = (1 - alpha) * self.xr[i][:3] + alpha * self.xr[i+1][:3]
-                    r1 = R.from_quat(self.xr[i][3:])
-                    r2 = R.from_quat(self.xr[i+1][3:])
-                    slerp_rot = R.slerp([0, 1], R.concatenate([r1, r2]))
-                    rot_interp = slerp_rot(alpha).as_quat()
+
+                    slerp = Slerp([0, 1], R.concatenate([R.from_quat(self.xr[i][3:]), R.from_quat(self.xr[i+1][3:])]))
+                    alpha = 0.5
+                    rot_interp = slerp(alpha).as_quat()
 
                     xr = np.concatenate([pos, rot_interp])
                     # xr = (1-alpha) * self.xr[i] + alpha * self.xr[i+1]
@@ -234,10 +233,18 @@ class MassageRobot:
                     ar = (1 - alpha) * self.ar[i] + alpha * self.ar[i + 1]
 
                     self.arm_state.desired_position = xr[:3]
-                    self.arm_state.desired_orientation = xr[3:]
+                    # self.arm_state.desired_orientation = xr[3:]
+                    # print(111)
+                    self.arm_state.desired_orientation = rot_interp
+                    # print(222)
                     self.arm_state.desired_twist = vr
                     self.arm_state.desired_acc = ar
 
+                    print("self.arm_state.desired_position",self.arm_state.desired_position)
+                    print("self.arm_state.desired_orientation",self.arm_state.desired_orientation)
+                    print("self.arm_state.desired_twist",self.arm_state.desired_twist)
+                    print("self.arm_state.desired_acc",self.arm_state.desired_acc)
+
                     self.controller_manager.step_traj(control_cycle)
                     # self.controller_manager.step(control_cycle)
                     command_pose = np.concatenate([
@@ -264,7 +271,7 @@ class MassageRobot:
                     if sleep_duration > 0:
                         time.sleep(sleep_duration)
                     print(f"real sleep:{time.time()-b2}")
-                    self.arm.dashboard.socket_dobot.sendall(tcp_command)
+                    # self.arm.dashboard.socket_dobot.sendall(tcp_command)
                     if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停
                         print("机械臂为暂停状态")
                         res = self.arm.dashboard.Continue()
@@ -329,7 +336,7 @@ class MassageRobot:
             # 线程
             self.exit_event.clear()
             self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop)
-            self.arm_control_thread = threading.Thread(target=self.arm_command_loop)
+            self.arm_control_thread = threading.Thread(target=self.arm_command_loop_traj)
             # 线程启动
             self.arm_measure_thread.start() ## 测量线程
             position,quat_rot = self.arm.get_arm_position()
@@ -437,19 +444,19 @@ class MassageRobot:
 
 if __name__ == "__main__":
     waypoints = [
-        {"time": 0.0, "position": [0, 0, 0], "velocity": [0, 0, 0],
+        {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0],
          "acceleration": [0, 0, 0],
          "orientation": R.from_euler("xyz", [0, 0, 0], degrees=True).as_quat()},
-        {"time": 2.0, "position": [1, 2, 0], "velocity": [0, 0, 0],
+        {"time": 5.0, "position": [0.30, -0.135, 0.3043], "velocity": [0, 0, 0],
          "acceleration": [0, 0, 0],
-         "orientation": R.from_euler("xyz", [0, 45, 90], degrees=True).as_quat()},
-    ]
+         "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
+    ] ## 单位 m deg
     myInterpolate = TrajectoryInterpolator(waypoints)
-    ts = np.linspace(0,2,100)
+    ts = np.linspace(0,5,100)
     robot = MassageRobot(arm_config_path="/home/jsfb/jsfb/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml")
     xr_list, vr_list, ar_list = [], [], []
     for t in ts:
-        xr, vr, ar = myInterpolate.interpolate()  
+        xr, vr, ar = myInterpolate.interpolate(t)  
         xr_list.append(xr)
         vr_list.append(vr)
         ar_list.append(ar)  
@@ -479,8 +486,8 @@ if __name__ == "__main__":
     
     robot.init_hardwares(ready_pose)
     time.sleep(3)
-    # robot.controller_manager.switch_controller('admittance')
-    robot.controller_manager.switch_controller('position')
+    robot.controller_manager.switch_controller('admittance')
+    # robot.controller_manager.switch_controller('position')
     
     
     

Massage/MassageControl/algorithms/admittance_controller.py

@@ -85,11 +85,19 @@ class AdmittanceController(BaseController):
         arm_ori_quat = R.from_quat(self.state.arm_orientation)
         arm_ori_mat = arm_ori_quat.as_matrix()
         # 位置误差
+        
         temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.desired_twist[:3] * dt
         self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
+
         # 姿态误差(四元数)
-        rot_err_quat = (R.from_quat(self.state.desired_twist[3:] * dt)).inv() * arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
+        angular_velocity = np.array(self.state.desired_twist[3:])  # 形状 (3,)
+
+        # 用旋转向量（小角度近似）
+        rotvec = angular_velocity * dt  # 旋转向量 = 角速度 × 时间
+        rot_quat = R.from_rotvec(rotvec).as_quat()  # 转成四元数，形状 (4,)
+        rot_err_quat = R.from_quat(rot_quat).inv() * arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
         self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
+
         # 期望加速度
         wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
         D_vel = self.D @ (self.state.arm_desired_twist - self.state.desired_twist + self.state.desired_acc*dt)

Massage/MassageControl/hardware/dobot_nova5.py

@@ -575,7 +575,7 @@ class dobot_nova5:
         print("res:",res)
         angle = [float(x.strip()) for x in angle_str.split(",")]
         return angle
-
+    
     def getForwardKin(self,joints_list,user=-1,tool=-1):
         res = self.dashboard.PositiveKin(*joints_list,user,tool)
         code = self.parseResultId(res)[0]
@@ -729,9 +729,41 @@ class dobot_nova5:
         else:
             print("机械臂初始化失败")
 
-        
+    def get_pose_at_flange(self):
+        '''
+        输出pos,quat
+        pos: [x,y,z] in m
+        quat: [qx,qy,qz,qw] 四元数
+        '''
+        pose = self.getPose(user=0, tool=0) # 获取初始法兰坐标系下的位姿
+        x,y,z,rx,ry,rz = pose # x y z in mm; rx ry rz in degree
+        x /= 1000
+        y /= 1000
+        z /= 1000
+        pos = np.array([x,y,z])
+        quat_rot = R.from_euler('xyz', [rx, ry, rz], degrees=True).as_quat()
+        return pos,quat_rot
+    
+    def get_pose_at_tool(self,tool=1):
+        '''
+        工具默认为1
+        输出pos,quat
+        pos: [x,y,z] in m
+        quat: [qx,qy,qz,qw] 四元数
+        '''
+        pose = self.getPose(user=0, tool=tool)
+        x,y,z,rx,ry,rz = pose # x y z in mm; rx ry rz in degree
+        x /= 1000
+        y /= 1000
+        z /= 1000
+        pos = np.array([x,y,z])
+        quat_rot = R.from_euler('xyz', [rx, ry, rz], degrees=True).as_quat()
+        return pos,quat_rot
 
     def get_arm_position(self):
+        '''
+        直接获得当前user/tool坐标系下的机械臂位置
+        '''
         '''
         输出pos,quat
         pos: [x,y,z] in m

Massage/MassageControl/tools/Trajectory.py

@@ -90,7 +90,7 @@ class TrajectoryInterpolator:
             return np.zeros(3)
         return axis / dt  # rotvec 本身是 axis * angle
 
-    def _estimate_angular_derivatives(self, t, delta=0.03):
+    def _estimate_angular_derivatives(self, t, delta=0.08):
         """数值估计角速度和角加速度，避免递归调用"""
         if t <= self.times[0] or t >= self.times[-1]:
             return np.zeros(3), np.zeros(3)
@@ -119,16 +119,16 @@ if __name__ == "__main__":
     import matplotlib.pyplot as plt
 
     waypoints = [
-        {"time": 0.0, "position": [0, 0, 0], "velocity": [0, 0, 0],
+        {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0],
          "acceleration": [0, 0, 0],
          "orientation": R.from_euler("xyz", [0, 0, 0], degrees=True).as_quat()},
-        {"time": 2.0, "position": [1, 2, 0], "velocity": [0, 0, 0],
+        {"time": 5.0, "position": [0.30, -0.135, 0.3043], "velocity": [0, 0, 0],
          "acceleration": [0, 0, 0],
-         "orientation": R.from_euler("xyz", [0, 45, 90], degrees=True).as_quat()},
-    ]
+         "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
+    ] ## 单位 m deg
 
     interpolator = TrajectoryInterpolator(waypoints)
-    ts = np.linspace(0, 2, 100)
+    ts = np.linspace(0, 5, 100)
 
     positions = []
     velocities = []