xarm-python-sdk 1.15.2__py3-none-any.whl

xarm/x3/modbus_tcp.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2023, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
+from .base import Base
+from .decorator import xarm_is_connected
+
+
+class ModbusTcp(Base):
+    def __init__(self):
+        super(ModbusTcp, self).__init__()
+    
+    @xarm_is_connected(_type='get')
+    def read_coil_bits(self, addr, quantity):
+        """
+        func_code: 0x01
+        """
+        return self.arm_cmd.read_coil_bits(addr, quantity)
+
+    @xarm_is_connected(_type='get')
+    def read_input_bits(self, addr, quantity):
+        """
+        func_code: 0x02
+        """
+        return self.arm_cmd.read_input_bits(addr, quantity)
+    
+    @xarm_is_connected(_type='get')
+    def read_holding_registers(self, addr, quantity, is_signed=False):
+        """
+        func_code: 0x03
+        """
+        return self.arm_cmd.read_holding_registers(addr, quantity, is_signed)
+    
+    @xarm_is_connected(_type='get')
+    def read_input_registers(self, addr, quantity, is_signed=False):
+        """
+        func_code: 0x04
+        """
+        return self.arm_cmd.read_input_registers(addr, quantity, is_signed)
+    
+    @xarm_is_connected(_type='set')
+    def write_single_coil_bit(self, addr, bit_val):
+        """
+        func_code: 0x05
+        """
+        return self.arm_cmd.write_single_coil_bit(addr, bit_val)
+    
+    @xarm_is_connected(_type='set')
+    def write_single_holding_register(self, addr, reg_val):
+        """
+        func_code: 0x06
+        """
+        return self.arm_cmd.write_single_holding_register(addr, reg_val)
+
+    @xarm_is_connected(_type='set')
+    def write_multiple_coil_bits(self, addr, bits):
+        """
+        func_code: 0x0F
+        """
+        return self.arm_cmd.write_multiple_coil_bits(addr, bits)
+
+    @xarm_is_connected(_type='set')
+    def write_multiple_holding_registers(self, addr, regs):
+        """
+        func_code: 0x10
+        """
+        return self.arm_cmd.write_multiple_holding_registers(addr, regs)
+
+    @xarm_is_connected(_type='set')
+    def mask_write_holding_register(self, addr, and_mask, or_mask):
+        """
+        func_code: 0x16
+        """
+        return self.arm_cmd.mask_write_holding_register(addr, and_mask, or_mask)
+
+    @xarm_is_connected(_type='get')
+    def write_and_read_holding_registers(self, r_addr, r_quantity, w_addr, w_regs, is_signed=False):
+        """
+        func_code: 0x17
+        """
+        return self.arm_cmd.write_and_read_holding_registers(r_addr, r_quantity, w_addr, w_regs, is_signed)

xarm/x3/parse.py

