class VRController: def __init__(self, host, ur_frame, base_frame, arm_name, init_posej=None): try: self.vr_init_pose = None self.ur_init_pose = None self.time = time.time() self.scale = 1 # 机器人移动距离与手臂的比值 self.ur_frame = ur_frame self.base_frame = base_frame # self.angle = None self.last_gripper_value = 0 self.stop_vr = False self.gripper = [False, False] self.force_mode = [False, False] self.tran_base_to_ur = TransformerTool(target_frame=ur_frame, source_frame=base_frame) self.ur_robot = URRobot(host) if init_posej is not None: self.ur_robot.movej(init_posej, acc=0.5, vel=1) rospy.Subscriber(name=arm_name + "/vr_pose4", data_class=Pose, callback=self.on_sub_pose, queue_size=3) rospy.Subscriber(name=arm_name + "/gripper", data_class=Int32, callback=self.on_sub_gripper, queue_size=1) except Exception, e: print(e) self.ur_robot.close() exit()
from urx import URRobot rbt = URRobot('192.168.31.53') import numpy as np def camera2base(pose_on_camera): Tm = np.array([[-0.9992, 0.0082, 0.0383, 0.0183], [-0.0036, 0.9543, -0.2987, -0.2376], [-0.0390, -0.2986, -0.9536, 1.1127], [0, 0, 0, 1.0000]]) #pose_on_base=np.dot([pose_on_camera.x,pose_on_camera.y,pose_on_camera.z,1],Tm.T) pose_on_base = np.dot( [pose_on_camera[0], pose_on_camera[1], pose_on_camera[2], 1], Tm.T) pose_on_base[0] = pose_on_base[0] - 0.03 pose_on_base[1] = pose_on_base[1] * 0.98 - 0.01 pose_on_base[2] = pose_on_base[2] * 0.94 + 0.09 return pose_on_base pose_on_camera = [0.12036043, -0.04154134, 1.215] pose_on_base = camera2base(pose_on_camera) destination = [pose_on_base[0], pose_on_base[1], pose_on_base[2], 3.14, 0, 0] print(destination) rbt.movel(destination, 0.1, 0.1) rbt.close()
class VRController: def __init__(self, host, ur_frame, base_frame, arm_name, init_posej=None): self.ur_robot = None try: self.vr_init_pose = None self.ur_init_pose = None self.time = time.time() self.scale = 1 # 机器人移动距离与手臂的比值 self.ur_frame = ur_frame self.base_frame = base_frame # self.angle = None self.last_gripper_value = 0 self.stop_vr = False self.gripper = [False, False] self.force_mode = [False, False] self.tran_base_to_ur = TransformerTool(target_frame=ur_frame, source_frame=base_frame) self.ur_robot = URRobot(host) if init_posej is not None: self.movej(init_posej) # rospy.Subscriber( # name=arm_name + "/vr_pose4", data_class=Pose, callback=self.on_sub_pose, queue_size=3) # rospy.Subscriber(name=arm_name + "/gripper", data_class=Int32, # callback=self.on_sub_gripper, queue_size=1) except Exception as e: print(e.message) if self.ur_robot is not None: self.ur_robot.close() exit() def movej(self, pose): self.ur_robot.movej(pose, acc=0.5, vel=1) def on_recv_data(self, data): # euler_pose = data['euler_pose'] quaternion_pose = data['quaternion_pose'] touched = data['touched'] pose = Pose() pose.position.x = quaternion_pose[0] pose.position.y = quaternion_pose[1] pose.position.z = quaternion_pose[2] pose.orientation.x = quaternion_pose[3] pose.orientation.y = quaternion_pose[4] pose.orientation.z = quaternion_pose[5] pose.orientation.w = quaternion_pose[6] self.change_status(touched['circleY']) self.grip(touched['gripper']) self.on_sub_pose(pose) def close(self): self.ur_robot.close() def set_force_mode(self, value): if value == self.force_mode[1]: self.force_mode[0] = False else: self.force_mode[1] = value self.force_mode[0] = True def grip(self, data): if data == self.last_gripper_value: self.gripper[0] = False return self.gripper[0] = True self.gripper[1] = data == 1 # self.ur_robot.set_digital_out(8, data.data != 1) self.last_gripper_value = data def on_sub_gripper(self, data): self.grip(data.data) def mat2rvec(self, mat): theta = math.acos((mat[0, 0] + mat[1, 1] + mat[2, 2] - 1.0) * 0.5) if theta < 0.001: return [0, 0, 0] else: rx = theta * (mat[2, 1] - mat[1, 2]) / (2 * math.sin(theta)) ry = theta * (mat[0, 2] - mat[2, 0]) / (2 * math.sin(theta)) rz = theta * (mat[1, 0] - mat[0, 1]) / (2 * math.sin(theta)) return [rx, ry, rz] def change_status(self, data): if data != 0: self.begin_control() else: self.stop_control() # if data > 0: # self.set_force_mode(True) # else: # self.set_force_mode(False) def on_sub_pose(self, data): try: current_pose = self.rvec_pose_from_msg_pose(data) ########################################### '调整世界坐标系下的角度' rotation_matrix = tf.transformations.quaternion_matrix([ data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w ]) target_rotation_matrix = np.dot( rotation_matrix, np.array([[0, 0, 1, 0], [1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 0, 1]])) angle = self.mat2rvec(target_rotation_matrix) # if self.ur_frame == "right_arm_base": # self.angle[2] -= 180 / math.pi current_pose[3] = angle[0] current_pose[4] = angle[1] current_pose[5] = angle[2] # self.angle = current_pose[3:6] ################################################ if self.stop_vr: self.vr_init_pose = None self.ur_robot.send_program( 'servoj(get_actual_joint_positions(),t=0.05,lookahead_time=0.03,gain=100)' ) return elif self.vr_init_pose is None: self.vr_init_pose = current_pose pose = self.ur_robot.getl() self.ur_init_pose = self.tranform_pose(TransformerTool( target_frame=self.base_frame, source_frame=self.ur_frame), pose=pose) target_pose = [] for i in range(0, 3): target_pose.append(self.ur_init_pose[i] + (current_pose[i] - self.vr_init_pose[i]) * self.scale) target_pose.append(angle[0]) target_pose.append(angle[1]) target_pose.append(angle[2]) target_pose = self.tranform_pose(self.tran_base_to_ur, pose=target_pose) self.move_arm_robot(target_pose) except Exception as e: print(e) # self.ur_robot.close() def rvec_pose_from_msg_pose(self, pose): quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) # tran = TransformerTool() rvec = self.tran_base_to_ur.quat2rvec(quaternion) return [ pose.position.x, pose.position.y, pose.position.z, rvec[0], rvec[1], rvec[2] ] def stop_control(self): # self.ur_robot.stopj() self.stop_vr = True def begin_control(self): self.stop_vr = False def move_arm_robot(self, target_pose): time_limit = 0.25 joing_speed_limit = [30.0, 30.0, 60.0, 90.0, 90.0, 120.0] joint_max_distance = [] for i in range(len(joing_speed_limit)): joint_max_distance.append(joing_speed_limit[i] / 180 * math.pi * time_limit) # 当工具距离机械臂基座标系较远时停止移动(此方法治标不治本,更合理的方式应为无法规划出有效路径时停止移动,暂未找到判断是否成功规划路径的方法) if math.sqrt( math.pow(target_pose[0], 2) + math.pow(target_pose[1], 2) + math.pow(target_pose[2], 2) > 0.65): self.ur_robot.send_program( 'servoj(get_actual_joint_positions(),t={},lookahead_time=0.1,gain=100)' .format(time_limit)) print('too long') return urscript = URScript() # 控制夹手 if self.gripper[0]: urscript.add_line_to_program('set_tool_digital_out(0,{})'.format( self.gripper[1])) # urscript.add_line_to_program( # 'textmsg("-------------------------------------------------")') urscript.add_line_to_program('pi={}'.format(math.pi)) # 添加pi常量 # urscript.add_line_to_program('textmsg("pose: [{},{},{},{},{},{}])")'.format( # target_pose[0], target_pose[1], target_pose[2], target_pose[3], target_pose[4], target_pose[5])) # 获取当前关节角度 urscript.add_line_to_program( 'current_joint=get_actual_joint_positions()') # 力模式 # if self.force_mode[0]: # if self.force_mode[1]: # urscript.add_line_to_program( # 'force_mode(target_pose,[0,0,0,1.1,1.1,1.1],[0,0,0,0,0,0],2,[100,100,100,20,20,20])') # urscript.add_line_to_program('sleep(0.1)') # else: # urscript.add_line_to_program('end_force_mode()') urscript.add_line_to_program('inverse_kin=current_joint') urscript.add_line_to_program( 'inverse_kin=get_inverse_kin(p[{},{},{},{},{},{}],maxPositionError=0.02,maxOrientationError=0.02)' .format(target_pose[0], target_pose[1], target_pose[2], target_pose[3], target_pose[4], target_pose[5])) # urscript.add_line_to_program('textmsg("inverse kin:",inverse_kin)') # 若计算出的关节值与当前值差距较大,保持当前状态(仅判断前几个关节,治标不治本) urscript.add_line_to_program('i=0') urscript.add_line_to_program('while i<3:') urscript.add_line_to_program( ' if norm(inverse_kin[i]-current_joint[i])>1:') urscript.add_line_to_program(' inverse_kin=current_joint') # urscript.add_line_to_program(' textmsg(i,": distance too big")') urscript.add_line_to_program(' break') urscript.add_line_to_program(' end') urscript.add_line_to_program(' i=i+1') urscript.add_line_to_program('end') # 添加速度限制 urscript.add_line_to_program( 'joint_max_distance=[{},{},{},{},{},{}]'.format( joint_max_distance[0], joint_max_distance[1], joint_max_distance[2], joint_max_distance[3], joint_max_distance[4], joint_max_distance[5])) # urscript.add_line_to_program('textmsg("current joint:",current_joint)') # 添加关节限制 urscript.add_line_to_program('i=0') urscript.add_line_to_program('while i<6:') # 添加关节速度限制 urscript.add_line_to_program( ' if inverse_kin[i]-current_joint[i]>joint_max_distance[i]:') urscript.add_line_to_program( ' inverse_kin[i]=current_joint[i]+joint_max_distance[i]') urscript.add_line_to_program( ' elif inverse_kin[i]-current_joint[i]<-joint_max_distance[i]:') urscript.add_line_to_program( ' inverse_kin[i]=current_joint[i]-joint_max_distance[i]') urscript.add_line_to_program(' end') # 添加关节角度限制 urscript.add_line_to_program( ' if i<5 and norm(inverse_kin[i])>{}:'.format(math.pi * 2 - 0.2)) urscript.add_line_to_program(' inverse_kin[i]=current_joint[i]') # urscript.add_line_to_program(' textmsg("***: ",i)') urscript.add_line_to_program(' end') urscript.add_line_to_program(' i=i+1') urscript.add_line_to_program('end') # urscript.add_line_to_program('textmsg("target joint:",inverse_kin)') # urscript.add_line_to_program( # 'if norm(inverse_kin[0])<pi*2 and norm(inverse_kin[1])<pi*2:') urscript.add_line_to_program( 'servoj(inverse_kin,t={},lookahead_time=0.05,gain=100)'.format( time_limit)) # urscript.add_line_to_program('end') prog = urscript() # prog = "servoj(get_inverse_kin(p[{},{},{},{},{},{}]),t=0.30,lookahead_time=0.00,gain=100)".format( # target_pose[0], target_pose[1], target_pose[2], target_pose[3], target_pose[4], target_pose[5]) self.ur_robot.secmon.send_program(prog) def tranform_pose(self, tran, pose): src_quat = tran.rvec2quat(pose[3:6]) src_pose = Pose() src_pose.position.x = pose[0] src_pose.position.y = pose[1] src_pose.position.z = pose[2] src_pose.orientation.x = src_quat[0] src_pose.orientation.y = src_quat[1] src_pose.orientation.z = src_quat[2] src_pose.orientation.w = src_quat[3] target_pose = tran.transformPose(pose=src_pose) result = self.rvec_pose_from_msg_pose(target_pose) ################################################## # if self.ur_frame == "right_arm_base": # print([target_pose.orientation.x, target_pose.orientation.y, # target_pose.orientation.z, target_pose.orientation.w]) # if result[5] < -3: # target_pose.orientation.x *= -1 # target_pose.orientation.y *= -1 # target_pose.orientation.z *= -1 # target_pose.orientation.w *= -1 # inverse_pose = self.rvec_pose_from_msg_pose(target_pose) # print("inverse rvec: " + str(inverse_pose[3:6])) # return inverse_pose ################################################## return result