stash
This commit is contained in:
LiangShiyun
parent
961a86d423
commit
3981c448c4
@ -1860,7 +1860,7 @@ class TaskBase(smach.State):
|
|||||||
|
|
||||||
if self.resources.robot.force_sensor:
|
if self.resources.robot.force_sensor:
|
||||||
self.resources.robot.force_sensor.stop_background_reading()
|
self.resources.robot.force_sensor.stop_background_reading()
|
||||||
self.resources.robot.force_sensor = XjcSensor(arm_ip=self.resources.robot.arm_config['arm_ip']) # 初始化实例化力传感器
|
self.resources.robot.force_sensor = XjcSensor() # 初始化实例化力传感器
|
||||||
time.sleep(0.2)
|
time.sleep(0.2)
|
||||||
code = self.resources.robot.sensor_set_zero()
|
code = self.resources.robot.sensor_set_zero()
|
||||||
if code != 0:
|
if code != 0:
|
||||||
@ -3037,7 +3037,7 @@ class EmergencyExitState(smach.State):
|
|||||||
# 这里可以处理一些清理工作
|
# 这里可以处理一些清理工作
|
||||||
self.resources.logger.log_error("Emergency exit triggered, shutting down...")
|
self.resources.logger.log_error("Emergency exit triggered, shutting down...")
|
||||||
self.resources.robot.stop()
|
self.resources.robot.stop()
|
||||||
self.resources.robot.arm.power_off()
|
# self.resources.robot.arm.power_off()
|
||||||
self.resources.stop_event.clear()
|
self.resources.stop_event.clear()
|
||||||
# 或者使用 sys.exit() 直接退出程序
|
# 或者使用 sys.exit() 直接退出程序
|
||||||
# sys.exit(1)
|
# sys.exit(1)
|
||||||
|
|||||||
@ -115,6 +115,7 @@ class MassageRobot:
|
|||||||
# arm 实例化时机械臂类内部进行通讯连接
|
# arm 实例化时机械臂类内部进行通讯连接
|
||||||
self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip'])
|
self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip'])
|
||||||
# self.arm.start()
|
# self.arm.start()
|
||||||
|
# self.arm.chooseRightFrame()
|
||||||
self.arm.init()
|
self.arm.init()
|
||||||
|
|
||||||
self.thermotherapy = None
|
self.thermotherapy = None
|
||||||
@ -165,15 +166,16 @@ class MassageRobot:
|
|||||||
self.skip_pos = np.zeros(6,dtype=np.float64)
|
self.skip_pos = np.zeros(6,dtype=np.float64)
|
||||||
|
|
||||||
#按摩头参数暂时使用本地数据
|
#按摩头参数暂时使用本地数据
|
||||||
massage_head_dir = self.arm_config['massage_head_dir']
|
# massage_head_dir = self.arm_config['massage_head_dir']
|
||||||
all_items = os.listdir(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))]
|
# head_config_files = [f for f in all_items if os.path.isfile(os.path.join(massage_head_dir, f))]
|
||||||
self.playload_dict = {}
|
# self.playload_dict = {}
|
||||||
for file in head_config_files:
|
# for file in head_config_files:
|
||||||
file_address = massage_head_dir + '/' + file
|
# file_address = massage_head_dir + '/' + file
|
||||||
play_load = read_yaml(file_address)
|
# play_load = read_yaml(file_address)
|
||||||
self.playload_dict[play_load['name']] = play_load
|
# self.playload_dict[play_load['name']] = play_load
|
||||||
# self.playload_dict = self.vtxdb.get("robot_config", "massage_head")
|
|
||||||
|
self.playload_dict = self.vtxdb.get("robot_config", "massage_head")
|
||||||
# print(self.playload_dict)
|
# print(self.playload_dict)
|
||||||
self.current_head = 'none'
|
self.current_head = 'none'
|
||||||
self.is_waitting = False
|
self.is_waitting = False
|
||||||
@ -220,6 +222,7 @@ class MassageRobot:
|
|||||||
# ---
|
# ---
|
||||||
self.last_time = -1
|
self.last_time = -1
|
||||||
self.cur_time = -1
|
self.cur_time = -1
|
||||||
|
self.tool_num = 0
|
||||||
|
|
||||||
# 预测步骤
|
# 预测步骤
|
||||||
def kalman_predict(self,x, P, Q):
|
def kalman_predict(self,x, P, Q):
|
||||||
@ -417,8 +420,10 @@ class MassageRobot:
|
|||||||
tcp_offset = self.playload_dict[name]["tcp_offset"]
|
tcp_offset = self.playload_dict[name]["tcp_offset"]
|
||||||
tcp_offset_str = "{" + ",".join(map(str, tcp_offset)) + "}"
|
tcp_offset_str = "{" + ",".join(map(str, tcp_offset)) + "}"
|
||||||
print("tcp_offset_str",tcp_offset_str)
|
print("tcp_offset_str",tcp_offset_str)
|
||||||
self.arm.setEndEffector(i=9,tool_i=tcp_offset_str)
|
self.tool_num = 9
|
||||||
self.arm.chooseEndEffector(i=9)
|
self.arm.setEndEffector(i=self.tool_num,tool_i=tcp_offset_str)
|
||||||
|
self.arm.chooseEndEffector(i=self.tool_num)
|
||||||
|
|
||||||
print(self.arm.get_arm_position())
|
print(self.arm.get_arm_position())
|
||||||
self.logger.log_info(f"切换到{name}按摩头")
|
self.logger.log_info(f"切换到{name}按摩头")
|
||||||
|
|
||||||
@ -554,9 +559,9 @@ class MassageRobot:
|
|||||||
time.sleep(sleep_duration)
|
time.sleep(sleep_duration)
|
||||||
# print(f"real sleep:{time.time()-b2}")
|
# print(f"real sleep:{time.time()-b2}")
|
||||||
|
|
||||||
# print("self111:",self.arm.feedbackData.robotMode)
|
# print("self111:",self.arm.feedbackData.robotMode,tcp_command)
|
||||||
self.arm.dashboard.socket_dobot.sendall(tcp_command)
|
self.arm.dashboard.socket_dobot.sendall(tcp_command)
|
||||||
# print("self111222:",self.arm.feedbackData.robotMode)
|
# print("self111222:",self.arm.feedbackData.robotMode,tcp_command)
|
||||||
if self.arm.feedbackData.robotMode in [3, 4, 6, 8, 9, 11] or not self.arm.feedbackData.EnableState:
|
if self.arm.feedbackData.robotMode in [3, 4, 6, 8, 9, 11] or not self.arm.feedbackData.EnableState:
|
||||||
self.logger.log_error(f"机械臂异常:{self.arm.feedbackData.robotMode}")
|
self.logger.log_error(f"机械臂异常:{self.arm.feedbackData.robotMode}")
|
||||||
self.arm.dashboard.Stop()
|
self.arm.dashboard.Stop()
|
||||||
@ -665,14 +670,20 @@ class MassageRobot:
|
|||||||
|
|
||||||
@track_function("set_position",True)
|
@track_function("set_position",True)
|
||||||
def set_position(self,pose,is_wait = False):
|
def set_position(self,pose,is_wait = False):
|
||||||
|
print("self.robotmode1:",self.arm.feedbackData.robotMode)
|
||||||
self.logger.log_info(f"set postion:{pose}")
|
self.logger.log_info(f"set postion:{pose}")
|
||||||
x,y,z = pose[0],pose[1],pose[2]
|
x,y,z = pose[0],pose[1],pose[2]
|
||||||
roll,pitch,yaw = pose[3],pose[4],pose[5]
|
roll,pitch,yaw = pose[3],pose[4],pose[5]
|
||||||
pose_command = np.concatenate([[x,y,z], [roll,pitch,yaw]])
|
pose_command = np.concatenate([np.array([x,y,z]), np.array([roll,pitch,yaw])])
|
||||||
time.sleep(0.1)
|
time.sleep(0.1)
|
||||||
code = self.arm.RunPoint_P_inPose_M_RAD(poses_list = pose_command)
|
print("self.robotmode2:",self.arm.feedbackData.robotMode)
|
||||||
|
# inverse_joint = self.arm.getInverseKin(poses_list = pose_command,tool = self.tool_num)
|
||||||
|
|
||||||
|
code = self.arm.RunPoint_P_inPose_M_RAD(pose = pose_command)
|
||||||
time.sleep(0.1)
|
time.sleep(0.1)
|
||||||
return code
|
# print("inverse_joint:",inverse_joint,np.array(inverse_joint)*np.pi/180)
|
||||||
|
# code = self.arm.move_joint(q = np.array(inverse_joint)*np.pi/180)
|
||||||
|
# return code
|
||||||
|
|
||||||
@track_function("move_to_point",True)
|
@track_function("move_to_point",True)
|
||||||
def move_to_point(self, t, pose, is_interrupt=True, wait=True, timeout=0.5, interpolation:Literal["linear","circle","cloud_point"] ='linear', algorithm:Literal["admittance","position","hybrid","admithybrid","hybridPid","hybridAdmit","admittanceZ"] = "admittance",is_switch_controller = False):
|
def move_to_point(self, t, pose, is_interrupt=True, wait=True, timeout=0.5, interpolation:Literal["linear","circle","cloud_point"] ='linear', algorithm:Literal["admittance","position","hybrid","admithybrid","hybridPid","hybridAdmit","admittanceZ"] = "admittance",is_switch_controller = False):
|
||||||
@ -1671,8 +1682,8 @@ if __name__ == "__main__":
|
|||||||
# robot.dt = ts[1] - ts[0]
|
# robot.dt = ts[1] - ts[0]
|
||||||
|
|
||||||
|
|
||||||
ready_pose = [0.1543467,-0.1349880,0.4553604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
|
ready_pose = [0.4543467,-0.1349880,0.4553604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
|
||||||
robot.current_head = 'finger_head'
|
robot.current_head = 'none'
|
||||||
|
|
||||||
robot.force_sensor.disable_active_transmission()
|
robot.force_sensor.disable_active_transmission()
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
@ -1683,15 +1694,24 @@ if __name__ == "__main__":
|
|||||||
robot.force_sensor.set_zero()
|
robot.force_sensor.set_zero()
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
robot.force_sensor.enable_active_transmission()
|
robot.force_sensor.enable_active_transmission()
|
||||||
|
robot.set_position(pose = ready_pose)
|
||||||
|
|
||||||
robot.init_hardwares(ready_pose)
|
robot.init_hardwares(ready_pose)
|
||||||
time.sleep(3)
|
time.sleep(3)
|
||||||
robot.switch_payload("none")
|
robot.switch_payload("finger_head")
|
||||||
robot.controller_manager.switch_controller('admittance')
|
robot.controller_manager.switch_controller('admittance')
|
||||||
position,quat_rot = robot.arm.get_arm_position()
|
position,quat_rot = robot.arm.get_arm_position()
|
||||||
euler_rot = R.from_quat(quat_rot).as_euler('xyz')
|
euler_rot = R.from_quat(quat_rot).as_euler('xyz')
|
||||||
target_point = [position[0], position[1], position[2], euler_rot[0], euler_rot[1], euler_rot[2]]
|
# target_point = [position[0], position[1], position[2], euler_rot[0], euler_rot[1], euler_rot[2]]
|
||||||
robot.move_to_point(pose=target_point,t=60)
|
target_point = [0.12191350715303009, -0.1636467057977184, 0.007338312730729687, 8.37758040957278e-05, 1.5708137800874167, -1.5708329787091884]
|
||||||
|
|
||||||
|
# target_point = [0.3543467,-0.1349880,0.4553604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
|
||||||
|
# robot.move_to_point(pose=target_point,t=60)
|
||||||
|
robot.arm.move_joint(q = robot.arm.off_pos)
|
||||||
|
robot.arm.move_joint(q = robot.arm.init_pos)
|
||||||
|
pose1 = [0.353467,-0.1349880,0.403604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
|
||||||
|
robot.set_position(pose = pose1)
|
||||||
|
robot.set_position([0.12189544299531646, -0.31364670468715944, 0.00733569473686102, 2.153932270623249, 0.6209756952505509, 0.0])
|
||||||
|
|
||||||
# robot.controller_manager.switch_controller('position')
|
# robot.controller_manager.switch_controller('position')
|
||||||
|
|
||||||
|
|||||||
@ -40,97 +40,105 @@ class AdmittanceController(BaseController):
|
|||||||
|
|
||||||
self.laset_print_time = 0
|
self.laset_print_time = 0
|
||||||
|
|
||||||
# def step(self,dt):
|
|
||||||
# # 方向统一
|
|
||||||
# if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
|
|
||||||
# self.state.arm_orientation = -self.state.arm_orientation
|
|
||||||
# # 缓存常用计算
|
|
||||||
# arm_ori_quat = R.from_quat(self.state.arm_orientation)
|
|
||||||
# arm_ori_mat = arm_ori_quat.as_matrix()
|
|
||||||
# # 位置误差
|
|
||||||
# temp_pose_error = self.state.arm_position - self.state.desired_position
|
|
||||||
# self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
|
|
||||||
# # 姿态误差(四元数)
|
|
||||||
# rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
|
|
||||||
# self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
|
|
||||||
# # 期望加速度
|
|
||||||
# wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
|
|
||||||
# D_vel = self.D @ (self.state.arm_desired_twist - self.state.desired_twist)
|
|
||||||
# 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
|
|
||||||
# self.clip_command(self.state.arm_desired_twist, "vel")
|
|
||||||
# # 计算位姿变化
|
|
||||||
# delta_pose = np.concatenate([
|
|
||||||
# arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt),
|
|
||||||
# self.state.arm_desired_twist[3:] * dt
|
|
||||||
# ])
|
|
||||||
# self.clip_command(delta_pose, "pose")
|
|
||||||
# # 更新四元数
|
|
||||||
# delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat()
|
|
||||||
# arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat)
|
|
||||||
# self.state.arm_orientation_command = arm_ori_quat_new.as_quat()
|
|
||||||
# # 归一化四元数
|
|
||||||
# self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
|
|
||||||
# # 更新位置
|
|
||||||
# self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
|
|
||||||
def step(self,dt):
|
def step(self,dt):
|
||||||
# 计算误差 位置直接作差，姿态误差以旋转向量表示
|
# 方向统一
|
||||||
|
|
||||||
temp_pose_error = self.state.arm_position - self.state.desired_position
|
|
||||||
# if time.time() - self.laset_print_time > 5:
|
|
||||||
# print(f'temp_pose_error: {temp_pose_error} ||| arm_position: {self.state.arm_position} ||| desired_position: {self.state.desired_position}')
|
|
||||||
if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
|
if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
|
||||||
self.state.arm_orientation = -self.state.arm_orientation
|
self.state.arm_orientation = -self.state.arm_orientation
|
||||||
|
# 缓存常用计算
|
||||||
self.state.pose_error[:3] = R.from_quat(self.state.arm_orientation).as_matrix().T @ temp_pose_error
|
arm_ori_quat = R.from_quat(self.state.arm_orientation)
|
||||||
# if time.time() - self.laset_print_time > 5:
|
arm_ori_mat = arm_ori_quat.as_matrix()
|
||||||
# print("pose_error:",self.state.pose_error[:3])
|
# 位置误差
|
||||||
# 计算误差 位置直接作差，姿态误差以旋转向量表示
|
temp_pose_error = self.state.arm_position - self.state.desired_position
|
||||||
#rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ R.from_quat(self.state.desired_orientation).as_matrix().T
|
self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error
|
||||||
rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix().T @ R.from_quat(self.state.desired_orientation).as_matrix()
|
# 姿态误差(四元数)
|
||||||
# print(f'rot_err_mat: {rot_err_mat} ||| arm_orientation: {R.from_quat(self.state.arm_orientation).as_euler('xyz',False)} ||| desired_orientation: {R.from_quat(self.state.desired_orientation).as_euler('xyz',False)}')
|
rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation)
|
||||||
rot_err_rotvex = R.from_matrix(rot_err_mat).as_rotvec(degrees=False)
|
self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False)
|
||||||
self.state.pose_error[3:] = -rot_err_rotvex
|
# 期望加速度
|
||||||
|
|
||||||
#wrench_err = self.state.external_wrench_base - self.state.desired_wrench
|
|
||||||
wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
|
wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
|
||||||
if time.time() - self.laset_print_time > 5:
|
D_vel = self.D @ (self.state.arm_desired_twist - self.state.desired_twist)
|
||||||
print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}')
|
K_pose = self.K @ self.state.pose_error
|
||||||
self.state.arm_desired_acc = np.linalg.inv(self.M) @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error)
|
self.state.arm_desired_acc = self.M_inv @ (wrench_err - D_vel - K_pose)
|
||||||
# if time.time() - self.laset_print_time > 5:
|
|
||||||
# print("@@@:",wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error)
|
|
||||||
self.clip_command(self.state.arm_desired_acc, "acc")
|
self.clip_command(self.state.arm_desired_acc, "acc")
|
||||||
self.state.arm_desired_twist = self.state.arm_desired_acc * dt + self.state.arm_desired_twist
|
## 更新速度和位姿
|
||||||
|
self.state.arm_desired_twist += self.state.arm_desired_acc * dt
|
||||||
self.clip_command(self.state.arm_desired_twist, "vel")
|
self.clip_command(self.state.arm_desired_twist, "vel")
|
||||||
delta_pose = self.state.arm_desired_twist * dt
|
# 计算位姿变化
|
||||||
|
delta_pose = np.concatenate([
|
||||||
delta_pose[:3] = self.pos_scale_factor * delta_pose[:3]
|
arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt),
|
||||||
delta_pose[3:] = self.rot_scale_factor * delta_pose[3:]
|
self.state.arm_desired_twist[3:] * dt
|
||||||
# if time.time() - self.laset_print_time > 5:
|
])
|
||||||
# print("delta_pose:",delta_pose)
|
|
||||||
|
|
||||||
delta_pose[:3] = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_pose[:3]
|
|
||||||
|
|
||||||
# if time.time() - self.laset_print_time > 5:
|
|
||||||
# print("tf_delta_pose:",delta_pose)
|
|
||||||
self.clip_command(delta_pose, "pose")
|
self.clip_command(delta_pose, "pose")
|
||||||
|
|
||||||
# testlsy
|
# # update position
|
||||||
delta_ori_mat = R.from_rotvec(delta_pose[3:]).as_matrix()
|
# delta_pose[:3] = arm_ori_mat @ delta_pose[:3]
|
||||||
|
|
||||||
#arm_ori_mat = delta_ori_mat @ R.from_quat(self.state.arm_orientation).as_matrix()
|
# 更新四元数
|
||||||
arm_ori_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_ori_mat
|
delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat()
|
||||||
self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat()
|
arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat)
|
||||||
|
self.state.arm_orientation_command = arm_ori_quat_new.as_quat()
|
||||||
# arm_ori_mat = R.from_quat(self.state.arm_orientation).as_rotvec(degrees=False) + delta_pose[3:]
|
# 归一化四元数
|
||||||
# self.state.arm_orientation_command = R.from_rotvec(arm_ori_mat).as_quat()
|
self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command)
|
||||||
|
# 更新位置
|
||||||
# self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat()
|
|
||||||
self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
|
self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
|
||||||
|
|
||||||
|
if time.time() - self.laset_print_time > 1:
|
||||||
|
print("11111111111111111111111:",self.state.desired_position,self.state.arm_position_command,self.state.arm_desired_acc)
|
||||||
|
|
||||||
|
# def step(self,dt):
|
||||||
|
# # 计算误差 位置直接作差，姿态误差以旋转向量表示
|
||||||
|
|
||||||
|
# temp_pose_error = self.state.arm_position - self.state.desired_position
|
||||||
|
# # if time.time() - self.laset_print_time > 5:
|
||||||
|
# # print(f'temp_pose_error: {temp_pose_error} ||| arm_position: {self.state.arm_position} ||| desired_position: {self.state.desired_position}')
|
||||||
|
# if self.state.desired_orientation.dot(self.state.arm_orientation) < 0:
|
||||||
|
# self.state.arm_orientation = -self.state.arm_orientation
|
||||||
|
|
||||||
|
# self.state.pose_error[:3] = R.from_quat(self.state.arm_orientation).as_matrix().T @ temp_pose_error
|
||||||
|
# # if time.time() - self.laset_print_time > 5:
|
||||||
|
# # print("pose_error:",self.state.pose_error[:3])
|
||||||
|
# # 计算误差 位置直接作差，姿态误差以旋转向量表示
|
||||||
|
# #rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ R.from_quat(self.state.desired_orientation).as_matrix().T
|
||||||
|
# rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix().T @ R.from_quat(self.state.desired_orientation).as_matrix()
|
||||||
|
# # print(f'rot_err_mat: {rot_err_mat} ||| arm_orientation: {R.from_quat(self.state.arm_orientation).as_euler('xyz',False)} ||| desired_orientation: {R.from_quat(self.state.desired_orientation).as_euler('xyz',False)}')
|
||||||
|
# rot_err_rotvex = R.from_matrix(rot_err_mat).as_rotvec(degrees=False)
|
||||||
|
# self.state.pose_error[3:] = -rot_err_rotvex
|
||||||
|
|
||||||
|
# #wrench_err = self.state.external_wrench_base - self.state.desired_wrench
|
||||||
|
# wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
|
||||||
|
# if time.time() - self.laset_print_time > 5:
|
||||||
|
# print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}')
|
||||||
|
# self.state.arm_desired_acc = np.linalg.inv(self.M) @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error)
|
||||||
|
# # if time.time() - self.laset_print_time > 5:
|
||||||
|
# # print("@@@:",wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error)
|
||||||
|
# self.clip_command(self.state.arm_desired_acc,"acc")
|
||||||
|
# self.state.arm_desired_twist = self.state.arm_desired_acc * dt + self.state.arm_desired_twist
|
||||||
|
# self.clip_command(self.state.arm_desired_twist,"vel")
|
||||||
|
# delta_pose = self.state.arm_desired_twist * dt
|
||||||
|
|
||||||
|
# delta_pose[:3] = self.pos_scale_factor * delta_pose[:3]
|
||||||
|
# delta_pose[3:] = self.rot_scale_factor * delta_pose[3:]
|
||||||
|
# # if time.time() - self.laset_print_time > 5:
|
||||||
|
# # print("delta_pose:",delta_pose)
|
||||||
|
|
||||||
|
# delta_pose[:3] = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_pose[:3]
|
||||||
|
|
||||||
|
# # if time.time() - self.laset_print_time > 5:
|
||||||
|
# # print("tf_delta_pose:",delta_pose)
|
||||||
|
# self.clip_command(delta_pose,"pose")
|
||||||
|
|
||||||
|
# # testlsy
|
||||||
|
# delta_ori_mat = R.from_rotvec(delta_pose[3:]).as_matrix()
|
||||||
|
|
||||||
|
# #arm_ori_mat = delta_ori_mat @ R.from_quat(self.state.arm_orientation).as_matrix()
|
||||||
|
# arm_ori_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_ori_mat
|
||||||
|
# self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat()
|
||||||
|
|
||||||
|
# # arm_ori_mat = R.from_quat(self.state.arm_orientation).as_rotvec(degrees=False) + delta_pose[3:]
|
||||||
|
# # self.state.arm_orientation_command = R.from_rotvec(arm_ori_mat).as_quat()
|
||||||
|
|
||||||
|
# # self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat()
|
||||||
|
# self.state.arm_position_command = self.state.arm_position + delta_pose[:3]
|
||||||
|
|
||||||
|
|
||||||
# if time.time() - self.laset_print_time > 1:
|
# if time.time() - self.laset_print_time > 1:
|
||||||
# print("11111111111111111111111:",self.state.desired_position,self.state.arm_position_command,self.state.arm_position)
|
# print("11111111111111111111111:",self.state.desired_position,self.state.arm_position_command,self.state.arm_position)
|
||||||
|
|||||||
@ -8,10 +8,10 @@ mass_rot: [0.11, 0.11, 0.11]
|
|||||||
# P
|
# P
|
||||||
# stiff_tran: [300, 300, 300]
|
# stiff_tran: [300, 300, 300]
|
||||||
# stiff_rot: [7, 7, 7]
|
# stiff_rot: [7, 7, 7]
|
||||||
# stiff_tran: [270, 270, 270]
|
stiff_tran: [270, 270, 270]
|
||||||
# stiff_rot: [11, 11, 11]
|
|
||||||
stiff_tran: [0, 0, 0]
|
|
||||||
stiff_rot: [11, 11, 11]
|
stiff_rot: [11, 11, 11]
|
||||||
|
# stiff_tran: [0, 0, 0]
|
||||||
|
# stiff_rot: [11, 11, 11]
|
||||||
# stiff_tran: [100, 100, 100]
|
# stiff_tran: [100, 100, 100]
|
||||||
# stiff_rot: [1, 1, 1]
|
# stiff_rot: [1, 1, 1]
|
||||||
# D
|
# D
|
||||||
|
|||||||
@ -1893,6 +1893,7 @@ class DobotApiDashboard(DobotApi):
|
|||||||
else:
|
else:
|
||||||
print("coordinateMode param is wrong")
|
print("coordinateMode param is wrong")
|
||||||
return ""
|
return ""
|
||||||
|
print("movJ:string,",string)
|
||||||
params = []
|
params = []
|
||||||
if user != -1:
|
if user != -1:
|
||||||
params.append('user={:d}'.format(user))
|
params.append('user={:d}'.format(user))
|
||||||
|
|||||||
@ -148,8 +148,13 @@ class dobot_nova5:
|
|||||||
self.feedFour = None
|
self.feedFour = None
|
||||||
|
|
||||||
atexit.register(self.exit_task)
|
atexit.register(self.exit_task)
|
||||||
|
self.chooseRightFrame()
|
||||||
self.init_pos = [-45.0009079*np.pi/180,55.5785141*np.pi/180,-120.68821716*np.pi/180,5.11103201*np.pi/180,90.00195312*np.pi/180,-90.00085449*np.pi/180]
|
self.init_pos = [-45.0009079*np.pi/180,55.5785141*np.pi/180,-120.68821716*np.pi/180,5.11103201*np.pi/180,90.00195312*np.pi/180,-90.00085449*np.pi/180]
|
||||||
|
self.standby_pos = [-45.0009079*np.pi/180,55.5785141*np.pi/180,-120.68821716*np.pi/180,5.11103201*np.pi/180,90.00195312*np.pi/180,-90.00085449*np.pi/180]
|
||||||
self.off_pos = [-44.996*np.pi/180,87.0092*np.pi/180,-147.0087*np.pi/180,-0.0048*np.pi/180,90.0010*np.pi/180,-90.0021*np.pi/180]
|
self.off_pos = [-44.996*np.pi/180,87.0092*np.pi/180,-147.0087*np.pi/180,-0.0048*np.pi/180,90.0010*np.pi/180,-90.0021*np.pi/180]
|
||||||
|
self.cam_pose = [-45.0050*np.pi/180,30.0682*np.pi/180,-66.1887*np.pi/180,-21.8836*np.pi/180,-90.3064*np.pi/180,-89.9977*np.pi/180]
|
||||||
|
self.cam_length = 1.75
|
||||||
|
self.level_base = [-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
|
||||||
self.is_exit = False
|
self.is_exit = False
|
||||||
self.is_standby = False
|
self.is_standby = False
|
||||||
self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
|
self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
|
||||||
@ -281,6 +286,7 @@ class dobot_nova5:
|
|||||||
|
|
||||||
def waitArrival(self, command):
|
def waitArrival(self, command):
|
||||||
sendCommandID = self.parseResultId(command)[1]
|
sendCommandID = self.parseResultId(command)[1]
|
||||||
|
print("self.feedbackData.robotMode6:",self.feedbackData.robotMode)
|
||||||
# print("self.parseResultId(command)",self.parseResultId(command))
|
# print("self.parseResultId(command)",self.parseResultId(command))
|
||||||
while True:
|
while True:
|
||||||
if self.feedbackData.robotMode in [3, 4, 6, 9, 10, 11]:
|
if self.feedbackData.robotMode in [3, 4, 6, 9, 10, 11]:
|
||||||
@ -290,7 +296,7 @@ class dobot_nova5:
|
|||||||
self.dashboard.EmergencyStop(mode=1)
|
self.dashboard.EmergencyStop(mode=1)
|
||||||
return -1
|
return -1
|
||||||
if self.feedbackData.EnableState:
|
if self.feedbackData.EnableState:
|
||||||
print("self.feedbackData.robotCurrentCommandID",self.feedbackData.robotCurrentCommandID)
|
# print("self.feedbackData.robotCurrentCommandID",self.feedbackData.robotCurrentCommandID)
|
||||||
if self.feedbackData.robotCurrentCommandID > sendCommandID:
|
if self.feedbackData.robotCurrentCommandID > sendCommandID:
|
||||||
break
|
break
|
||||||
else:
|
else:
|
||||||
@ -315,11 +321,13 @@ class dobot_nova5:
|
|||||||
|
|
||||||
is_arrived = self.waitArrival(command)
|
is_arrived = self.waitArrival(command)
|
||||||
if is_arrived == 0:
|
if is_arrived == 0:
|
||||||
|
# self.dashboard.MovJ()
|
||||||
print(f'Arrived at the joint position: {q}')
|
print(f'Arrived at the joint position: {q}')
|
||||||
return 0
|
return 0
|
||||||
else:
|
else:
|
||||||
print(f'Failed to arrive at the joint position: {q}')
|
print(f'Failed to arrive at the joint position: {q}')
|
||||||
return -1
|
return -1
|
||||||
|
|
||||||
print(f'After {cnt} retries, still failed to arrive at the joint position: {q}')
|
print(f'After {cnt} retries, still failed to arrive at the joint position: {q}')
|
||||||
return -1
|
return -1
|
||||||
|
|
||||||
@ -333,7 +341,7 @@ class dobot_nova5:
|
|||||||
targetAngle = self.__transform_rad_2_deg(q)
|
targetAngle = self.__transform_rad_2_deg(q)
|
||||||
currentAngle = np.array(currentAngle)
|
currentAngle = np.array(currentAngle)
|
||||||
targetAngle = np.array(targetAngle)
|
targetAngle = np.array(targetAngle)
|
||||||
self.dashboard.SpeedFactor(70)
|
self.dashboard.SpeedFactor(50)
|
||||||
|
|
||||||
for i in range(len(targetAngle)):
|
for i in range(len(targetAngle)):
|
||||||
while np.abs(currentAngle[i] - targetAngle[i]) >threshold:
|
while np.abs(currentAngle[i] - targetAngle[i]) >threshold:
|
||||||
@ -356,12 +364,15 @@ class dobot_nova5:
|
|||||||
# 单位 m/rad -> mm/deg
|
# 单位 m/rad -> mm/deg
|
||||||
max_retry_count = params.get('max_retry_count', 3)
|
max_retry_count = params.get('max_retry_count', 3)
|
||||||
cnt = 0
|
cnt = 0
|
||||||
|
print("self.robotmode3:",self.feedbackData.robotMode)
|
||||||
poses_list_mm_deg = self.__transform_m_rad_2_mm_deg(pose)
|
poses_list_mm_deg = self.__transform_m_rad_2_mm_deg(pose)
|
||||||
while cnt < max_retry_count:
|
while cnt < max_retry_count:
|
||||||
cnt += 1
|
cnt += 1
|
||||||
|
print("self.robotmode4:",self.feedbackData.robotMode)
|
||||||
command = self.dashboard.MovJ(*poses_list_mm_deg,0)
|
command = self.dashboard.MovJ(*poses_list_mm_deg,0)
|
||||||
|
print("self.robotmode5:",self.feedbackData.robotMode)
|
||||||
|
|
||||||
is_arrived = self.waitArrival(command)
|
is_arrived = self.waitArrival(command = command)
|
||||||
if is_arrived == 0:
|
if is_arrived == 0:
|
||||||
print(f'Arrived at the joint position: {pose}')
|
print(f'Arrived at the joint position: {pose}')
|
||||||
return 0
|
return 0
|
||||||
@ -588,7 +599,7 @@ class dobot_nova5:
|
|||||||
pose = [float(x.strip()) for x in pose_str.split(",")]
|
pose = [float(x.strip()) for x in pose_str.split(",")]
|
||||||
return pose
|
return pose
|
||||||
|
|
||||||
def getInverseKin(self,poses_list,user=-1,tool=-1,useJointNear=-1,JointNear=''):
|
def getInverseKin(self,poses_list,user=1,tool=-1,useJointNear=1,JointNear=''):
|
||||||
'''
|
'''
|
||||||
poses_list X Y Z RX RY RZ
|
poses_list X Y Z RX RY RZ
|
||||||
useJointNear 0 或不携带 表示 JointNear无效
|
useJointNear 0 或不携带 表示 JointNear无效
|
||||||
@ -747,7 +758,7 @@ class dobot_nova5:
|
|||||||
pos: [x,y,z] in m
|
pos: [x,y,z] in m
|
||||||
quat: [qx,qy,qz,qw] 四元数
|
quat: [qx,qy,qz,qw] 四元数
|
||||||
'''
|
'''
|
||||||
pose = self.getPose(user=0, tool=tool)
|
pose = self.getPose(user=1, tool=tool)
|
||||||
x,y,z,rx,ry,rz = pose # x y z in mm; rx ry rz in degree
|
x,y,z,rx,ry,rz = pose # x y z in mm; rx ry rz in degree
|
||||||
x /= 1000
|
x /= 1000
|
||||||
y /= 1000
|
y /= 1000
|
||||||
@ -842,6 +853,8 @@ class dobot_nova5:
|
|||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
|
|
||||||
|
# print(R.from_euler("xyz", np.array([180, 0, 0]), degrees=True).as_matrix())
|
||||||
# sleep(5)
|
# sleep(5)
|
||||||
|
|
||||||
dobot = dobot_nova5("192.168.5.1")
|
dobot = dobot_nova5("192.168.5.1")
|
||||||
@ -852,22 +865,43 @@ if __name__ == "__main__":
|
|||||||
# dobot.dashboard.MoveJog("J1+")
|
# dobot.dashboard.MoveJog("J1+")
|
||||||
# time.sleep(1)
|
# time.sleep(1)
|
||||||
# dobot.dashboard.MoveJog("")
|
# dobot.dashboard.MoveJog("")
|
||||||
# print(dobot.getAngel())
|
print("dobot.getangle::",dobot.getAngel())
|
||||||
# # while True:
|
# # while True:
|
||||||
# # time.sleep(1)
|
# # time.sleep(1)
|
||||||
|
|
||||||
# # dobot.start()
|
# # dobot.start()
|
||||||
# posJ = [0,30,-120,0,90,0]
|
# posJ = [0,30,-120,0,90,0]
|
||||||
|
# time.sleep(1)
|
||||||
|
joint = [-45.0050*np.pi/180,30.0682*np.pi/180,-66.1887*np.pi/180,-21.8836*np.pi/180,-90.3064*np.pi/180,-89.9977*np.pi/180]
|
||||||
|
# dobot.move_joint(q = joint)
|
||||||
|
time.sleep(1)
|
||||||
|
|
||||||
|
joint1 = [-44.8416*np.pi/180, 39.5626*np.pi/180, -80.5683*np.pi/180, -18.0396*np.pi/180, -89.4168*np.pi/180, -90.0026*np.pi/180]
|
||||||
|
try:
|
||||||
|
dobot.start_drag()
|
||||||
|
# dobot.move_joint(q = joint)
|
||||||
|
# dobot.move_joint(q = joint1)
|
||||||
|
|
||||||
|
# pose1 = [0.353467,-0.1349880,0.403604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
|
||||||
|
|
||||||
|
# dobot.RunPoint_P_inPose_M_RAD(pose=pose1)
|
||||||
|
|
||||||
|
while True:
|
||||||
|
time.sleep(1)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
# robot_thread.join()
|
||||||
|
print("用户中断操作。")
|
||||||
|
except Exception as e:
|
||||||
|
# robot_thread.join()
|
||||||
|
print("Exception occurred at line:", e.__traceback__.tb_lineno)
|
||||||
|
print("发生异常：", e)
|
||||||
|
# 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())
|
||||||
|
|
||||||
|
# dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90])
|
||||||
|
|
||||||
# dobot.start_drag()
|
# dobot.start_drag()
|
||||||
# time.sleep(60)
|
dobot.disableRobot()
|
||||||
# dobot.stop_drag()
|
|
||||||
# # 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())
|
|
||||||
|
|
||||||
# # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90])
|
|
||||||
|
|
||||||
# # dobot.start_drag()
|
|
||||||
# dobot.disableRobot()
|
|
||||||
@ -427,12 +427,12 @@ if __name__ == "__main__":
|
|||||||
xjc_sensor.set_zero()
|
xjc_sensor.set_zero()
|
||||||
# time.sleep(1)
|
# time.sleep(1)
|
||||||
# xjc_sensor.enable_active_transmission()
|
# xjc_sensor.enable_active_transmission()
|
||||||
while True:
|
# while True:
|
||||||
# sensor_data = xjc_sensor.read()
|
# # sensor_data = xjc_sensor.read()
|
||||||
sensor_data = xjc_sensor.read_data_f32()
|
# sensor_data = xjc_sensor.read_data_f32()
|
||||||
current_time = time.strftime("%Y-%m-%d_%H-%M-%S")
|
# current_time = time.strftime("%Y-%m-%d_%H-%M-%S")
|
||||||
if sensor_data is None:
|
# if sensor_data is None:
|
||||||
print('failed to get force sensor data!')
|
# print('failed to get force sensor data!')
|
||||||
print(f"{current_time}-----{sensor_data}")
|
# print(f"{current_time}-----{sensor_data}")
|
||||||
rate.sleep(False)
|
# rate.sleep(False)
|
||||||
# break
|
# # break
|
||||||
|
|||||||
@ -0,0 +1,235 @@
|
|||||||
|
import requests
|
||||||
|
import base64
|
||||||
|
import cv2
|
||||||
|
import open3d as o3d
|
||||||
|
import numpy as np
|
||||||
|
import paramiko
|
||||||
|
import time
|
||||||
|
class ToolCamera:
|
||||||
|
def __init__(self, host):
|
||||||
|
"""
|
||||||
|
初始化 CameraClient 类。
|
||||||
|
"""
|
||||||
|
self.host = host
|
||||||
|
self.port = 22 # SSH端口号，默认是22
|
||||||
|
self.username = "jsfb" # SSH用户名
|
||||||
|
self.password = "jsfb" # SSH密码
|
||||||
|
self.root_password = "jsfb"
|
||||||
|
|
||||||
|
# 要执行的命令
|
||||||
|
self.start_command = "systemctl restart cam_server"
|
||||||
|
self.stop_command = "systemctl stop cam_server"
|
||||||
|
|
||||||
|
def start(self):
|
||||||
|
self.execute_command_on_remote(
|
||||||
|
host=self.host,
|
||||||
|
username=self.username,
|
||||||
|
password=self.password,
|
||||||
|
root_password=self.root_password,
|
||||||
|
command=self.start_command
|
||||||
|
)
|
||||||
|
|
||||||
|
def stop(self):
|
||||||
|
self.execute_command_on_remote(
|
||||||
|
host=self.host,
|
||||||
|
username=self.username,
|
||||||
|
password=self.password,
|
||||||
|
root_password=self.root_password,
|
||||||
|
command=self.stop_command
|
||||||
|
)
|
||||||
|
|
||||||
|
def execute_command_on_remote(self, host, username, password, root_password, command, port=22):
|
||||||
|
# 创建SSH客户端
|
||||||
|
ssh = paramiko.SSHClient()
|
||||||
|
ssh.set_missing_host_key_policy(paramiko.AutoAddPolicy())
|
||||||
|
|
||||||
|
try:
|
||||||
|
# 连接到远程服务器，指定端口
|
||||||
|
ssh.connect(hostname=host, port=port, username=username, password=password)
|
||||||
|
|
||||||
|
# 构建完整的命令字符串，将root密码传递给sudo
|
||||||
|
sudo_command = f'echo {root_password} | sudo -S {command}'
|
||||||
|
|
||||||
|
# 获取通道对象
|
||||||
|
transport = ssh.get_transport()
|
||||||
|
channel = transport.open_session()
|
||||||
|
channel.get_pty() # 获取伪终端
|
||||||
|
channel.exec_command(sudo_command) # 执行命令
|
||||||
|
|
||||||
|
# 检查命令是否在后台执行
|
||||||
|
while True:
|
||||||
|
# 如果命令在后台运行, channel.exit_status_ready() 将会一直是 False
|
||||||
|
if channel.exit_status_ready():
|
||||||
|
break
|
||||||
|
|
||||||
|
# 非阻塞方式读取输出
|
||||||
|
if channel.recv_ready():
|
||||||
|
output = channel.recv(1024).decode('utf-8')
|
||||||
|
print(output)
|
||||||
|
|
||||||
|
# 非阻塞方式读取错误输出
|
||||||
|
if channel.recv_stderr_ready():
|
||||||
|
error = channel.recv_stderr(1024).decode('utf-8')
|
||||||
|
print(error)
|
||||||
|
|
||||||
|
# 延时，防止CPU占用过高
|
||||||
|
time.sleep(0.1)
|
||||||
|
|
||||||
|
return channel.recv_exit_status()
|
||||||
|
finally:
|
||||||
|
# 关闭SSH连接
|
||||||
|
ssh.close()
|
||||||
|
|
||||||
|
def get_latest_frame(self):
|
||||||
|
"""
|
||||||
|
发送请求到服务器以获取最新的RGB和深度图像数据。
|
||||||
|
|
||||||
|
返回:
|
||||||
|
tuple: 包含RGB图像和深度图像的元组 (rgb_image, depth_image)，如果请求失败则返回 (None, None)。
|
||||||
|
"""
|
||||||
|
try:
|
||||||
|
# 发送GET请求到服务器
|
||||||
|
response = requests.get("http://" + self.host + ":8000/get_latest_frame", timeout=(3, 5))
|
||||||
|
|
||||||
|
# 检查请求是否成功
|
||||||
|
if response.status_code == 200:
|
||||||
|
data = response.json()
|
||||||
|
|
||||||
|
# 获取Base64编码的RGB和深度图像数据
|
||||||
|
rgb_image_base64 = data['rgb_image']
|
||||||
|
depth_image_base64 = data['depth_image']
|
||||||
|
camera_intrinsics = data['camera_intrinsics']
|
||||||
|
|
||||||
|
# 解码Base64为二进制数据
|
||||||
|
rgb_image_data = base64.b64decode(rgb_image_base64)
|
||||||
|
depth_image_data = base64.b64decode(depth_image_base64)
|
||||||
|
|
||||||
|
# 转换为NumPy数组
|
||||||
|
rgb_image_np = np.frombuffer(rgb_image_data, np.uint8)
|
||||||
|
depth_image_np = np.frombuffer(depth_image_data, np.uint8)
|
||||||
|
|
||||||
|
# 解码为OpenCV图像
|
||||||
|
rgb_image = cv2.imdecode(rgb_image_np, cv2.IMREAD_COLOR)
|
||||||
|
depth_image = cv2.imdecode(depth_image_np, cv2.IMREAD_UNCHANGED)
|
||||||
|
|
||||||
|
|
||||||
|
return rgb_image, depth_image, camera_intrinsics
|
||||||
|
else:
|
||||||
|
print(f"Failed to retrieve data from server, status code: {response.status_code}")
|
||||||
|
return None, None, None
|
||||||
|
except Exception as e:
|
||||||
|
print(f"Exception occurred: {e}")
|
||||||
|
return None, None, None
|
||||||
|
|
||||||
|
def display_images(self, rgb_image, depth_image):
|
||||||
|
"""
|
||||||
|
显示RGB和深度图像。
|
||||||
|
|
||||||
|
参数:
|
||||||
|
rgb_image (numpy.ndarray): RGB图像。
|
||||||
|
depth_image (numpy.ndarray): 深度图像。
|
||||||
|
"""
|
||||||
|
if rgb_image is not None and depth_image is not None:
|
||||||
|
cv2.imshow('RGB Image', rgb_image)
|
||||||
|
cv2.imshow('Depth Image', depth_image)
|
||||||
|
cv2.waitKey(0)
|
||||||
|
cv2.destroyAllWindows()
|
||||||
|
else:
|
||||||
|
print("No image data to display.")
|
||||||
|
|
||||||
|
def display_rgb_point_cloud(self, rgb_image, depth_image, camera_intrinsics):
|
||||||
|
"""
|
||||||
|
显示RGB点云，将RGB图像和深度图像转换为点云并显示。
|
||||||
|
|
||||||
|
参数:
|
||||||
|
rgb_image (np.ndarray): RGB图像。
|
||||||
|
depth_image (np.ndarray): 深度图像。
|
||||||
|
camera_intrinsics (dict): 相机的内参字典，包含 'fx', 'fy', 'cx', 'cy'。
|
||||||
|
|
||||||
|
返回:
|
||||||
|
None
|
||||||
|
"""
|
||||||
|
# 获取RGB图像和深度图像的尺寸
|
||||||
|
rgb_h, rgb_w = rgb_image.shape[:2]
|
||||||
|
depth_h, depth_w = depth_image.shape[:2]
|
||||||
|
|
||||||
|
# 计算裁剪区域
|
||||||
|
start_x = (rgb_w - depth_w) // 2
|
||||||
|
start_y = (rgb_h - depth_h) // 2
|
||||||
|
|
||||||
|
# 裁剪RGB图像以匹配深度图像的尺寸
|
||||||
|
rgb_image_cropped = rgb_image[start_y:start_y + depth_h, start_x:start_x + depth_w]
|
||||||
|
rgb_image_cropped = cv2.cvtColor(rgb_image_cropped, cv2.COLOR_RGB2BGR)
|
||||||
|
|
||||||
|
# 将深度图像转换为浮点型并将单位从毫米转换为米（假设深度图像以毫米为单位）
|
||||||
|
depth_image = depth_image.astype(np.float32) / 1000.0
|
||||||
|
|
||||||
|
# 创建点云的空数组
|
||||||
|
points = []
|
||||||
|
colors = []
|
||||||
|
|
||||||
|
# 相机内参
|
||||||
|
fx = camera_intrinsics['fx']
|
||||||
|
fy = camera_intrinsics['fy']
|
||||||
|
cx = camera_intrinsics['cx']
|
||||||
|
cy = camera_intrinsics['cy']
|
||||||
|
|
||||||
|
# 遍历每个像素，将其转换为3D点
|
||||||
|
for v in range(depth_h):
|
||||||
|
for u in range(depth_w):
|
||||||
|
z = depth_image[v, u]
|
||||||
|
if z > 0: # 排除无效深度
|
||||||
|
x = (u - cx) * z / fx
|
||||||
|
y = (v - cy) * z / fy
|
||||||
|
points.append([x, y, z])
|
||||||
|
colors.append(rgb_image_cropped[v, u] / 255.0) # 颜色归一化到[0,1]
|
||||||
|
|
||||||
|
# 将点云和颜色转换为NumPy数组
|
||||||
|
points = np.array(points)
|
||||||
|
colors = np.array(colors)
|
||||||
|
|
||||||
|
# 创建Open3D点云对象
|
||||||
|
point_cloud = o3d.geometry.PointCloud()
|
||||||
|
point_cloud.points = o3d.utility.Vector3dVector(points)
|
||||||
|
point_cloud.colors = o3d.utility.Vector3dVector(colors)
|
||||||
|
|
||||||
|
# 显示点云
|
||||||
|
o3d.visualization.draw_geometries([point_cloud])
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
import time
|
||||||
|
import os
|
||||||
|
# 初始化客户端并指定服务器地址
|
||||||
|
cam = ToolCamera(host='127.0.0.1')
|
||||||
|
# cam.stop()
|
||||||
|
cam.start()
|
||||||
|
time.sleep(2)
|
||||||
|
# 获取最新的RGB和深度图像
|
||||||
|
rgb_image, depth_image, camera_intrinsics = cam.get_latest_frame()
|
||||||
|
print(camera_intrinsics)
|
||||||
|
|
||||||
|
max_depth = np.max(depth_image)
|
||||||
|
print(np.min(depth_image),np.max(depth_image))
|
||||||
|
# print(depth_image[200, 320])
|
||||||
|
# depth_image = (depth_image / max_depth * 65535).astype(np.uint16)
|
||||||
|
# print(np.min(depth_image),np.max(depth_image))
|
||||||
|
# 对图像进行水平翻转
|
||||||
|
# rgb_image = cv2.flip(rgb_image, 1)
|
||||||
|
# depth_image = cv2.flip(depth_image, 1)
|
||||||
|
|
||||||
|
# 显示图像
|
||||||
|
cam.display_images(rgb_image, depth_image)
|
||||||
|
cv2.imwrite("aucpuncture2point/configs/using_img/color.png",rgb_image)
|
||||||
|
cv2.imwrite("aucpuncture2point/configs/using_img/depth.png",depth_image)
|
||||||
|
# 获取当前时间并格式化为文件夹名 (格式: 年-月-日_时-分-秒)
|
||||||
|
current_time = time.strftime("%Y-%m-%d_%H-%M-%S")
|
||||||
|
base_dir = "/home/jsfb/jsfb_ws/collected_data/test_images"
|
||||||
|
# 创建一个根据时间命名的新文件夹
|
||||||
|
folder_path = os.path.join(base_dir, current_time)
|
||||||
|
os.makedirs(folder_path, exist_ok=True) # 如果文件夹不存在则创建
|
||||||
|
|
||||||
|
cv2.imwrite(os.path.join(folder_path, "color.png"), rgb_image)
|
||||||
|
cv2.imwrite(os.path.join(folder_path, "depth.png"), depth_image)
|
||||||
|
|
||||||
|
# 显示RGB点云
|
||||||
|
# cam.display_rgb_point_cloud(rgb_image, depth_image, camera_intrinsics)
|
||||||
293
Massage/MassageControl/tools/auto_visual_calibration.py
Normal file
293
Massage/MassageControl/tools/auto_visual_calibration.py
Normal file
@ -0,0 +1,293 @@
|
|||||||
|
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.objp = self._create_object_points()
|
||||||
|
self.image_points = []
|
||||||
|
self.robot_poses = []
|
||||||
|
self.intrinsics = None
|
||||||
|
self.distortion_cofficients = None
|
||||||
|
self.cam = ToolCamera(host='127.0.0.1')
|
||||||
|
self.cam.start()
|
||||||
|
time.sleep(5)
|
||||||
|
self.size = (400,640)
|
||||||
|
rgb_image, depth_image, camera_intrinsics = self.cam.get_latest_frame()
|
||||||
|
if rgb_image is None :
|
||||||
|
print(f'===============相机连接失败请检查并重试==============')
|
||||||
|
sys.exit(1)
|
||||||
|
else:
|
||||||
|
print(f'===============相机连接成功==============')
|
||||||
|
self.size = rgb_image.shape[:2]
|
||||||
|
self.intrinsics = camera_intrinsics
|
||||||
|
|
||||||
|
self.arm = AuboC5()
|
||||||
|
self.arm.init()
|
||||||
|
self.obj_points = []
|
||||||
|
print(f'===============机械臂连接成功==============')
|
||||||
|
date_time_str = time.strftime("%Y%m%d%H%M%S", time.localtime())
|
||||||
|
folder_name = f"calibration_data_{date_time_str}"
|
||||||
|
file_folder+=folder_name
|
||||||
|
os.makedirs(file_folder, exist_ok=True)
|
||||||
|
print(f'===============创建文件夹成功=============')
|
||||||
|
self.file_address = file_folder
|
||||||
|
|
||||||
|
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"\n\n\n\n\nsize 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("++++++++++相机标定完成++++++++++++++")
|
||||||
|
# 从mtx中提取内参
|
||||||
|
fx = mtx[0, 0] # 焦距 fx
|
||||||
|
fy = mtx[1, 1] # 焦距 fy
|
||||||
|
cx = mtx[0, 2] # 主点 cx
|
||||||
|
cy = mtx[1, 2] # 主点 cy
|
||||||
|
|
||||||
|
# 从 distortion_array 提取畸变系数
|
||||||
|
k1 = dist[0][0] # 畸变系数 k1
|
||||||
|
k2 = dist[0][1] # 畸变系数 k2
|
||||||
|
p1 = dist[0][2] # 畸变系数 p1
|
||||||
|
p2 = dist[0][3] # 畸变系数 p2
|
||||||
|
k3 = dist[0][4] # 畸变系数 k3
|
||||||
|
# self.intrinsics = mtx
|
||||||
|
self.intrinsics = {
|
||||||
|
'fx': fx,
|
||||||
|
'fy': fy,
|
||||||
|
'cx': cx,
|
||||||
|
'cy': cy,
|
||||||
|
'distortion_coeffs': {
|
||||||
|
'k1': k1,
|
||||||
|
'k2': k2,
|
||||||
|
'p1': p1,
|
||||||
|
'p2': p2,
|
||||||
|
'k3': k3
|
||||||
|
}
|
||||||
|
}
|
||||||
|
self.distortion_cofficients = 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()
|
||||||
|
print(rvecs)
|
||||||
|
print(tvecs)
|
||||||
|
print(r_arm)
|
||||||
|
print(t_arm)
|
||||||
|
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("+++++++++++手眼标定完成+++++++++++++++")
|
||||||
|
self.arm.exit_task()
|
||||||
|
return R, t ,self.intrinsics
|
||||||
|
|
||||||
|
#得到图片以后 根据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.03 #unit - meter
|
||||||
|
systemPath = "/home/jsfb/Documents/"
|
||||||
|
|
||||||
|
|
||||||
|
calib = Calibration(calibration_board_size,calibration_board_square_size,systemPath)
|
||||||
|
calib.collect_data()
|
||||||
|
R, t,intrinsics = calib.calibrate()
|
||||||
|
print("旋转矩阵：")
|
||||||
|
print(R)
|
||||||
|
print("平移向量：")
|
||||||
|
print(t)
|
||||||
|
print("内参：")
|
||||||
|
print(intrinsics)
|
||||||
@ -94,13 +94,21 @@ class Identifier:
|
|||||||
x y z : mm
|
x y z : mm
|
||||||
r p y : deg
|
r p y : deg
|
||||||
'''
|
'''
|
||||||
pose_command = np.concatenate([[x,y,z], [roll,pitch,yaw]])
|
# pose_command = np.concatenate([[x,y,z]/1000, [roll,pitch,yaw]*np.pi/180])
|
||||||
|
pose_command = np.concatenate([
|
||||||
|
np.array([x, y, z]) / 1000.0, # mm ➝ m
|
||||||
|
np.array([roll, pitch, yaw]) * np.pi / 180.0 # deg ➝ rad
|
||||||
|
])
|
||||||
|
|
||||||
cur = self.arm.getAngel() # 接口调用: 获取机器人的关节位置
|
cur = self.arm.getAngel() # 接口调用: 获取机器人的关节位置
|
||||||
cur_str = "{" + ",".join(map(str, cur)) + "}"
|
cur_str = "{" + ",".join(map(str, cur)) + "}"
|
||||||
# print("cur_str",cur_str)
|
# print("cur_str",cur_str)
|
||||||
target_J = self.arm.getInverseKin(poses_list=pose_command,useJointNear=1,JointNear=cur_str)
|
print("pose_command:",pose_command)
|
||||||
print(target_J)
|
self.arm.RunPoint_P_inPose_M_RAD(pose = pose_command)
|
||||||
self.arm.RunPoint_P_inJoint(target_J)
|
|
||||||
|
# target_J = self.arm.getInverseKin(poses_list=pose_command,useJointNear=1,JointNear=cur_str)
|
||||||
|
# print(target_J)
|
||||||
|
# self.arm.RunPoint_P_inJoint(target_J)
|
||||||
|
|
||||||
def identify_param_auto(self,ready_pose,cnt,sample_num = 5):
|
def identify_param_auto(self,ready_pose,cnt,sample_num = 5):
|
||||||
if cnt < 15:
|
if cnt < 15:
|
||||||
@ -170,19 +178,20 @@ class Identifier:
|
|||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
arm = dobot_nova5("192.168.5.1")
|
arm = dobot_nova5()
|
||||||
arm.start()
|
# arm.start()
|
||||||
arm.setEndEffector(i=9,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}")
|
arm.setEndEffector(i=9,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}")
|
||||||
arm.chooseEndEffector(i=9)
|
arm.chooseEndEffector(i=9)
|
||||||
arm.init()
|
arm.init()
|
||||||
sensor = XjcSensor("192.168.5.1", 60000)
|
sensor = XjcSensor()
|
||||||
sensor.connect()
|
sensor.connect()
|
||||||
sensor.disable_active_transmission()
|
sensor.disable_active_transmission()
|
||||||
sensor.disable_active_transmission()
|
sensor.disable_active_transmission()
|
||||||
sensor.disable_active_transmission()
|
sensor.disable_active_transmission()
|
||||||
atexit.register(sensor.disconnect)
|
atexit.register(sensor.disconnect)
|
||||||
# identifier = Identifier(arm=arm,sensor=sensor,config_path="/home/jsfb/jsfb/MassageRobot_Dobot/Massage/MassageControl/config/massage_head/finger_playload.yaml")
|
identifier = Identifier(arm=arm,sensor=sensor,config_path="/home/jsfb/jsfb_ws/global_config/massage_head/finger_playload.yaml")
|
||||||
|
ready_pose = [0.353467*1000,-0.1349880*1000,0.403604*1000,-180.0000,-30.0000,0.0042]
|
||||||
# ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0]
|
# ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0]
|
||||||
# time.sleep(1)
|
time.sleep(1)
|
||||||
# identifier.identify_param_auto(ready_pose,45)
|
identifier.identify_param_auto(ready_pose,15)
|
||||||
arm.disableRobot()
|
arm.disableRobot()
|
||||||
328
Massage/MassageControl/tools/replay_trajectory_from_csv.py
Normal file
328
Massage/MassageControl/tools/replay_trajectory_from_csv.py
Normal file
@ -0,0 +1,328 @@
|
|||||||
|
# cython: language_level=3
|
||||||
|
import time
|
||||||
|
import csv
|
||||||
|
import atexit
|
||||||
|
from pathlib import Path
|
||||||
|
import sys
|
||||||
|
import ctypes
|
||||||
|
import threading
|
||||||
|
import pygame # 添加pygame导入
|
||||||
|
|
||||||
|
sys.path.append(str(Path(__file__).resolve().parent.parent))
|
||||||
|
from hardware.aubo_C5 import AuboC5
|
||||||
|
from hardware.force_sensor_aubo import XjcSensor
|
||||||
|
|
||||||
|
# 初始化libc
|
||||||
|
try:
|
||||||
|
libc = ctypes.CDLL("libc.so.6")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"Failed to load libc: {e}")
|
||||||
|
libc = None
|
||||||
|
|
||||||
|
class JointTrajectoryPlayer:
|
||||||
|
def __init__(self, csv_path: str, arm: AuboC5 = None, sensor: XjcSensor = None):
|
||||||
|
"""
|
||||||
|
初始化轨迹回放器
|
||||||
|
:param csv_path: 轨迹CSV文件路径
|
||||||
|
:param arm: 可选，机械臂控制器
|
||||||
|
:param sensor: 可选，力传感器
|
||||||
|
"""
|
||||||
|
self.csv_path = csv_path
|
||||||
|
self.arm = arm
|
||||||
|
self.sensor = sensor
|
||||||
|
self.file = None
|
||||||
|
self.reader = None
|
||||||
|
self.header = None
|
||||||
|
self.keep_playing = True
|
||||||
|
self.play_thread = None
|
||||||
|
|
||||||
|
def __enter__(self):
|
||||||
|
"""上下文管理器入口，打开文件"""
|
||||||
|
self.file = open(self.csv_path, 'r')
|
||||||
|
self.reader = csv.reader(self.file)
|
||||||
|
self.header = next(self.reader) # 读取表头
|
||||||
|
return self
|
||||||
|
|
||||||
|
def __exit__(self, exc_type, exc_val, exc_tb):
|
||||||
|
"""上下文管理器退出，关闭文件"""
|
||||||
|
if self.file:
|
||||||
|
self.file.close()
|
||||||
|
|
||||||
|
def read_next_joint_angles(self):
|
||||||
|
"""
|
||||||
|
读取下一行关节角度
|
||||||
|
:return: (timestamp, [j1,j2,j3,j4,j5,j6]) 或 None(文件结束)
|
||||||
|
"""
|
||||||
|
try:
|
||||||
|
row = next(self.reader)
|
||||||
|
timestamp = float(row[0])
|
||||||
|
angles = [float(x) for x in row[1:7]] # 读取6个关节角度
|
||||||
|
return timestamp, angles
|
||||||
|
except (StopIteration, ValueError, IndexError):
|
||||||
|
return None
|
||||||
|
|
||||||
|
def playback(self, speed_factor: float = 1.0):
|
||||||
|
"""
|
||||||
|
回放轨迹
|
||||||
|
:param speed_factor: 回放速度因子(1.0为原速)
|
||||||
|
"""
|
||||||
|
while self.keep_playing: # 外部可以通过设置keep_playing为False来停止播放
|
||||||
|
# 每次循环开始时重新打开文件和读取器
|
||||||
|
self.file = open(self.csv_path, 'r')
|
||||||
|
self.reader = csv.reader(self.file)
|
||||||
|
self.header = next(self.reader) # 读取表头
|
||||||
|
|
||||||
|
start_time = time.time()
|
||||||
|
first_timestamp = None
|
||||||
|
|
||||||
|
while self.keep_playing: # 内部循环，播放单次轨迹
|
||||||
|
data = self.read_next_joint_angles()
|
||||||
|
if data is None:
|
||||||
|
break
|
||||||
|
|
||||||
|
timestamp, angles = data
|
||||||
|
|
||||||
|
# 初始化第一个时间戳
|
||||||
|
if first_timestamp is None:
|
||||||
|
first_timestamp = timestamp
|
||||||
|
continue
|
||||||
|
|
||||||
|
usleep(15000)
|
||||||
|
|
||||||
|
# 执行运动
|
||||||
|
if self.arm:
|
||||||
|
self.arm.mc.servoJoint(angles, 0, 0, 0.15, 0.1, 200)
|
||||||
|
|
||||||
|
print(f"时间: {timestamp:.3f}s, 角度: {angles}")
|
||||||
|
|
||||||
|
# 关闭当前文件
|
||||||
|
self.file.close()
|
||||||
|
|
||||||
|
return 0
|
||||||
|
|
||||||
|
def start(self, speed_factor: float = 1.0):
|
||||||
|
"""启动轨迹播放，返回是否成功启动"""
|
||||||
|
if self.play_thread and self.play_thread.is_alive():
|
||||||
|
return False # 已经在运行中
|
||||||
|
|
||||||
|
self.keep_playing = True
|
||||||
|
self.play_thread = threading.Thread(target=self.playback, kwargs={'speed_factor': speed_factor})
|
||||||
|
self.play_thread.daemon = True
|
||||||
|
self.play_thread.start()
|
||||||
|
return True
|
||||||
|
|
||||||
|
def stop(self):
|
||||||
|
"""停止轨迹播放，返回是否成功停止"""
|
||||||
|
if not self.play_thread or not self.play_thread.is_alive():
|
||||||
|
return False # 未在运行或已停止
|
||||||
|
|
||||||
|
self.keep_playing = False
|
||||||
|
return True
|
||||||
|
|
||||||
|
|
||||||
|
class RobotDanceController:
|
||||||
|
"""机器人舞蹈控制器，单例模式"""
|
||||||
|
_instance = None
|
||||||
|
|
||||||
|
def __new__(cls, *args, **kwargs):
|
||||||
|
if cls._instance is None:
|
||||||
|
cls._instance = super(RobotDanceController, cls).__new__(cls)
|
||||||
|
cls._instance._initialized = False
|
||||||
|
return cls._instance
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
if self._initialized:
|
||||||
|
return
|
||||||
|
|
||||||
|
self.arm = None
|
||||||
|
self.player = None
|
||||||
|
self.is_running = False
|
||||||
|
self._initialized = True
|
||||||
|
self.music_playing = False
|
||||||
|
|
||||||
|
def initialize(self):
|
||||||
|
"""初始化机械臂和轨迹播放器"""
|
||||||
|
try:
|
||||||
|
# 初始化机械臂
|
||||||
|
self.arm = AuboC5()
|
||||||
|
self.arm.init()
|
||||||
|
|
||||||
|
# 设置CSV路径
|
||||||
|
script_dir = Path(__file__).resolve().parent
|
||||||
|
csv_path = script_dir / "joint_processed_0.025s_6.csv"
|
||||||
|
|
||||||
|
# 创建轨迹播放器
|
||||||
|
self.player = JointTrajectoryPlayer(str(csv_path), self.arm)
|
||||||
|
|
||||||
|
return True
|
||||||
|
except Exception as e:
|
||||||
|
print(f"初始化失败: {e}")
|
||||||
|
return False
|
||||||
|
|
||||||
|
def play_music(self):
|
||||||
|
"""播放背景音乐，从头开始，循环播放"""
|
||||||
|
try:
|
||||||
|
# 初始化pygame混音器
|
||||||
|
pygame.mixer.init()
|
||||||
|
|
||||||
|
# 获取音乐文件路径
|
||||||
|
script_dir = Path(__file__).resolve().parent
|
||||||
|
# music_path = script_dir / "demo_music.mp3"
|
||||||
|
music_path = script_dir / "demo_music1.mp3"
|
||||||
|
|
||||||
|
# 加载并播放音乐从头开始，设置循环播放（loops=-1表示无限循环）
|
||||||
|
pygame.mixer.music.load(str(music_path))
|
||||||
|
pygame.mixer.music.play(loops=-1)
|
||||||
|
self.music_playing = True
|
||||||
|
print("背景音乐已开始播放（循环模式）")
|
||||||
|
|
||||||
|
return True
|
||||||
|
except Exception as e:
|
||||||
|
print(f"播放音乐失败: {e}")
|
||||||
|
return False
|
||||||
|
|
||||||
|
def stop_music(self):
|
||||||
|
"""停止背景音乐"""
|
||||||
|
if self.music_playing:
|
||||||
|
try:
|
||||||
|
pygame.mixer.music.stop()
|
||||||
|
pygame.mixer.quit()
|
||||||
|
self.music_playing = False
|
||||||
|
print("背景音乐已停止")
|
||||||
|
except Exception as e:
|
||||||
|
print(f"停止音乐失败: {e}")
|
||||||
|
|
||||||
|
def start_demo(self):
|
||||||
|
"""开始演示，返回是否成功启动"""
|
||||||
|
if self.is_running:
|
||||||
|
return False
|
||||||
|
|
||||||
|
# 如果未初始化，先初始化
|
||||||
|
if not self.arm or not self.player:
|
||||||
|
if not self.initialize():
|
||||||
|
return False
|
||||||
|
|
||||||
|
try:
|
||||||
|
# 移动到舞蹈准备姿势
|
||||||
|
self.arm.move_joint([1.571,0.578,2.35,0.205,1.569,0.0])
|
||||||
|
time.sleep(2)
|
||||||
|
|
||||||
|
# 先播放背景音乐
|
||||||
|
self.play_music()
|
||||||
|
|
||||||
|
# 创建一个线程等待14秒后启动机器人运动
|
||||||
|
def start_robot_movement_at_music_mark():
|
||||||
|
try:
|
||||||
|
start_time = time.time()
|
||||||
|
# wait_time = 8.0 # 8秒
|
||||||
|
wait_time = 4.0 # 4秒
|
||||||
|
print(f"等待音乐播放到{wait_time}秒...")
|
||||||
|
time.sleep(wait_time)
|
||||||
|
|
||||||
|
# 检查是否仍在运行状态（可能用户已经停止了演示）
|
||||||
|
if self.is_running:
|
||||||
|
print(f"音乐已播放{wait_time}秒，开始机器人运动")
|
||||||
|
# 启动轨迹播放器
|
||||||
|
# 启动伺服
|
||||||
|
self.arm.enable_servo()
|
||||||
|
time.sleep(1)
|
||||||
|
self.arm.enable_servo()
|
||||||
|
time.sleep(1)
|
||||||
|
self.arm.enable_servo()
|
||||||
|
time.sleep(1)
|
||||||
|
self.player.start()
|
||||||
|
except Exception as e:
|
||||||
|
print(f"启动机器人运动失败: {e}")
|
||||||
|
|
||||||
|
# 设置为正在运行状态，这样线程中可以检查状态
|
||||||
|
self.is_running = True
|
||||||
|
|
||||||
|
# 启动等待线程
|
||||||
|
threading.Thread(target=start_robot_movement_at_music_mark, daemon=True).start()
|
||||||
|
|
||||||
|
return True
|
||||||
|
except Exception as e:
|
||||||
|
# 发生错误时，确保停止音乐
|
||||||
|
self.stop_music()
|
||||||
|
self.is_running = False
|
||||||
|
print(f"启动演示失败: {e}")
|
||||||
|
return False
|
||||||
|
|
||||||
|
def stop_demo(self):
|
||||||
|
"""停止演示，返回是否成功停止"""
|
||||||
|
if not self.is_running:
|
||||||
|
return False
|
||||||
|
|
||||||
|
try:
|
||||||
|
# 停止背景音乐
|
||||||
|
self.stop_music()
|
||||||
|
|
||||||
|
# 停止播放器
|
||||||
|
if self.player:
|
||||||
|
self.player.stop()
|
||||||
|
|
||||||
|
# 关闭伺服
|
||||||
|
if self.arm:
|
||||||
|
self.arm.disable_servo()
|
||||||
|
time.sleep(1)
|
||||||
|
self.arm.disable_servo()
|
||||||
|
time.sleep(1)
|
||||||
|
self.arm.disable_servo()
|
||||||
|
time.sleep(1)
|
||||||
|
|
||||||
|
# 回到初始位置
|
||||||
|
self.arm.move_joint(self.arm.off_pos)
|
||||||
|
|
||||||
|
self.is_running = False
|
||||||
|
return True
|
||||||
|
except Exception as e:
|
||||||
|
print(f"停止演示失败: {e}")
|
||||||
|
# 即使发生异常，也将状态设为未运行
|
||||||
|
self.is_running = False
|
||||||
|
return False
|
||||||
|
|
||||||
|
def cleanup(self):
|
||||||
|
"""清理资源"""
|
||||||
|
try:
|
||||||
|
if self.is_running:
|
||||||
|
self.stop_demo()
|
||||||
|
else:
|
||||||
|
# 确保音乐停止
|
||||||
|
self.stop_music()
|
||||||
|
|
||||||
|
if self.arm:
|
||||||
|
self.arm.power_off()
|
||||||
|
|
||||||
|
return True
|
||||||
|
except Exception as e:
|
||||||
|
print(f"清理资源失败: {e}")
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
def usleep(microseconds):
|
||||||
|
"""Linux 下的微秒级休眠（比 time.sleep 更精确）"""
|
||||||
|
if libc and hasattr(libc, 'usleep'):
|
||||||
|
libc.usleep(microseconds)
|
||||||
|
else:
|
||||||
|
# 如果libc不可用，使用time.sleep作为备选
|
||||||
|
time.sleep(microseconds / 1000000.0)
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
# 作为独立脚本运行时，提供简单的CLI接口
|
||||||
|
controller = RobotDanceController()
|
||||||
|
|
||||||
|
try:
|
||||||
|
print("开始机器人舞蹈演示...")
|
||||||
|
if controller.start_demo():
|
||||||
|
print("演示已开始，按Ctrl+C停止...")
|
||||||
|
|
||||||
|
# 等待用户按Ctrl+C
|
||||||
|
while True:
|
||||||
|
time.sleep(1)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
print("\n接收到停止信号，正在停止演示...")
|
||||||
|
finally:
|
||||||
|
controller.stop_demo()
|
||||||
|
controller.cleanup()
|
||||||
|
print("演示已停止，资源已清理。")
|
||||||
Binary file not shown.
|
Before Width: | Height: | Size: 376 KiB After Width: | Height: | Size: 366 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 376 KiB After Width: | Height: | Size: 366 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 376 KiB After Width: | Height: | Size: 366 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 376 KiB After Width: | Height: | Size: 366 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 376 KiB After Width: | Height: | Size: 366 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 376 KiB After Width: | Height: | Size: 366 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 2.0 MiB After Width: | Height: | Size: 1.8 MiB |
Binary file not shown.
|
Before Width: | Height: | Size: 455 KiB After Width: | Height: | Size: 417 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 112 KiB After Width: | Height: | Size: 106 KiB |
@ -64,8 +64,11 @@ class CoordinateTransformer:
|
|||||||
self.camera_matrix = None
|
self.camera_matrix = None
|
||||||
self.camera_trans = None
|
self.camera_trans = None
|
||||||
|
|
||||||
self.camera_matrix = np.array(self.vtxdb.get("robot_config","camera.camera_matrix"))
|
self.camera_matrix = np.array([[0.9996735,-0.0251487,-0.00260522],[-0.02535628,-0.9994429,0.02170123],[-0.00315538,-0.02162809,-0.99976111]])
|
||||||
self.camera_trans = np.array(self.vtxdb.get("robot_config","camera.camera_trans"))
|
self.camera_trans = np.array([-0.00767654,-0.00675844,-0.1762473])
|
||||||
|
|
||||||
|
# self.camera_matrix = np.array(self.vtxdb.get("robot_config","camera.camera_matrix"))
|
||||||
|
# self.camera_trans = np.array(self.vtxdb.get("robot_config","camera.camera_trans"))
|
||||||
|
|
||||||
camera_offset = self.vtxdb.get("robot_config","camera.camera_offset")
|
camera_offset = self.vtxdb.get("robot_config","camera.camera_offset")
|
||||||
if camera_offset:
|
if camera_offset:
|
||||||
@ -342,12 +345,14 @@ class CoordinateTransformer:
|
|||||||
camera_coordinate = np.array(camera_coordinate)
|
camera_coordinate = np.array(camera_coordinate)
|
||||||
# 将相机坐标系转换到机械臂末端坐标系
|
# 将相机坐标系转换到机械臂末端坐标系
|
||||||
arm_coordinate = [0, 0, 0]
|
arm_coordinate = [0, 0, 0]
|
||||||
if self.camera_matrix is not None and self.camera_trans is not None:
|
|
||||||
camera_matrix = copy.deepcopy(self.camera_matrix)
|
camera_matrix = copy.deepcopy(self.camera_matrix)
|
||||||
camera_trans = copy.deepcopy(self.camera_trans)
|
camera_trans = copy.deepcopy(self.camera_trans)
|
||||||
else:
|
# if self.camera_matrix is not None and self.camera_trans is not None:
|
||||||
camera_matrix = np.array([[-0.04169144,0.99888175,-0.02229508],[0.99912694,0.04174075,0.00175093],[0.00267958,-0.02220262,-0.9997499]])
|
# camera_matrix = copy.deepcopy(self.camera_matrix)
|
||||||
camera_trans = np.array([-0.00209053,-0.01021561,-0.16877656])
|
# camera_trans = copy.deepcopy(self.camera_trans)
|
||||||
|
# else:
|
||||||
|
# camera_matrix = np.array([[-0.04169144,0.99888175,-0.02229508],[0.99912694,0.04174075,0.00175093],[0.00267958,-0.02220262,-0.9997499]])
|
||||||
|
# camera_trans = np.array([-0.00209053,-0.01021561,-0.16877656])
|
||||||
arm_coordinate = camera_matrix @ (camera_coordinate/1000) + camera_trans
|
arm_coordinate = camera_matrix @ (camera_coordinate/1000) + camera_trans
|
||||||
# 将机械臂末端坐标系转为机械臂基座坐标系
|
# 将机械臂末端坐标系转为机械臂基座坐标系
|
||||||
targetPosition = R.from_quat(self.quaternion).as_matrix() @ arm_coordinate + self.position
|
targetPosition = R.from_quat(self.quaternion).as_matrix() @ arm_coordinate + self.position
|
||||||
|
|||||||
BIN
UI_next/static/images/tmp_images/Pixel_Visualization_0.jpg
Normal file
BIN
UI_next/static/images/tmp_images/Pixel_Visualization_0.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 366 KiB |
BIN
UI_next/static/images/tmp_images/Pixel_Visualization_1.jpg
Normal file
BIN
UI_next/static/images/tmp_images/Pixel_Visualization_1.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 366 KiB |
BIN
UI_next/static/images/tmp_images/Pixel_Visualization_2.jpg
Normal file
BIN
UI_next/static/images/tmp_images/Pixel_Visualization_2.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 366 KiB |
BIN
UI_next/static/images/tmp_images/Pixel_Visualization_3.jpg
Normal file
BIN
UI_next/static/images/tmp_images/Pixel_Visualization_3.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 366 KiB |
BIN
UI_next/static/images/tmp_images/Pixel_Visualization_4.jpg
Normal file
BIN
UI_next/static/images/tmp_images/Pixel_Visualization_4.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 366 KiB |
BIN
UI_next/static/images/tmp_images/back_acupoints.png
Normal file
BIN
UI_next/static/images/tmp_images/back_acupoints.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 1.8 MiB |
BIN
UI_next/static/images/tmp_images/color.png
Executable file
BIN
UI_next/static/images/tmp_images/color.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 417 KiB |
Loading…
x
Reference in New Issue
Block a user