set tcp to have sleep of 300 ms because of no status for feedback to follow

Author: hganchev

build/lib/pyUR/realtime/realtime_statuses.py

@@ -5,6 +5,7 @@
 '''
 _program_state : float = None
 _robot_mode : float = None
+_digital_outputs: list = None
 
 _q_act_base:float = None
 _q_act_shoulder:float = None
@@ -13,7 +14,13 @@
 _q_act_wrist2: float = None
 _q_act_wrist3:float = None
 
-_digital_outputs: list = None
+_tool_act_x:float = None
+_tool_act_y:float = None
+_tool_act_z:float = None
+_tool_act_rx:float = None
+_tool_act_ry:float = None
+_tool_act_rz:float = None
+
 
 '''
 Class for unpacking status from robot
@@ -26,9 +33,10 @@ def __init__():
 :param status: status to unpack
 '''
 def unpack(data:bytes):
-    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
+    global _program_state, _robot_mode,_digital_outputs,\
+    _q_act_base,_q_act_shoulder,_q_act_elbow,_q_act_wrist1,_q_act_wrist2,_q_act_wrist3,\
+    _tool_act_x,_tool_act_y,_tool_act_z,_tool_act_rx,_tool_act_ry,_tool_act_rz
+    
 
     packlen = (struct.unpack('!i', data[0:4]))[0]
     if packlen == 1220: #for version 5.10 package
@@ -70,6 +78,15 @@ def unpack(data:bytes):
         _q_act_wrist2   = (struct.unpack('!d', data[35*8:36*8]))[0]
         _q_act_wrist3   = (struct.unpack('!d', data[36*8:37*8]))[0]
 
+        # actual robot position coordinates - x,y,z,rx,ry,rz
+        _tool_act_x = (struct.unpack('!d', data[55*8:56*8]))[0]
+        _tool_act_y = (struct.unpack('!d', data[56*8:57*8]))[0]
+        _tool_act_z = (struct.unpack('!d', data[57*8:58*8]))[0]
+        _tool_act_rx = (struct.unpack('!d', data[58*8:59*8]))[0]
+        _tool_act_ry = (struct.unpack('!d', data[59*8:60*8]))[0]
+        _tool_act_rz = (struct.unpack('!d', data[60*8:61*8]))[0]
+
+
 def get_program_state():
     global _program_state
     return _program_state
@@ -79,14 +96,17 @@ def get_robot_mode():
     return _robot_mode
 
 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
+    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 get_robot_act_pose():
+    global _tool_act_x, _tool_act_y, _tool_act_z, _tool_act_rx, _tool_act_ry, _tool_act_rz
+    return list[_tool_act_x, _tool_act_y, _tool_act_z, _tool_act_rx, _tool_act_ry, _tool_act_rz]
+
 def _double_to_8bit_list(d):
     # Convert the integer to its binary representation as a string
     binary_str = bin(int(d))[2:].zfill(8)

build/lib/pyURControl/ur_control.py

@@ -41,9 +41,11 @@ def break_release():
 '''
 set tcp
 :param tcp_offset: tcp offset
+:param delay: delay for command to be executed 0.3s by default
 '''
-def set_tcp(tcp_offset: list=[0, 0, 0, 0, 0, 0]):
+def set_tcp(tcp_offset: list=[0, 0, 0, 0, 0, 0], delay: float=0.3):
     realtime.send(realtime_commands.set_tcp(tcp_offset))
+    sleep(delay) # set sleep for command to be executed
 
 '''
 set payload
