CS系列机器人采用MDH方法建模，MDH数据见 https://bbs.elibot.cn/forum/detail/topic/186.html

用户可以通过解析机器人30001端口的数据，获得当前机器人的MDH数据

关于30001返回数据解析，可以通过以下链接获得

链接：https://pan.baidu.com/s/1jRJHMvMvGpNiS9_V3XtT1g
提取码：06r0

以下示例为，机器人获取本机MDH参数，并计算当前机器人位置下的机器人正运动学。可以看到计算结果正确。

import socket
import struct
import time

HOST = "127.0.0.1"  # The server's hostname or IP address
PORT = 30001  # The port used by the server

global a
global d
global alpha
global theta
a = [0, 0, 0, 0, 0, 0]
d = [0, 0, 0, 0, 0, 0]
alpha = [0, 0, 0, 0, 0, 0]
theta = [0, 0, 0, 0, 0, 0]

# CS机器人获取到的是MDH参数
bFinish = False
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
    s.connect((HOST, PORT))

    while bFinish == False:
        __buf = s.recv(4096)
        #print(len(__buf))
        if len(__buf) <= 0:
            time.sleep(0.01)

        while(len(__buf) > 0):
            data_length = struct.unpack(">i", __buf[0:4])[0]
            data_type = struct.unpack("B", __buf[4:5])[0]
            #print(data_length, data_type)
            data, __buf = __buf[0:data_length], __buf[data_length:]

            if data_type == 16:
                # MESSAGE_TYPE_ROBOT_STATE = 16
                bFinish = True
                sub_type = 0
                
                data = data[5:data_length]
                # 去掉前5个字节的报文头
                while sub_type != 6:
                    #ROBOT_STATE_PACKAGE_TYPE_CONFIGURATION_DATA = 6
                    sub_length = struct.unpack(">i", data[0:4])[0]
                    sub_type = struct.unpack("B", data[4:5])[0]
                    data1, data = data[0:sub_length], data[sub_length:]

                curr_data_add = 5+32*6+40
				
                for i in range(0, 6):
                        a[i] = struct.unpack(
                                ">d", data1[curr_data_add:curr_data_add+8])[0]
                        curr_data_add = curr_data_add+8

                for i in range(0, 6):
                        d[i] = struct.unpack(
                                ">d", data1[curr_data_add:curr_data_add+8])[0]
                        curr_data_add = curr_data_add+8

                for i in range(0, 6):
                        alpha[i] = struct.unpack(
                                ">d", data1[curr_data_add:curr_data_add+8])[0]
                        curr_data_add = curr_data_add+8

                for i in range(0, 6):
                        theta[i] = struct.unpack(
                                ">d", data1[curr_data_add:curr_data_add+8])[0]
                        curr_data_add = curr_data_add+8


def mdh2pose(a,alpha,d,theta):
    # Rx(alpha) * Dx(a) * Rz(theta) * Dz(d)
    pose = [0, 0, 0, 0, 0, 0]
    pose[0] = a
    pose[3] = alpha

    pose = pose_trans(pose, [0, 0, 0, 0, 0, theta])
    pose = pose_trans(pose, [0, 0, d, 0, 0, 0])
    return pose
end

def ForwardKinCal(c_joint):
    # 输入当前角度，进行机器人正运动学计算
    out = [0,0,0,0,0,0]
    for i in range(0,6):
        out = pose_trans(out,mdh2pose(a[i], alpha[i], d[i], theta[i]+c_joint[i]))
    return out
	# 返回的是当前法兰盘末端pose

global c_pose
# xyz单位是米，rpy单位是rad

global c_pose1
# xyz单位是mm，rpy单位是deg

joint = get_actual_joint_positions()
c_pose = ForwardKinCal(joint)
c_pose1=[c_pose[0]*1000,c_pose[1]*1000,c_pose[2]*1000,c_pose[3]/3.14159*180,c_pose[4]/3.14159*180,c_pose[5]/3.14159*180]