CS脚本计算用户坐标系及对准功能

用户可以使用以下代码,计算坐标系,p1为原点,p1p2为x方向,p1p2p3构成xy平面

# author: chenliao@elibot.cn
import math def dot_product(v1, v2): return sum(a*b for a, b in zip(v1, v2)) def cross_product(v1, v2): return [v1[1]*v2[2] - v1[2]*v2[1], v1[2]*v2[0] - v1[0]*v2[2], v1[0]*v2[1] - v1[1]*v2[0]] def Mat2RPY(rotation_matrix): # 计算 yaw 角 yaw = math.atan2(rotation_matrix[1][0], rotation_matrix[0][0]) # 计算 pitch 角 pitch = math.atan2(-rotation_matrix[2][0], math.sqrt(rotation_matrix[2][1]**2 + rotation_matrix[2][2]**2)) # 计算 roll 角 roll = math.atan2(rotation_matrix[2][1], rotation_matrix[2][2]) return ([roll,pitch,yaw]) def cal_user(p1,p2,p3): # p1为原点, p1p2为x方向, p1p2p3构成xy平面 # 返回user # 计算两个向量 v1 = [0,0,0] v2 = [0,0,0] for i in range(0,3): v1[i] = p2[i]-p1[i] v2[i] = p3[i]-p1[i] # 计算法向量 normal = cross_product(v1,v2) # 确保法向量为单位向量 normal = normalize(normal) # 计算机器人坐标系的 x、y、z 轴单位向量 x_axis = normalize(v1) z_axis = normal y_axis = cross_product(z_axis, x_axis) y_axis = normalize(y_axis) mat_rot = [[x_axis[0],y_axis[0],z_axis[0]], [x_axis[1],y_axis[1],z_axis[1]], [x_axis[2],y_axis[2],z_axis[2]]] user_rad = Mat2RPY(mat_rot) return [p1[0],p1[1],p1[2],user_rad[0],user_rad[1],user_rad[2]] def alignTCP(user=[0,0,0,0,0,0]): # 将当前末端姿态的z垂直用户坐标系的xy平面 p_in_base =get_actual_tcp_pose() p_in_user = pose_trans(pose_inv(user),p_in_base) rx = p_in_user[3] ry = p_in_user[4] rz = p_in_user[5] if (rx>=90) or (rx<=-90): rx = 180 else: rx = 0 ry = 0 p_in_user[3] = rx p_in_user[4] = ry movel(pose_trans(user,p_in_user),v=0.1) # # 三个关键点的坐标 p1 = [0.4870015119080839, -0.14749998942204118, 0.49750784416160615] p2 = [0.5620267993968734, -0.1928286859223005, 0.49750488511249974] p3 = [0.49155327034810975, -0.278376837186979, 0.5244138572890163] user = cal_user(p1, p2, p3) # 返回坐标系单位m,rad
点击显示全文
赞同0
发表评论
分享

手机扫码分享
0
217
收藏
举报
收起
登录
  • 密码登录
  • 验证码登录
还没有账号,立即注册
还没有账号,立即注册
注册
已有账号,立即登录
选择发帖板块
举报
请选择举报理由
举报
举报说明