@@ -79,11 +81,21 @@ def move_linear_pose(pose: list=[0, 0, 0, 0, 0, 0], a: float=1.4, v: float=1.05,
     _wait_robot_move_done()
 
 '''
-wait for robot to finish moving
+get program state
+:1 - normal
+:2 - running
 '''
-def _wait_robot_move_done():
-    _robot_started_moving()
-    _robot_done_moving()
+def _get_program_state() -> float:
+    # Receive responce
+    responce = realtime.receive_status()
+
+    # Unpack responce
+    realtime_statuses.unpack(responce)
+
+    # Get program state
+    prgstate = realtime_statuses.get_program_state()
+    return prgstate
+
 '''
 robot started moving
 '''
@@ -105,26 +117,43 @@ def _robot_done_moving() -> bool:
         sleep(0.001)
 
 '''
-get program state
-:1 - normal
-:2 - running
+wait for robot to finish moving
+'''
+def _wait_robot_move_done():
+    _robot_started_moving()
+    _robot_done_moving()
+
+'''
+set digital output
+:param output: output number 0-7
+:param value: output value
 '''
-def _get_program_state():
+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
-    prgstate = realtime_statuses.get_program_state()
-    return prgstate
+    digital_outputs = realtime_statuses.get_digital_outputs()
+    return digital_outputs
 
 '''
-set digital output
+wait for digital output to be set
 :param output: output number 0-7
 :param value: output value
 '''
-def set_digital_output(output: int=0, value: bool=False):
-    print(realtime_commands.set_digital_output(output, value))
-    realtime.send(realtime_commands.set_digital_output(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)

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

@@ -21,8 +21,7 @@ def program():
 
 def PickAndPlace():
     # set tcp
-    ur_control.set_tcp([0, 0, 0.1, 0, 0, 0])
-    sleep(0.3)
+    ur_control.set_tcp([0, 0, 0.2, 0, 0, 0])
 
     # set payload if there is any
     ur_control.set_payload(0, [0, 0, 0], [0,0,0,0,0,0])
@@ -45,6 +44,7 @@ def PickAndPlace():
     # 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
@@ -53,6 +53,7 @@ def PickAndPlace():
     # 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

@@ -5,6 +5,7 @@
 '''
 _program_state : float = None
 _robot_mode : float = None
+_digital_outputs: list = None
 
 _q_act_base:float = None
 _q_act_shoulder:float = None
@@ -13,7 +14,13 @@
 _q_act_wrist2: float = None
 _q_act_wrist3:float = None
 
-_digital_outputs: list = None
+_tool_act_x:float = None
+_tool_act_y:float = None
+_tool_act_z:float = None
+_tool_act_rx:float = None
+_tool_act_ry:float = None
+_tool_act_rz:float = None
+
 
 '''
 Class for unpacking status from robot
@@ -26,9 +33,10 @@ def __init__():
 :param status: status to unpack
 '''
 def unpack(data:bytes):
-    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
+    global _program_state, _robot_mode,_digital_outputs,\
+    _q_act_base,_q_act_shoulder,_q_act_elbow,_q_act_wrist1,_q_act_wrist2,_q_act_wrist3,\
+    _tool_act_x,_tool_act_y,_tool_act_z,_tool_act_rx,_tool_act_ry,_tool_act_rz
+    
 
     packlen = (struct.unpack('!i', data[0:4]))[0]
     if packlen == 1220: #for version 5.10 package
@@ -70,6 +78,15 @@ def unpack(data:bytes):
         _q_act_wrist2   = (struct.unpack('!d', data[35*8:36*8]))[0]
         _q_act_wrist3   = (struct.unpack('!d', data[36*8:37*8]))[0]
 
+        # actual robot position coordinates - x,y,z,rx,ry,rz
+        _tool_act_x = (struct.unpack('!d', data[55*8:56*8]))[0]
+        _tool_act_y = (struct.unpack('!d', data[56*8:57*8]))[0]
+        _tool_act_z = (struct.unpack('!d', data[57*8:58*8]))[0]
+        _tool_act_rx = (struct.unpack('!d', data[58*8:59*8]))[0]
+        _tool_act_ry = (struct.unpack('!d', data[59*8:60*8]))[0]
+        _tool_act_rz = (struct.unpack('!d', data[60*8:61*8]))[0]
+
+
 def get_program_state():
     global _program_state
     return _program_state
@@ -79,14 +96,17 @@ def get_robot_mode():
     return _robot_mode
 
 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
+    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 get_robot_act_pose():
+    global _tool_act_x, _tool_act_y, _tool_act_z, _tool_act_rx, _tool_act_ry, _tool_act_rz
+    return list[_tool_act_x, _tool_act_y, _tool_act_z, _tool_act_rx, _tool_act_ry, _tool_act_rz]
+
 def _double_to_8bit_list(d):
     # Convert the integer to its binary representation as a string
     binary_str = bin(int(d))[2:].zfill(8)

main.py

@@ -41,9 +41,11 @@ def break_release():
 '''
 set tcp
 :param tcp_offset: tcp offset
+:param delay: delay for command to be executed 0.3s by default
 '''
-def set_tcp(tcp_offset: list=[0, 0, 0, 0, 0, 0]):
+def set_tcp(tcp_offset: list=[0, 0, 0, 0, 0, 0], delay: float=0.3):
     realtime.send(realtime_commands.set_tcp(tcp_offset))
+    sleep(delay) # set sleep for command to be executed
 
 '''
 set payload
@@ -79,11 +81,21 @@ def move_linear_pose(pose: list=[0, 0, 0, 0, 0, 0], a: float=1.4, v: float=1.05,
     _wait_robot_move_done()
 
 '''
-wait for robot to finish moving
+get program state
+:1 - normal
+:2 - running
 '''
-def _wait_robot_move_done():
-    _robot_started_moving()
-    _robot_done_moving()
+def _get_program_state() -> float:
+    # Receive responce
+    responce = realtime.receive_status()
+
+    # Unpack responce
+    realtime_statuses.unpack(responce)
+
+    # Get program state
+    prgstate = realtime_statuses.get_program_state()
+    return prgstate
+
 '''
 robot started moving
 '''
@@ -105,20 +117,11 @@ def _robot_done_moving() -> bool:
         sleep(0.001)
 
 '''
-get program state
-:1 - normal
-:2 - running
+wait for robot to finish moving
 '''
-def _get_program_state() -> float:
-    # Receive responce
-    responce = realtime.receive_status()
-
-    # Unpack responce
-    realtime_statuses.unpack(responce)
-
-    # Get program state
-    prgstate = realtime_statuses.get_program_state()
-    return prgstate
+def _wait_robot_move_done():
+    _robot_started_moving()
+    _robot_done_moving()
 
 '''
 set digital output