【CSJP020】スクリプトを使用して TCP をユーザー座標系に合わせる方法
  • ユーザー座標系に合わせる
Blinders
这个人很懒,没有留下如何东西~

ユーザーは次のコードを使用して座標系を計算できます。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 平面を構成
    # 戻り値 ユーザー座標

    # 二つのベクトルを計算
    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
285
收藏
举报
收起
登录
  • 密码登录
  • 验证码登录
还没有账号,立即注册
还没有账号,立即注册
注册
已有账号,立即登录
选择发帖板块
上传文件
举报
请选择举报理由
举报
举报说明