debug

Author: hganchev

.vscode/launch.json

@@ -10,7 +10,8 @@
             "request": "launch",
             "program": "${file}",
             "console": "integratedTerminal",
-            "justMyCode": true
+            "justMyCode": false,
+            "purpose": ["debug-test"]
         }
     ]
 }

build/lib/pyUR/dashboard/__init__.py

@@ -0,0 +1 @@
+# __all__=["dashboard", "dashboard_commands", "dashboard_statuses"]

build/lib/pyUR/realtime/__init__.py

@@ -0,0 +1 @@
+# __all__=["realtime", "realtime_commands", "realtime_statuses"]

dist/pyURControl-0.0.1-py3-none-any.whl

@@ -43,12 +43,16 @@ def PickAndPlace():
     ur_control.move_joint_with_pose(list(map(add,p2_pick,p2_offset_approach)), a=1.4, v=1.05, t=0, r=0) # move to p2_pick with offset for approach
     ur_control.move_linear_pose(p2_pick, a=1.4, v=1.05, t=0, r=0) # move to p2_pick with linear motion
     # pick the piece - close gripper
+    ur_control.set_digital_output(0, True)
+    ur_control.set_digital_output(1, False)
     ur_control.move_joint_with_pose(list(map(add,p2_pick,p2_offset_leave)), a=1.4, v=1.05, t=0, r=0) # move to p2_pick with offset to leave
 
     # Place
     ur_control.move_joint_with_pose(list(map(add,p3_place,p3_offset_approach)), a=1.4, v=1.05, t=0, r=0) # move to p3_place with offset for approach
     ur_control.move_linear_pose(p3_place, a=1.4, v=1.05, t=0, r=0) # move to p3_place with linear motion
     # place the piece - open gripper
+    ur_control.set_digital_output(0, False)
+    ur_control.set_digital_output(1, True)
     ur_control.move_joint_with_pose(list(map(add,p3_place,p3_offset_leave)), a=1.4, v=1.05, t=0, r=0) # move to p3_place with offset to leave
 
 

dist/pyURControl-0.0.1.tar.gz

@@ -91,3 +91,18 @@ def pose_translate(pose1:list=[0, 0, 0, 0, 0, 0], pose2:list=[0, 0, 0, 0, 0, 0])
 '''
 def pose_inverse(pose:list=[0, 0, 0, 0, 0, 0]) -> str:
     return "pose_inv(p" + str(pose) + ")"
+
+'''
+calculate sub pose of two poses
+:param pose1: pose position - [x, y, z, rx, ry, rz]
+:param pose2: pose position - [x, y, z, rx, ry, rz]
+'''
+def pose_sub(pose1:list=[0, 0, 0, 0, 0, 0], pose2:list=[0, 0, 0, 0, 0, 0]) -> str:
+    return "pose_sub(p" + str(pose1) + ", p" + str(pose2) + ")"
+
+'''
+set digital output
+:param output: output number - 0-7
+'''
+def set_digital_output(output:int=0, value:bool=False) -> str:
+    return "set_standard_digital_out(" + str(output) + ", " + str(value) +")" + '\n'

main.py

@@ -6,12 +6,14 @@
 _program_state : float = None
 _robot_mode : float = None
 
-_q_act_x:float = None
-_q_act_y:float = None
-_q_act_z:float = None
-_q_act_rx: float = None
-_q_act_ry: float = None
-_q_act_rz:float = None
+_q_act_base:float = None
+_q_act_shoulder:float = None
+_q_act_elbow:float = None
+_q_act_wrist1: float = None
+_q_act_wrist2: float = None
+_q_act_wrist3:float = None
+
+_digital_outputs: list = None
 
 '''
 Class for unpacking status from robot
