DevelopmentTeam/MassageRobot_Dobot
15
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

解决点云投影与姿态欧拉角的转换关系

Browse Source
This commit is contained in:
swayneleong 2025-05-27 17:38:55 +08:00
parent 1967009793
commit 82480f4a7b
25 changed files with 119 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
IoT/iot.service
View File

@ -5,7 +5,7 @@ Wants=network-online.target
[Service]
Type=simple
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_aubo/IoT
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_Dobot/IoT
Environment="PATH=/home/jsfb/anaconda3/envs/CPU_robotarm/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin"
ExecStart=/home/jsfb/anaconda3/envs/CPU_robotarm/bin/python iot_lite.pyc
Restart=always

6
IoT/iot_lite.py
View File

@ -976,10 +976,10 @@ class OTAListener(OTAListener):
# 获取当前脚本所在目录的上一级目录
base_path = os.path.dirname(os.path.dirname(current_path))
# 检查当前路径是否包含 MassageRobot_aubo- 开头的文件夹名
if "MassageRobot_aubo-" in base_path:
# 检查当前路径是否包含 MassageRobot_Dobot- 开头的文件夹名
if "MassageRobot_Dobot-" in base_path:
# 提取版本号部分并去除后续目录部分
version = base_path.split("MassageRobot_aubo-")[-1].split("/")[0]
version = base_path.split("MassageRobot_Dobot-")[-1].split("/")[0]
else:
# 如果没有匹配的版本号，则返回默认值
version = "default"

6
Language/LLM/scripts/LLM.py
View File

@ -14,11 +14,11 @@ current_file_path = os.path.abspath(__file__)
Language_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
# print("Language_Path:",Language_Path)
# 找到上三级目录的父级（project_root），然后拼接目标文件夹路径
MassageRobot_aubo_Path = os.path.dirname(Language_Path)
# print("MassageRobot_aubo_Path:",MassageRobot_aubo_Path)
MassageRobot_Dobot_Path = os.path.dirname(Language_Path)
# print("MassageRobot_Dobot_Path:",MassageRobot_Dobot_Path)
# 添加目标文件夹到系统路径
sys.path.append(MassageRobot_aubo_Path)
sys.path.append(MassageRobot_Dobot_Path)
# # 测试
try:

6
Language/LLM/scripts/LLM_back.py
View File

@ -13,11 +13,11 @@ current_file_path = os.path.abspath(__file__)
Language_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
# print("Language_Path:",Language_Path)
# 找到上三级目录的父级（project_root），然后拼接目标文件夹路径
MassageRobot_aubo_Path = os.path.dirname(Language_Path)
# print("MassageRobot_aubo_Path:",MassageRobot_aubo_Path)
MassageRobot_Dobot_Path = os.path.dirname(Language_Path)
# print("MassageRobot_Dobot_Path:",MassageRobot_Dobot_Path)
# 添加目标文件夹到系统路径
sys.path.append(MassageRobot_aubo_Path)
sys.path.append(MassageRobot_Dobot_Path)
# # 测试
try:

6
Language/Language.py
View File

@ -30,9 +30,9 @@ import os
import copy
current_file_path = os.path.abspath(__file__)
Language_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
MassageRobot_aubo_Path = os.path.dirname(Language_Path)
print("MassageRobot_aubo_Path:",MassageRobot_aubo_Path)
sys.path.append(MassageRobot_aubo_Path)
MassageRobot_Dobot_Path = os.path.dirname(Language_Path)
print("MassageRobot_Dobot_Path:",MassageRobot_Dobot_Path)
sys.path.append(MassageRobot_Dobot_Path)
from VortXDB.client import VTXClient
# 自定义输出类，将输出同时发送到终端和日志文件
class MultiWriter:

8
Language/Speech_processor/scripts/Audio_player.py
View File

