Compare commits
7 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
b07c0ee2a6 | ||
|
|
aa09db487f | ||
|
|
feaccd1410 | ||
|
|
0a18dec9a0 | ||
|
|
f5a3aaaab0 | ||
|
|
4e210c38fa | ||
|
|
66ddb21f80 |
26
.gitignore
vendored
26
.gitignore
vendored
@ -1,26 +0,0 @@
|
||||
# 忽略 Python 缓存文件和编译文件
|
||||
*.pyc
|
||||
__pycache__/
|
||||
|
||||
# 忽略虚拟环境
|
||||
venv/
|
||||
.env/
|
||||
.venv/
|
||||
env/
|
||||
|
||||
# 忽略日志、临时文件
|
||||
*.log
|
||||
*.tmp
|
||||
|
||||
# 忽略 Jupyter 缓存
|
||||
.ipynb_checkpoints/
|
||||
|
||||
# 忽略 VS Code 或 PyCharm 等 IDE 配置
|
||||
.vscode/
|
||||
.idea/
|
||||
|
||||
# 忽略构建目录
|
||||
build/
|
||||
dist/
|
||||
*.egg-info/
|
||||
.eggs/
|
||||
@ -108,20 +108,12 @@ class MassageRobot:
|
||||
|
||||
self.vtxdb = VTXClient()
|
||||
|
||||
# 当前执行的函数
|
||||
self.current_function = None
|
||||
self.arm_state = ArmState()
|
||||
self.arm_config = read_yaml(arm_config_path)
|
||||
# arm 实例化时机械臂类内部进行通讯连接
|
||||
# 初始化坐标系 TOOL=0 BASE=1
|
||||
self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip'])
|
||||
self.arm.start()
|
||||
|
||||
self.arm.chooseRightFrame()
|
||||
|
||||
self.arm.init()
|
||||
|
||||
|
||||
self.thermotherapy = None
|
||||
self.shockwave = None
|
||||
self.stone = None
|
||||
@ -135,16 +127,25 @@ class MassageRobot:
|
||||
# 控制器,初始为导纳控制
|
||||
self.controller_manager = ControllerManager(self.arm_state)
|
||||
self.controller_manager.add_controller(AdmittanceController,'admittance',self.arm_config['controller'][0])
|
||||
# self.controller_manager.add_controller(HybridController,'hybrid',self.arm_config['controller'][1])
|
||||
self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][1])
|
||||
# self.controller_manager.add_controller(AdmitHybridController,'admithybrid',self.arm_config['controller'][3])
|
||||
# self.controller_manager.add_controller(HybridPidController,'hybridPid',self.arm_config['controller'][4])
|
||||
# self.controller_manager.add_controller(HybridAdmitController,'hybridAdmit',self.arm_config['controller'][5])
|
||||
# self.controller_manager.add_controller(PositionerSensorController,'positionerSensor',self.arm_config['controller'][6])
|
||||
# self.controller_manager.add_controller(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7])
|
||||
self.controller_manager.add_controller(HybridController,'hybrid',self.arm_config['controller'][1])
|
||||
self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][2])
|
||||
self.controller_manager.add_controller(AdmitHybridController,'admithybrid',self.arm_config['controller'][3])
|
||||
self.controller_manager.add_controller(HybridPidController,'hybridPid',self.arm_config['controller'][4])
|
||||
self.controller_manager.add_controller(HybridAdmitController,'hybridAdmit',self.arm_config['controller'][5])
|
||||
self.controller_manager.add_controller(PositionerSensorController,'positionerSensor',self.arm_config['controller'][6])
|
||||
self.controller_manager.add_controller(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7])
|
||||
|
||||
self.controller_manager.switch_controller('admittance')
|
||||
|
||||
# 按摩头参数暂时使用本地数据
|
||||
massage_head_dir = self.arm_config['massage_head_dir']
|
||||
all_items = os.listdir(massage_head_dir)
|
||||
head_config_files = [f for f in all_items if os.path.isfile(os.path.join(massage_head_dir, f))]
|
||||
self.playload_dict = {}
|
||||
for file in head_config_files:
|
||||
file_address = massage_head_dir + '/' + file
|
||||
play_load = read_yaml(file_address)
|
||||
self.playload_dict[play_load['name']] = play_load
|
||||
self.current_head = 'none'
|
||||
|
||||
# 读取数据
|
||||
self.gravity_base = None
|
||||
@ -153,7 +154,7 @@ class MassageRobot:
|
||||
self.tool_position = None
|
||||
self.mass_center_position = None
|
||||
self.s_tf_matrix_t = None
|
||||
arm_orientation_set0 = np.array([-180,-30,0])
|
||||
arm_orientation_set0 = np.array([-180,0,-90])
|
||||
self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
|
||||
|
||||
# 频率数据赋值
|
||||
@ -176,30 +177,15 @@ class MassageRobot:
|
||||
# 按摩调整
|
||||
self.adjust_wrench_envent = threading.Event()
|
||||
self.adjust_wrench_envent.clear() # 调整初始化为False
|
||||
self.is_execute = False
|
||||
self.pos_increment = np.zeros(3,dtype=np.float64)
|
||||
self.adjust_wrench = np.zeros(6,dtype=np.float64)
|
||||
self.skip_pos = np.zeros(6,dtype=np.float64)
|
||||
|
||||
# 按摩头参数暂时使用本地数据
|
||||
massage_head_dir = self.arm_config['massage_head_dir']
|
||||
all_items = os.listdir(massage_head_dir)
|
||||
head_config_files = [f for f in all_items if os.path.isfile(os.path.join(massage_head_dir, f))]
|
||||
self.playload_dict = {}
|
||||
for file in head_config_files:
|
||||
file_address = massage_head_dir + '/' + file
|
||||
play_load = read_yaml(file_address)
|
||||
self.playload_dict[play_load['name']] = play_load
|
||||
# self.playload_dict = self.vtxdb.get("robot_config", "massage_head")
|
||||
# print(self.playload_dict)
|
||||
self.current_head = 'none'
|
||||
self.is_waitting = False
|
||||
|
||||
self.last_print_time = 0
|
||||
self.last_record_time = 0
|
||||
self.last_command_time = 0
|
||||
self.move_to_point_count = 0
|
||||
self.width_default = 5
|
||||
# 卡尔曼滤波相关初始化
|
||||
self.x_base = np.zeros(6)
|
||||
self.P_base = np.eye(6)
|
||||
@ -217,16 +203,7 @@ class MassageRobot:
|
||||
# 传感器故障计数器
|
||||
self.sensor_fail_count = 0
|
||||
# 机械臂初始化,适配中间层
|
||||
# 读取数据
|
||||
self.gravity_base = None
|
||||
self.force_zero = None
|
||||
self.torque_zero = None
|
||||
self.tool_position = None
|
||||
self.mass_center_position = None
|
||||
self.s_tf_matrix_t = None
|
||||
arm_orientation_set0 = np.array([-180,0,-90])
|
||||
self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
|
||||
|
||||
self.arm.init()
|
||||
|
||||
# REF TRAJ
|
||||
self.xr = []
|
||||
@ -238,36 +215,6 @@ class MassageRobot:
|
||||
self.last_time = -1
|
||||
self.cur_time = -1
|
||||
|
||||
# 预测步骤
|
||||
def kalman_predict(self,x, P, Q):
|
||||
# 预测状态（这里假设状态不变）
|
||||
x_predict = x
|
||||
# 预测误差协方差
|
||||
P_predict = P + Q
|
||||
return x_predict, P_predict
|
||||
|
||||
# 更新步骤
|
||||
def kalman_update(self,x_predict, P_predict, z, R):
|
||||
# 卡尔曼增益
|
||||
# K = P_predict @ np.linalg.inv(P_predict + R)
|
||||
S = P_predict + R
|
||||
s = np.diag(S) # shape (6,)
|
||||
p_diag = np.diag(P_predict)
|
||||
K_diag = np.zeros_like(s)
|
||||
nonzero_mask = s != 0
|
||||
K_diag[nonzero_mask] = p_diag[nonzero_mask] / s[nonzero_mask]
|
||||
K = np.diag(K_diag)
|
||||
# 更新状态
|
||||
x_update = x_predict + K @ (z - x_predict)
|
||||
# 更新误差协方差
|
||||
P_update = (np.eye(len(K)) - K) @ P_predict
|
||||
return x_update, P_update
|
||||
def init_hardwares(self,ready_pose):
|
||||
self.ready_pose = np.array(ready_pose)
|
||||
self.switch_payload(self.current_head)
|
||||
print(self.arm.get_arm_position())
|
||||
time.sleep(0.5)
|
||||
|
||||
|
||||
def sensor_set_zero(self):
|
||||
self.arm_position_set0,self.arm_orientation_set0 = self.arm.get_arm_position()
|
||||
@ -376,7 +323,30 @@ class MassageRobot:
|
||||
return -1
|
||||
return 0
|
||||
|
||||
# 预测步骤
|
||||
def kalman_predict(self,x, P, Q):
|
||||
# 预测状态（这里假设状态不变）
|
||||
x_predict = x
|
||||
# 预测误差协方差
|
||||
P_predict = P + Q
|
||||
return x_predict, P_predict
|
||||
|
||||
# 更新步骤
|
||||
def kalman_update(self,x_predict, P_predict, z, R):
|
||||
# 卡尔曼增益
|
||||
# K = P_predict @ np.linalg.inv(P_predict + R)
|
||||
S = P_predict + R
|
||||
s = np.diag(S) # shape (6,)
|
||||
p_diag = np.diag(P_predict)
|
||||
K_diag = np.zeros_like(s)
|
||||
nonzero_mask = s != 0
|
||||
K_diag[nonzero_mask] = p_diag[nonzero_mask] / s[nonzero_mask]
|
||||
K = np.diag(K_diag)
|
||||
# 更新状态
|
||||
x_update = x_predict + K @ (z - x_predict)
|
||||
# 更新误差协方差
|
||||
P_update = (np.eye(len(K)) - K) @ P_predict
|
||||
return x_update, P_update
|
||||
|
||||
def update_wrench(self):
|
||||
# 当前的机械臂到末端转换 (实时)
|
||||
@ -414,29 +384,6 @@ class MassageRobot:
|
||||
self.arm_state.last_external_wrench_tcp = self.arm_state.external_wrench_tcp
|
||||
return 0
|
||||
|
||||
def switch_payload(self,name):
|
||||
if name in self.playload_dict:
|
||||
self.stop()
|
||||
self.current_head = name
|
||||
|
||||
compensation_config = self.playload_dict[self.current_head]
|
||||
|
||||
# 读取数据
|
||||
self.gravity_base = np.array(compensation_config['gravity_base'])
|
||||
self.force_zero = np.array(compensation_config['force_zero'])
|
||||
self.torque_zero = np.array(compensation_config['torque_zero'])
|
||||
self.tool_position = np.array(compensation_config['tcp_offset'])
|
||||
self.mass_center_position = np.array(compensation_config['mass_center_position'])
|
||||
self.s_tf_matrix_t = self.get_tf_matrix(self.tool_position[:3], R.from_euler('xyz',self.tool_position[3:],degrees=False).as_quat())
|
||||
|
||||
tcp_offset = self.playload_dict[name]["tcp_offset"]
|
||||
tcp_offset_str = "{" + ",".join(map(str, tcp_offset)) + "}"
|
||||
print("tcp_offset_str",tcp_offset_str)
|
||||
self.arm.setEndEffector(i=9,tool_i=tcp_offset_str)
|
||||
self.arm.chooseEndEffector(i=9)
|
||||
print(self.arm.get_arm_position())
|
||||
self.logger.log_info(f"切换到{name}按摩头")
|
||||
|
||||
# ####################### 位姿伺服 ##########################
|
||||
|
||||
def arm_measure_loop(self):
|
||||
@ -553,9 +500,8 @@ class MassageRobot:
|
||||
command_pose[:3],
|
||||
command_pose[3:]
|
||||
)
|
||||
# pose_processed = command_pose
|
||||
print(f"pose_processed:{pose_processed}")
|
||||
print(self.arm.feedbackData.robotMode)
|
||||
# print(f"pose_processed:{pose_processed}")
|
||||
|
||||
tcp_command = (
|
||||
f"ServoP({pose_processed[0]:.3f},{pose_processed[1]:.3f},"
|
||||
f"{pose_processed[2]:.3f},{pose_processed[3]:.3f},"
|
||||
@ -570,7 +516,6 @@ class MassageRobot:
|
||||
time.sleep(sleep_duration)
|
||||
print(f"real sleep:{time.time()-b2}")
|
||||
self.arm.dashboard.socket_dobot.sendall(tcp_command)
|
||||
print("currentCommandID",self.arm.feedbackData.robotCurrentCommandID)
|
||||
if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
print("机械臂为暂停状态")
|
||||
res = self.arm.dashboard.Continue()
|
||||
@ -590,7 +535,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()
|
||||
@ -639,8 +584,37 @@ class MassageRobot:
|
||||
self.arm.stop_motion()
|
||||
self.logger.log_info("MassageRobot停止")
|
||||
|
||||
def init_hardwares(self,ready_pose):
|
||||
self.ready_pose = np.array(ready_pose)
|
||||
self.switch_payload(self.current_head)
|
||||
print(self.arm.get_arm_position())
|
||||
time.sleep(0.5)
|
||||
|
||||
def switch_payload(self,name):
|
||||
if name in self.playload_dict:
|
||||
self.stop()
|
||||
self.current_head = name
|
||||
|
||||
compensation_config = self.playload_dict[self.current_head]
|
||||
|
||||
# 读取数据
|
||||
self.gravity_base = np.array(compensation_config['gravity_base'])
|
||||
self.force_zero = np.array(compensation_config['force_zero'])
|
||||
self.torque_zero = np.array(compensation_config['torque_zero'])
|
||||
self.tool_position = np.array(compensation_config['tcp_offset'])
|
||||
self.mass_center_position = np.array(compensation_config['mass_center_position'])
|
||||
self.s_tf_matrix_t = self.get_tf_matrix(self.tool_position[:3], R.from_euler('xyz',self.tool_position[3:],degrees=False).as_quat())
|
||||
|
||||
tcp_offset = self.playload_dict[name]["tcp_offset"]
|
||||
tcp_offset_str = "{" + ",".join(map(str, tcp_offset)) + "}"
|
||||
print("tcp_offset_str",tcp_offset_str)
|
||||
self.arm.setEndEffector(i=9,tool_i=tcp_offset_str)
|
||||
self.arm.chooseEndEffector(i=9)
|
||||
print(self.arm.get_arm_position())
|
||||
self.logger.log_info(f"切换到{name}按摩头")
|
||||
self.controller_manager.switch_controller('position')
|
||||
else:
|
||||
self.logger.log_error(f"未找到{name}按摩头")
|
||||
|
||||
def log_thread(self):
|
||||
while True:
|
||||
@ -1581,35 +1555,35 @@ class MassageRobot:
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# waypoints = [
|
||||
# {"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": 5.0, "position": [0.30, -0.135, 0.3043], "velocity": [0, 0, 0],
|
||||
# "acceleration": [0, 0, 0],
|
||||
# "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
|
||||
# ] ## 单位 m deg
|
||||
# myInterpolate = TrajectoryInterpolator(waypoints)
|
||||
# ts = np.linspace(0,5,100)
|
||||
robot = MassageRobot(arm_config_path="/home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml")
|
||||
# xr_list, vr_list, ar_list = [], [], []
|
||||
# for t in ts:
|
||||
# xr, vr, ar = myInterpolate.interpolate(t)
|
||||
# xr_list.append(xr)
|
||||
# vr_list.append(vr)
|
||||
# ar_list.append(ar)
|
||||
# xr_array = np.array(xr_list)
|
||||
# vr_array = np.array(vr_list)
|
||||
# ar_array = np.array(ar_list)
|
||||
waypoints = [
|
||||
{"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": 5.0, "position": [0.30, -0.135, 0.3043], "velocity": [0, 0, 0],
|
||||
"acceleration": [0, 0, 0],
|
||||
"orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
|
||||
] ## 单位 m deg
|
||||
myInterpolate = TrajectoryInterpolator(waypoints)
|
||||
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(t)
|
||||
xr_list.append(xr)
|
||||
vr_list.append(vr)
|
||||
ar_list.append(ar)
|
||||
xr_array = np.array(xr_list)
|
||||
vr_array = np.array(vr_list)
|
||||
ar_array = np.array(ar_list)
|
||||
|
||||
# robot.xr = xr_array
|
||||
# robot.vr = vr_array
|
||||
# robot.ar = ar_array
|
||||
# robot.ts = ts
|
||||
# robot.dt = ts[1] - ts[0]
|
||||
robot.xr = xr_array
|
||||
robot.vr = vr_array
|
||||
robot.ar = ar_array
|
||||
robot.ts = ts
|
||||
robot.dt = ts[1] - ts[0]
|
||||
|
||||
|
||||
ready_pose = [204.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042]
|
||||
ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0]
|
||||
robot.current_head = 'finger_head'
|
||||
|
||||
robot.force_sensor.disable_active_transmission()
|
||||
@ -1630,7 +1604,7 @@ if __name__ == "__main__":
|
||||
|
||||
|
||||
atexit.register(robot.force_sensor.disconnect)
|
||||
robot.arm_state.desired_wrench = np.array([0,0,-1,0,0,0])
|
||||
robot.arm_state.desired_wrench = np.array([0,0,-0.5,0,0,0])
|
||||
try:
|
||||
robot_thread = threading.Thread(target=robot.start)
|
||||
robot_thread.start()
|
||||
|
||||
@ -10,8 +10,6 @@ mass_rot: [0.11, 0.11, 0.11]
|
||||
# stiff_rot: [7, 7, 7]
|
||||
stiff_tran: [270, 270, 270]
|
||||
stiff_rot: [11, 11, 11]
|
||||
# stiff_tran: [0, 0, 0]
|
||||
# stiff_rot: [11, 11, 11]
|
||||
# stiff_tran: [100, 100, 100]
|
||||
# stiff_rot: [1, 1, 1]
|
||||
# D
|
||||
|
||||
@ -1,124 +0,0 @@
|
||||
import os
|
||||
import sys
|
||||
import cv2
|
||||
import threading
|
||||
import numpy as np
|
||||
import time
|
||||
|
||||
current_file_path = os.path.abspath(__file__)
|
||||
dobot_nova5_path = os.path.dirname(os.path.dirname(current_file_path))
|
||||
sys.path.append(dobot_nova5_path)
|
||||
|
||||
from hardware.dobot_nova5 import dobot_nova5
|
||||
from hardware.remote_cam import ToolCamera
|
||||
|
||||
class Getpose:
|
||||
def __init__(self):
|
||||
self.running = True
|
||||
self.arm = dobot_nova5()
|
||||
self.arm.start()
|
||||
self.arm.init()
|
||||
self.cam = ToolCamera(host='127.0.0.1')
|
||||
self.cam.start()
|
||||
time.sleep(2)
|
||||
|
||||
self.pose_data = []
|
||||
self.image_data = []
|
||||
# self.arm.start_drag()
|
||||
self.rgb = None
|
||||
|
||||
def test_show(self):
|
||||
self.rgb, self.depth, intrinsics = self.cam.get_latest_frame()
|
||||
if self.rgb is not None:
|
||||
cv2.imshow("Video Feed", self.rgb)
|
||||
cv2.waitKey(1000)
|
||||
|
||||
def add_data(self):
|
||||
angle = self.arm.getAngel()
|
||||
self.pose_data.append(angle)
|
||||
print(f"Current angle: {angle}")
|
||||
|
||||
def save_data(self):
|
||||
if self.pose_data:
|
||||
data = np.array(self.pose_data)
|
||||
np.savetxt('pose.txt', data, fmt='%.5f')
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images'
|
||||
if not os.path.exists(save_directory):
|
||||
os.makedirs(save_directory)
|
||||
|
||||
myTest = Getpose()
|
||||
|
||||
myTest.arm.start_drag()
|
||||
|
||||
time.sleep(0.5)
|
||||
try:
|
||||
count = 1
|
||||
i = 1
|
||||
while True:
|
||||
# 读取摄像头的帧
|
||||
r, d, intrinsics = myTest.cam.get_latest_frame()
|
||||
# ret, frame = cam.read()
|
||||
frame = r
|
||||
|
||||
# if not ret:
|
||||
# print("Error: Could not read frame from video device.")
|
||||
# break
|
||||
|
||||
# 显示摄像头的帧
|
||||
cv2.imshow('found', frame)
|
||||
|
||||
# 等待按键事件
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
|
||||
|
||||
# if key == ord('r'):
|
||||
# pose = increase_dof(poselist[index])
|
||||
# index+=1
|
||||
# code = cp.arm.move_joint(pose, 4,wait=True)
|
||||
# if code == -1:
|
||||
# print("运动到拍照位置失败")
|
||||
# else:
|
||||
|
||||
# 按下's'键保存照片
|
||||
if key == ord('s'):
|
||||
img_path = f'photo{i}'
|
||||
if not os.path.exists(os.path.join(save_directory, img_path)):
|
||||
os.makedirs(os.path.join(save_directory, img_path))
|
||||
filename = os.path.join(save_directory, f'{img_path}/rgb_image.png')
|
||||
cv2.imwrite(filename, frame) # 保存照片
|
||||
filename1 = os.path.join(save_directory, f'{img_path}/depth_image.png')
|
||||
cv2.imwrite(filename1, d) # 保存照片
|
||||
angle = myTest.arm.Get_feedInfo_angle()
|
||||
print("angle = ",angle)
|
||||
filename2 = os.path.join(save_directory, f'{img_path}/angle.txt')
|
||||
np.savetxt(filename2, angle.shape(-1,6),delimiter=',')
|
||||
print(f'Saved {filename}')
|
||||
|
||||
# pose = increase_dof(poselist[index])
|
||||
# index+=1
|
||||
# code = cp.arm.move_joint(poselist[index], 4,wait=True)
|
||||
# if code == -1:
|
||||
# print("运动到拍照位置失败")
|
||||
count += 1 # 增加照片计数
|
||||
i += 1
|
||||
# 按下'q'键退出循环
|
||||
elif key == ord('q'):
|
||||
break
|
||||
|
||||
|
||||
except KeyboardInterrupt:
|
||||
# 处理Ctrl+C中断
|
||||
print("\nCamera session ended by user.")
|
||||
finally:
|
||||
# 释放资源
|
||||
myTest.arm.stop_drag()
|
||||
time.sleep(0.5)
|
||||
myTest.arm.disableRobot()
|
||||
myTest.cam.stop()
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
# 总结拍摄的照片
|
||||
print(f'Total number of images captured: {count}')
|
||||
@ -13,7 +13,6 @@ import threading
|
||||
from time import sleep,time
|
||||
import re
|
||||
from tools.log import CustomLogger
|
||||
# from tools.log
|
||||
import copy
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
@ -95,7 +94,6 @@ class dobot_nova5:
|
||||
self.feedbackData.QActual = feedInfo['QActual'][0]
|
||||
self.feedbackData.ActualQuaternion = feedInfo['ActualQuaternion'][0]
|
||||
self.feedbackData.ToolVectorActual = feedInfo['ToolVectorActual'][0]
|
||||
self.feedbackData.QDActual = feedInfo['QDActual'][0]
|
||||
# 自定义添加所需反馈数据
|
||||
'''
|
||||
self.feedData.DigitalOutputs = int(feedInfo['DigitalOutputs'][0])
|
||||
@ -108,6 +106,12 @@ class dobot_nova5:
|
||||
def start(self):
|
||||
self.dashboard = DobotApiDashboard(self.ip,self.dashboardPort)
|
||||
self.feedFour = DobotApiFeedBack(self.ip,self.feedFourPort)
|
||||
self.dashboard.EmergencyStop(mode=0) # 松开急停
|
||||
self.clearError() # 清除报警
|
||||
if self.parseResultId(self.dashboard.EnableRobot())[0] != 0:
|
||||
print("使能失败:检查29999端口是否被占用")
|
||||
return
|
||||
print("机械臂使能成功")
|
||||
|
||||
# 状态反馈线程
|
||||
feedback_thread = threading.Thread(
|
||||
@ -115,19 +119,6 @@ class dobot_nova5:
|
||||
)
|
||||
feedback_thread.daemon = True
|
||||
feedback_thread.start()
|
||||
self.dashboard.RequestControl()
|
||||
self.dashboard.EmergencyStop(mode=0) # 松开急停
|
||||
self.clearError() # 清除报警
|
||||
# 关闭碰撞检测
|
||||
self.dashboard.SetCollisionLevel(level=1)
|
||||
# lower the velocity
|
||||
self.dashboard.SpeedFactor(50)
|
||||
if self.parseResultId(self.dashboard.EnableRobot())[0] != 0:
|
||||
print("使能失败:检查29999端口是否被占用")
|
||||
return
|
||||
print("机械臂使能成功")
|
||||
|
||||
|
||||
|
||||
def RunPoint_P_inPose(self,poses_list):
|
||||
'''
|
||||
@ -740,13 +731,11 @@ class dobot_nova5:
|
||||
self.last_input_command = None
|
||||
self.tcp_offset = None
|
||||
self.init_J = [-45.0009079,55.5785141,-120.68821716,5.11103201,90.00195312,-90.00085449]
|
||||
self.init_P = [654.3467,-134.9880,305.3604,-180.0000,-30.0000,0.0042]
|
||||
self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
|
||||
self.filter_matrix = np.zeros((1,6)) # 角度伺服用
|
||||
sleep(1)
|
||||
self.is_standby = False
|
||||
# code = self.RunPoint_P_inJoint(self.init_J)
|
||||
code = self.RunPoint_P_inPose(self.init_P)
|
||||
code = self.RunPoint_P_inJoint(self.init_J)
|
||||
if code == 0:
|
||||
print("机械臂初始化成功且到达初始位置")
|
||||
else:
|
||||
@ -862,38 +851,20 @@ class dobot_nova5:
|
||||
|
||||
return -1, None
|
||||
|
||||
def chooseRightFrame(self):
|
||||
self.chooseBaseFrame(i=1)
|
||||
# self.chooseEndEffector(i=0)
|
||||
return
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# sleep(5)
|
||||
dobot = dobot_nova5("192.168.5.1")
|
||||
dobot.start()
|
||||
# posJ_home = [-45,87,-147,0,90,-90]
|
||||
# posJ_ready = [-45,55.5785,-120.6882,5.111,90,-90]
|
||||
# dobot.RunPoint_P_inJoint(posJ_ready)
|
||||
posJ = [0,30,-120,0,90,0]
|
||||
pos_end=[]
|
||||
dobot.RunPoint_P_inJoint(posJ)
|
||||
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("Arm pose:",dobot.getPose())
|
||||
|
||||
# # sleep(1)
|
||||
dobot.chooseRightFrame()
|
||||
# print("Arm pose1:",dobot.getPose())
|
||||
# # print("Arm pose:",dobot.getPose(user=1,tool=0))
|
||||
dobot.setEndEffector(i=8,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}")
|
||||
dobot.chooseEndEffector(i=8)
|
||||
print("Arm pose2:",dobot.getPose(user=1,tool=8))
|
||||
# dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90])
|
||||
|
||||
# print(dobot.get_arm_position())
|
||||
|
||||
dobot.RunPoint_L_inPose([179.087702,-136.993776,425.209977,179.418397,-28.9718402,0.06])
|
||||
# cur_pose = dobot.getPose(user=1,tool=8)
|
||||
# cur_pose[0] += 30
|
||||
# target_pose = cur_pose
|
||||
# print("target",target_pose)
|
||||
# dobot.RunPoint_P_inPose(target_pose)
|
||||
# dobot.ServoPose(dobot.getPose(user=1,tool=8))
|
||||
# print("new ready joint",dobot.feedbackData.QActual)
|
||||
# # dobot.start_drag()
|
||||
# dobot.stop_drag()
|
||||
# dobot.start_drag()
|
||||
dobot.disableRobot()
|
||||
@ -1,11 +1,9 @@
|
||||
import struct
|
||||
import serial
|
||||
import serial.tools.list_ports
|
||||
import numpy as np
|
||||
import atexit
|
||||
import threading
|
||||
import time
|
||||
import yaml
|
||||
|
||||
import subprocess
|
||||
import psutil
|
||||
@ -16,8 +14,7 @@ sys.path.append(os.path.abspath(os.path.join(os.getcwd(), "Massage/MassageContro
|
||||
|
||||
class XjcSensor:
|
||||
def __init__(self, port=None, baudrate=115200, rate=250):
|
||||
# self.port = '/dev/ttyUSB0'
|
||||
self.port = '/dev/ttyS0'
|
||||
self.port = port
|
||||
self.baudrate = baudrate
|
||||
self.rate = rate
|
||||
|
||||
|
||||
@ -41,16 +41,12 @@ class Calibration:
|
||||
self.size = None
|
||||
self.intrinsics_mtx = None
|
||||
self.disorder = None
|
||||
time.sleep(2)
|
||||
rgb_image, depth_image, camera_intrinsics = self.cam.get_latest_frame()
|
||||
self.save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images'
|
||||
|
||||
if rgb_image is None :
|
||||
print(f'===============相机连接失败请检查并重试==============')
|
||||
else:
|
||||
print(f'===============相机连接成功==============')
|
||||
self.size = rgb_image.shape[:2]
|
||||
print(f'===============相机连接成功============== ',self.size)
|
||||
|
||||
self.intrinsics = camera_intrinsics
|
||||
|
||||
self.arm = dobot_nova5("192.168.5.1")
|
||||
@ -66,7 +62,7 @@ class Calibration:
|
||||
|
||||
def collect_data(self):
|
||||
#移动到初始位置
|
||||
pose = [-45.0009079,55.5785141,-120.68821716,5.11103201,-90.00195312,-90.00085449]
|
||||
pose = [0,30,-120,0,90,0]
|
||||
code = self.arm.RunPoint_P_inJoint(pose)
|
||||
if code == -1:
|
||||
print("运动到拍照位置失败")
|
||||
@ -74,21 +70,16 @@ class Calibration:
|
||||
self.check_corners()
|
||||
#运动到下一个位置
|
||||
# 目前只设计了44条轨迹
|
||||
for i in range (0,18):
|
||||
for i in range (0,45):
|
||||
next_pose = self.get_next_pose_ugly(i)
|
||||
code=self.arm.RunPoint_P_inJoint(next_pose)
|
||||
time.sleep(0.5)
|
||||
self.arm.RunPoint_P_inJoint(next_pose)
|
||||
if code == -1:
|
||||
print("运动到拍照位置失败")
|
||||
else:
|
||||
self.check_corners()
|
||||
time.sleep(0.5)
|
||||
|
||||
def check_corners(self):
|
||||
rgb, depth, intrinsics = self.cam.get_latest_frame()
|
||||
# img_path = f'photo{i}'
|
||||
# filename = os.path.join(self.save_directory, f'{img_path}/rgb_image.png')
|
||||
# rgb=cv2.imread(filename)
|
||||
gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY)
|
||||
ret, corners = cv2.findChessboardCorners(gray, self.board_size, None)
|
||||
if ret:
|
||||
@ -126,7 +117,7 @@ class Calibration:
|
||||
def get_pose_homomat(self):
|
||||
arm_position,arm_orientation = self.arm.get_arm_position()
|
||||
rotation_matrix = R.from_quat(arm_orientation).as_matrix()
|
||||
translation_vector = arm_position
|
||||
translation_vector = arm_position.reshape(3, 1)
|
||||
homo_mat = np.eye(4) # 创建 4x4 单位矩阵
|
||||
homo_mat[:3, :3] = rotation_matrix
|
||||
homo_mat[:3, 3] = translation_vector
|
||||
@ -193,24 +184,52 @@ class Calibration:
|
||||
return None
|
||||
def get_next_pose_ugly(self,index):
|
||||
poselist =[
|
||||
[-4.513801574707031250e+01,5.518265151977539062e+01,-1.237858963012695312e+02,9.085088729858398438e+00,-9.337700653076171875e+01,-9.000016784667968750e+01],
|
||||
[-4.594435882568359375e+01,3.636156082153320312e+01,-1.064173660278320312e+02,-2.125140428543090820e+00,-9.337675476074218750e+01,-9.000016784667968750e+01],
|
||||
[-4.595432662963867188e+01,1.059147930145263672e+01,-8.284246063232421875e+01,-1.484527587890625000e+01,-9.337491607666015625e+01,-9.000013732910156250e+01],
|
||||
[-4.638331604003906250e+01,-6.824899196624755859e+00,-6.758448791503906250e+01,-2.286166954040527344e+01,-9.337348937988281250e+01,-9.000016784667968750e+01],
|
||||
[-4.636801910400390625e+01,-1.807893753051757812e+01,-6.532709503173828125e+01,-2.261142921447753906e+01,-9.336659240722656250e+01,-9.000010681152343750e+01],
|
||||
[-4.637482833862304688e+01,-4.540402412414550781e+00,-1.098018112182617188e+02,3.086268234252929688e+01,-9.305430603027343750e+01,-9.000022125244140625e+01],
|
||||
[-4.640084075927734375e+01,8.068140029907226562e+00,-1.362033843994140625e+02,6.259086990356445312e+01,-9.334789276123046875e+01,-9.000019073486328125e+01],
|
||||
[-4.640056610107421875e+01,8.061657905578613281e+00,-1.511797027587890625e+02,8.927394104003906250e+01,-9.331819915771484375e+01,-9.000019073486328125e+01],
|
||||
[-4.638466262817382812e+01,3.287191867828369141e+00,-1.566032409667968750e+02,1.035896453857421875e+02,-9.330465698242187500e+01,-9.000022125244140625e+01],
|
||||
[-3.178509473800659180e+00,6.977911472320556641e+00,-1.417592010498046875e+02,6.005132293701171875e+01,-1.280102996826171875e+02,-9.000010681152343750e+01],
|
||||
[-1.424913024902343750e+01,-2.980865538120269775e-01,-1.193396759033203125e+02,3.752756500244140625e+01,-1.278784713745117188e+02,-8.999975585937500000e+01],
|
||||
[-2.393187522888183594e+01,-2.973175048828125000e-01,-1.121088027954101562e+02,3.928178024291992188e+01,-1.183067703247070312e+02,-8.999983215332031250e+01],
|
||||
[-3.699544525146484375e+01,-2.981689572334289551e-01,-1.078408889770507812e+02,3.178039932250976562e+01,-1.024006271362304688e+02,-8.999983215332031250e+01],
|
||||
[-6.734904479980468750e+01,-4.368630886077880859e+00,-1.035898971557617188e+02,2.342595481872558594e+01,-7.085527801513671875e+01,-8.999980926513671875e+01],
|
||||
[-8.632910919189453125e+01,-1.039798259735107422e+01,-1.038105545043945312e+02,2.330961036682128906e+01,-5.476583099365234375e+01,-8.999980926513671875e+01],
|
||||
[-8.156823730468750000e+01,5.878097534179687500e+00,-1.439221954345703125e+02,6.846669006347656250e+01,-4.482905197143554688e+01,-8.999922943115234375e+01],
|
||||
[-8.601764678955078125e+01,3.570293045043945312e+01,-1.308228912353515625e+02,2.318510818481445312e+01,-5.378013610839843750e+01,-8.999922943115234375e+01],
|
||||
[-7.132132053375244141e+00,3.722338104248046875e+01,-1.065378265380859375e+02,-3.337371826171875000e+00,-1.185582199096679688e+02,-8.999931335449218750e+01]
|
||||
[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]
|
||||
|
||||
@ -221,7 +240,7 @@ if __name__ == '__main__':
|
||||
# 标定板的width 对应的角点的个数 6
|
||||
# 标定板的height 对应的角点的个数 3
|
||||
calibration_board_size = [6,3]
|
||||
calibration_board_square_size = 0.03 #unit - meter
|
||||
calibration_board_square_size = 0.027 #unit - meter
|
||||
systemPath = "/home/jsfb/Documents/"
|
||||
|
||||
now = datetime.now()
|
||||
@ -231,22 +250,10 @@ if __name__ == '__main__':
|
||||
os.makedirs(systemPath, exist_ok=True)
|
||||
|
||||
calib = Calibration(calibration_board_size,calibration_board_square_size,systemPath)
|
||||
|
||||
time.sleep(0.5)
|
||||
calib.arm.chooseBaseFrame(i=1)
|
||||
calib.arm.chooseEndEffector(i=0)
|
||||
time.sleep(0.5)
|
||||
|
||||
calib.collect_data()
|
||||
print("=================数据采集完成===================")
|
||||
R, t,intrin = calib.calibrate()
|
||||
|
||||
print("旋转矩阵：")
|
||||
print(R)
|
||||
print("平移向量：")
|
||||
print(t)
|
||||
print(f"内参： {intrin}")
|
||||
time.sleep(0.5)
|
||||
calib.cam.stop()
|
||||
time.sleep(0.5)
|
||||
print('camera stopped')
|
||||
|
||||
69
Massage/MassageControl/tools/capture.py
Normal file
69
Massage/MassageControl/tools/capture.py
Normal file
@ -0,0 +1,69 @@
|
||||
import os
|
||||
import sys
|
||||
current_file_path = os.path.abspath(__file__)
|
||||
dobot_nova5_path = os.path.dirname(os.path.dirname(current_file_path))
|
||||
print(dobot_nova5_path)
|
||||
sys.path.append(dobot_nova5_path)
|
||||
|
||||
from hardware.dobot_nova5 import dobot_nova5
|
||||
import cv2
|
||||
from hardware.remote_cam import ToolCamera
|
||||
import threading
|
||||
import numpy as np
|
||||
import time
|
||||
class Getpose:
|
||||
def __init__(self):
|
||||
self.arm = dobot_nova5()
|
||||
self.arm.start()
|
||||
self.arm.init()
|
||||
self.cam = ToolCamera(host='127.0.0.1')
|
||||
self.cam.start()
|
||||
time.sleep(2)
|
||||
self.thread1=threading.Thread(target=self.show)
|
||||
self.thread1.start()
|
||||
self.pose_data=[]
|
||||
self.image_data=[]
|
||||
self.arm.start_drag()
|
||||
|
||||
def show(self):
|
||||
while True:
|
||||
self.rgb, self.depth, intrinsics = self.cam.get_latest_frame()
|
||||
# print("cv:",self.rgb)
|
||||
cv2.imshow("1111",self.rgb)
|
||||
cv2.waitKey(1)
|
||||
time.sleep(0.2)
|
||||
|
||||
def add_data(self):
|
||||
angle=self.arm.getAngel()
|
||||
self.pose_data=self.pose_data.append(angle)
|
||||
print(angle)
|
||||
|
||||
def save_data(self):
|
||||
data=np.array(self.pose_data)
|
||||
np.savetxt('pose.txt', data ,fmt='%.5f')
|
||||
|
||||
if __name__ == "__main__":
|
||||
save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images'
|
||||
if not os.path.exists(save_directory):
|
||||
os.makedirs(save_directory)
|
||||
sele=Getpose()
|
||||
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
while True:
|
||||
i=1
|
||||
if key == ord('s'):
|
||||
img_path = f'photo{i}.png'
|
||||
filename = os.path.join(save_directory, img_path)
|
||||
cv2.imwrite(filename, sele.rgb) # 保存照片
|
||||
sele.add_data()
|
||||
|
||||
filename2 = os.path.join(save_directory, f'pose.txt')
|
||||
np.savetxt(filename2, sele.pose_data)
|
||||
# pose = increase_dof(poselist[index])
|
||||
|
||||
i += 1
|
||||
# 按下'q'键退出循环
|
||||
elif key == ord('q'):
|
||||
sele.arm.disableRobot()
|
||||
break
|
||||
|
||||
0
Massage/controller.service
Executable file → Normal file
0
Massage/controller.service
Executable file → Normal file
0
Massage/test_manual.py
Executable file → Normal file
0
Massage/test_manual.py
Executable file → Normal file
0
Restart_Speaker.py
Executable file → Normal file
0
Restart_Speaker.py
Executable file → Normal file
0
clear_pyc.sh
Executable file → Normal file
0
clear_pyc.sh
Executable file → Normal file
@ -1,7 +0,0 @@
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
import numpy as np
|
||||
|
||||
arm_orientation_set0 = np.array([-180,-30,0])
|
||||
b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
|
||||
tcp_offset = b_rotation_s_set0 @ np.array([0,0,154.071])
|
||||
print(tcp_offset)
|
||||
0
py2json.py
Executable file → Normal file
0
py2json.py
Executable file → Normal file
0
restart_language.sh
Executable file → Normal file
0
restart_language.sh
Executable file → Normal file
Loading…
x
Reference in New Issue
Block a user