@@ -0,0 +1,110 @@
+#!/usr/bin/env python3
+# Software License Agreement (MIT License)
+#
+# Copyright (c) 2018, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
+
+import re
+
+GCODE_PARAM_X = 'X'  # TCP-X
+GCODE_PARAM_Y = 'Y'  # TCP-Y
+GCODE_PARAM_Z = 'Z'  # TCP-Z
+GCODE_PARAM_A = 'A'  # TCP-Roll
+GCODE_PARAM_B = 'B'  # TCP-Pitch
+GCODE_PARAM_C = 'C'  # TCP-Yaw
+GCODE_PARAM_R = 'R'  # TCP-Radius
+GCODE_PARAM_I = 'I'  # Joint-1
+GCODE_PARAM_J = 'J'  # Joint-2
+GCODE_PARAM_K = 'K'  # Joint-3
+GCODE_PARAM_L = 'L'  # Joint-4
+GCODE_PARAM_M = 'M'  # Joint-5
+GCODE_PARAM_N = 'N'  # Joint-6
+GCODE_PARAM_O = 'O'  # Joint-7
+GCODE_PARAM_F = 'F'  # Move-Speed
+GCODE_PARAM_Q = 'Q'  # Move-Acc
+GCODE_PARAM_T = 'T'  # Move-Time
+GCODE_PARAM_V = 'V'  # Value
+GCODE_PARAM_D = 'D'  # Addr
+
+
+class GcodeParser:
+    def __init__(self):
+        self._int_val = 0
+        self._float_val = 0.0
+
+    @staticmethod
+    def __get_value(string, ch, return_type, default=None):
+        pattern = r'{}(\-?\d+\.?\d*)'.format(ch)
+        data = re.findall(pattern, string)
+        if len(data) > 0:
+            return return_type(data[0])
+        return default
+
+    @staticmethod
+    def __get_hex_value(string, ch, default=None):
+        pattern = r'{}(-?\w{{3,4}})'.format(ch)
+        data = re.findall(pattern, string)
+        if len(data) > 0:
+            return int(data[0], base=16)
+        return default
+
+    def _get_int_value(self, string, ch, default=None):
+        return self.__get_value(string, ch, int, default=default)
+
+    def _get_float_value(self, string, ch, default=None):
+        return self.__get_value(string, ch, float, default=default)
+
+    def get_int_value(self, string, default=None):
+        if default is None:
+            default = self._int_val
+            self._int_val = self._get_int_value(string, GCODE_PARAM_V, default=default)
+            return self._int_val
+        else:
+            return self._get_int_value(string, GCODE_PARAM_V, default=default)
+
+    def get_float_value(self, string, default=0):
+        return self._get_float_value(string, GCODE_PARAM_V, default=default)
+
+    def get_addr(self, string, default=0):
+        return self.__get_hex_value(string, GCODE_PARAM_D, default=default)
+
+    def get_gcode_cmd_num(self, string, ch):
+        return self._get_int_value(string, ch, default=-1)
+
+    def get_mvvelo(self, string, default=None):
+        return self._get_float_value(string, GCODE_PARAM_F, default=default)
+
+    def get_mvacc(self, string, default=None):
+        return self._get_float_value(string, GCODE_PARAM_Q, default=default)
+
+    def get_mvtime(self, string, default=None):
+        return self._get_float_value(string, GCODE_PARAM_T, default=default)
+
+    def get_mvradius(self, string, default=None):
+        return self._get_float_value(string, GCODE_PARAM_R, default=default)
+
+    def get_id_num(self, string, default=None):
+        return self._get_int_value(string, GCODE_PARAM_I, default=default)
+
+    def get_poses(self, string, default=None):
+        pose = [None] * 6
+        pose[0] = self._get_float_value(string[2:], GCODE_PARAM_X, default=default)
+        pose[1] = self._get_float_value(string[2:], GCODE_PARAM_Y, default=default)
+        pose[2] = self._get_float_value(string[2:], GCODE_PARAM_Z, default=default)
+        pose[3] = self._get_float_value(string[2:], GCODE_PARAM_A, default=default)
+        pose[4] = self._get_float_value(string[2:], GCODE_PARAM_B, default=default)
+        pose[5] = self._get_float_value(string[2:], GCODE_PARAM_C, default=default)
+        return pose
+
+    def get_joints(self, string, default=None):
+        joints = [None] * 7
+        joints[0] = self._get_float_value(string[2:], GCODE_PARAM_I, default=default)
+        joints[1] = self._get_float_value(string[2:], GCODE_PARAM_J, default=default)
+        joints[2] = self._get_float_value(string[2:], GCODE_PARAM_K, default=default)
+        joints[3] = self._get_float_value(string[2:], GCODE_PARAM_L, default=default)
+        joints[4] = self._get_float_value(string[2:], GCODE_PARAM_M, default=default)
+        joints[5] = self._get_float_value(string[2:], GCODE_PARAM_N, default=default)
+        joints[6] = self._get_float_value(string[2:], GCODE_PARAM_O, default=default)
+        return joints

xarm/x3/record.py