@ -149,7 +149,7 @@ if __name__ == "__main__":
# 启动线程A，优先级为5
threading.Thread(
target=thread_play,
args=("/home/jsfb/jsfb_ws/MassageRobot_aubo/Language/pre_mp3/按摩已结束.mp3", 10),
args=("/home/jsfb/jsfb_ws/MassageRobot_Dobot/Language/pre_mp3/按摩已结束.mp3", 10),
).start()
# time.sleep(2)
@ -157,7 +157,7 @@ if __name__ == "__main__":
# 启动线程B，优先级为7（更高），强制停止A并播放新的音频
threading.Thread(
target=thread_play,
args=("/home/jsfb/jsfb_ws/MassageRobot_aubo/Language/pre_mp3/我在/audio_1.mp3", 10),
args=("/home/jsfb/jsfb_ws/MassageRobot_Dobot/Language/pre_mp3/我在/audio_1.mp3", 10),
).start()
time.sleep(2)
@ -165,7 +165,7 @@ if __name__ == "__main__":
# 启动线程C，优先级为3（更低），不会打断线程B的播放
threading.Thread(
target=thread_play,
args=("/home/jsfb/jsfb_ws/MassageRobot_aubo/Language/pre_mp3/完成复位.mp3", 3),
args=("/home/jsfb/jsfb_ws/MassageRobot_Dobot/Language/pre_mp3/完成复位.mp3", 3),
).start()
time.sleep(2)
@ -173,7 +173,7 @@ if __name__ == "__main__":
# 启动线程D，优先级为5
threading.Thread(
target=thread_play,
args=("/home/jsfb/jsfb_ws/MassageRobot_aubo/Language/pre_mp3/connect_successfully.mp3", 5),
args=("/home/jsfb/jsfb_ws/MassageRobot_Dobot/Language/pre_mp3/connect_successfully.mp3", 5),
).start()
time.sleep(2)

8
Language/Speech_processor/scripts/Recognizer.py
View File

@ -14,9 +14,9 @@ from tools.log import CustomLogger
import os
current_file_path = os.path.abspath(__file__)
Language_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
MassageRobot_aubo_Path = os.path.dirname(Language_Path)
print("MassageRobot_aubo_Path:",MassageRobot_aubo_Path)
sys.path.append(MassageRobot_aubo_Path)
MassageRobot_Dobot_Path = os.path.dirname(Language_Path)
print("MassageRobot_Dobot_Path:",MassageRobot_Dobot_Path)
sys.path.append(MassageRobot_Dobot_Path)
from VortXDB.client import VTXClient
# 获取当前文件的父目录的上三级路径
@ -296,5 +296,5 @@ if __name__ == '__main__':
# text,if_timeout,remaining_time = recognizer.speech_recognize()
# print(text,if_timeout)
## 音频语音识别
# recognizer.speech_recognize_UI("/home/jsfb/jsfb_ws/MassageRobot_aubo/tmp/speech_audio.mp3")
# recognizer.speech_recognize_UI("/home/jsfb/jsfb_ws/MassageRobot_Dobot/tmp/speech_audio.mp3")
recognizer.test_token()

8
Language/Speech_processor/scripts/Synthesizer.py
View File

@ -13,9 +13,9 @@ from tools.log import CustomLogger
import os
current_file_path = os.path.abspath(__file__)
Language_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
MassageRobot_aubo_Path = os.path.dirname(Language_Path)
print("MassageRobot_aubo_Path:",MassageRobot_aubo_Path)
sys.path.append(MassageRobot_aubo_Path)
MassageRobot_Dobot_Path = os.path.dirname(Language_Path)
print("MassageRobot_Dobot_Path:",MassageRobot_Dobot_Path)
sys.path.append(MassageRobot_Dobot_Path)
from VortXDB.client import VTXClient
# aliyun语音合成
@ -129,7 +129,7 @@ if __name__ == '__main__':
from Audio_player import AudioPlayer
def parse_args():
parser = argparse.ArgumentParser(description='Speech processor')
parser.add_argument('--synthesizer_config_path', type=str, default='/home/jsfb/jsfb_ws/MassageRobot_aubo/Language/Speech_processor/config/aliyun_synthesize_config.yaml')
parser.add_argument('--synthesizer_config_path', type=str, default='/home/jsfb/jsfb_ws/MassageRobot_Dobot/Language/Speech_processor/config/aliyun_synthesize_config.yaml')
args = parser.parse_args()
return args

2
Language/language.service
View File

@ -5,7 +5,7 @@ Wants=network-online.target
[Service]
Type=simple
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_aubo/Language
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_Dobot/Language
Environment="PATH=/home/jsfb/anaconda3/envs/CPU_robotarm/bin"
Environment="PULSE_SERVER=unix:/run/user/1000/pulse/native"
Environment="DISPLAY=:0"

2
License/license_client.service
View File

