stash
This commit is contained in:
LiangShiyun
parent
5dcb63a51e
commit
961a86d423
@ -247,7 +247,9 @@ class MassageRobot:
|
||||
return x_update, P_update
|
||||
def init_hardwares(self,ready_pose):
|
||||
self.ready_pose = np.array(ready_pose)
|
||||
self.switch_payload(self.current_head)
|
||||
self.arm_state.desired_position = self.ready_pose[:3]
|
||||
euler_angles = self.ready_pose[3:]
|
||||
self.arm_state.desired_orientation = R.from_euler('xyz',euler_angles).as_quat()
|
||||
print(self.arm.get_arm_position())
|
||||
time.sleep(0.5)
|
||||
|
||||
@ -551,12 +553,15 @@ class MassageRobot:
|
||||
if sleep_duration > 0:
|
||||
time.sleep(sleep_duration)
|
||||
# print(f"real sleep:{time.time()-b2}")
|
||||
|
||||
|
||||
# print("self111:",self.arm.feedbackData.robotMode)
|
||||
self.arm.dashboard.socket_dobot.sendall(tcp_command)
|
||||
# print("self111222:",self.arm.feedbackData.robotMode)
|
||||
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.arm.dashboard.Stop()
|
||||
self.arm.dashboard.EmergencyStop(mode=1)
|
||||
self.stop()
|
||||
return -1
|
||||
if self.arm.feedbackData.EnableState and self.arm.feedbackData.robotMode == 10:
|
||||
if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停
|
||||
@ -591,11 +596,12 @@ class MassageRobot:
|
||||
self.arm_state.arm_orientation = quat_rot
|
||||
for i in range(20):
|
||||
self.controller_manager.step(self.control_rate.to_sec())
|
||||
self.logger.log_blue(f"position command: {self.arm_state.arm_position_command}")
|
||||
self.logger.log_blue(f"orientation command: {R.from_quat(self.arm_state.arm_orientation_command).as_euler('xyz',degrees=True)}")
|
||||
# self.logger.log_blue(f"position command: {self.arm_state.arm_position_command}")
|
||||
# self.logger.log_blue(f"orientation command: {R.from_quat(self.arm_state.arm_orientation_command).as_euler('xyz',degrees=True)}")
|
||||
position, quat_rot = self.arm.get_arm_position()
|
||||
self.logger.log_blue(f"position current:{position}")
|
||||
self.logger.log_blue(f"orientation current: {R.from_quat(quat_rot).as_euler('xyz',degrees=False)}")
|
||||
# self.logger.log_blue(f"position current:{position}")
|
||||
# self.logger.log_blue(f"orientation current: {R.from_quat(quat_rot).as_euler('xyz',degrees=False)}")
|
||||
print("self.arm.feedbackData.robotMode:",self.arm.feedbackData.robotMode)
|
||||
self.command_rate.precise_sleep()
|
||||
|
||||
position ,quat_rot = self.arm.get_arm_position()
|
||||
@ -1613,6 +1619,26 @@ class MassageRobot:
|
||||
'wrench': wrench_traj
|
||||
}
|
||||
return traj
|
||||
|
||||
# 工具函数
|
||||
############################################################################################################
|
||||
def convert_to_7d(self,matrix):
|
||||
matrix = np.array(matrix)
|
||||
positions = matrix[:, :3]
|
||||
rpy_angles = matrix[:, 3:]
|
||||
# 将RPY角度转换为四元数
|
||||
quaternions = R.from_euler('xyz', rpy_angles).as_quat()
|
||||
# 将位置和四元数组合成一个新的矩阵
|
||||
result_matrix = np.hstack([positions, quaternions])
|
||||
# 加入当前位置
|
||||
current_quat = self.arm_state.arm_orientation
|
||||
current_position = self.arm_state.arm_position
|
||||
if quaternions[0].dot(current_quat) < 0:
|
||||
current_quat = -current_quat
|
||||
current_pose = np.hstack([current_position,current_quat])
|
||||
result_matrix = np.vstack([current_pose,result_matrix])
|
||||
return result_matrix
|
||||
|
||||
|
||||
|
||||
|
||||
@ -1645,7 +1671,7 @@ if __name__ == "__main__":
|
||||
# robot.dt = ts[1] - ts[0]
|
||||
|
||||
|
||||
ready_pose = [204.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042]
|
||||
ready_pose = [0.1543467,-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.force_sensor.disable_active_transmission()
|
||||
@ -1660,7 +1686,13 @@ if __name__ == "__main__":
|
||||
|
||||
robot.init_hardwares(ready_pose)
|
||||
time.sleep(3)
|
||||
robot.switch_payload("none")
|
||||
robot.controller_manager.switch_controller('admittance')
|
||||
position,quat_rot = robot.arm.get_arm_position()
|
||||
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]]
|
||||
robot.move_to_point(pose=target_point,t=60)
|
||||
|
||||
# robot.controller_manager.switch_controller('position')
|
||||
|
||||
|
||||
|
||||
@ -40,42 +40,100 @@ class AdmittanceController(BaseController):
|
||||
|
||||
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):
|
||||
# 方向统一
|
||||
# 计算误差 位置直接作差，姿态误差以旋转向量表示
|
||||
|
||||
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
|
||||
# 缓存常用计算
|
||||
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)
|
||||
# 期望加速度
|
||||
|
||||
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
|
||||
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)
|
||||
# 更新位置
|
||||
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:
|
||||
# print("11111111111111111111111:",self.state.desired_position,self.state.arm_position_command,self.state.arm_position)
|
||||
|
||||
def step_traj(self,dt):
|
||||
# 方向统一
|
||||
|
||||
@ -150,6 +150,10 @@ class dobot_nova5:
|
||||
atexit.register(self.exit_task)
|
||||
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.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.is_exit = False
|
||||
self.is_standby = False
|
||||
self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
|
||||
self.filter_matrix = np.zeros((1,6)) # 角度伺服用
|
||||
|
||||
# 为状态管理而封装的初始化函数
|
||||
def init(self):
|
||||
@ -164,6 +168,8 @@ class dobot_nova5:
|
||||
self.filter_matrix = np.zeros((1,6)) # 角度伺服用
|
||||
sleep(1)
|
||||
self.is_standby = False
|
||||
self.move_joint_jog(q=self.init_pos)
|
||||
time.sleep(0.5)
|
||||
code = self.move_joint(self.init_pos)
|
||||
|
||||
if code == 0:
|
||||
@ -275,14 +281,16 @@ class dobot_nova5:
|
||||
|
||||
def waitArrival(self, command):
|
||||
sendCommandID = self.parseResultId(command)[1]
|
||||
# print("self.parseResultId(command)",self.parseResultId(command))
|
||||
while True:
|
||||
if self.feedbackData.robotMode in [3, 4, 6, 8, 9, 10, 11]:
|
||||
if self.feedbackData.robotMode in [3, 4, 6, 9, 10, 11]:
|
||||
self.logger.log_error(f"机械臂异常:{self.feedbackData.robotMode}")
|
||||
print(robot_mode_types.get(self.feedbackData.robotMode, "未知模式"))
|
||||
self.dashboard.Stop()
|
||||
self.dashboard.EmergencyStop(mode=1)
|
||||
return -1
|
||||
if self.feedbackData.EnableState:
|
||||
print("self.feedbackData.robotCurrentCommandID",self.feedbackData.robotCurrentCommandID)
|
||||
if self.feedbackData.robotCurrentCommandID > sendCommandID:
|
||||
break
|
||||
else:
|
||||
@ -314,7 +322,32 @@ class dobot_nova5:
|
||||
return -1
|
||||
print(f'After {cnt} retries, still failed to arrive at the joint position: {q}')
|
||||
return -1
|
||||
|
||||
|
||||
def move_joint_jog(self,q,**params):
|
||||
'''
|
||||
move_joint_jog 以JOINT jOG运动方式运动到目标点
|
||||
MovJ flag = 1 通过关节角度走点，输入为[J1 J2 J3 J4 J5 J6] unit: rad
|
||||
'''
|
||||
threshold = params.get('threshold', 10)
|
||||
currentAngle = self.getAngel()
|
||||
targetAngle = self.__transform_rad_2_deg(q)
|
||||
currentAngle = np.array(currentAngle)
|
||||
targetAngle = np.array(targetAngle)
|
||||
self.dashboard.SpeedFactor(70)
|
||||
|
||||
for i in range(len(targetAngle)):
|
||||
while np.abs(currentAngle[i] - targetAngle[i]) >threshold:
|
||||
if currentAngle[i] < targetAngle[i]:
|
||||
self.dashboard.MoveJog(f"J{i+1}+")
|
||||
else:
|
||||
self.dashboard.MoveJog(f"J{i+1}-")
|
||||
time.sleep(0.5)
|
||||
|
||||
currentAngle = self.getAngel()
|
||||
currentAngle = np.array(currentAngle)
|
||||
self.dashboard.MoveJog("")
|
||||
# self.dashboard.SpeedFactor(95)
|
||||
|
||||
def RunPoint_P_inPose_M_RAD(self,pose,**params):
|
||||
'''
|
||||
RUNPOINT_P 以关节运动方式运动到目标点
|
||||
@ -810,16 +843,31 @@ class dobot_nova5:
|
||||
|
||||
if __name__ == "__main__":
|
||||
# sleep(5)
|
||||
|
||||
dobot = dobot_nova5("192.168.5.1")
|
||||
dobot.start()
|
||||
posJ = [0,30,-120,0,90,0]
|
||||
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.init()
|
||||
|
||||
# dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90])
|
||||
# dobot.move_joint_jog(q=dobot.init_pos)
|
||||
|
||||
# dobot.dashboard.MoveJog("J1+")
|
||||
# time.sleep(1)
|
||||
# dobot.dashboard.MoveJog("")
|
||||
# print(dobot.getAngel())
|
||||
# # while True:
|
||||
# # time.sleep(1)
|
||||
|
||||
# # dobot.start()
|
||||
# posJ = [0,30,-120,0,90,0]
|
||||
# dobot.start_drag()
|
||||
dobot.disableRobot()
|
||||
# time.sleep(60)
|
||||
# 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()
|
||||
Loading…
x
Reference in New Issue
Block a user