Skip to content

Commit a471f9f

Browse files
SchoolMaterSchoolMater
committed
[feat]1.Added the interface for obtaining trajectory validity;2.Compatible with 1305 joint range;3.Added the interface for running gcode files;4.Adjust the interface information notes of "set_cgpio_digital" and "set_cgpio_digital";5.Modify the code translated by running Blockly controls in Blockly and PythonIDE;6.Added Get Holding Register Control
1 parent 32b9259 commit a471f9f

File tree

10 files changed

+176
-20
lines changed

10 files changed

+176
-20
lines changed

‎xarm/core/config/x_code.py

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -248,6 +248,16 @@
248248
'desc': '请通过控制器上的紧急停止按钮重启机械臂。如多次重启无效，请联系技术支持。',
249249
}
250250
},
251+
58: {
252+
'en': {
253+
'title': 'Torque Command Timeout',
254+
'desc': 'Please check the connection between the Control Box and the robot.',
255+
},
256+
'cn': {
257+
'title': '转矩指令超时',
258+
'desc': '请检查控制器与机械臂的连接。',
259+
}
260+
},
251261
'other': {
252262
'en': {
253263
'title': 'Joint Error',

‎xarm/core/config/x_config.py

Lines changed: 29 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,9 @@ class Type:
4141
XARM6_X11 = 11
4242
XARM6_X12 = 12
4343
XARM7_X13 = 13
44+
XARM5_X4_1305 = 51305
45+
XARM6_X4_1305 = 61305
46+
XARM7_X4_1305 = 71305
4447

4548
JOINT_LIMITS = {
4649
Axis.XARM5: {
@@ -51,6 +54,13 @@ class Type:
5154
(-1.692969, math.pi), # (-1.75, math.pi),
5255
(-2 * math.pi, 2 * math.pi)
5356
],
57+
Type.XARM5_X4_1305: [
58+
(-2 * math.pi, 2 * math.pi),
59+
(-2.042035, 2.024581),
60+
(-3.822271, 0.174532),
61+
(-1.692969, math.pi), # (-1.75, math.pi),
62+
(-2 * math.pi, 2 * math.pi)
63+
],
5464
},
5565
Axis.XARM6: {
5666
Type.XARM6_X4: [
@@ -61,6 +71,14 @@ class Type:
6171
(-1.692969, math.pi), # (-1.75, math.pi),
6272
(-2 * math.pi, 2 * math.pi)
6373
],
74+
Type.XARM6_X4_1305: [
75+
(-2 * math.pi, 2 * math.pi),
76+
(-2.042035, 2.024581),
77+
(-3.822271, 0.174532),
78+
(-2 * math.pi, 2 * math.pi),
79+
(-1.692969, math.pi), # (-1.75, math.pi),
80+
(-2 * math.pi, 2 * math.pi)
81+
],
6482
Type.XARM6_X8: [
6583
(-2 * math.pi, 2 * math.pi),
6684
(-2.059488, 2.094395), # (-2.18, 2.18),
@@ -114,6 +132,15 @@ class Type:
114132
(-1.692969, math.pi), # (-1.75, math.pi),
115133
(-2 * math.pi, 2 * math.pi)
116134
],
135+
Type.XARM7_X4_1305: [
136+
(-2 * math.pi, 2 * math.pi),
137+
(-2.042035, 2.024581),
138+
(-2 * math.pi, 2 * math.pi),
139+
(-0.10472, 3.92699),
140+
(-2 * math.pi, 2 * math.pi),
141+
(-1.692969, math.pi), # (-1.75, math.pi),
142+
(-2 * math.pi, 2 * math.pi)
143+
],
117144
Type.XARM7_X13: [
118145
(-2 * math.pi, 2 * math.pi),
119146
(-2.094395, 2.059488), # (-2.18, 2.18),
@@ -388,7 +415,8 @@ class UxbusReg:
388415
IMPEDANCE_CTRL_MBK = 210
389416
IMPEDANCE_CTRL_CONFIG = 211
390417
FTSENSOR_GET_CONFIG = 212
391-
418+
419+
GET_TRAJ_SPEEDING = 230
392420
GET_MAX_JOINT_VELOCITY = 231
393421
SET_COMMON_PARAM = 232
394422
GET_COMMON_PARAM = 233

‎xarm/core/wrapper/uxbus_cmd.py

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1432,3 +1432,14 @@ def get_common_info(self, param_type):
14321432
else:
14331433
data[0] = XCONF.UxbusState.ERR_PARAM
14341434
return data
1435+
1436+
def get_traj_speeding(self, rate):
1437+
txdata = bytes([rate])
1438+
ret = self.getset_nu8(XCONF.UxbusReg.GET_TRAJ_SPEEDING, txdata, 1, -1)
1439+
data = [0] * 4
1440+
data[0] = ret[0]
1441+
if ret[0] != XCONF.UxbusState.ERR_NOTTCP:
1442+
data[1] = convert.bytes_to_int32(ret[1:5])
1443+
data[2] = convert.bytes_to_int32(ret[5:9])
1444+
data[3] = round(math.degrees(convert.bytes_to_fp32(ret[9:])),3)
1445+
return data

‎xarm/tools/blockly/_blockly_base.py

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -217,6 +217,11 @@ def _get_condition_expression(self, value_block, arg_map=None):
217217
format(direction_li.index(direction), direction_li.index(direction))
218218
elif block.attrib['type'] == 'get_counter':
219219
return 'self._arm.count'
220+
elif block.attrib['type'] == 'get_single_holding_register':
221+
fields = self._get_nodes('field', root=block)
222+
addr = int(fields[0].text.replace(' ', '').replace('0x', '').replace(',', '').replace('\xa0', ''), 16)
223+
return "list(map(lambda x: hex(x).split('0x')[1].upper().zfill(4)[:2] + ' ' + hex(x).split('0x')[1]." \
224+
"upper().zfill(4)[2:], self._arm.read_holding_registers({}, 1)[1]))[0]".format(str(addr))
220225

221226

222227
def __get_logic_compare(self, block, arg_map=None):

‎xarm/tools/blockly/_blockly_handler.py

Lines changed: 16 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -398,8 +398,11 @@ def _handle_tool_console_with_variable(self, block, indent=0, arg_map=None):
398398

399399
def _handle_wait(self, block, indent=0, arg_map=None):
400400
value = self._get_node('value', root=block)
401-
value = self._get_nodes('field', root=value, descendant=True)[0].text
402-
self._append_main_code('time.sleep({})'.format(value), indent + 2)
401+
value = float(self._get_nodes('field', root=value, descendant=True)[0].text)
402+
self._append_main_code('for i in range({}):'.format(int(value / 0.1)), indent + 2)
403+
self._append_main_code(' time.sleep(0.1)', indent + 2)
404+
self._append_main_code(' if not self.is_alive:', indent=indent + 2)
405+
self._append_main_code(' return', indent=indent + 2)
403406

404407
def _handle_gpio_get_digital(self, block, indent=0, arg_map=None):
405408
io = self._get_node('field', block).text
@@ -1211,5 +1214,15 @@ def _handle_studio_run_gcode(self, block, indent=0, arg_map=None):
12111214
projectName = fields[0].text
12121215
fileName = fields[1].text
12131216
times = fields[2].text
1214-
self._append_main_code('start_run_gcode(projectName="{}", fileName="{}", times={})'.format(projectName, fileName, times), indent + 2)
1217+
if self._is_exec:
1218+
self._append_main_code('start_run_gcode(projectName="{}", fileName="{}", times={})'.format(projectName, fileName, times), indent + 2)
1219+
else:
1220+
self._append_main_code("for i in range({}):".format(times), indent + 2)
1221+
self._append_main_code(' self._arm.run_gcode_app(path="{}")'.format(projectName+fileName), indent + 2)
1222+
1223+
def _handle_write_single_holding_register(self, block, indent=0, arg_map=None):
1224+
fields = self._get_nodes('field', root=block)
1225+
addr = int(fields[0].text.replace(' ', '').replace('0x','').replace(',','').replace('\xa0', ''), 16)
1226+
value = int(fields[1].text.replace(' ', '').replace('0x','').replace(',','').replace('\xa0', ''), 16)
1227+
self._append_main_code('self._arm.write_single_holding_register({}, {})'.format(addr, value), indent + 2)
12151228

‎xarm/tools/blockly/_blockly_tool.py

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -267,8 +267,10 @@ def __define_listen_gpio_thread_func(self):
267267

268268
self._append_main_init_code(' def _listen_gpio_thread(self):')
269269
if self._listen_tgpio_digital or len(self._tgpio_digital_callbacks):
270+
self._append_main_init_code(' _, values2 = self._arm.get_tgpio_digital(2)')
270271
self._append_main_init_code(' _, values = self._arm.get_tgpio_digital()')
271-
self._append_main_init_code(' tgpio_digitals = values if _ == 0 else [0] * 2')
272+
self._append_main_init_code(' values.insert(2, values2)')
273+
self._append_main_init_code(' tgpio_digitals = values if _ == 0 else [0] * 5')
272274
if self._listen_tgpio_analog or len(self._tgpio_analog_callbacks):
273275
self._append_main_init_code(' _, values = self._arm.get_tgpio_analog()')
274276
self._append_main_init_code(' tgpio_analogs = values if _ == 0 else [0] * 2')
@@ -281,10 +283,12 @@ def __define_listen_gpio_thread_func(self):
281283

282284
self._append_main_init_code(' while self.alive:')
283285
if self._listen_tgpio_digital or len(self._tgpio_digital_callbacks):
286+
self._append_main_init_code(' _, values2 = self._arm.get_tgpio_digital(2)')
284287
self._append_main_init_code(' _, values = self._arm.get_tgpio_digital()')
288+
self._append_main_init_code(' values.insert(2, values2)')
285289
self._append_main_init_code(' if _ == 0 and tgpio_digitals is not None:')
286290
self._append_main_init_code(' for item in self._tgpio_digital_callbacks:')
287-
self._append_main_init_code(' for io in range(2):')
291+
self._append_main_init_code(' for io in range(5):')
288292
self._append_main_init_code(' if item[\'io\'] == io and eval(\'{} {} {}\'.format(values[io], item[\'op\'], item[\'trigger\'])) and not eval(\'{} {} {}\'.format(tgpio_digitals[io], item[\'op\'], item[\'trigger\'])):')
289293
self._append_main_init_code(' self._callback_que.put(item[\'callback\'])')
290294
self._append_main_init_code(' tgpio_digitals = values if _ == 0 else tgpio_digitals')
@@ -361,7 +365,7 @@ def __define_run_blockly_func(self):
361365
if self._is_run_blockly and not self._is_exec:
362366
self._append_main_init_code(' def _start_run_blockly(self, fileName, times):')
363367
self._append_main_init_code(' for i in range(times):')
364-
self._append_main_init_code(' self._arm.run_blockly_app(fileName, init=False, axis_type=[self._arm.axis, self._arm.device_type])\n')
368+
self._append_main_init_code(' code = self._arm.run_blockly_app(fileName, init=False, is_exec=True, axis_type=[self._arm.axis, self._arm.device_type])\n')
365369

366370
def __define_robot_init_func(self, init=True, wait_seconds=1, mode=0, state=0, error_exit=True, stop_exit=True):
367371
# Define XArm Init Function:
@@ -462,9 +466,11 @@ def _init_main_codes(self, arm=None):
462466
self._append_main_code('RobotMain.pprint(\'xArm-Python-SDK Version:{}\'.format(version.__version__))', indent=indent)
463467
if arm is None:
464468
self._append_main_code('arm = XArmAPI(sys.argv[1], baud_checkset=False)', indent=indent)
469+
self._append_main_code('time.sleep(0.5)', indent=indent)
465470
elif isinstance(arm, str):
466471
self._append_main_code('arm = XArmAPI(\'{}\', baud_checkset=False)'.format(arm), indent=indent)
467-
472+
self._append_main_code('time.sleep(0.5)', indent=indent)
473+
468474
self._append_main_code('robot_main = RobotMain(arm)', indent=indent)
469475
self._append_main_code('robot_main.run()', indent=indent)
470476
self._append_main_code('', indent=-1)

‎xarm/version.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1 +1 @@
1-
__version__ = '1.14.4'
1+
__version__ = '1.14.5'

‎xarm/wrapper/xarm_api.py

Lines changed: 27 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1947,7 +1947,7 @@ def get_cgpio_digital(self, ionum=None):
19471947
"""
19481948
Get the digital value of the specified Controller GPIO
19491949
1950-
:param ionum: 0~15 or None(both 0~15), default is None
1950+
:param ionum: 0~7(CI0~CI7), 8~15(DI0~DI7) or None(both 0~15), default is None
19511951
:return: tuple((code, value or value list)), only when code is 0, the returned result is correct.
19521952
code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
19531953
"""
@@ -1966,7 +1966,7 @@ def set_cgpio_digital(self, ionum, value, delay_sec=None, sync=True):
19661966
"""
19671967
Set the digital value of the specified Controller GPIO
19681968
1969-
:param ionum: 0~15
1969+
:param ionum: 0~7(CO0~CO7), 8~15(DO0~DO7)
19701970
:param value: value
19711971
:param delay_sec: delay effective time from the current start, in seconds, default is None(effective immediately)
19721972
:param sync: whether to execute in the motion queue, set to False to execute immediately(default is True)
@@ -1993,7 +1993,7 @@ def set_cgpio_analog(self, ionum, value, sync=True):
19931993
def set_cgpio_digital_input_function(self, ionum, fun):
19941994
"""
19951995
Set the digital input functional mode of the Controller GPIO
1996-
:param ionum: 0~15
1996+
:param ionum: 0~7(CI0~CI7), 8~15(DI0~DI7)
19971997
:param fun: functional mode
19981998
0: general input
19991999
1: external emergency stop
@@ -2010,7 +2010,7 @@ def set_cgpio_digital_input_function(self, ionum, fun):
20102010
def set_cgpio_digital_output_function(self, ionum, fun):
20112011
"""
20122012
Set the digital output functional mode of the specified Controller GPIO
2013-
:param ionum: 0~15
2013+
:param ionum: 0~7(CO0~CO7), 8~15(DO0~DO7)
20142014
:param fun: functionnal mode
20152015
0: general output
20162016
1: emergency stop
@@ -4198,3 +4198,26 @@ def get_c38_error_info(self, is_radian=None):
41984198
return 0, err_info
41994199
# ret[1][1] = ret[1][1] if is_rad else math.degrees(ret[1][1])
42004200
return ret
4201+
4202+
def run_gcode_app(self, path, **kwargs):
4203+
"""
4204+
Run gcode project file by xArmStudio software
4205+
:param path: gcode file path
4206+
4207+
:return: code, only when code is 0, the returned result is correct.
4208+
"""
4209+
return self._arm.run_gcode_app(path, **kwargs)
4210+
4211+
def get_traj_speeding(self, rate):
4212+
"""
4213+
Obtain the joint and velocity values of joint overspeed during trajectory recording
4214+
:param rate: speed rate, It can only be 1/2/4
4215+
4216+
:return: tuple((code, speed_info)), only when code is 0, the returned result is correct.
4217+
code: See the [API Code Documentation](./xarm_api_code.md#api-code) for details.
4218+
speed_info: [result_code, servo_id, servo_speed]
4219+
result_code: 0：Pass, -1: Fail, >0: abnormal(1:Trajectory not loaded or incorrect status;2:The input magnification is incorrect)
4220+
servo_id: Effective only when result_code is -1
4221+
servo_speed: Effective only when result_code is -1
4222+
"""
4223+
return self._arm.get_traj_speeding(rate)

‎xarm/x3/base.py

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -2328,7 +2328,7 @@ def _wait_feedback(self, timeout=None, trans_id=-1, ignore_log=False):
23282328
time.sleep(0.05)
23292329
return APIState.WAIT_FINISH_TIMEOUT, -1
23302330

2331-
def wait_move(self, timeout=None, trans_id=-1):
2331+
def wait_move(self, timeout=None, trans_id=-1, set_cnt=2):
23322332
if self._support_feedback and trans_id > 0:
23332333
return self._wait_feedback(timeout, trans_id)[0]
23342334
if timeout is not None:
@@ -2338,7 +2338,7 @@ def wait_move(self, timeout=None, trans_id=-1):
23382338
_, state = self.get_state()
23392339
cnt = 0
23402340
state5_cnt = 0
2341-
max_cnt = 2 if _ == 0 and state == 1 else 10
2341+
max_cnt = set_cnt if _ == 0 and state == 1 else 10
23422342
while timeout is None or time.monotonic() < expired:
23432343
if not self.connected:
23442344
self.log_api_info('wait_move, xarm is disconnect', code=APIState.NOT_CONNECTED)
@@ -2367,7 +2367,7 @@ def wait_move(self, timeout=None, trans_id=-1):
23672367
continue
23682368
if state == 0 or state == 1:
23692369
cnt = 0
2370-
max_cnt = 2
2370+
max_cnt = set_cnt
23712371
time.sleep(0.05)
23722372
continue
23732373
else:
@@ -2596,4 +2596,9 @@ def get_common_param(self, param_type, return_val=True):
25962596
def get_common_info(self, param_type, return_val=True):
25972597
ret = self.arm_cmd.get_common_info(param_type)
25982598
ret[0] = self._check_code(ret[0])
2599-
return ret[0], ret[1] if return_val else ret[1:]
2599+
return ret[0], ret[1] if return_val else ret[1:]
2600+
2601+
def get_traj_speeding(self, rate):
2602+
ret = self.arm_cmd.get_traj_speeding(rate)
2603+
ret[0] = self._check_code(ret[0])
2604+
return ret[0], ret[1:]

‎xarm/x3/xarm.py

Lines changed: 58 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,8 @@ def _is_out_of_tcp_range(self, value, i):
6666
def _is_out_of_joint_range(self, angle, i):
6767
if not self._check_joint_limit or self._stream_type != 'socket' or not self._enable_report or angle == math.inf:
6868
return False
69-
joint_limit = XCONF.Robot.JOINT_LIMITS.get(self.axis).get(self.device_type, [])
69+
device_type = int('{}1305'.format(self.axis)) if self.sn and int(self.sn[2:6]) >= 1305 else self.device_type
70+
joint_limit = XCONF.Robot.JOINT_LIMITS.get(self.axis).get(device_type, [])
7071
if i < len(joint_limit):
7172
angle_range = joint_limit[i]
7273
if angle < angle_range[0] - math.radians(0.1) or angle > angle_range[1] + math.radians(0.1):
@@ -766,7 +767,8 @@ def set_reduced_joint_range(self, joint_range, is_radian=None):
766767
# limits = list(map(lambda x: round(math.radians(x), 3), limits))
767768

768769
for i in range(self.axis):
769-
joint_limit = XCONF.Robot.JOINT_LIMITS.get(self.axis).get(self.device_type, [])
770+
device_type = int('{}1305'.format(self.axis)) if self.sn and int(self.sn[2:6]) >= 1305 else self.device_type
771+
joint_limit = XCONF.Robot.JOINT_LIMITS.get(self.axis).get(device_type, [])
770772
if i < len(joint_limit):
771773
angle_range = joint_limit[i]
772774
# angle_range = list(map(lambda x: round(x, 3), joint_limit[i]))
@@ -1860,4 +1862,57 @@ def send_hex_cmd(self, datas, timeout=10):
18601862

18611863
@xarm_is_connected(_type='get')
18621864
def get_trans_id(self):
1863-
return self.arm_cmd.get_trans_id()
1865+
return self.arm_cmd.get_trans_id()
1866+
1867+
@xarm_is_connected(_type='set')
1868+
def run_gcode_app(self, path=None, **kwargs):
1869+
sock = None
1870+
try:
1871+
if not os.path.exists(path):
1872+
dir_name = 'lite6' if self.axis == 6 and self.device_type == 9 else '850' if self.axis == 6 and self.device_type == 12 else 'xarm7T' if self.axis == 7 and self.device_type == 13 else 'xarm{}'.format(
1873+
self.axis)
1874+
path = os.path.join('/home/uf' if sys.platform.startswith('linux') else os.path.expanduser('~'), '.UFACTORY', 'projects', 'test', dir_name, 'gcode', path)
1875+
if not os.path.exists(path):
1876+
raise FileNotFoundError('{} is not found'.format(path))
1877+
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
1878+
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
1879+
sock.setblocking(True)
1880+
sock.settimeout(10)
1881+
sock.connect((self._port, 504))
1882+
with open(path, 'r') as f:
1883+
datas = f.read()
1884+
lines = datas.split('\n')
1885+
is_continue = False
1886+
code = APIState.NORMAL
1887+
for line in lines:
1888+
line = line.strip()
1889+
if line.startswith(';('):
1890+
is_continue = True
1891+
if not line or is_continue:
1892+
if line.endswith(')'):
1893+
is_continue = False
1894+
continue
1895+
if line.startswith(';'):
1896+
continue
1897+
GCODE_PATTERN = r'([A-Z])([-+]?[0-9.]+)'
1898+
CLEAN_PATTERN = r'\s+|\(.*?\)|;.*'
1899+
data = re.sub(CLEAN_PATTERN, '', line.strip().upper())
1900+
if not data:
1901+
return -14
1902+
if data[0] == '%':
1903+
return -15
1904+
if not re.findall(GCODE_PATTERN, data):
1905+
return -16
1906+
1907+
sock.send(line.strip().encode('utf-8', 'replace') + b'\n')
1908+
err, status, code, buffer1, buffer2 = sock.recv(5)
1909+
if err != 0 or code != 0:
1910+
return err if err != 0 else code
1911+
self.wait_move(set_cnt=5)
1912+
return code
1913+
except Exception as e:
1914+
code = -13
1915+
return code
1916+
finally:
1917+
if sock:
1918+
sock.close()

0 commit comments

Comments
 (0)