@ -5,7 +5,7 @@ Wants=network-online.target
[Service]
Type=simple
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_aubo/License
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_Dobot/License
Environment="PATH=/home/jsfb/anaconda3/envs/CPU_robotarm/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin"
ExecStart=/home/jsfb/anaconda3/envs/CPU_robotarm/bin/python license_client.pyc
Restart=always

4
Massage/Massage.py
View File

@ -26,7 +26,7 @@ import requests
from tools.yaml_operator import read_yaml
from tools.log import CustomLogger
from typing import Literal
from MassageControl.MassageRobot_aubo import MassageRobot
from MassageControl.MassageRobot_nova5 import MassageRobot
from aucpuncture2point import ToolCamera, AbdomenDetector,BackDetector,LegDetector, CoordinateTransformer
from scipy.spatial.transform import Rotation as R
from scipy.interpolate import make_interp_spline
@ -34,7 +34,7 @@ import matplotlib.pyplot as plt
import datetime
import shutil
import importlib.util
from MassageControl.hardware.force_sensor_aubo import XjcSensor
from MassageControl.hardware.force_sensor import XjcSensor
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from VortXDB.client import VTXClient

31
Massage/MassageControl/hardware/force_sensor.py
View File

@ -13,7 +13,7 @@ sys.path.append(os.path.abspath(os.path.join(os.getcwd(), "Massage/MassageContro
class XjcSensor:
def __init__(self, port="/dev/ttyUSB0", baudrate=115200, rate=100):
def __init__(self, port=None, baudrate=115200, rate=250):
self.port = port
self.baudrate = baudrate
self.rate = rate
@ -28,6 +28,9 @@ class XjcSensor:
self._stop_background = False
self._last_reading = None
self._reading_lock = threading.Lock()
if not self.port:
self.port = self.find_sensor_port()
try:
self.connect()
@ -48,6 +51,32 @@ class XjcSensor:
def connect(self):
self.ser = serial.Serial(self.port, self.baudrate,timeout=0.1)
def find_sensor_port(self):
"""寻找对应六维力传感器的串口"""
try:
# 目标 YAML 文件路径
yaml_file_path = '/home/jsfb/jsfb_ws/global_config/force_sensor.yaml'
# 读取现有的 YAML 文件内容
with open(yaml_file_path, 'r') as file:
data = yaml.safe_load(file) # 读取并解析 YAML 内容
# 获取 target_usb_device_path 的值
target_usb_device_path = data.get('target_usb_device_path')
print(f"已成功将 target_usb_device_path 添加到 {yaml_file_path}")
ports = serial.tools.list_ports.comports()
for port in ports:
print(port.usb_device_path)
if target_usb_device_path == port.usb_device_path:
print(f"找到六维力传感器在端口: {port.usb_device_path}")
return port.device
print("未找到具有指定描述的六维力传感器。")
except Exception as e:
print(f"查找串口时发生错误: {e}")
return None
@staticmethod
def generate_crc16_table():
table = []

40
Massage/MassageControl/hardware/test.py Normal file
View File

@ -0,0 +1,40 @@
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from scipy.spatial.transform import Rotation as R
# 定义你的坐标系（原点 + 欧拉角，单位为角度）
frames = [
{"origin": [0, 0, 0], "euler": [0, 0, 0]},
{"origin": [2, 2, 2], "euler": [0, 150, 90]},
{"origin": [3, 3, 3], "euler": [-30, -180, 0]},
]
def draw_frame(ax, origin, rot_matrix, length=0.5):
origin = np.array(origin)
x_axis = origin + rot_matrix @ np.array([length, 0, 0])
y_axis = origin + rot_matrix @ np.array([0, length, 0])
z_axis = origin + rot_matrix @ np.array([0, 0, length])
ax.quiver(*origin, *(x_axis - origin), color='r')
ax.quiver(*origin, *(y_axis - origin), color='g')
ax.quiver(*origin, *(z_axis - origin), color='b')
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_box_aspect([1,1,1])
for frame in frames:
origin = frame["origin"]
euler_deg = frame["euler"]
rotation = R.from_euler('xyz', euler_deg, degrees=True)
rot_matrix = rotation.as_matrix()
draw_frame(ax, origin, rot_matrix)
ax.set_xlim(-1, 3)
ax.set_ylim(-1, 3)
ax.set_zlim(-1, 3)
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
plt.title("Multiple 3D Coordinate Frames (Euler Angles)")
plt.show()

2
Massage/MassageControl/tools/auto_visual_valibration.py
View File

@ -8,7 +8,7 @@ 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')
sys.path.append('/home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage/MassageControl')
from hardware.remote_cam import ToolCamera
from hardware.aubo_C5 import AuboC5

8
Massage/aucpuncture2point/scripts/coordinate_transform.py
View File

@ -380,9 +380,13 @@ class CoordinateTransformer:
temp_point[:, :3] = point[:, :3]
# 计算欧拉角
# temp_point[0][3] = -math.asin(point[0][4])
# temp_point[0][4] = -math.atan2(-point[0][3], point[0][5])
# temp_point[0][5] = 0.0
temp_point[0][3] = -math.asin(point[0][4])
temp_point[0][4] = -math.atan2(-point[0][3], point[0][5])
temp_point[0][5] = 0.0
temp_point[0][4] = math.atan2(point[0][3], point[0][5])
temp_point[0][5] = -np.pi/2
# 确保角度在[-pi, pi]范围内
if temp_point[0][5] > np.pi:

4
Massage/aucpuncture2point/scripts/rgb_point_cloud.py
View File

@ -68,6 +68,6 @@ def display_rgb_point_cloud(rgb_image, depth_image, camera_intrinsics):
o3d.visualization.draw_geometries([point_cloud])
intrinsics = {'fx': 454.114136, 'fy': 454.114136, 'cx': 325.119751, 'cy': 235.403824}
color_image = cv2.imread('/home/jsfb/jsfb_ws/MassageRobot_aubo/Massage/aucpuncture2point/configs/using_img/color.png')
depth_img = cv2.imread('/home/jsfb/jsfb_ws/MassageRobot_aubo/Massage/aucpuncture2point/configs/using_img/depth.png', cv2.IMREAD_UNCHANGED)
color_image = cv2.imread('/home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage/aucpuncture2point/configs/using_img/color.png')
depth_img = cv2.imread('/home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage/aucpuncture2point/configs/using_img/depth.png', cv2.IMREAD_UNCHANGED)
display_rgb_point_cloud(color_image, depth_img, intrinsics)

2
Massage/controller.service
View File

@ -5,7 +5,7 @@ Wants=network-online.target
[Service]
Type=simple
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_aubo/Massage/MassageControl/hardware/
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage/MassageControl/hardware/
Environment="PATH=/home/jsfb/anaconda3/envs/CPU_robotarm/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin"
ExecStart=/home/jsfb/anaconda3/envs/CPU_robotarm/bin/python controller.pyc
Restart=on-failure

4
Massage/massage.service
View File

@ -5,9 +5,9 @@ Wants=network-online.target
[Service]
Type=simple
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_aubo/Massage
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage
Environment="PATH=/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/home/jsfb/anaconda3/envs/CPU_robotarm/bin"
ExecStart=/bin/bash -c "source /opt/ros/noetic/setup.bash; echo $PATH; exec /home/jsfb/anaconda3/envs/CPU_robotarm/bin/python /home/jsfb/jsfb_ws/MassageRobot_aubo/Massage/Massage.pyc"
ExecStart=/bin/bash -c "source /opt/ros/noetic/setup.bash; echo $PATH; exec /home/jsfb/anaconda3/envs/CPU_robotarm/bin/python /home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage/Massage.pyc"
Restart=on-failure
User=jsfb
Group=jsfb

2
UI_next/force_sensor_aubo.py
View File

@ -8,7 +8,7 @@ import time
import subprocess
import psutil
import sys
# sys.path.append("/home/jsfb/jsfb_ws/MassageRobot_aubo/Massage/MassageControl")
# sys.path.append("/home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage/MassageControl")
from tools.ssh_tools import execute_command_on_remote
from tools.serial_tools import start_virtual_serial,stop_virtual_serial

4
UI_next/modules/thermal/thermal_trajectory.py
View File

@ -8,8 +8,8 @@ current_file_path = os.path.abspath(__file__)
current_file_floder = os.path.dirname(current_file_path)
Path = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(current_file_path))))
print(Path)
MassageRobot_aubo_Path = os.path.dirname(Path)
sys.path.append(MassageRobot_aubo_Path)
MassageRobot_Dobot_Path = os.path.dirname(Path)
sys.path.append(MassageRobot_Dobot_Path)
from VortXDB.client import VTXClient

