diff --git a/Massage/MassageControl/algorithms/admittance_controller.py b/Massage/MassageControl/algorithms/admittance_controller.py index b010858..9443c75 100644 --- a/Massage/MassageControl/algorithms/admittance_controller.py +++ b/Massage/MassageControl/algorithms/admittance_controller.py @@ -54,9 +54,9 @@ class AdmittanceController(BaseController): self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) # 期望加速度 wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench - KD_vel = self.D @ (self.state.arm_desired_twist - self.state.desired_twist) - KD_pose = self.K @ self.state.pose_error - self.state.arm_desired_acc = self.M_inv @ (wrench_err - KD_vel - KD_pose) + D_vel = self.D @ (self.state.arm_desired_twist - self.state.desired_twist) + K_pose = self.K @ self.state.pose_error + self.state.arm_desired_acc = self.M_inv @ (wrench_err - D_vel - K_pose) self.clip_command(self.state.arm_desired_acc, "acc") ## 更新速度和位姿 self.state.arm_desired_twist += self.state.arm_desired_acc * dt diff --git a/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc index 1e114cc..047fd18 100644 Binary files a/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc and b/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc differ diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py index a476d9f..721c927 100644 --- a/Massage/MassageControl/hardware/dobot_nova5.py +++ b/Massage/MassageControl/hardware/dobot_nova5.py @@ -812,33 +812,10 @@ if __name__ == "__main__": sleep(1) dobot.setEndEffector(i=9,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}") dobot.chooseEndEffector(i=9) - # print(dobot.getPose()) - # print(dobot.get_arm_position()) - # pos,rot_quat = dobot.get_arm_position() - # print("pos",pos) - # print("rot_quat",rot_quat) - - # while(True): - # print("反馈角度",dobot.feedbackData.QActual) - # print("反馈四元数",dobot.feedbackData.ActualQuaternion) - # print("反馈TCP笛卡尔实际坐标值",dobot.feedbackData.ToolVectorActual) - # sleep(0.008) - - dobot.Get_feedInfo_angle() - sleep(0.008) - dobot.Get_feedInfo_pose() - sleep(0.5) - - cur = dobot.Get_feedInfo_angle() # 参考关节角度 - short_cur = [f"{x:.3f}" for x in cur] - - target = dobot.getInverseKin([250.000045,-134.999537,344.115807,-179.999982,-0.000408,-90.000642],-1,-1,1,"{" + ",".join(map(str, short_cur)) + "}") - print(target) - # for i in range(400): - # posJ[2] += 0.04 - # dobot.ServoJoint(posJ,t=0.008,aheadtime=20,gain=200) - # sleep(0.007) - + print("Arm pose:",dobot.getPose()) + + # dobot.RunPoint_P_inPose([, 0, 154.071, 0, 0, 0]) + # dobot.start_drag() dobot.disableRobot() \ No newline at end of file diff --git a/Massage/MassageControl/tools/auto_visual_valibration.py b/Massage/MassageControl/tools/auto_visual_valibration.py new file mode 100644 index 0000000..f3db12b --- /dev/null +++ b/Massage/MassageControl/tools/auto_visual_valibration.py @@ -0,0 +1,260 @@ +import cv2 +import os +from datetime import datetime +import numpy as np +import sys +from pathlib import Path +import time +from scipy.spatial.transform import Rotation as R + +sys.path.append(str(Path(__file__).resolve().parent.parent)) +sys.path.append('/home/jsfb/jsfb_ws/MassageRobot_aubo/Massage/MassageControl') +from hardware.remote_cam import ToolCamera +from hardware.aubo_C5 import AuboC5 + +np.set_printoptions(precision=8, suppress=True) +# 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001 +criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001) + + +# 标定计算类 +# -- 连接AuboC5 +# -- 连接 ToolCamera +# -- 运动到初始位置 计算下一步运动位姿 +# -- 开始手眼标定 +# -- 输出结果 +class Calibration: + def __init__(self, board_size, square_size, file_folder): + """ + 初始化标定计算类。 + board_size: 标定板的格子数，格式为 (width, height) + square_size: 格子的大小，单位为米 + """ + self.board_size = board_size + self.square_size = square_size + self.file_address = file_folder + self.objp = self._create_object_points() + self.image_points = [] + self.robot_poses = [] + self.cam = ToolCamera(host='127.0.0.1') + self.cam.start() + self.size = None + self.intrinsics_mtx = None + self.disorder = None + rgb_image, depth_image, camera_intrinsics = self.cam.get_latest_frame() + if rgb_image is None : + print(f'===============相机连接失败请检查并重试==============') + else: + print(f'===============相机连接成功==============') + self.size = rgb_image.shape[:2] + self.intrinsics = camera_intrinsics + + self.arm = AuboC5() + self.arm.init() + self.obj_points = [] + print(f'===============机械臂连接成功==============') + + def _create_object_points(self): + """创建标定板的3D点""" + objp = np.zeros((self.board_size[0] * self.board_size[1], 3), np.float32) + objp[:, :2] = np.mgrid[0:self.board_size[0], 0:self.board_size[1]].T.reshape(-1, 2) * self.square_size + return objp + + def collect_data(self): + #移动到初始位置 + pose = [(90) * (np.pi / 180), 4.23 * (np.pi / 180), 124.33 * (np.pi / 180), 61.56 * (np.pi / 180), -89.61 * (np.pi / 180), 0 * (np.pi / 180)] + code = self.arm.move_joint(pose,4,wait=True) + if code == -1: + print("运动到拍照位置失败") + #拍照 + self.check_corners() + #运动到下一个位置 + # 目前只设计了44条轨迹 + for i in range (0,45): + next_pose = self.get_next_pose_ugly(i) + self.arm.move_joint(next_pose, 4,wait=True) + if code == -1: + print("运动到拍照位置失败") + else: + self.check_corners() + + def check_corners(self): + rgb, depth, intrinsics = self.cam.get_latest_frame() + gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY) + ret, corners = cv2.findChessboardCorners(gray, self.board_size, None) + if ret: + robot_pose = self.get_pose_homomat() + self.add_sample(gray,corners,robot_pose) + print(f"robot_pose = {robot_pose}") + + pic_name = self.file_address + "/"+str(len(self.image_points)) + ".jpg" + print(f"save path as: {pic_name}") + # 绘制检测到的角点 (用于可视化，可选) + cv2.drawChessboardCorners(rgb, self.board_size, corners, ret) + cv2.imwrite(pic_name, rgb) + + return True + return False + + + def add_sample(self, gray, corners,robot_pose): + """ + 添加标定样本。 + image: 捕获的图像 + robot_pose: 机械臂的位姿，格式为 [x, y, z, roll, pitch, yaw] + """ + # 检测棋盘角点 + + self.obj_points.append(self.objp) + corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria) # 在原角点的基础上寻找亚像素角点 + if [corners2]: + self.image_points.append(corners2) + else: + self.image_points.append(corners) + + self.robot_poses.append(robot_pose) + + def get_pose_homomat(self): + pose = self.arm.robot_rpc_client.getRobotInterface(self.arm.robot_name).getRobotState().getTcpPose() + x, y, z, rx, ry, rz = pose + r = R.from_euler('xyz', [rx, ry, rz]) + rotation_matrix = r.as_matrix() + translation_vector = np.array([x, y, z]).reshape(3, 1) + homo_mat = np.eye(4) # 创建 4x4 单位矩阵 + homo_mat[:3, :3] = rotation_matrix + homo_mat[:3, 3] = translation_vector.flatten() + return homo_mat + + def camera_calib(self): + # print(f"size is {self.size}") + ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(self.obj_points, self.image_points, (self.size[1], self.size[0]), None, None) + # print("rvecs,tvecs:",rvecs,tvecs) + if ret: + print("内参矩阵:\n", mtx) # 内参数矩阵 + print("畸变系数:\n", dist) # 畸变系数 distortion cofficients = (k_1,k_2,p_1,p_2,k_3) + print("++++++++++相机标定完成++++++++++++++") + self.intrinsics_mtx = mtx + self.disorder = dist + else: + print("-----------相机标定失败--------------") + return rvecs, tvecs + + def split_robot_pose(self): + """ + 将包含 4x4 齐次矩阵的 robot_pose 列表拆分成 R_arm 和 t_arm。 + Args: + robot_pose: 包含 4x4 齐次矩阵的 Python list。 + Returns: + R_arm: 旋转矩阵的 Python list, 每个元素是 3x3 numpy 数组。 + t_arm: 平移向量的 Python list, 每个元素是 3x1 numpy 数组。 + """ + R_arm = [] + t_arm = [] + + for pose_matrix in self.robot_poses: + # # 1. 提取旋转矩阵 (3x3) + rotation_matrix = pose_matrix[:3, :3] + R_arm.append(rotation_matrix) + # R_arm.append(pose_matrix[:3]) + + + # 2. 提取平移向量 (3x1) + translation_vector = pose_matrix[:3, 3] # 获取齐次矩阵的平移部分 (1x3 numpy 数组) + translation_vector = translation_vector.reshape(3, 1) # 转换为 3x1 的列向量 + t_arm.append(translation_vector) + + return R_arm, t_arm + + def calibrate(self): + rvecs, tvecs = self.camera_calib() + r_arm, t_arm= self.split_robot_pose() + rvecs_array = np.array(rvecs) + tvecs_array = np.array(tvecs) + t_arma = np.array(t_arm) + r_arma = np.array(r_arm) + print(rvecs_array.shape) + print(tvecs_array.shape) + print(t_arma.shape) + print(r_arma.shape) + R, t = cv2.calibrateHandEye(r_arma, t_arma, rvecs_array, tvecs_array, cv2.CALIB_HAND_EYE_TSAI) + print("+++++++++++手眼标定完成+++++++++++++++") + return R, t ,self.intrinsics_mtx + + #得到图片以后 根据pose判断方向： + def get_next_pose_future(self): + return None + def get_next_pose_ugly(self,index): + poselist =[ + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.5116296651522887, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.4592697875924587, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0220648099678793, -1.5116296651522887, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080494, -1.4592697875924587, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9173450548482196, -1.4069099100326288, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.6163494202719486, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.7210691753916085, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.4941763726323454, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.424363202552572, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.3545500324727988, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.2847368623930255, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.633802712791892, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.7036158828716652, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], + [1.5707963267948966, 0.06510078109938851, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0], + [1.5707963267948966, 0.05637413483941686, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0], + [1.5707963267948966, 0.047647488579445216, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], + [1.562069680534925, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0], + [1.5533430342749535, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0], + [1.544616388014982, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0], + [1.5358897417550104, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0], + [1.5271630954950388, 0.07382742735936015, 2.16996785900455, 0.8998917623282761, -1.2149236923132523, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], + [1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.5988961277520053, 0.0], + [1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.633802712791892, 0.0], + [1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0], + [1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.7036158828716652, 0.0], + [1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.2489576127271425, -1.7385224679115518, 0.0], + [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], + [1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.581442835232062, 0.0], + [1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.5988961277520053, 0.0], + [1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.6163494202719486, 0.0], + [1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.283864197767029, -1.633802712791892, 0.0], + [1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.336224075326859, -1.6512560053118353, 0.0] + ] + return poselist[index] + + + +if __name__ == '__main__': + + # 标定板的width 对应的角点的个数 6 + # 标定板的height 对应的角点的个数 3 + calibration_board_size = [6,3] + calibration_board_square_size = 0.027 #unit - meter + systemPath = "/home/jsfb/Documents/" + + now = datetime.now() + date_time_str = now.strftime("%d_%H_%M") + folder_name = f"calibration_data_{date_time_str}" + systemPath+=folder_name + os.makedirs(systemPath, exist_ok=True) + + calib = Calibration(calibration_board_size,calibration_board_square_size,systemPath) + calib.collect_data() + R, t,intrin = calib.calibrate() + print("旋转矩阵：") + print(R) + print("平移向量：") + print(t) + print(f"内参： {intrin}") diff --git a/Massage/MassageControl/tools/full_auto_visual_calibration.py b/Massage/MassageControl/tools/full_auto_visual_calibration.py new file mode 100644 index 0000000..c920873 --- /dev/null +++ b/Massage/MassageControl/tools/full_auto_visual_calibration.py @@ -0,0 +1,550 @@ +import cv2 +import os +from datetime import datetime +import math +import numpy as np +import sys +import random +from pathlib import Path +import time +from scipy.spatial.transform import Rotation as R +from scipy.interpolate import CubicSpline + +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D + +sys.path.append(str(Path(__file__).resolve().parent.parent)) +sys.path.append('/home/jsfb/jsfb_ws/MassageRobot_aubo/Massage/MassageControl') +from hardware.remote_cam import ToolCamera +from hardware.aubo_C5 import AuboC5 + +np.set_printoptions(precision=8, suppress=True) +# 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001 +criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001) + + +# 标定计算类 +# -- 连接AuboC5 +# -- 连接 ToolCamera +# -- 运动到初始位置 计算下一步运动位姿 +# -- 开始手眼标定 +# -- 输出结果 +class Calibration: + def __init__(self, board_size, square_size, file_folder): + """ + 初始化标定计算类。 + board_size: 标定板的格子数，格式为 (width, height) + square_size: 格子的大小，单位为米 + """ + self.board_size = board_size + self.square_size = square_size + self.objp = self._create_object_points() + self.image_points = [] + self.robot_poses = [] + self.poselist = [] + self.cam = ToolCamera(host='127.0.0.1') + self.cam.start() + self.file_address = file_folder + rgb_image, depth_image, camera_intrinsics = self.cam.get_latest_frame() + cv2.imshow("rgb1",rgb_image) + cv2.waitKey(0) + if rgb_image is None : + print(f'===============相机连接失败请检查并重试==============') + else: + print(f'===============相机连接成功==============') + self.size = rgb_image.shape[:2] + self.intrinsics = camera_intrinsics + + self.arm = AuboC5() + self.arm.init() + self.obj_points = [] + print(f'===============机械臂连接成功==============') + + def _create_object_points(self): + """创建标定板的3D点""" + objp = np.zeros((self.board_size[0] * self.board_size[1], 3), np.float32) + objp[:, :2] = np.mgrid[0:self.board_size[0], 0:self.board_size[1]].T.reshape(-1, 2) * self.square_size + return objp + + def collect_data(self): + #移动到初始位置 + # move = [(90) * (np.pi / 180), 4.23 * (np.pi / 180), 124.33 * (np.pi / 180), 61.56 * (np.pi / 180), -89.61 * (np.pi / 180), 0 * (np.pi / 180)] + move = [(89.74) * (np.pi / 180), 11.21 * (np.pi / 180), 97.12 * (np.pi / 180), 29.06 * (np.pi / 180), -89.61 * (np.pi / 180), 0 * (np.pi / 180)] + + code = self.arm.move_joint(move,4,wait=True) + if code == -1: + print("运动到拍照位置失败") + # time.sleep(3) + #拍照 + rgb, depth, intrinsics = self.cam.get_latest_frame() + cv2.imshow("rgb2",rgb) + cv2.waitKey(0) + + if self.check_corners_add_sample() is False: + print("初始位置下没有找到标定板") + rgb, depth, intrinsics = self.cam.get_latest_frame() + pose = self.arm.robot_rpc_client.getRobotInterface(self.arm.robot_name).getRobotState().getTcpPose() + cv2.imshow("not right",rgb) + cv2.waitKey(0) + + + rgb, depth, intrinsics = self.cam.get_latest_frame() + gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY) + ret, corners = cv2.findChessboardCorners(gray, self.board_size, None) + robot_pose = self.get_pose_homomat() + + print(f"EE pose:{robot_pose}") + + + center = self.init_chessboard_center(rgb) + print(f"current center EE: {center}") + + + ''' Z轴抖动 ''' + # EE坐标系下的点 + trajectory, rpy_values = self.generate_cyclic_trajectory_above_center(center) + self.visualization(center,trajectory,rpy_values) + homo_trajectory = np.hstack((trajectory, np.ones((len(trajectory), 1)))) + tcp_pose = self.get_pose_homomat() + # 把生成的trajectory 移动到 世界坐标下 + trajectory = (tcp_pose @ homo_trajectory.T).T[:, :3] + rpy_angles = [] + for tool_point in trajectory: + vector_center_to_tool = tool_point - center + roll, pitch, yaw = self.get_roll_pitch_yaw(vector_center_to_tool) + rpy_angles.append((roll, pitch, yaw)) + rpy_angles = np.array(rpy_angles) + + pose = self.concatenate_path_and_rpy(trajectory,rpy_angles ) + + #世界坐标系下的中心 + center_homo = np.append(center, 1).reshape(4, 1) + tcp_pose = self.get_pose_homomat() + center_world_homo = tcp_pose @ center_homo # 4x4 *4x1 =4x1 + center_world = center_world_homo[:3].flatten() + + print(f"world center: {center_world} \n top pose:\n {pose[:5]}") + + lenOfResult = self.traversal_position_direction(pose) + + ''' Z轴抖动 ''' + + assert lenOfResult > 20, "没有收集到足够多的数据来完成精确的标定" + + def generate_cyclic_trajectory_above_center(self,center, num_steps=12, x_amplitude=0.12, z_amplitude=0.20, frequency=4, min_z_offset=0.60,step_z=0.01): + """ + 生成所有轨迹点都在中心点上方的周期性波动轨迹。 + """ + trajectory = [] + rpy_values = [] + y_constant = center[1] + x_values = np.linspace(-x_amplitude, x_amplitude, num_steps) + y_values = [center[1]+0.05,center[1]+0.15] + # y_values = [center[1]+0.05,center[1]+0.15,center[1]+0.30] + # y_values = [center[1]+0.15,center[1]+0.03,center[1]+0.06,center[1]+0.10,center[1]+0.15] + # y_values = [center[1],center[1],center[1]] + + + for i in range(len(y_values)): + y = y_values[i] + for x in x_values: + # 使用绝对值正弦函数确保 Z 始终在 center[2] 之上 + z = z_amplitude * np.abs(np.sin(frequency * x * np.pi)) + center[2] + min_z_offset + step_z*i + + tool_point = np.array([x + center[0], y, z]) + + trajectory.append(tool_point) + + # 计算从中心点射向轨迹点的向量 + vector_center_to_tool = tool_point - center + roll, pitch, yaw = self.get_roll_pitch_yaw(vector_center_to_tool) + rpy_values.append((roll, pitch, yaw)) + + print(f"above part center:{center}\n {trajectory[:5]}") + return np.array(trajectory), np.array(rpy_values) + + def get_roll_pitch_yaw(self,vector): + """ + 计算从原点指向给定向量的 roll, pitch, yaw (弧度). + 假设 Z 轴向前，Y 轴向上，X 轴向右。 + """ + x, y, z = vector + roll = math.atan2(y, z) + pitch = math.atan2(-x, math.sqrt(y**2 + z**2)) + yaw = 0 # 假设指向 tool 的方向没有偏航，或者根据实际需求调整 + return roll, pitch, yaw + + def concatenate_path_and_rpy(self,path, rpy_angles): + """ + Concatenates a list of positions [x, y, z] with a corresponding list of RPY angles [roll, pitch, yaw]. + + Args: + path (list of lists): A list where each inner list contains [x, y, z]. + rpy_angles (list of lists or tuples): A list where each inner list or tuple contains [roll, pitch, yaw]. + + Returns: + list of lists: A new list where each inner list is a concatenation of the + corresponding position and RPY angles: [x, y, z, roll, pitch, yaw]. + """ + if len(path) != len(rpy_angles): + raise ValueError("The lengths of the path and rpy_angles lists must be the same.") + + combined_list = [] + for i in range(len(path)): + position = path[i] + orientation = rpy_angles[i] + # combined = np.concatenate((position,orientation)) + combined = np.hstack((position.flatten(), orientation)) + + combined_list.append(combined) + print(f"combined xyz|rpy :{combined}") + + return combined_list + + def check_corners_add_sample(self): + rgb, depth, intrinsics = self.cam.get_latest_frame() + gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY) + ret, corners = cv2.findChessboardCorners(gray, self.board_size) + pic_name = self.file_address + "/"+str(len(self.image_points)) + ".jpg" + + if ret: + robot_pose = self.get_pose_homomat() + self.add_sample(gray,corners,robot_pose) + + print(f"save path as: {pic_name}") + # 绘制检测到的角点 (用于可视化，可选) + # cv2.drawChessboardCorners(rgb, self.board_size, corners, ret) + cv2.imwrite(pic_name, rgb) + return True + + cv2.imwrite(pic_name, rgb) + return False + + def add_sample(self, gray, corners,robot_pose): + """ + 添加标定样本。 + image: 捕获的图像 + robot_pose: 机械臂的位姿，格式为 [x, y, z, roll, pitch, yaw] + """ + # 检测棋盘角点 + + corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) # 在原角点的基础上寻找亚像素角点 + # if self._validate_corners(corners2): + if [corners2]: + self.obj_points.append(self.objp) + self.image_points.append(corners2) + self.robot_poses.append(robot_pose) + print(f"insert {robot_pose}") + + def get_pose_homomat(self): + pose = self.arm.robot_rpc_client.getRobotInterface(self.arm.robot_name).getRobotState().getTcpPose() + x, y, z, rx, ry, rz = pose + r = R.from_euler('xyz', [rx, ry, rz]) + rotation_matrix = r.as_matrix() + translation_vector = np.array([x, y, z]).reshape(3, 1) + homo_mat = np.eye(4) # 创建 4x4 单位矩阵 + homo_mat[:3, :3] = rotation_matrix + homo_mat[:3, 3] = translation_vector.flatten() + return homo_mat + + def camera_calib(self): + + ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(self.obj_points, self.image_points, (self.size[1],self.size[0]) , None, None) + if ret: + print("内参矩阵:\n", mtx) # 内参数矩阵 + print("畸变系数:\n", dist) # 畸变系数 distortion cofficients = (k_1,k_2,p_1,p_2,k_3) + print("++++++++++相机标定完成++++++++++++++") + else: + print("-----------相机标定失败--------------") + return rvecs, tvecs + + def split_robot_pose(self): + """ + 将包含 4x4 齐次矩阵的 robot_pose 列表拆分成 R_arm 和 t_arm。 + Args: + robot_pose: 包含 4x4 齐次矩阵的 Python list。 + Returns: + R_arm: 旋转矩阵的 Python list, 每个元素是 3x3 numpy 数组。 + t_arm: 平移向量的 Python list, 每个元素是 3x1 numpy 数组。 + """ + R_arm = [] + t_arm = [] + + for pose_matrix in self.robot_poses: + # 1. 提取旋转矩阵 (3x3) + rotation_matrix = pose_matrix[:3, :3] + R_arm.append(rotation_matrix) + + # 2. 提取平移向量 (3x1) + translation_vector = pose_matrix[:3, 3] # 获取齐次矩阵的平移部分 (1x3 numpy 数组) + translation_vector = translation_vector.reshape(3, 1) # 转换为 3x1 的列向量 + t_arm.append(translation_vector) + + return R_arm, t_arm + + def calibrate(self): + rvecs, tvecs = self.camera_calib() + r_arm, t_arm= self.split_robot_pose() + + rvecs_array = np.array(rvecs) + tvecs_array = np.array(tvecs) + t_arma = np.array(t_arm) + r_arma = np.array(r_arm) + print(rvecs_array.shape) + print(tvecs_array.shape) + print(t_arma.shape) + print(r_arma.shape) + R, t = cv2.calibrateHandEye(r_arma, t_arma, rvecs_array, tvecs_array, cv2.CALIB_HAND_EYE_HORAUD) + print("+++++++++++手眼标定完成+++++++++++++++") + return R, t + + def init_chessboard_center(self,image): + ''' + 返回的是EE坐标下的中点 + ''' + intrinsics = self.intrinsics + # 检测标定板获取初始位置 + ret, corners = cv2.findChessboardCorners(image, self.board_size) + if not ret: raise ValueError("标定板未检测到") + + # 计算标定板中心在相机坐标系中的位置 + obj_pts = np.array([[i*self.square_size, j*self.square_size, 0] for i in range(self.board_size[0]) for j in range(self.board_size[1])], dtype=np.float32) + + print(intrinsics) + camera_matrix = np.array([ + [intrinsics['fx'], 0, intrinsics['cx']], + [0, intrinsics['fy'], intrinsics['cy']], + [0, 0, 1] + ], dtype=np.float32) + dist_coeffs = np.array([ + intrinsics['distortion_coeffs']['k1'], + intrinsics['distortion_coeffs']['k2'], + intrinsics['distortion_coeffs']['p1'], + intrinsics['distortion_coeffs']['p2'], + intrinsics['distortion_coeffs']['k3'] + ], dtype=np.float32) # 同样确保数据类型 + + print("obj_pts shape:", obj_pts.shape) + print("corners shape:", corners.shape) + ret, rvec, tvec = cv2.solvePnP(obj_pts, corners, camera_matrix, dist_coeffs) + + # 这里我们假设标定板的初始位置是原点，没有旋转和平移 + rvec_initial = np.zeros((3, 1), dtype=np.float32) # 初始旋转向量 + tvec_initial = np.zeros((3, 1), dtype=np.float32) # 初始平移向量 + + # 初始变换（标定板到初始坐标系） + initial_transform = np.eye(4) + initial_transform[:3, :3], _ = cv2.Rodrigues(rvec_initial) + initial_transform[:3, 3] = tvec_initial.flatten() + print(f"board itselt {initial_transform}") + # PnP 结果（初始坐标系到相机） + cam2initial = np.eye(4) + cam2initial[:3, :3], _ = cv2.Rodrigues(rvec) + cam2initial[:3, 3] = tvec.flatten() + + # 合成最终变换：标定板 -> 初始坐标系 -> 相机 + board2cam = initial_transform @ cam2initial + print(f"board2cam\n{board2cam}") + # 手眼标定矩阵的逆（相机到TCP的固定变换） + cam2tool = np.eye(4) + cam2tool[:3, :3] = np.array([[0.0, 1.0 , 0.0], + [1.0 , 0.0 , 0.0], + [0.0, 0.0 , -1.0]]) + cam2tool[:3, 3] = np.array([0.00209053, 0.01021561, -0.16877656]).flatten() + + ###找到棋盘格中心 + camera_center = None + chessboard_center_object = self.get_chessboard_center_object_points() + if chessboard_center_object is not None: + # Convert to homogeneous coordinates + chessboard_center_homogeneous = np.append(chessboard_center_object, 1) + + # Transform the center to camera coordinates + camera_center_homogeneous = cam2tool@ board2cam @ chessboard_center_homogeneous + camera_center = camera_center_homogeneous[:3] + + return camera_center + + def get_chessboard_center_object_points(self): + rows, cols = self.board_size + obj_pts = self.objp + if obj_pts is None or len(obj_pts) != rows * cols: + return None + + # Get the range of x and y indices + x_indices = obj_pts[:, 0].flatten() + y_indices = obj_pts[:, 1].flatten() + + center_x_index = (np.max(x_indices) + np.min(x_indices)) / 2.0 + center_y_index = (np.max(y_indices) + np.min(y_indices)) / 2.0 + center_z = 0 # Assuming the chessboard is on the Z=0 plane in its frame + + return np.array([center_x_index, center_y_index, center_z]) + + def generate_perturbed_joint_angles_rad(self,original_angle): + """ + Args: + original_angle (list or tuple): 包含6个角度值的列表或元组 (单位：度). + + Returns: + numpy.ndarray: 一个形状为 (6, 6) 的 numpy 数组，包含6组6个角度值（单位：弧度）。 + 每组数据的含义如下： + - 第1组: 原始角度（转换为弧度）。 + - 第2组: 第五个关节角度 + rand5（转换为弧度）。 + - 第3组: 第五个关节角度 - rand5（转换为弧度）。 + - 第4组: 第四个关节角度 + rand4（转换为弧度）。 + - 第5组: 第四个关节角度 - rand4（转换为弧度）。 + - 第6组: 第四个关节角度 + rand4，第五个关节角度 + rand5（转换为弧度）。 + """ + original_angle = np.array(original_angle) + if original_angle.shape != (6,): + raise ValueError("输入的角度列表必须包含6个元素。") + original_angle[5] = 0.0 + + # 生成随机数 + rand5 = random.uniform(-20*np.pi/180, 20*np.pi/180) # 第五个关节的随机数，范围可以调整 + rand4 = random.uniform(-10*np.pi/180, 10*np.pi/180) # 第四个关节的随机数，范围可以调整 + + + angle_array = [] + + # 1. 原始角度 + angle_array.append(original_angle.copy()) + # 4. += rand4 + angle_c = original_angle.copy() + angle_c[3] -= rand4 + angle_c[4] -= rand5 + angle_array.append(angle_c) + + # 5. -= rand4 + angle_d = original_angle.copy() + angle_d[3] += rand4 + angle_d[4] += rand5 + angle_array.append(angle_d) + + # 6. +=rand4，-= rand5 + angle_e = original_angle.copy() + angle_e[3] += rand4 + angle_e[4] -= rand5 + angle_array.append(angle_e) + + # 6. -= rand4，第五个关节 += rand5 + angle_f = original_angle.copy() + angle_f[3] -= rand4 + angle_f[4] += rand5 + angle_array.append(angle_f) + + # 6. -= rand4，第五个关节 += rand5 + angle_g = original_angle.copy() + angle_g[3] -= rand4/2 + angle_g[4] += rand5/2 + angle_array.append(angle_g) + + angle_h = original_angle.copy() + angle_h[3] += rand4/2 + angle_h[4] -= rand5/2 + angle_array.append(angle_h) + + # 将所有角度从度转换为弧度 + # modified_angles_rad = np.array([np.deg2rad(angles) for angles in angle_array]) + # import pdb + # pdb.set_trace() + + return angle_array + + def traversal_position_direction(self,path): + + for pose in path: + current_pose = self.arm.robot_rpc_client.getRobotInterface(self.arm.robot_name).getRobotState().getTcpPose() + print(f" elipse path is {pose}\n current pose is {current_pose}" ) + code = self.arm.move_line( pose ) + + if code == -1: + print(f"运动到拍照位置失败:{pose}") + else: + joint = self.arm.robot_rpc_client.getRobotInterface(self.arm.robot_name).getRobotState().getJointPositions() + angle_list = self.generate_perturbed_joint_angles_rad(joint) + print(f"\n {angle_list}\n Angles Position ") + for joint in angle_list: + print(f"current joint: {joint}") + code = self.arm.move_joint(joint,4,wait=True) + if code != -1: + print(f"move success") + self.check_corners_add_sample() + + return len(self.robot_poses) + + def visualization(self,center,trajectory,rpy_values): + # 可视化 + fig = plt.figure(figsize=(10, 8)) + ax = fig.add_subplot(111, projection='3d') + + # 绘制中心点 + ax.scatter(*center, color='r', marker='o', s=100, label='Center Point') + + # 绘制轨迹 + ax.plot(trajectory[:, 0], trajectory[:, 1], trajectory[:, 2], color='b', label='Tool Trajectory') + + # 可视化 RPY 值 (每隔几个点显示一个) + num_samples_rpy = 20 + indices = np.linspace(0, len(rpy_values) - 1, num_samples_rpy, dtype=int) + for i in indices: + tool_point = trajectory[i] + roll, pitch, yaw = rpy_values[i] + vector = tool_point - center + # 将箭头的起始点移动到 tool_point + ax.quiver(tool_point[0], tool_point[1], tool_point[2], + vector[0], vector[1], vector[2], + length=0.2, color='g', alpha=0.7, label='Vector Center to Tool' if i == indices[0] else "") + # 将文本标签的位置移动到 tool_point 附近 + ax.text(tool_point[0] + 0.05, tool_point[1] + 0.05, tool_point[2] + 0.05, + f'RPY:({math.degrees(roll):.2f},{math.degrees(pitch):.2f},{math.degrees(yaw):.2f})', + color='g', fontsize=8) + + ax.set_xlabel('X') + ax.set_ylabel('Y') + ax.set_zlabel('Z') + ax.set_title('Cyclic Trajectory Above Center Point') + ax.legend() + ax.view_init(azim=-120, elev=30) + plt.grid(True) + plt.show() + + # 打印 RPY 值 (示例) + # print("\nRoll, Pitch, Yaw values (degrees) along the trajectory:") + # for i, (r, p, y) in enumerate(rpy_values): + # if i % 10 == 0: # Print every 10th value + # print(f"Step {i}: Roll: {math.degrees(r):.2f}, Pitch: {math.degrees(p):.2f}, Yaw: {math.degrees(y):.2f}") + + +if __name__ == '__main__': + + # 标定板的width 对应的角点的个数 6 + # 标定板的height 对应的角点的个数 3 + calibration_board_size = [9,6] + calibration_board_square_size = 0.020 #unit - meter + systemPath = "/home/jsfb/Documents/" + + now = datetime.now() + date_time_str = now.strftime("%d_%H_%M") + folder_name = f"calibration_data_{date_time_str}" + systemPath+=folder_name + os.makedirs(systemPath, exist_ok=True) + + calib = Calibration(calibration_board_size,calibration_board_square_size,systemPath) + calib.collect_data() + r, t = calib.calibrate() + print("旋转矩阵：") + print(r) + print("平移向量：") + print(t) + + + + # arm = AuboC5() + # arm.init() + # aubo_algorithm = arm.robot_rpc_client.getRobotInterface(arm.robot_name).getRobotAlgorithm().inverseKinematics() + + # # 查看 rpc_client 对象的所有属性和方法 + # print(dir(aubo_algorithm)) + diff --git a/logs/MassageRobot_nova5_test.log b/logs/MassageRobot_nova5_test.log index e69de29..4f39b77 100644 --- a/logs/MassageRobot_nova5_test.log +++ b/logs/MassageRobot_nova5_test.log @@ -0,0 +1,84 @@ +2025-05-26 15:02:07,413 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 +2025-05-26 15:02:10,922 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 +2025-05-26 15:02:10,923 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000011 -0.13499942 0.3442838 ] +2025-05-26 15:02:10,925 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999939e+02 1.09863257e-04 -9.00000000e+01] +2025-05-26 15:02:10,927 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13499942 0.3443389 ] +2025-05-26 15:02:10,928 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 1.91747555e-06 -1.57079633e+00] +2025-05-26 15:02:10,929 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000011 -0.13499942 0.34423872] +2025-05-26 15:02:10,930 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999939e+02 1.09626792e-04 -9.00001567e+01] +2025-05-26 15:02:10,931 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999965 -0.13499989 0.3443392 ] +2025-05-26 15:02:10,932 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 1.43810701e-06 -1.57079872e+00] +2025-05-26 15:02:10,947 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999248 -0.13499964 0.3442024 ] +2025-05-26 15:02:10,948 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998049e+02 5.63537890e-05 -9.00001768e+01] +2025-05-26 15:02:10,949 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000008 -0.13499965 0.34433948] +2025-05-26 15:02:10,951 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 9.58738475e-07 -1.57079633e+00] +2025-05-26 15:02:10,965 - 测试日志 - INFO - log.py:106 - position command: [ 0.24998647 -0.13499941 0.34417218] +2025-05-26 15:02:10,966 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79996532e+02 1.10826636e-04 -9.00001933e+01] +2025-05-26 15:02:10,967 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13499954 0.34433917] +2025-05-26 15:02:10,968 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 1.91747644e-06 -1.57079726e+00] +2025-05-26 15:02:10,984 - 测试日志 - INFO - log.py:106 - position command: [ 0.24997959 -0.13499632 0.34414674] +2025-05-26 15:02:10,985 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79995110e+02 -7.71629391e-04 -9.00002723e+01] +2025-05-26 15:02:10,988 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999971 -0.13499977 0.34433887] +2025-05-26 15:02:10,990 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 9.58737078e-07 -1.57079726e+00] +2025-05-26 15:02:11,003 - 测试日志 - INFO - log.py:106 - position command: [ 0.24997405 -0.1349941 0.34412699] +2025-05-26 15:02:11,004 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79993630e+02 -1.21707493e-03 -9.00001773e+01] +2025-05-26 15:02:11,005 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999979 -0.13499953 0.34433914] +2025-05-26 15:02:11,010 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 1.43810727e-06 -1.57079633e+00] +2025-05-26 15:02:11,022 - 测试日志 - INFO - log.py:106 - position command: [ 0.24996921 -0.13499329 0.34411049] +2025-05-26 15:02:11,023 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79992392e+02 -1.41061365e-03 -9.00001617e+01] +2025-05-26 15:02:11,027 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999976 -0.13499954 0.34433859] +2025-05-26 15:02:11,028 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 1.91747466e-06 -1.57079726e+00] +2025-05-26 15:02:11,040 - 测试日志 - INFO - log.py:106 - position command: [ 0.24996431 -0.13499314 0.344097 ] +2025-05-26 15:02:11,042 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79991336e+02 -1.42291447e-03 -9.00001738e+01] +2025-05-26 15:02:11,044 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13499965 0.34433951] +2025-05-26 15:02:11,044 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 1.43810905e-06 -1.57079779e+00] +2025-05-26 15:02:11,059 - 测试日志 - INFO - log.py:106 - position command: [ 0.24996024 -0.13499153 0.34408569] +2025-05-26 15:02:11,060 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79990418e+02 -1.83792583e-03 -9.00001532e+01] +2025-05-26 15:02:11,061 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13499954 0.34433936] +2025-05-26 15:02:11,062 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 1.91747644e-06 -1.57079726e+00] +2025-05-26 15:02:11,077 - 测试日志 - INFO - log.py:106 - position command: [ 0.24995717 -0.13499 0.34407685] +2025-05-26 15:02:11,078 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79989529e+02 -2.20403839e-03 -9.00001363e+01] +2025-05-26 15:02:11,079 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13499953 0.34433917] +2025-05-26 15:02:11,080 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 1.43810740e-06 -1.57079686e+00] +2025-05-26 15:02:11,094 - 测试日志 - INFO - log.py:106 - position command: [ 0.249954 -0.13498974 0.34406968] +2025-05-26 15:02:11,095 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79988802e+02 -2.26643396e-03 -9.00001428e+01] +2025-05-26 15:02:11,098 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13499965 0.34433942] +2025-05-26 15:02:11,099 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 1.91747619e-06 -1.57079726e+00] +2025-05-26 15:02:11,112 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499512 -0.13498704 0.34406287] +2025-05-26 15:02:11,114 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79988304e+02 -2.84823642e-03 -9.00001578e+01] +2025-05-26 15:02:11,115 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.13499977 0.34433896] +2025-05-26 15:02:11,118 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 1.43810701e-06 -1.57079819e+00] +2025-05-26 15:02:11,130 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994961 -0.13498602 0.34405822] +2025-05-26 15:02:11,131 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79987684e+02 -3.11853240e-03 -9.00000925e+01] +2025-05-26 15:02:11,132 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.13499989 0.34433929] +2025-05-26 15:02:11,133 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 9.58737967e-07 -1.57079726e+00] +2025-05-26 15:02:11,149 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994811 -0.13498334 0.34405508] +2025-05-26 15:02:11,150 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79987145e+02 -3.78674921e-03 -9.00001559e+01] +2025-05-26 15:02:11,150 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13499989 0.34433951] +2025-05-26 15:02:11,151 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 9.58739364e-07 -1.57079779e+00] +2025-05-26 15:02:11,168 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994679 -0.13498323 0.3440513 ] +2025-05-26 15:02:11,171 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79986832e+02 -3.73867209e-03 -9.00000707e+01] +2025-05-26 15:02:11,173 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.13499989 0.34433929] +2025-05-26 15:02:11,175 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 1.43810841e-06 -1.57079872e+00] +2025-05-26 15:02:11,189 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994505 -0.1349838 0.3440481 ] +2025-05-26 15:02:11,190 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79986625e+02 -3.72589441e-03 -9.00001151e+01] +2025-05-26 15:02:11,192 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13499965 0.34433942] +2025-05-26 15:02:11,193 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 1.91747670e-06 -1.57079819e+00] +2025-05-26 15:02:11,204 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994443 -0.1349842 0.34404624] +2025-05-26 15:02:11,208 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79986397e+02 -3.43709370e-03 -9.00001074e+01] +2025-05-26 15:02:11,209 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13499965 0.34433908] +2025-05-26 15:02:11,211 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 1.43810651e-06 -1.57079726e+00] +2025-05-26 15:02:11,223 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994332 -0.13498559 0.34404457] +2025-05-26 15:02:11,226 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79986153e+02 -3.16623999e-03 -9.00001230e+01] +2025-05-26 15:02:11,228 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999966 -0.13499989 0.34433914] +2025-05-26 15:02:11,228 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 9.58738475e-07 -1.57079779e+00] +2025-05-26 15:02:11,242 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994286 -0.13498704 0.34404302] +2025-05-26 15:02:11,242 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79986006e+02 -2.85488773e-03 -9.00000897e+01] +2025-05-26 15:02:11,243 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000018 -0.13499977 0.34433942] +2025-05-26 15:02:11,244 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 9.58738475e-07 -1.57079686e+00] +2025-05-26 15:02:11,259 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994203 -0.13498865 0.34404123] +2025-05-26 15:02:11,260 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79985975e+02 -2.50918022e-03 -9.00001638e+01] +2025-05-26 15:02:11,261 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000014 -0.13499953 0.34433948] +2025-05-26 15:02:11,262 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 9.58738920e-07 -1.57079633e+00] +2025-05-26 15:02:11,277 - 测试日志 - INFO - log.py:106 - MassageRobot启动 +2025-05-26 15:02:11,277 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动