@@ -0,0 +1,216 @@
+#!/usr/bin/env python3
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2019, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
+
+import json
+import time
+import uuid
+from urllib import request
+from .code import APIState
+from ..core.config.x_config import XCONF
+from ..core.utils.log import logger
+from .base import Base
+from .decorator import xarm_is_connected
+
+
+class Record(Base):
+    def __init__(self):
+        super(Record, self).__init__()
+
+    @xarm_is_connected(_type='get')
+    def get_trajectories(self, ip=None):
+        if ip is None:
+            url = 'http://{}:18333/cmd'.format(self._port)
+        else:
+            url = 'http://{}:18333/cmd'.format(ip)
+        try:
+            data = {'cmd': 'xarm_list_trajs'}
+            req = request.Request(url, headers={'Content-Type': 'application/json'}, data=json.dumps(data).encode('utf-8'))
+            res = request.urlopen(req)
+            if res.code == 200:
+                result = json.loads(res.read().decode('utf-8'))
+                return result['res'][0], [{'name': item['name'], 'duration': item['count'] / 100} for item in result['res'][1]]
+            else:
+                return APIState.API_EXCEPTION, []
+        except Exception as e:
+            return APIState.API_EXCEPTION, []
+
+    @xarm_is_connected(_type='set')
+    def start_record_trajectory(self):
+        ret = self.arm_cmd.set_record_traj(1)
+        self.log_api_info('API -> start_record_trajectory -> code={}'.format(ret[0]), code=ret[0])
+        return ret[0]
+
+    @xarm_is_connected(_type='set')
+    def stop_record_trajectory(self, filename=None, **kwargs):
+        ret = self.arm_cmd.set_record_traj(0)
+        if isinstance(filename, str) and filename.strip():
+            ret2 = self.save_record_trajectory(filename, wait=True, timeout=10, **kwargs)
+            if ret2 != 0:
+                return ret2
+        self.log_api_info('API -> stop_record_trajectory -> code={}'.format(ret[0]), code=ret[0])
+        return ret[0]
+
+    @xarm_is_connected(_type='set')
+    def save_record_trajectory(self, filename, wait=True, timeout=2, **kwargs):
+        assert isinstance(filename, str) and filename.strip()
+        filename = filename.strip()
+        if not filename.endswith('.traj'):
+            full_filename = '{}.traj'.format(filename)
+        else:
+            full_filename = filename
+        self.get_trajectory_rw_status()
+        feedback_key, studio_wait = self._gen_feedback_key(wait, **kwargs)
+        ret = self.arm_cmd.save_traj(full_filename, wait_time=0, feedback_key=feedback_key)
+        trans_id = self._get_feedback_transid(feedback_key, studio_wait)
+        self.log_api_info('API -> save_record_trajectory -> code={}'.format(ret[0]), code=ret[0])
+        ret[0] = self._check_code(ret[0])
+        if ret[0] == 0 and wait:
+            return self.__wait_save_traj(timeout, trans_id, filename)
+        if ret[0] != 0:
+            logger.error('Save {} failed, ret={}'.format(filename, ret))
+        return ret[0]
+    
+    def __check_traj_status(self, status, filename='unknown'):
+        if status == XCONF.TrajState.LOAD_SUCCESS:
+            logger.info('Load {} success'.format(filename))
+            return 0
+        elif status == XCONF.TrajState.LOAD_FAIL:
+            logger.error('Load {} failed'.format(filename))
+            return APIState.TRAJ_RW_FAILED
+        elif status == XCONF.TrajState.SAVE_SUCCESS:
+            logger.info('Save {} success'.format(filename))
+            return 0
+        elif status == XCONF.TrajState.SAVE_FAIL:
+            logger.error('Save {} failed'.format(filename))
+            return APIState.TRAJ_RW_FAILED
+        return -1
+    
+    def __wait_traj_op(self, timeout, trans_id, filename='unknown', op='Load'):
+        if self._support_feedback and trans_id > 0:
+            code, feedback_code = self._wait_feedback(timeout, trans_id)
+            _, status = self.get_trajectory_rw_status()
+            if code == 0:
+                success_status = XCONF.TrajState.LOAD_SUCCESS if op == 'Load' else XCONF.TrajState.SAVE_SUCCESS
+                failure_status = XCONF.TrajState.LOAD_FAIL if op == 'Load' else XCONF.TrajState.SAVE_FAIL
+                status = success_status if feedback_code == XCONF.FeedbackCode.SUCCESS else failure_status if feedback_code == XCONF.FeedbackCode.FAILURE else status            
+            return self.__check_traj_status(status, filename)
+        else:
+            expired = (time.monotonic() + timeout) if timeout is not None else 0
+            idle_cnts = 0
+            while timeout is None or time.monotonic() < expired:
+                time.sleep(0.1)
+                code, status = self.get_trajectory_rw_status()
+                if self._check_code(code) == 0:
+                    if status == XCONF.TrajState.IDLE:
+                        idle_cnts += 1
+                        if idle_cnts >= 5:
+                            logger.info('{} {} failed, idle'.format(op, filename))
+                            return APIState.TRAJ_RW_FAILED
+                    else:
+                        code = self.__check_traj_status(status, filename)
+                        if code >= 0:
+                            return code
+            logger.warning('{} {} timeout'.format(op, filename))
+            return APIState.TRAJ_RW_TOUT
+    
+    def __wait_load_traj(self, timeout, trans_id, filename='unknown'):
+        return self.__wait_traj_op(timeout, trans_id, filename, 'Load')
+
+    def __wait_save_traj(self, timeout, trans_id, filename='unknown'):
+        return self.__wait_traj_op(timeout, trans_id, filename, 'Save')
+    
+    def __wait_play_traj(self, timeout, trans_id, times=1):
+        if self._support_feedback and trans_id > 0:
+            code, feedback_code = self._wait_feedback(timeout, trans_id)
+            if feedback_code == XCONF.FeedbackCode.FAILURE:
+                code = APIState.TRAJ_PLAYBACK_FAILED
+        else:
+            start_time = time.monotonic()
+            while self.state != 1:
+                if self.state in [4]:
+                    return APIState.STATE_NOT_READY
+                if time.monotonic() - start_time > 5:
+                    return APIState.TRAJ_PLAYBACK_TOUT
+                time.sleep(0.1)
+            max_count = int((time.monotonic() - start_time) / 0.1)
+            max_count = max_count if max_count > 10 else 10
+            start_time = time.monotonic()
+            while self.mode != 11:
+                if self.state == 1:
+                    start_time = time.monotonic()
+                    time.sleep(0.1)
+                    continue
+                if self.state in [4]:
+                    return APIState.STATE_NOT_READY
+                if time.monotonic() - start_time > 5:
+                    return APIState.TRAJ_PLAYBACK_TOUT
+                time.sleep(0.1)
+            time.sleep(0.1)
+            count = 0
+            while self.state not in [4]:
+                if self.state == 2:
+                    if times == 1:
+                        break
+                    count += 1
+                else:
+                    count = 0
+                if count > max_count:
+                    break
+                time.sleep(0.1)
+            code = 0 if self.state != 4 else APIState.STATE_NOT_READY
+
+        if self.state not in [4]:
+            self.set_mode(0)
+            self.set_state(0)
+        self._sync()
+        return code
+
+    @xarm_is_connected(_type='set')
+    def load_trajectory(self, filename, wait=True, timeout=None, **kwargs):
+        assert isinstance(filename, str) and filename.strip()
+        filename = filename.strip()
+        if not filename.endswith('.traj'):
+            full_filename = '{}.traj'.format(filename)
+        else:
+            full_filename = filename
+        self.get_trajectory_rw_status()
+        feedback_key, studio_wait = self._gen_feedback_key(wait, **kwargs)
+        ret = self.arm_cmd.load_traj(full_filename, wait_time=0, feedback_key=feedback_key)
+        trans_id = self._get_feedback_transid(feedback_key, studio_wait)
+        self.log_api_info('API -> load_trajectory -> code={}'.format(ret[0]), code=ret[0])
+        if ret[0] == 0 and wait:
+            return self.__wait_load_traj(timeout, trans_id, filename)
+        if ret[0] != 0:
+            logger.error('Load {} failed, ret={}'.format(filename, ret))
+        return ret[0]
+
+    @xarm_is_connected(_type='set')
+    def playback_trajectory(self, times=1, filename=None, wait=False, double_speed=1, **kwargs):
+        assert isinstance(times, int)
+        times = times if times > 0 else -1
+        if isinstance(filename, str) and filename.strip():
+            ret = self.load_trajectory(filename, wait=True, timeout=None)
+            if ret != 0:
+                return ret
+        if self.state in [4]:
+            return APIState.NOT_READY
+        feedback_key, studio_wait = self._gen_feedback_key(wait, **kwargs)
+        if self.version_is_ge(1, 2, 11):
+            ret = self.arm_cmd.playback_traj(times, double_speed, feedback_key=feedback_key)
+        else:
+            ret = self.arm_cmd.playback_traj_old(times)
+        trans_id = self._get_feedback_transid(feedback_key, studio_wait)
+        self.log_api_info('API -> playback_trajectory -> code={}'.format(ret[0]), code=ret[0])
+        if ret[0] == 0 and wait:
+            return self.__wait_play_traj(None, trans_id, times)
+        return ret[0]
+
+    @xarm_is_connected(_type='get')
+    def get_trajectory_rw_status(self):
+        ret = self.arm_cmd.get_traj_rw_status()
+        return ret[0], ret[1]