2
UI_next/tools/log_utils.py
View File

@ -1,6 +1,6 @@
import os
# 获取项目根目录的绝对路径（MassageRobot_aubo 目录）
# 获取项目根目录的绝对路径（MassageRobot_Dobot 目录）
BASE_DIR = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
def read_log_file(log_type, keyword=None, page=1 , page_size=400):

2
UI_next/ui_next_app.service
View File

@ -5,7 +5,7 @@ Wants=network-online.target
[Service]
Type=simple
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_aubo/UI_next
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_Dobot/UI_next
Environment="PATH=/home/jsfb/anaconda3/envs/CPU_robotarm/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin"
Environment="PULSE_SERVER=unix:/run/user/1000/pulse/native"
Environment="DISPLAY=:0"

2
UI_next/ui_next_app_net.service
View File

@ -6,7 +6,7 @@ Requires=NetworkManager.service
[Service]
Type=simple
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_aubo/UI_next
WorkingDirectory=/home/jsfb/jsfb_ws/MassageRobot_Dobot/UI_next
Environment="PATH=/home/jsfb/anaconda3/envs/CPU_robotarm/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin"
Environment="PULSE_SERVER=unix:/run/user/1000/pulse/native"
Environment="DISPLAY=:0"

2
setup.sh
View File