@@ -24,30 +26,50 @@ def __init__():
 :param status: status to unpack
 '''
 def unpack(data:bytes):
-    global _program_state, _robot_mode, _q_act_x,\
-    _q_act_y,_q_act_x,_q_act_rx,_q_act_ry,_q_act_rz
+    global _program_state, _robot_mode, _q_act_base,\
+    _q_act_shoulder,_q_act_elbow,_q_act_wrist1,_q_act_wrist2,_q_act_wrist3,\
+    _digital_outputs
 
     packlen = (struct.unpack('!i', data[0:4]))[0]
     if packlen == 1220: #for version 5.10 package
         data = data[4:]
+        # program state
         _program_state = (struct.unpack('!d', data[131*8:132*8]))[0]
+
+        # robot mode
+        _robot_mode = (struct.unpack('!d', data[94*8:95*8]))[0]
+
+        # digital outputs
+        _digital_outputs = _double_to_8bit_list((struct.unpack('!d', data[130*8:131*8]))[0])
+        
+        # actual joint positions
+        _q_act_base     = (struct.unpack('!d', data[31*8:32*8]))[0]
+        _q_act_shoulder = (struct.unpack('!d', data[32*8:33*8]))[0]
+        _q_act_elbow    = (struct.unpack('!d', data[33*8:34*8]))[0]
+        _q_act_wrist1   = (struct.unpack('!d', data[34*8:35*8]))[0]
+        _q_act_wrist2   = (struct.unpack('!d', data[35*8:36*8]))[0]
+        _q_act_wrist3   = (struct.unpack('!d', data[36*8:37*8]))[0]
+
     elif packlen == 1140: #for version 5.9 package
         data = data[4:]
-        
-        # actual positions
-        _q_act_x = (struct.unpack('!d', data[31*8:32*8]))[0]
-        _q_act_y = (struct.unpack('!d', data[32*8:33*8]))[0]
-        _q_act_z = (struct.unpack('!d', data[33*8:34*8]))[0]
-        _q_act_rx = (struct.unpack('!d', data[34*8:35*8]))[0]
-        _q_act_ry = (struct.unpack('!d', data[35*8:36*8]))[0]
-        _q_act_rz = (struct.unpack('!d', data[36*8:37*8]))[0]
-        
-        # program state
+
+        # program state - 0 -? 1 - normal 2 - running
         _program_state = (struct.unpack('!d', data[131*8:132*8]))[0]
 
         # robot mode
         _robot_mode = (struct.unpack('!d', data[94*8:95*8]))[0]
 
+        # digital outputs
+        _digital_outputs = _double_to_8bit_list(struct.unpack('!d', data[130*8:131*8])[0])
+        
+         # actual joint positions
+        _q_act_base     = (struct.unpack('!d', data[31*8:32*8]))[0]
+        _q_act_shoulder = (struct.unpack('!d', data[32*8:33*8]))[0]
+        _q_act_elbow    = (struct.unpack('!d', data[33*8:34*8]))[0]
+        _q_act_wrist1   = (struct.unpack('!d', data[34*8:35*8]))[0]
+        _q_act_wrist2   = (struct.unpack('!d', data[35*8:36*8]))[0]
+        _q_act_wrist3   = (struct.unpack('!d', data[36*8:37*8]))[0]
+
 def get_program_state():
     global _program_state
     return _program_state
@@ -56,7 +78,20 @@ def get_robot_mode():
     global _robot_mode
     return _robot_mode
 
-def get_q_act_pose():
-    global _q_act_x, _q_act_y, _q_act_z,\
-    _q_act_rx, _q_act_ry, _q_act_rz
-    return list[_q_act_x, _q_act_y, _q_act_z, _q_act_rx, _q_act_ry, _q_act_rz]
+def get_q_act_joint_positions():
+    global _q_act_base, _q_act_shoulder, _q_act_elbow,\
+    _q_act_wrist1, _q_act_wrist2, _q_act_wrist3
+    return list[_q_act_base, _q_act_shoulder, _q_act_elbow, _q_act_wrist1, _q_act_wrist2, _q_act_wrist3]
+
+def get_digital_outputs():
+    global _digital_outputs
+    return _digital_outputs
+
+def _double_to_8bit_list(d):
+    # Convert the integer to its binary representation as a string
+    binary_str = bin(int(d))[2:].zfill(8)
+
+    #Convert the binary string to a list of 0 and 1 values
+    binary_array = [bool(int(bit)) for bit in binary_str]
+
+    return binary_array[::-1]

src/pyUR/realtime/realtime_commands.py

@@ -16,7 +16,7 @@ License-File: LICENSE.md
 
 # python-ur-control
 This is package for basic control of Universal Robots using inbuild intefaces
-like Dashboard, Realtime, RTDE.
+like Dashboard, Realtime, RTDE. link to UR documentation: https://www.universal-robots.com/articles/ur/interface-communication/overview-of-client-interfaces/
 
 >:warning: **The package is in developement. It can have some bugs or missed functionalities.
 I am open for project ideas or cooperation to make it better. **

src/pyUR/realtime/realtime_statuses.py

@@ -109,7 +109,7 @@ def _robot_done_moving() -> bool:
 :1 - normal
 :2 - running
 '''
-def _get_program_state():
+def _get_program_state() -> float:
     # Receive responce
     responce = realtime.receive_status()
 
@@ -118,4 +118,39 @@ def _get_program_state():
 
     # Get program state
     prgstate = realtime_statuses.get_program_state()
-    return prgstate
+    return prgstate
+
+'''
+set digital output
+:param output: output number 0-7
+:param value: output value
+'''
+def set_digital_output(output: int=0, value: bool=False):
+    realtime.send(realtime_commands.set_digital_output(output, value))
+    _wait_digital_output_is_set(output, value)
+
+'''
+get digital outputs
+'''
+def get_digital_outputs() -> list:
+    # Receive responce
+    responce = realtime.receive_status()
+
+    # Unpack responce
+    realtime_statuses.unpack(responce)
+
+    # Get program state
+    digital_outputs = realtime_statuses.get_digital_outputs()
+    return digital_outputs
+
+'''
+wait for digital output to be set
+:param output: output number 0-7
+:param value: output value
+'''
+def _wait_digital_output_is_set(output: int=0, value: bool=False):
+    while True:
+        digital_outputs = get_digital_outputs()
+        if digital_outputs[output] == value:
+            break
+        sleep(0.001)