운동학 캘리브레이션 이후 실제 운동학 파라미터를 얻는 방법

Introduction

신규 제품에는 운동학 캘리브레이션 기능이 추가되었으며, 실제 운동학 파라미터는 당사에서 제공하는 기준 DH 파라미터와 약간 다를 수 있습니다. get_forward_kinematics() API와 Studio의 결과에는 이미 해당 캘리브레이션 값이 반영되어 있습니다.

Robot Arm Model	Kinematics Calibration
xArm1303 또는 그 이하 버전	No
xArm1304 또는 Lite6	Support Team에 SN을 공유해 주세요
xArm1305 또는 850	Yes

Python Script

gen_kinematics_params.py

import os
import sys
import socket
import struct

try:
    from yaml import dump
except:
    def dump(data, f, indent=0, **kwargs):
        buf = []
        for key, val in data.items():
            if isinstance(val, dict):
                buf.append('{}{}:'.format(' ' * indent, key))
                buf += dump(val, None, indent=indent+2, **kwargs)
            else:
                buf.append('{}{}: {}'.format(' ' * indent, key, val))
        if f is not None:
            f.write('\n'.join(buf))
        return buf
if __name__ == '__main__':
    if len(sys.argv) < 3:
        print('Usage: {} {{robot_ip}} {{kinematics_suffix (to distinguish your multiple arms)}}'.format(sys.argv[0]))
        exit(1)
    robot_ip = sys.argv[1]
    kinematics_suffix = sys.argv[2]
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.connect((robot_ip, 502))

    send_data = [0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x08]
    sock.send(bytes(send_data))
    recv_data = sock.recv(179)
    if len(recv_data) == 179 and recv_data[8]:
        robot_dof = recv_data[9]
        robot_type = recv_data[10]
        robot_name = 'uf850' if robot_dof == 6 and robot_type == 12 else 'lite6' if robot_dof == 6 and robot_type == 9 else 'xarm{}'.format(robot_dof)
        ouput_dir = os.path.join(os.path.dirname(__file__), 'user')
        if not os.path.exists(ouput_dir):
            os.makedirs(ouput_dir)
        output_file = os.path.join(ouput_dir, '{}_kinematics_{}.yaml'.format(robot_name, kinematics_suffix))
        params = struct.unpack('<42f', recv_data[11:])
        kinematics = {}
        data = {'kinematics': kinematics}
        for i in range(robot_dof):
            joint_param = {}
            kinematics['joint{}'.format(i + 1)] = joint_param
            joint_param['x'] = params[i * 6]
            joint_param['y'] = params[i * 6 + 1]
            joint_param['z'] = params[i * 6 + 2]
            joint_param['roll'] = params[i * 6 + 3]
            joint_param['pitch'] = params[i * 6 + 4]
            joint_param['yaw'] = params[i * 6 + 5]
        with open(output_file, 'w', encoding='utf-8') as f:
            try:
                dump(data, f, default_flow_style=False, allow_unicode=True, sort_keys=False)
            except:
                dump(data, f, default_flow_style=False, allow_unicode=True)
        print('[Success] save to {}'.format(output_file))
    else:
        print('[Failed] recv_len={}, valid={}'.format(len(recv_data), 0 if len(recv_data) < 9 else recv_data[8]))

운동학 파라미터 파일 생성

python gen_kinematics_params.py [robot_ip] [kinematics_suffix]

python gen_kinematics_params.py 192.168.1.212 ip212

robot_ip는 로봇 암의 IP를 의미합니다. 실제 파라미터를 얻으려면 로봇 암에 연결되어 있어야 합니다.

kinematics_suffix는 생성되는 파라미터 파일의 접미사를 의미합니다.

성공하면 설정 파일은 /user 디렉터리에 생성됩니다. kinematics_suffix가 AAA인 경우, 해당 파일명은 아래와 같습니다.

• xarm5: user/xarm5_kinematics_AAA.yaml

• xarm6: user/xarm6_kinematics_AAA.yaml

• xarm7: user/xarm7_kinematics_AAA.yaml

• lite6: user/lite6_kinematics_AAA.yaml

• uf850: user/uf850_kinematics_AAA.yaml

형식은 더 이상 DH가 아니며, 인접한 각 관절 좌표계 사이의 6자유도 (x, y, z, roll, pitch, yaw) 변환 관계입니다. 단위는 미터(Meters)와 라디안(Radians)입니다.