@ -7,7 +7,7 @@ PASSWORD="jsfb"
CURRENT_DIR=$(pwd)
# 定义要替换的路径和新路径
OLD_PATH="/home/jsfb/jsfb_ws/MassageRobot_aubo"
OLD_PATH="/home/jsfb/jsfb_ws/MassageRobot_Dobot"
NEW_PATH="$CURRENT_DIR"
# Conda 环境名称

21
版本说明.txt
View File

@ -1,20 +1 @@
MassageRobot_aubo-1.2.5-alpha-rc1 # 发往日本的稳定版本（无多语言）
MassageRobot_aubo-1.2.5-alpha-rc2 # 发往日本的稳定版本（多语言）10.25更新
MassageRobot_aubo-1.2.5-alpha-rc3 # 发往日本的稳定版本（多语言）10.25更新:修复断网可继续播音频问题
MassageRobot_aubo-1.2.5-alpha-rc4 # 发往日本的稳定版本（多语言）10.25更新:jiaru lichuanganqizhuantaipanduantangchuan
MassageRobot_aubo-1.2.5-alpha-rc5-yoyo # 发往日本的稳定版本（多语言）10.26更新:这个唤醒词为yoyo，修改按摩头、语音路径
MassageRobot_aubo-1.2.5-alpha-rc5-sakuna # 发往日本的稳定版本（多语言）10.26更新:这个唤醒词为heisakuna，修改按摩头、语音路径
MassageRobot_aubo-1.2.5-alpha-rc6-yoyo # 发往日本的稳定版本（多语言）10.26更新:这个唤醒词为yoyo，修改按摩头、语音路径,ota
MassageRobot_aubo-1.2.5-alpha-rc6-sakuna # 发往日本的稳定版本（多语言）10.26更新:这个唤醒词为heisakuna，修改按摩头、语音路径ota
MassageRobot_aubo-1.2.6-alpha-rc8-cn # 发往安徽版本 11.2更新:取消用戶取頭置零操作，以及新的視覺模型修改
MassageRobot_aubo-1.2.8-alpha-rc1-cn # 新发出去的三台前的稳定版本，加入稳定冲击波和滚珠
MassageRobot_aubo-1.2.8-alpha-rc2-cn # 加入滚珠揉腹部，设置中加入语音重启
MassageRobot_aubo-1.2.8-alpha-rc3-cn # 加入开发者虚拟模式支持空跑，进入设置需要密码
MassageRobot_aubo-1.2.8-alpha-rc4-cn # 去掉冲击波绝对路径
MassageRobot_aubo-1.2.8-alpha-rc8-cn # 1.滚珠改成不过脊柱轨迹；2.加入hybridPid控制方式
MassageRobot_aubo-1.2.8-alpha-rc9-cn # 旧版OTA的最后一个版本，用于衔接新版OTA
MassageRobot_aubo-1.2.9-alpha-cn # 新版OTA 语音加入唤醒词可选
MassageRobot_aubo-1.2.9-alpha-rc1-cn # ???
MassageRobot_aubo-1.2.9-alpha-rc2-cn # ???
MassageRobot_aubo-1.2.9-alpha-rc2-iot-cn # rc2版本的iot分支，用于测试iot lite版
MassageRobot_aubo-1.2.9-alpha-rc3-cn # 添加WiFi配网功能