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