xarm/x3/report.py

@@ -0,0 +1,204 @@
+from xarm.core.utils import convert
+
+
+class ReportHandler(object):
+    def __init__(self, report_type):
+        self.buffer = b''
+        self.report_size = 0
+        self.report_type = report_type
+        if self.report_type == 'devlop':
+            self.parse_handler = self._parse_report_tcp_develop_data
+        elif self.report_type == 'normal':
+            self.parse_handler = self._parse_report_tcp_normal_data
+        elif self.report_type == 'rich':
+            self.parse_handler = self._parse_report_tcp_rich_data
+        else:
+            self.parse_handler = None
+        self.source_data = b''
+        self.parse_dict = {}
+
+    def reset(self):
+        self.buffer = b''
+        self.report_size = 0
+
+    def process_report_data(self, recv_data):
+        if recv_data == -1:
+            return
+        self.buffer += recv_data
+        if len(self.buffer) < 4:
+            return
+        if self.report_size == 0:
+            self.report_size = convert.bytes_to_u32(self.buffer[:4])
+        if len(self.buffer) < self.report_size:
+            return
+        if self.report_type == 'rich' and self.report_size == 233 and len(self.buffer) >= 245:
+            # 兼容某几版固件上报的数据的数据长度和实际数据的长度不一致
+            try:
+                if len(self.buffer) >= 249:
+                    if convert.bytes_to_u32(self.buffer[245:249]) != self.report_size:
+                        if convert.bytes_to_u32(self.buffer[233:237]) == self.report_size:
+                            data = self.buffer[:self.report_size]
+                            self.buffer = self.buffer[self.report_size:]
+                        else:
+                            self.reset()
+                            # TODO reconnect
+                            return -1
+                    else:
+                        data = self.buffer[:245]
+                        self.buffer = self.buffer[245:]
+                else:
+                    if convert.bytes_to_u32(self.buffer[233:237]) != self.report_size:
+                        data = self.buffer[:245]
+                        self.buffer = self.buffer[245:]
+                    else:
+                        data = self.buffer[:self.report_size]
+                        self.buffer = self.buffer[self.report_size:]
+            except:
+                self.reset()
+                # TODO reconnect
+                return -1
+        else:
+            data = self.buffer[:self.report_size]
+            self.buffer = self.buffer[self.report_size:]
+        self.source_data = data
+        if self.parse_handler:
+            return self.parse_handler(data)
+
+    def __parse_report_common_data(self, rx_data):
+        # length = convert.bytes_to_u32(rx_data[0:4])
+        length = len(rx_data)
+        state, mode = rx_data[4] & 0x0F, rx_data[4] >> 4
+        cmd_num = convert.bytes_to_u16(rx_data[5:7])
+        angles = convert.bytes_to_fp32s(rx_data[7:7 * 4 + 7], 7)
+        pose = convert.bytes_to_fp32s(rx_data[35:6 * 4 + 35], 6)
+        torque = convert.bytes_to_fp32s(rx_data[59:7 * 4 + 59], 7)
+        self.parse_dict['length'] = length
+        self.parse_dict['state'] = state
+        self.parse_dict['mode'] = mode
+        self.parse_dict['cmd_num'] = cmd_num
+        self.parse_dict['angles'] = angles
+        self.parse_dict['pose'] = pose
+        self.parse_dict['torque'] = torque
+        return [length, state, mode, cmd_num, angles, pose, torque]
+
+    def _parse_report_tcp_develop_data(self, rx_data):
+        ret = self.__parse_report_common_data(rx_data)
+        return ret
+
+    def _parse_report_tcp_normal_data(self, rx_data):
+        ret = self.__parse_report_common_data(rx_data)
+        mtbrake, mtable, error_code, warn_code = rx_data[87:91]
+        tcp_offset = convert.bytes_to_fp32s(rx_data[91:6 * 4 + 91], 6)
+        tcp_load = convert.bytes_to_fp32s(rx_data[115:4 * 4 + 115], 4)
+        collis_sens, teach_sens = rx_data[131:133]
+        gravity_direction = convert.bytes_to_fp32s(rx_data[133:3 * 4 + 133], 3)
+        mtbrake = [mtbrake & 0x01, mtbrake >> 1 & 0x01, mtbrake >> 2 & 0x01, mtbrake >> 3 & 0x01,
+                   mtbrake >> 4 & 0x01, mtbrake >> 5 & 0x01, mtbrake >> 6 & 0x01, mtbrake >> 7 & 0x01]
+        mtable = [mtable & 0x01, mtable >> 1 & 0x01, mtable >> 2 & 0x01, mtable >> 3 & 0x01,
+                  mtable >> 4 & 0x01, mtable >> 5 & 0x01, mtable >> 6 & 0x01, mtable >> 7 & 0x01]
+        self.parse_dict['mtbrake'] = mtbrake
+        self.parse_dict['mtable'] = mtable
+        self.parse_dict['tcp_offset'] = tcp_offset
+        self.parse_dict['tcp_load'] = tcp_load
+        self.parse_dict['error_code'] = error_code
+        self.parse_dict['warn_code'] = warn_code
+        self.parse_dict['collis_sens'] = collis_sens
+        self.parse_dict['teach_sens'] = teach_sens
+        self.parse_dict['gravity_direction'] = gravity_direction
+        ret.extend([mtbrake, mtable, error_code, warn_code, tcp_offset, tcp_load, collis_sens, teach_sens, gravity_direction])
+        return ret
+
+    def _parse_report_tcp_rich_data(self, rx_data):
+        ret = self._parse_report_tcp_normal_data(rx_data)
+        length = ret[0]
+        if length >= 151:
+            arm_type, arm_axis, arm_master_id, arm_slave_id, arm_motor_tid, arm_motor_fid = rx_data[145:151]
+            self.parse_dict['arm_type'] = arm_type
+            self.parse_dict['arm_axis'] = arm_axis
+            self.parse_dict['arm_master_id'] = arm_master_id
+            self.parse_dict['arm_slave_id'] = arm_slave_id
+            self.parse_dict['arm_motor_tid'] = arm_motor_tid
+            self.parse_dict['arm_motor_fid'] = arm_motor_fid
+            ret.extend([arm_type, arm_axis, arm_master_id, arm_slave_id, arm_motor_tid, arm_motor_fid])
+        if length >= 181:
+            version = str(rx_data[151:180], 'utf-8')
+            self.parse_dict['version'] = version
+            ret.append(version)
+        if length >= 201:
+            trs_msg = convert.bytes_to_fp32s(rx_data[181:201], 5)
+            tcp_jerk, min_tcp_acc, max_tcp_acc, min_tcp_speed, max_tcp_speed = trs_msg
+            self.parse_dict['tcp_jerk'] = tcp_jerk
+            self.parse_dict['min_tcp_acc'] = min_tcp_acc
+            self.parse_dict['max_tcp_acc'] = max_tcp_acc
+            self.parse_dict['min_tcp_speed'] = min_tcp_speed
+            self.parse_dict['max_tcp_speed'] = max_tcp_speed
+            ret.extend([tcp_jerk, min_tcp_acc, max_tcp_acc, min_tcp_speed, max_tcp_speed])
+        if length >= 221:
+            p2p_msg = convert.bytes_to_fp32s(rx_data[201:221], 5)
+            joint_jerk, min_joint_acc, max_joint_acc, min_joint_speed, max_joint_speed = p2p_msg
+            self.parse_dict['joint_jerk'] = joint_jerk
+            self.parse_dict['min_joint_acc'] = min_joint_acc
+            self.parse_dict['max_joint_acc'] = max_joint_acc
+            self.parse_dict['min_joint_speed'] = min_joint_speed
+            self.parse_dict['max_joint_speed'] = max_joint_speed
+            ret.extend([joint_jerk, min_joint_acc, max_joint_acc, min_joint_speed, max_joint_speed])
+        if length >= 229:
+            rot_msg = convert.bytes_to_fp32s(rx_data[221:229], 2)
+            rot_jerk, max_rot_acc = rot_msg
+            self.parse_dict['rot_jerk'] = rot_jerk
+            self.parse_dict['max_rot_acc'] = max_rot_acc
+            ret.extend([rot_jerk, max_rot_acc])
+        if length >= 245:
+            servo_code = [val for val in rx_data[229:245]]
+            self.parse_dict['servo_code'] = servo_code
+            ret.extend([servo_code[:-2], servo_code[-2:]])
+        if length >= 252:
+            temperatures = list(map(int, rx_data[245:252]))
+            self.parse_dict['temperatures'] = temperatures
+            ret.append(temperatures)
+        if length >= 284:
+            speeds = convert.bytes_to_fp32s(rx_data[252:8 * 4 + 252], 8)
+            self.parse_dict['speeds'] = speeds
+            ret.append(speeds)
+        if length >= 288:
+            count = convert.bytes_to_u32(rx_data[284:288])
+            self.parse_dict['count'] = count
+            ret.append(count)
+        if length >= 312:
+            world_offset = convert.bytes_to_fp32s(rx_data[288:6 * 4 + 288], 6)
+            self.parse_dict['world_offset'] = world_offset
+            ret.append(world_offset)
+        if length >= 314:
+            cgpio_reset_enable, tgpio_reset_enable = rx_data[312:314]
+            self.parse_dict['cgpio_reset_enable'] = cgpio_reset_enable
+            self.parse_dict['tgpio_reset_enable'] = tgpio_reset_enable
+            ret.extend([cgpio_reset_enable, tgpio_reset_enable])
+        if length >= 416:
+        # if length >= 340:
+            is_collision_check, collision_tool_type = rx_data[314:316]
+            collision_tool_params = convert.bytes_to_fp32s(rx_data[316:340], 6)
+            self.parse_dict['is_collision_check'] = is_collision_check
+            self.parse_dict['collision_tool_type'] = collision_tool_type
+            self.parse_dict['collision_tool_params'] = collision_tool_params
+            ret.extend([is_collision_check, collision_tool_type, collision_tool_params])
+        # if length >= 354:
+            voltages = convert.bytes_to_u16s(rx_data[340:354], 7)
+            voltages = list(map(lambda x: x / 100, voltages))
+            self.parse_dict['voltages'] = voltages
+            ret.append(voltages)
+        # if length >= 382:
+            currents = convert.bytes_to_fp32s(rx_data[354:382], 7)
+            self._currents = currents
+            self.parse_dict['currents'] = currents
+            ret.append(currents)
+        # if length >= 416:
+            cgpio_states = []
+            cgpio_states.extend(rx_data[382:384])
+            cgpio_states.extend(convert.bytes_to_u16s(rx_data[384:400], 8))
+            cgpio_states[6:10] = list(map(lambda x: x / 4095.0 * 10.0, cgpio_states[6:10]))
+            cgpio_states.append(list(map(int, rx_data[400:408])))
+            cgpio_states.append(list(map(int, rx_data[408:416])))
+            self.parse_dict['cgpio_states'] = cgpio_states
+            ret.append(cgpio_states)
+        return ret
+