From 3a732affafd4c3a5d9e72a40233f47048787926d Mon Sep 17 00:00:00 2001 From: Moritz Fey Date: Fri, 10 Aug 2018 13:50:44 +0200 Subject: [PATCH 01/13] add prototypical implementation of OnRobot RG2 support, has to be tested on real hardware --- urx/onrobot_rg2_gripper.py | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 urx/onrobot_rg2_gripper.py diff --git a/urx/onrobot_rg2_gripper.py b/urx/onrobot_rg2_gripper.py new file mode 100644 index 0000000..07cd70a --- /dev/null +++ b/urx/onrobot_rg2_gripper.py @@ -0,0 +1,28 @@ + +from urx.urscript import URScript + + +class OnRobotGripperRG2Script(URScript): + + def __init__(self): + super(OnRobotGripperRG2Script, self).__init__() + + def _rg2_command(self, target_width=110, target_force=40, payload=0.0, set_payload=False, depth_compensation=False, slave=False): + self.add_line_to_program("RG2(target_width={}, target_force={}, payload={}, set_payload={}, depth_compensation={}, slave={})" + .format(target_width, target_force, payload, set_payload, depth_compensation, slave)) + + +class OnRobotGripperRG2(object): + + def __init__(self, robot): + self.robot = robot + + def open_gripper(self): + urscript = OnRobotGripperRG2Script() + urscript._rg2_command(target_width=110) + self.robot.send_program(urscript) + + def close_gripper(self): + urscript = OnRobotGripperRG2Script() + urscript._rg2_command(target_width=0) + self.robot.send_program(urscript) From cc2936a85131e6cef7d27afa21b495d34876ba3a Mon Sep 17 00:00:00 2001 From: Moritz Fey Date: Fri, 10 Aug 2018 15:11:51 +0200 Subject: [PATCH 02/13] working prototypical implementation of onrobot rg2 gripper --- urx/onrobot.script | 237 +++++++++++++++++++++++++++++++++++++ urx/onrobot_rg2_gripper.py | 20 +++- 2 files changed, 251 insertions(+), 6 deletions(-) create mode 100644 urx/onrobot.script diff --git a/urx/onrobot.script b/urx/onrobot.script new file mode 100644 index 0000000..03dbbc5 --- /dev/null +++ b/urx/onrobot.script @@ -0,0 +1,237 @@ + set_standard_analog_input_domain(0, 1) + set_standard_analog_input_domain(1, 1) + set_tool_analog_input_domain(0, 1) + set_tool_analog_input_domain(1, 1) + set_analog_outputdomain(0, 0) + set_analog_outputdomain(1, 0) + set_tool_voltage(0) + set_input_actions_to_default() + set_tcp(p[0.0,0.0,0.0,0.0,0.0,0.0]) + set_payload(0.0) + set_gravity([0.0, 0.0, 9.82]) + # begin: URCap Installation Node + # Source: RG - On Robot, 1.9.0, OnRobot A/S + # Type: RG Configuration + global measure_width=0 + global grip_detected=False + global lost_grip=False + global zsysx=0 + global zsysy=0 + global zsysz=0.06935 + global zsysm=0.7415 + global zmasx=0 + global zmasy=0 + global zmasz=0.18659 + global zmasm=0 + global zmasm=0 + global zslax=0 + global zslay=0 + global zslaz=0 + global zslam=0 + global zslam=0 + thread lost_grip_thread(): + while True: + set_tool_voltage(24) + if True ==get_digital_in(9): + sleep(0.024) + if True == grip_detected: + if False == get_digital_in(8): + grip_detected=False + lost_grip=True + end + end + set_tool_analog_input_domain(0, 1) + set_tool_analog_input_domain(1, 1) + zscale = (get_analog_in(2)-0.026)/2.9760034 + zangle = zscale*1.57079633+-0.08726646 + zwidth = 5.0+110*sin(zangle) + global measure_width = (floor(zwidth*10))/10-9.2 + end + sync() + end + end + lg_thr = run lost_grip_thread() + def RG2(target_width=110, target_force=40, payload=0.0, set_payload=False, depth_compensation=False, slave=False): + grip_detected=False + if slave: + slave_grip_detected=False + else: + master_grip_detected=False + end + timeout = 0 + timeout_limit = 750000 + while get_digital_in(9) == False: + if timeout > timeout_limit: + break + end + timeout = timeout+1 + sync() + end + def bit(input): + msb=65536 + local i=0 + local output=0 + while i<17: + set_digital_out(8,True) + if input>=msb: + input=input-msb + set_digital_out(9,False) + else: + set_digital_out(9,True) + end + if get_digital_in(8): + out=1 + end + sync() + set_digital_out(8,False) + sync() + input=input*2 + output=output*2 + i=i+1 + end + return output + end + target_width=target_width+9.2 + if target_force>40: + target_force=40 + end + if target_force<4: + target_force=4 + end + if target_width>110: + target_width=110 + end + if target_width<0: + target_width=0 + end + rg_data=floor(target_width)*4 + rg_data=rg_data+floor(target_force/2)*4*111 + rg_data=rg_data+32768 + if slave: + rg_data=rg_data+16384 + end + bit(rg_data) + if depth_compensation: + finger_length = 55.0/1000 + finger_heigth_disp = 5.0/1000 + center_displacement = 7.5/1000 + + start_pose = get_forward_kin() + set_analog_inputrange(2, 1) + zscale = (get_analog_in(2)-0.026)/2.9760034 + zangle = zscale*1.57079633+-0.08726646 + zwidth = 5.0+110*sin(zangle) + + start_depth = cos(zangle)*finger_length + + sleep(0.016) + timeout = 0 + while get_digital_in(9) == True: + timeout=timeout+1 + sleep(0.008) + if timeout > 20: + break + end + end + timeout = 0 + timeout_limit = 750000 + while get_digital_in(9) == False: + zscale = (get_analog_in(2)-0.026)/2.9760034 + zangle = zscale*1.57079633+-0.08726646 + zwidth = 5.0+110*sin(zangle) + measure_depth = cos(zangle)*finger_length + compensation_depth = (measure_depth - start_depth) + target_pose = pose_trans(start_pose,p[0,0,-compensation_depth,0,0,0]) + if timeout > timeout_limit: + break + end + timeout=timeout+1 + # servoj(get_inverse_kin(target_pose), t=0.008, lookahead_time=0.033, gain=1500) + # textmsg(point_dist(target_pose, get_forward_kin())) + #end + #textmsg("end gripper move!!!!!") + #nspeedthr = 0.001 + #nspeed = norm(get_actual_tcp_speed()) + #while nspeed > nspeedthr: + # servoj(get_inverse_kin(target_pose), t=0.008, lookahead_time=0.033, gain=1500) + # nspeed = norm(get_actual_tcp_speed()) + # textmsg(point_dist(target_pose, get_forward_kin())) + #end + servoj(get_inverse_kin(target_pose),0,0,0.008,0.01,2000) + if point_dist(target_pose, get_forward_kin()) > 0.005: + popup("Lower grasping force or max width",title="RG-lag threshold exceeded", warning=False, error=False, blocking=False) + end + end + nspeed = norm(get_actual_tcp_speed()) + while nspeed > 0.001: + servoj(get_inverse_kin(target_pose),0,0,0.008,0.01,2000) + nspeed = norm(get_actual_tcp_speed()) + end + stopj(2) + end + if depth_compensation==False: + timeout = 0 + timeout_count=20*0.008/0.008 + while get_digital_in(9) == True: + timeout = timeout+1 + sync() + if timeout > timeout_count: + break + end + end + timeout = 0 + timeout_limit = 750000 + while get_digital_in(9) == False: + timeout = timeout+1 + sync() + if timeout > timeout_limit: + break + end + end + end + sleep(0.024) + if set_payload: + if slave: + if get_analog_in(3) < 2: + zslam=0 + else: + zslam=payload + end + else: + if get_digital_in(8) == False: + zmasm=0 + else: + zmasm=payload + end + end + zload=zmasm+zslam+zsysm + set_payload(zload,[(zsysx*zsysm+zmasx*zmasm+zslax*zslam)/zload,(zsysy*zsysm+zmasy*zmasm+zslay*zslam)/zload,(zsysz*zsysm+zmasz*zmasm+zslaz*zslam)/zload]) + end + master_grip_detected=False + master_lost_grip=False + slave_grip_detected=False + slave_lost_grip=False + if True == get_digital_in(8): + master_grip_detected=True + end + if get_analog_in(3)>2: + slave_grip_detected=True + end + grip_detected=False + lost_grip=False + if True == get_digital_in(8): + grip_detected=True + end + zscale = (get_analog_in(2)-0.026)/2.9760034 + zangle = zscale*1.57079633+-0.08726646 + zwidth = 5.0+110*sin(zangle) + global measure_width = (floor(zwidth*10))/10-9.2 + if slave: + slave_measure_width=measure_width + else: + master_measure_width=measure_width + end + return grip_detected + end + set_tool_voltage(24) + set_tcp(p[0,0,0.18659,0,-0,0]) \ No newline at end of file diff --git a/urx/onrobot_rg2_gripper.py b/urx/onrobot_rg2_gripper.py index 07cd70a..95c1dc0 100644 --- a/urx/onrobot_rg2_gripper.py +++ b/urx/onrobot_rg2_gripper.py @@ -1,16 +1,21 @@ from urx.urscript import URScript - +import time +import os class OnRobotGripperRG2Script(URScript): def __init__(self): super(OnRobotGripperRG2Script, self).__init__() + print(os.listdir(".")) + with open("../urx/onrobot.script", "r") as f: + for line in f.readlines(): + self.add_line_to_program(line) - def _rg2_command(self, target_width=110, target_force=40, payload=0.0, set_payload=False, depth_compensation=False, slave=False): + def _rg2_command(self, target_width=110, target_force=40, payload=0.5, set_payload=False, depth_compensation=False, slave=False): self.add_line_to_program("RG2(target_width={}, target_force={}, payload={}, set_payload={}, depth_compensation={}, slave={})" .format(target_width, target_force, payload, set_payload, depth_compensation, slave)) - + # self.add_line_to_program("RG2(110)") class OnRobotGripperRG2(object): @@ -20,9 +25,12 @@ def __init__(self, robot): def open_gripper(self): urscript = OnRobotGripperRG2Script() urscript._rg2_command(target_width=110) - self.robot.send_program(urscript) - + print(urscript()) + self.robot.send_program(urscript()) + time.sleep(2) def close_gripper(self): urscript = OnRobotGripperRG2Script() urscript._rg2_command(target_width=0) - self.robot.send_program(urscript) + print(urscript()) + self.robot.send_program(urscript()) + time.sleep(2) From 3151cae50a0c3efeb8d831c784cc2bef5b6d1d68 Mon Sep 17 00:00:00 2001 From: Moritz Fey Date: Mon, 13 Aug 2018 09:14:18 +0200 Subject: [PATCH 03/13] basic implementation of Robotique RG2 gripper with boilerplate code --- urx/onrobot_rg2_gripper.py | 259 +++++++++++++++++++++++++++++++++++-- 1 file changed, 248 insertions(+), 11 deletions(-) diff --git a/urx/onrobot_rg2_gripper.py b/urx/onrobot_rg2_gripper.py index 95c1dc0..09d976c 100644 --- a/urx/onrobot_rg2_gripper.py +++ b/urx/onrobot_rg2_gripper.py @@ -3,34 +3,271 @@ import time import os +boilerplate = """ + set_standard_analog_input_domain(0, 1) + set_standard_analog_input_domain(1, 1) + set_tool_analog_input_domain(0, 1) + set_tool_analog_input_domain(1, 1) + set_analog_outputdomain(0, 0) + set_analog_outputdomain(1, 0) + set_tool_voltage(0) + set_input_actions_to_default() + set_tcp(p[0.0,0.0,0.0,0.0,0.0,0.0]) + set_payload(0.0) + set_gravity([0.0, 0.0, 9.82]) + # begin: URCap Installation Node + # Source: RG - On Robot, 1.9.0, OnRobot A/S + # Type: RG Configuration + global measure_width=0 + global grip_detected=False + global lost_grip=False + global zsysx=0 + global zsysy=0 + global zsysz=0.06935 + global zsysm=0.7415 + global zmasx=0 + global zmasy=0 + global zmasz=0.18659 + global zmasm=0 + global zmasm=0 + global zslax=0 + global zslay=0 + global zslaz=0 + global zslam=0 + global zslam=0 + thread lost_grip_thread(): + while True: + set_tool_voltage(24) + if True ==get_digital_in(9): + sleep(0.024) + if True == grip_detected: + if False == get_digital_in(8): + grip_detected=False + lost_grip=True + end + end + set_tool_analog_input_domain(0, 1) + set_tool_analog_input_domain(1, 1) + zscale = (get_analog_in(2)-0.026)/2.9760034 + zangle = zscale*1.57079633+-0.08726646 + zwidth = 5.0+110*sin(zangle) + global measure_width = (floor(zwidth*10))/10-9.2 + end + sync() + end + end + lg_thr = run lost_grip_thread() + def RG2(target_width=110, target_force=40, payload=0.0, set_payload=False, depth_compensation=False, slave=False): + grip_detected=False + if slave: + slave_grip_detected=False + else: + master_grip_detected=False + end + timeout = 0 + timeout_limit = 750000 + while get_digital_in(9) == False: + if timeout > timeout_limit: + break + end + timeout = timeout+1 + sync() + end + def bit(input): + msb=65536 + local i=0 + local output=0 + while i<17: + set_digital_out(8,True) + if input>=msb: + input=input-msb + set_digital_out(9,False) + else: + set_digital_out(9,True) + end + if get_digital_in(8): + out=1 + end + sync() + set_digital_out(8,False) + sync() + input=input*2 + output=output*2 + i=i+1 + end + return output + end + target_width=target_width+9.2 + if target_force>40: + target_force=40 + end + if target_force<4: + target_force=4 + end + if target_width>110: + target_width=110 + end + if target_width<0: + target_width=0 + end + rg_data=floor(target_width)*4 + rg_data=rg_data+floor(target_force/2)*4*111 + rg_data=rg_data+32768 + if slave: + rg_data=rg_data+16384 + end + bit(rg_data) + if depth_compensation: + finger_length = 55.0/1000 + finger_heigth_disp = 5.0/1000 + center_displacement = 7.5/1000 + + start_pose = get_forward_kin() + set_analog_inputrange(2, 1) + zscale = (get_analog_in(2)-0.026)/2.9760034 + zangle = zscale*1.57079633+-0.08726646 + zwidth = 5.0+110*sin(zangle) + + start_depth = cos(zangle)*finger_length + + sleep(0.016) + timeout = 0 + while get_digital_in(9) == True: + timeout=timeout+1 + sleep(0.008) + if timeout > 20: + break + end + end + timeout = 0 + timeout_limit = 750000 + while get_digital_in(9) == False: + zscale = (get_analog_in(2)-0.026)/2.9760034 + zangle = zscale*1.57079633+-0.08726646 + zwidth = 5.0+110*sin(zangle) + measure_depth = cos(zangle)*finger_length + compensation_depth = (measure_depth - start_depth) + target_pose = pose_trans(start_pose,p[0,0,-compensation_depth,0,0,0]) + if timeout > timeout_limit: + break + end + timeout=timeout+1 + # servoj(get_inverse_kin(target_pose), t=0.008, lookahead_time=0.033, gain=1500) + # textmsg(point_dist(target_pose, get_forward_kin())) + #end + #textmsg("end gripper move!!!!!") + #nspeedthr = 0.001 + #nspeed = norm(get_actual_tcp_speed()) + #while nspeed > nspeedthr: + # servoj(get_inverse_kin(target_pose), t=0.008, lookahead_time=0.033, gain=1500) + # nspeed = norm(get_actual_tcp_speed()) + # textmsg(point_dist(target_pose, get_forward_kin())) + #end + servoj(get_inverse_kin(target_pose),0,0,0.008,0.01,2000) + if point_dist(target_pose, get_forward_kin()) > 0.005: + popup("Lower grasping force or max width",title="RG-lag threshold exceeded", warning=False, error=False, blocking=False) + end + end + nspeed = norm(get_actual_tcp_speed()) + while nspeed > 0.001: + servoj(get_inverse_kin(target_pose),0,0,0.008,0.01,2000) + nspeed = norm(get_actual_tcp_speed()) + end + stopj(2) + end + if depth_compensation==False: + timeout = 0 + timeout_count=20*0.008/0.008 + while get_digital_in(9) == True: + timeout = timeout+1 + sync() + if timeout > timeout_count: + break + end + end + timeout = 0 + timeout_limit = 750000 + while get_digital_in(9) == False: + timeout = timeout+1 + sync() + if timeout > timeout_limit: + break + end + end + end + sleep(0.024) + if set_payload: + if slave: + if get_analog_in(3) < 2: + zslam=0 + else: + zslam=payload + end + else: + if get_digital_in(8) == False: + zmasm=0 + else: + zmasm=payload + end + end + zload=zmasm+zslam+zsysm + set_payload(zload,[(zsysx*zsysm+zmasx*zmasm+zslax*zslam)/zload,(zsysy*zsysm+zmasy*zmasm+zslay*zslam)/zload,(zsysz*zsysm+zmasz*zmasm+zslaz*zslam)/zload]) + end + master_grip_detected=False + master_lost_grip=False + slave_grip_detected=False + slave_lost_grip=False + if True == get_digital_in(8): + master_grip_detected=True + end + if get_analog_in(3)>2: + slave_grip_detected=True + end + grip_detected=False + lost_grip=False + if True == get_digital_in(8): + grip_detected=True + end + zscale = (get_analog_in(2)-0.026)/2.9760034 + zangle = zscale*1.57079633+-0.08726646 + zwidth = 5.0+110*sin(zangle) + global measure_width = (floor(zwidth*10))/10-9.2 + if slave: + slave_measure_width=measure_width + else: + master_measure_width=measure_width + end + return grip_detected + end + set_tool_voltage(24) + set_tcp(p[0,0,0.18659,0,-0,0]) + """ class OnRobotGripperRG2Script(URScript): def __init__(self): super(OnRobotGripperRG2Script, self).__init__() - print(os.listdir(".")) - with open("../urx/onrobot.script", "r") as f: - for line in f.readlines(): - self.add_line_to_program(line) + # copy the boilerplate to the start of the script to make the RG2() function available + self.add_line_to_program(boilerplate) def _rg2_command(self, target_width=110, target_force=40, payload=0.5, set_payload=False, depth_compensation=False, slave=False): self.add_line_to_program("RG2(target_width={}, target_force={}, payload={}, set_payload={}, depth_compensation={}, slave={})" .format(target_width, target_force, payload, set_payload, depth_compensation, slave)) - # self.add_line_to_program("RG2(110)") class OnRobotGripperRG2(object): def __init__(self, robot): self.robot = robot - def open_gripper(self): + def open_gripper(self, target_width=110, target_force=40, payload=0.5, set_payload=False, depth_compensation=False, slave=False): urscript = OnRobotGripperRG2Script() - urscript._rg2_command(target_width=110) - print(urscript()) + urscript._rg2_command(target_width, target_force, payload, set_payload, depth_compensation, slave) self.robot.send_program(urscript()) time.sleep(2) - def close_gripper(self): + + def close_gripper(self, target_width=-10, target_force=40, payload=0.5, set_payload=False, depth_compensation=False, slave=False): urscript = OnRobotGripperRG2Script() - urscript._rg2_command(target_width=0) - print(urscript()) + urscript._rg2_command(target_width, target_force, payload, set_payload, depth_compensation, slave) self.robot.send_program(urscript()) time.sleep(2) + + From c5964acaacd336a44187442520da20942014f248 Mon Sep 17 00:00:00 2001 From: Moritz Fey Date: Mon, 13 Aug 2018 09:17:05 +0200 Subject: [PATCH 04/13] remove onrobot.script --- urx/onrobot.script | 237 --------------------------------------------- 1 file changed, 237 deletions(-) delete mode 100644 urx/onrobot.script diff --git a/urx/onrobot.script b/urx/onrobot.script deleted file mode 100644 index 03dbbc5..0000000 --- a/urx/onrobot.script +++ /dev/null @@ -1,237 +0,0 @@ - set_standard_analog_input_domain(0, 1) - set_standard_analog_input_domain(1, 1) - set_tool_analog_input_domain(0, 1) - set_tool_analog_input_domain(1, 1) - set_analog_outputdomain(0, 0) - set_analog_outputdomain(1, 0) - set_tool_voltage(0) - set_input_actions_to_default() - set_tcp(p[0.0,0.0,0.0,0.0,0.0,0.0]) - set_payload(0.0) - set_gravity([0.0, 0.0, 9.82]) - # begin: URCap Installation Node - # Source: RG - On Robot, 1.9.0, OnRobot A/S - # Type: RG Configuration - global measure_width=0 - global grip_detected=False - global lost_grip=False - global zsysx=0 - global zsysy=0 - global zsysz=0.06935 - global zsysm=0.7415 - global zmasx=0 - global zmasy=0 - global zmasz=0.18659 - global zmasm=0 - global zmasm=0 - global zslax=0 - global zslay=0 - global zslaz=0 - global zslam=0 - global zslam=0 - thread lost_grip_thread(): - while True: - set_tool_voltage(24) - if True ==get_digital_in(9): - sleep(0.024) - if True == grip_detected: - if False == get_digital_in(8): - grip_detected=False - lost_grip=True - end - end - set_tool_analog_input_domain(0, 1) - set_tool_analog_input_domain(1, 1) - zscale = (get_analog_in(2)-0.026)/2.9760034 - zangle = zscale*1.57079633+-0.08726646 - zwidth = 5.0+110*sin(zangle) - global measure_width = (floor(zwidth*10))/10-9.2 - end - sync() - end - end - lg_thr = run lost_grip_thread() - def RG2(target_width=110, target_force=40, payload=0.0, set_payload=False, depth_compensation=False, slave=False): - grip_detected=False - if slave: - slave_grip_detected=False - else: - master_grip_detected=False - end - timeout = 0 - timeout_limit = 750000 - while get_digital_in(9) == False: - if timeout > timeout_limit: - break - end - timeout = timeout+1 - sync() - end - def bit(input): - msb=65536 - local i=0 - local output=0 - while i<17: - set_digital_out(8,True) - if input>=msb: - input=input-msb - set_digital_out(9,False) - else: - set_digital_out(9,True) - end - if get_digital_in(8): - out=1 - end - sync() - set_digital_out(8,False) - sync() - input=input*2 - output=output*2 - i=i+1 - end - return output - end - target_width=target_width+9.2 - if target_force>40: - target_force=40 - end - if target_force<4: - target_force=4 - end - if target_width>110: - target_width=110 - end - if target_width<0: - target_width=0 - end - rg_data=floor(target_width)*4 - rg_data=rg_data+floor(target_force/2)*4*111 - rg_data=rg_data+32768 - if slave: - rg_data=rg_data+16384 - end - bit(rg_data) - if depth_compensation: - finger_length = 55.0/1000 - finger_heigth_disp = 5.0/1000 - center_displacement = 7.5/1000 - - start_pose = get_forward_kin() - set_analog_inputrange(2, 1) - zscale = (get_analog_in(2)-0.026)/2.9760034 - zangle = zscale*1.57079633+-0.08726646 - zwidth = 5.0+110*sin(zangle) - - start_depth = cos(zangle)*finger_length - - sleep(0.016) - timeout = 0 - while get_digital_in(9) == True: - timeout=timeout+1 - sleep(0.008) - if timeout > 20: - break - end - end - timeout = 0 - timeout_limit = 750000 - while get_digital_in(9) == False: - zscale = (get_analog_in(2)-0.026)/2.9760034 - zangle = zscale*1.57079633+-0.08726646 - zwidth = 5.0+110*sin(zangle) - measure_depth = cos(zangle)*finger_length - compensation_depth = (measure_depth - start_depth) - target_pose = pose_trans(start_pose,p[0,0,-compensation_depth,0,0,0]) - if timeout > timeout_limit: - break - end - timeout=timeout+1 - # servoj(get_inverse_kin(target_pose), t=0.008, lookahead_time=0.033, gain=1500) - # textmsg(point_dist(target_pose, get_forward_kin())) - #end - #textmsg("end gripper move!!!!!") - #nspeedthr = 0.001 - #nspeed = norm(get_actual_tcp_speed()) - #while nspeed > nspeedthr: - # servoj(get_inverse_kin(target_pose), t=0.008, lookahead_time=0.033, gain=1500) - # nspeed = norm(get_actual_tcp_speed()) - # textmsg(point_dist(target_pose, get_forward_kin())) - #end - servoj(get_inverse_kin(target_pose),0,0,0.008,0.01,2000) - if point_dist(target_pose, get_forward_kin()) > 0.005: - popup("Lower grasping force or max width",title="RG-lag threshold exceeded", warning=False, error=False, blocking=False) - end - end - nspeed = norm(get_actual_tcp_speed()) - while nspeed > 0.001: - servoj(get_inverse_kin(target_pose),0,0,0.008,0.01,2000) - nspeed = norm(get_actual_tcp_speed()) - end - stopj(2) - end - if depth_compensation==False: - timeout = 0 - timeout_count=20*0.008/0.008 - while get_digital_in(9) == True: - timeout = timeout+1 - sync() - if timeout > timeout_count: - break - end - end - timeout = 0 - timeout_limit = 750000 - while get_digital_in(9) == False: - timeout = timeout+1 - sync() - if timeout > timeout_limit: - break - end - end - end - sleep(0.024) - if set_payload: - if slave: - if get_analog_in(3) < 2: - zslam=0 - else: - zslam=payload - end - else: - if get_digital_in(8) == False: - zmasm=0 - else: - zmasm=payload - end - end - zload=zmasm+zslam+zsysm - set_payload(zload,[(zsysx*zsysm+zmasx*zmasm+zslax*zslam)/zload,(zsysy*zsysm+zmasy*zmasm+zslay*zslam)/zload,(zsysz*zsysm+zmasz*zmasm+zslaz*zslam)/zload]) - end - master_grip_detected=False - master_lost_grip=False - slave_grip_detected=False - slave_lost_grip=False - if True == get_digital_in(8): - master_grip_detected=True - end - if get_analog_in(3)>2: - slave_grip_detected=True - end - grip_detected=False - lost_grip=False - if True == get_digital_in(8): - grip_detected=True - end - zscale = (get_analog_in(2)-0.026)/2.9760034 - zangle = zscale*1.57079633+-0.08726646 - zwidth = 5.0+110*sin(zangle) - global measure_width = (floor(zwidth*10))/10-9.2 - if slave: - slave_measure_width=measure_width - else: - master_measure_width=measure_width - end - return grip_detected - end - set_tool_voltage(24) - set_tcp(p[0,0,0.18659,0,-0,0]) \ No newline at end of file From 50186ddcb37163312cdbcb032d0b6620c58b748b Mon Sep 17 00:00:00 2001 From: Moritz Fey Date: Mon, 27 Aug 2018 08:27:57 +0200 Subject: [PATCH 05/13] add possibility to move to a pose via movej --- urx/urrobot.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/urx/urrobot.py b/urx/urrobot.py index a46ea05..e3b61a7 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -264,14 +264,18 @@ def speedx(self, command, velocities, acc, min_time): prog = "{}([{},{},{},{},{},{}], a={}, t_min={})".format(command, *vels) self.send_program(prog) - def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold=None): + def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold=None, move_to_pose=False): """ move in joint space """ + if move_to_pose == True: + prefix = "p" + else: + prefix = "" if relative: l = self.getj() joints = [v + l[i] for i, v in enumerate(joints)] - prog = self._format_move("movej", joints, acc, vel) + prog = self._format_move("movej", joints, acc, vel, prefix=prefix) self.send_program(prog) if wait: self._wait_for_move(joints[:6], threshold=threshold, joints=True) From 8a55f0127b7c278809cbbbf05e86c4efe9818a4f Mon Sep 17 00:00:00 2001 From: Moritz Fey Date: Mon, 27 Aug 2018 11:15:14 +0200 Subject: [PATCH 06/13] add movej_to_pose method --- urx/urrobot.py | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/urx/urrobot.py b/urx/urrobot.py index e3b61a7..44d116e 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -264,23 +264,30 @@ def speedx(self, command, velocities, acc, min_time): prog = "{}([{},{},{},{},{},{}], a={}, t_min={})".format(command, *vels) self.send_program(prog) - def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold=None, move_to_pose=False): + def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold=None): """ move in joint space """ - if move_to_pose == True: - prefix = "p" - else: - prefix = "" if relative: l = self.getj() joints = [v + l[i] for i, v in enumerate(joints)] - prog = self._format_move("movej", joints, acc, vel, prefix=prefix) + prog = self._format_move("movej", joints, acc, vel) self.send_program(prog) if wait: self._wait_for_move(joints[:6], threshold=threshold, joints=True) return self.getj() + def movej_to_pose(self, tpose, acc=0.1, vel=0.05, wait=True, relative=False, threshold=None): + if relative: + l = self.getl() + tpose = [v + l[i] for i, v in enumerate(tpose)] + prog = self._format_move("movej", tpose, acc, vel, prefix="p") + print(prog) + self.send_program(prog) + if wait: + self._wait_for_move(tpose[:6], threshold=threshold, joints=False) + return self.getj() + def movel(self, tpose, acc=0.01, vel=0.01, wait=True, relative=False, threshold=None): """ Send a movel command to the robot. See URScript documentation. From 5dc231324acfc73a4b933091e96d864a5552eb7e Mon Sep 17 00:00:00 2001 From: Moritz Fey Date: Fri, 31 Aug 2018 11:26:24 +0200 Subject: [PATCH 07/13] add radius to movej --- urx/urrobot.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/urx/urrobot.py b/urx/urrobot.py index 44d116e..a99e1f2 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -277,11 +277,11 @@ def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold= self._wait_for_move(joints[:6], threshold=threshold, joints=True) return self.getj() - def movej_to_pose(self, tpose, acc=0.1, vel=0.05, wait=True, relative=False, threshold=None): + def movej_to_pose(self, tpose, acc=0.1, vel=0.05, radius=0, wait=True, relative=False, threshold=None): if relative: l = self.getl() tpose = [v + l[i] for i, v in enumerate(tpose)] - prog = self._format_move("movej", tpose, acc, vel, prefix="p") + prog = self._format_move("movej", tpose, acc, vel, prefix="p", radius=radius) print(prog) self.send_program(prog) if wait: From 211967a2fbaf7cd24290f08c277f9ae2c42ed764 Mon Sep 17 00:00:00 2001 From: Moritz Fey Date: Fri, 31 Aug 2018 12:51:10 +0200 Subject: [PATCH 08/13] Revert "add radius to movej" This reverts commit 5dc2313 --- urx/urrobot.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/urx/urrobot.py b/urx/urrobot.py index a99e1f2..44d116e 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -277,11 +277,11 @@ def movej(self, joints, acc=0.1, vel=0.05, wait=True, relative=False, threshold= self._wait_for_move(joints[:6], threshold=threshold, joints=True) return self.getj() - def movej_to_pose(self, tpose, acc=0.1, vel=0.05, radius=0, wait=True, relative=False, threshold=None): + def movej_to_pose(self, tpose, acc=0.1, vel=0.05, wait=True, relative=False, threshold=None): if relative: l = self.getl() tpose = [v + l[i] for i, v in enumerate(tpose)] - prog = self._format_move("movej", tpose, acc, vel, prefix="p", radius=radius) + prog = self._format_move("movej", tpose, acc, vel, prefix="p") print(prog) self.send_program(prog) if wait: From 4914f8c59c20110e967c35460769dedbfe57d6b7 Mon Sep 17 00:00:00 2001 From: Moritz Fey Date: Fri, 31 Aug 2018 13:02:09 +0200 Subject: [PATCH 09/13] experimental implementation of multiple movejs --- urx/urrobot.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/urx/urrobot.py b/urx/urrobot.py index 44d116e..84e078a 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -351,6 +351,9 @@ def movec(self, pose_via, pose_to, acc=0.01, vel=0.01, wait=True, threshold=None self._wait_for_move(pose_to, threshold=threshold) return self.getl() + def movej_to_pose_list(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=None): + return self.movexs("movej", pose_list, acc, vel, radius, wait, threshold=threshold) + def movels(self, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=None): """ Concatenate several movel commands and applies a blending radius @@ -378,6 +381,8 @@ def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, self.send_program(prog) if wait: self._wait_for_move(target=pose_list[-1], threshold=threshold) + if command == "movej": + return self.getj() return self.getl() def stopl(self, acc=0.5): From 7757f3c3681261fb475eb279699a843c6f987508 Mon Sep 17 00:00:00 2001 From: Moritz Fey Date: Wed, 19 Sep 2018 09:42:40 +0200 Subject: [PATCH 10/13] add parameter for sleep duration --- urx/onrobot_rg2_gripper.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/urx/onrobot_rg2_gripper.py b/urx/onrobot_rg2_gripper.py index 09d976c..657eb5a 100644 --- a/urx/onrobot_rg2_gripper.py +++ b/urx/onrobot_rg2_gripper.py @@ -258,16 +258,16 @@ class OnRobotGripperRG2(object): def __init__(self, robot): self.robot = robot - def open_gripper(self, target_width=110, target_force=40, payload=0.5, set_payload=False, depth_compensation=False, slave=False): + def open_gripper(self, target_width=110, target_force=40, payload=0.5, set_payload=False, depth_compensation=False, slave=False, wait=2): urscript = OnRobotGripperRG2Script() urscript._rg2_command(target_width, target_force, payload, set_payload, depth_compensation, slave) self.robot.send_program(urscript()) - time.sleep(2) + time.sleep(wait) - def close_gripper(self, target_width=-10, target_force=40, payload=0.5, set_payload=False, depth_compensation=False, slave=False): + def close_gripper(self, target_width=-10, target_force=40, payload=0.5, set_payload=False, depth_compensation=False, slave=False, wait=2): urscript = OnRobotGripperRG2Script() urscript._rg2_command(target_width, target_force, payload, set_payload, depth_compensation, slave) self.robot.send_program(urscript()) - time.sleep(2) + time.sleep(wait) From 98aa4d6676196138ff8e5900ed7ae574f8fb5da7 Mon Sep 17 00:00:00 2001 From: michael-hart Date: Thu, 18 Jul 2019 14:59:38 +0100 Subject: [PATCH 11/13] Reorganise grippers into package and add README docs --- README.md | 80 +++++++++++++++---- urx/gripper/__init__.py | 8 ++ urx/{ => gripper}/onrobot_rg2_gripper.py | 11 --- .../robotiq_two_finger_gripper.py | 0 4 files changed, 72 insertions(+), 27 deletions(-) create mode 100644 urx/gripper/__init__.py rename urx/{ => gripper}/onrobot_rg2_gripper.py (93%) rename urx/{ => gripper}/robotiq_two_finger_gripper.py (100%) diff --git a/README.md b/README.md index 9120c0e..805c6f9 100644 --- a/README.md +++ b/README.md @@ -92,34 +92,82 @@ csys = rob.new_csys_from_xpy() # generate a new csys from 3 points: X, origin, rob.set_csys(csys) ``` +# Supported Grippers -# Robotiq Gripper +## Robotiq Gripper urx can also control a Robotiq gripper attached to the UR robot. The robotiq class was primarily developed by [Mark Silliman](https://github.com/markwsilliman). -## Example use: +### Example use: ```python import sys import urx -from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper +from urx.gripper import Robotiq_Two_Finger_Gripper if __name__ == '__main__': - rob = urx.Robot("192.168.0.100") - robotiqgrip = Robotiq_Two_Finger_Gripper() + rob = urx.Robot("192.168.0.100") + robotiqgrip = Robotiq_Two_Finger_Gripper() - if(len(sys.argv) != 2): - print "false" - sys.exit() + if(len(sys.argv) != 2): + print "false" + sys.exit() - if(sys.argv[1] == "close") : - robotiqgrip.close_gripper() - if(sys.argv[1] == "open") : - robotiqgrip.open_gripper() + if(sys.argv[1] == "close") : + robotiqgrip.close_gripper() + if(sys.argv[1] == "open") : + robotiqgrip.open_gripper() - rob.send_program(robotiqgrip.ret_program_to_run()) + rob.send_program(robotiqgrip.ret_program_to_run()) - rob.close() - print "true" - sys.exit() + rob.close() + print "true" + sys.exit() ``` + +## OnRobot RG2 Gripper + +urx can control an RG2 gripper as well. The class was primarily developed by [Moritz Fey](https://github.com/Mofeywalker). + +### Example use:?> + +```python +import sys +import urx +from urx.gripper import OnRobotGripperRG2 + +if __name__ == '__main__': + rob = urx.Robot("192.168.0.100") + gripper = OnRobotGripperRG2() + + if(len(sys.argv) != 2): + print "false" + sys.exit() + + if(sys.argv[1] == "close") : + gripper.close_gripper() + if(sys.argv[1] == "open") : + gripper.open_gripper() + + rob.send_program(robotiqgrip.ret_program_to_run()) + + rob.close() + print "true" + sys.exit() +``` + +Various parameters can be controlled in the open and close functions. The parameters that can be set are: + +```python + gripper.open_gripper( + target_width=110, # Width in mm, 110 is fully open + target_force=40, # Maximum force applied in N, 40 is maximum + payload=0.5, # Payload in kg + set_payload=False, # If any payload is attached + depth_compensation=False, # Whether to compensate for finger depth + slave=False, # Is this gripper the master or slave gripper? + wait=2 # Wait up to 2s for movement + ) +``` + +The parameters are the same for opening and closing the gripper. diff --git a/urx/gripper/__init__.py b/urx/gripper/__init__.py new file mode 100644 index 0000000..8378f4c --- /dev/null +++ b/urx/gripper/__init__.py @@ -0,0 +1,8 @@ +from .robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper +from .onrobot_rg2_gripper import OnRobotGripperRG2 + + +__all__ = [ + "Robotiq_Two_Finger_Gripper", + "OnRobotGripperRG2", +] diff --git a/urx/onrobot_rg2_gripper.py b/urx/gripper/onrobot_rg2_gripper.py similarity index 93% rename from urx/onrobot_rg2_gripper.py rename to urx/gripper/onrobot_rg2_gripper.py index 657eb5a..6c160c4 100644 --- a/urx/onrobot_rg2_gripper.py +++ b/urx/gripper/onrobot_rg2_gripper.py @@ -152,17 +152,6 @@ def bit(input): break end timeout=timeout+1 - # servoj(get_inverse_kin(target_pose), t=0.008, lookahead_time=0.033, gain=1500) - # textmsg(point_dist(target_pose, get_forward_kin())) - #end - #textmsg("end gripper move!!!!!") - #nspeedthr = 0.001 - #nspeed = norm(get_actual_tcp_speed()) - #while nspeed > nspeedthr: - # servoj(get_inverse_kin(target_pose), t=0.008, lookahead_time=0.033, gain=1500) - # nspeed = norm(get_actual_tcp_speed()) - # textmsg(point_dist(target_pose, get_forward_kin())) - #end servoj(get_inverse_kin(target_pose),0,0,0.008,0.01,2000) if point_dist(target_pose, get_forward_kin()) > 0.005: popup("Lower grasping force or max width",title="RG-lag threshold exceeded", warning=False, error=False, blocking=False) diff --git a/urx/robotiq_two_finger_gripper.py b/urx/gripper/robotiq_two_finger_gripper.py similarity index 100% rename from urx/robotiq_two_finger_gripper.py rename to urx/gripper/robotiq_two_finger_gripper.py From 7b63d3ff314c46074f62fafeaffcd8ba216f4468 Mon Sep 17 00:00:00 2001 From: michael-hart Date: Thu, 18 Jul 2019 15:23:30 +0100 Subject: [PATCH 12/13] Add ability to measure grip width and object gripped --- urx/gripper/onrobot_rg2_gripper.py | 11 +++++++++++ urx/urrobot.py | 6 ------ urx/ursecmon.py | 13 +++++++++++-- 3 files changed, 22 insertions(+), 8 deletions(-) diff --git a/urx/gripper/onrobot_rg2_gripper.py b/urx/gripper/onrobot_rg2_gripper.py index 6c160c4..ea67716 100644 --- a/urx/gripper/onrobot_rg2_gripper.py +++ b/urx/gripper/onrobot_rg2_gripper.py @@ -1,5 +1,6 @@ from urx.urscript import URScript +import math import time import os @@ -259,4 +260,14 @@ def close_gripper(self, target_width=-10, target_force=40, payload=0.5, set_payl self.robot.send_program(urscript()) time.sleep(wait) + @property.getter + def width(self): + zscale = (self.robot.get_analog_in(2) - 0.026) / 2.9760034 + zangle = zscale * 1.57079633 + -0.08726646 + zwidth = 5.0 + 110 * math.sin(zangle) + measure_width = (math.floor(zwidth * 10)) / 10 - 9.2 + return measure_width + @property.getter + def object_gripped(self): + return self.robot.get_digital_in(16) diff --git a/urx/urrobot.py b/urx/urrobot.py index c01abca..f955c44 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -422,16 +422,10 @@ def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, prog += end self.send_program(prog) if wait: -<<<<<<< HEAD if command == 'movel': self._wait_for_move(target=pose_list[-1], threshold=threshold, joints=False) elif command == 'movej': self._wait_for_move(target=pose_list[-1], threshold=threshold, joints=True) -======= - self._wait_for_move(target=pose_list[-1], threshold=threshold) - if command == "movej": - return self.getj() ->>>>>>> 7757f3c3681261fb475eb279699a843c6f987508 return self.getl() def stopl(self, acc=0.5): diff --git a/urx/ursecmon.py b/urx/ursecmon.py index e7b3189..994807f 100644 --- a/urx/ursecmon.py +++ b/urx/ursecmon.py @@ -411,13 +411,22 @@ def get_analog_in(self, nb, wait=False): if wait: self.wait() with self._dictLock: - return self._dict["MasterBoardData"]["analogInput" + str(nb)] + analog_input = "analogInput" + str(nb) + if analog_input in self._dict["MasterBoardData"]: + return self._dict["MasterBoardData"][analog_input] + else: + return self._dict["ToolData"][analog_input] def get_analog_inputs(self, wait=False): if wait: self.wait() with self._dictLock: - return self._dict["MasterBoardData"]["analogInput0"], self._dict["MasterBoardData"]["analogInput1"] + return ( + self._dict["MasterBoardData"]["analogInput0"], + self._dict["MasterBoardData"]["analogInput1"], + self._dict["ToolData"]["analogInput2"], + self._dict["ToolData"]["analogInput3"], + ) def is_program_running(self, wait=False): """ From 7b1212a0ed3a800b41b750feb80af8060546382c Mon Sep 17 00:00:00 2001 From: Michael Hart Date: Thu, 18 Jul 2019 17:06:23 +0100 Subject: [PATCH 13/13] Fix syntax errors and documentation --- README.md | 18 +- urx/gripper/onrobot_rg2_gripper.py | 392 ++++++++++++++--------------- 2 files changed, 204 insertions(+), 206 deletions(-) diff --git a/README.md b/README.md index 805c6f9..5d76393 100644 --- a/README.md +++ b/README.md @@ -109,13 +109,13 @@ if __name__ == '__main__': rob = urx.Robot("192.168.0.100") robotiqgrip = Robotiq_Two_Finger_Gripper() - if(len(sys.argv) != 2): + if len(sys.argv) != 2: print "false" sys.exit() - if(sys.argv[1] == "close") : + if sys.argv[1] == "close": robotiqgrip.close_gripper() - if(sys.argv[1] == "open") : + if sys.argv[1] == "open": robotiqgrip.open_gripper() rob.send_program(robotiqgrip.ret_program_to_run()) @@ -129,7 +129,7 @@ if __name__ == '__main__': urx can control an RG2 gripper as well. The class was primarily developed by [Moritz Fey](https://github.com/Mofeywalker). -### Example use:?> +### Example use: ```python import sys @@ -138,19 +138,17 @@ from urx.gripper import OnRobotGripperRG2 if __name__ == '__main__': rob = urx.Robot("192.168.0.100") - gripper = OnRobotGripperRG2() + gripper = OnRobotGripperRG2(rob) - if(len(sys.argv) != 2): + if len(sys.argv) != 2: print "false" sys.exit() - if(sys.argv[1] == "close") : + if sys.argv[1] == "close": gripper.close_gripper() - if(sys.argv[1] == "open") : + if sys.argv[1] == "open": gripper.open_gripper() - rob.send_program(robotiqgrip.ret_program_to_run()) - rob.close() print "true" sys.exit() diff --git a/urx/gripper/onrobot_rg2_gripper.py b/urx/gripper/onrobot_rg2_gripper.py index ea67716..6507084 100644 --- a/urx/gripper/onrobot_rg2_gripper.py +++ b/urx/gripper/onrobot_rg2_gripper.py @@ -37,198 +37,198 @@ global zslam=0 global zslam=0 thread lost_grip_thread(): - while True: - set_tool_voltage(24) - if True ==get_digital_in(9): - sleep(0.024) - if True == grip_detected: - if False == get_digital_in(8): - grip_detected=False - lost_grip=True - end - end - set_tool_analog_input_domain(0, 1) - set_tool_analog_input_domain(1, 1) - zscale = (get_analog_in(2)-0.026)/2.9760034 - zangle = zscale*1.57079633+-0.08726646 - zwidth = 5.0+110*sin(zangle) - global measure_width = (floor(zwidth*10))/10-9.2 - end - sync() - end + while True: + set_tool_voltage(24) + if True ==get_digital_in(9): + sleep(0.024) + if True == grip_detected: + if False == get_digital_in(8): + grip_detected=False + lost_grip=True + end + end + set_tool_analog_input_domain(0, 1) + set_tool_analog_input_domain(1, 1) + zscale = (get_analog_in(2)-0.026)/2.9760034 + zangle = zscale*1.57079633+-0.08726646 + zwidth = 5.0+110*sin(zangle) + global measure_width = (floor(zwidth*10))/10-9.2 + end + sync() + end end lg_thr = run lost_grip_thread() def RG2(target_width=110, target_force=40, payload=0.0, set_payload=False, depth_compensation=False, slave=False): - grip_detected=False - if slave: - slave_grip_detected=False - else: - master_grip_detected=False - end - timeout = 0 - timeout_limit = 750000 - while get_digital_in(9) == False: - if timeout > timeout_limit: - break - end - timeout = timeout+1 - sync() - end - def bit(input): - msb=65536 - local i=0 - local output=0 - while i<17: - set_digital_out(8,True) - if input>=msb: - input=input-msb - set_digital_out(9,False) - else: - set_digital_out(9,True) - end - if get_digital_in(8): - out=1 - end - sync() - set_digital_out(8,False) - sync() - input=input*2 - output=output*2 - i=i+1 - end - return output - end - target_width=target_width+9.2 - if target_force>40: - target_force=40 - end - if target_force<4: - target_force=4 - end - if target_width>110: - target_width=110 - end - if target_width<0: - target_width=0 - end - rg_data=floor(target_width)*4 - rg_data=rg_data+floor(target_force/2)*4*111 - rg_data=rg_data+32768 - if slave: - rg_data=rg_data+16384 - end - bit(rg_data) - if depth_compensation: - finger_length = 55.0/1000 - finger_heigth_disp = 5.0/1000 - center_displacement = 7.5/1000 + grip_detected=False + if slave: + slave_grip_detected=False + else: + master_grip_detected=False + end + timeout = 0 + timeout_limit = 750000 + while get_digital_in(9) == False: + if timeout > timeout_limit: + break + end + timeout = timeout+1 + sync() + end + def bit(input): + msb=65536 + local i=0 + local output=0 + while i<17: + set_digital_out(8,True) + if input>=msb: + input=input-msb + set_digital_out(9,False) + else: + set_digital_out(9,True) + end + if get_digital_in(8): + out=1 + end + sync() + set_digital_out(8,False) + sync() + input=input*2 + output=output*2 + i=i+1 + end + return output + end + target_width=target_width+9.2 + if target_force>40: + target_force=40 + end + if target_force<4: + target_force=4 + end + if target_width>110: + target_width=110 + end + if target_width<0: + target_width=0 + end + rg_data=floor(target_width)*4 + rg_data=rg_data+floor(target_force/2)*4*111 + rg_data=rg_data+32768 + if slave: + rg_data=rg_data+16384 + end + bit(rg_data) + if depth_compensation: + finger_length = 55.0/1000 + finger_heigth_disp = 5.0/1000 + center_displacement = 7.5/1000 - start_pose = get_forward_kin() - set_analog_inputrange(2, 1) - zscale = (get_analog_in(2)-0.026)/2.9760034 - zangle = zscale*1.57079633+-0.08726646 - zwidth = 5.0+110*sin(zangle) + start_pose = get_forward_kin() + set_analog_inputrange(2, 1) + zscale = (get_analog_in(2)-0.026)/2.9760034 + zangle = zscale*1.57079633+-0.08726646 + zwidth = 5.0+110*sin(zangle) - start_depth = cos(zangle)*finger_length + start_depth = cos(zangle)*finger_length - sleep(0.016) - timeout = 0 - while get_digital_in(9) == True: - timeout=timeout+1 - sleep(0.008) - if timeout > 20: - break - end - end - timeout = 0 - timeout_limit = 750000 - while get_digital_in(9) == False: - zscale = (get_analog_in(2)-0.026)/2.9760034 - zangle = zscale*1.57079633+-0.08726646 - zwidth = 5.0+110*sin(zangle) - measure_depth = cos(zangle)*finger_length - compensation_depth = (measure_depth - start_depth) - target_pose = pose_trans(start_pose,p[0,0,-compensation_depth,0,0,0]) - if timeout > timeout_limit: - break - end - timeout=timeout+1 - servoj(get_inverse_kin(target_pose),0,0,0.008,0.01,2000) - if point_dist(target_pose, get_forward_kin()) > 0.005: - popup("Lower grasping force or max width",title="RG-lag threshold exceeded", warning=False, error=False, blocking=False) - end - end - nspeed = norm(get_actual_tcp_speed()) - while nspeed > 0.001: - servoj(get_inverse_kin(target_pose),0,0,0.008,0.01,2000) - nspeed = norm(get_actual_tcp_speed()) - end - stopj(2) - end - if depth_compensation==False: - timeout = 0 - timeout_count=20*0.008/0.008 - while get_digital_in(9) == True: - timeout = timeout+1 - sync() - if timeout > timeout_count: - break - end - end - timeout = 0 - timeout_limit = 750000 - while get_digital_in(9) == False: - timeout = timeout+1 - sync() - if timeout > timeout_limit: - break - end - end - end - sleep(0.024) - if set_payload: - if slave: - if get_analog_in(3) < 2: - zslam=0 - else: - zslam=payload - end - else: - if get_digital_in(8) == False: - zmasm=0 - else: - zmasm=payload - end - end - zload=zmasm+zslam+zsysm - set_payload(zload,[(zsysx*zsysm+zmasx*zmasm+zslax*zslam)/zload,(zsysy*zsysm+zmasy*zmasm+zslay*zslam)/zload,(zsysz*zsysm+zmasz*zmasm+zslaz*zslam)/zload]) - end - master_grip_detected=False - master_lost_grip=False - slave_grip_detected=False - slave_lost_grip=False - if True == get_digital_in(8): - master_grip_detected=True - end - if get_analog_in(3)>2: - slave_grip_detected=True - end - grip_detected=False - lost_grip=False - if True == get_digital_in(8): - grip_detected=True - end - zscale = (get_analog_in(2)-0.026)/2.9760034 - zangle = zscale*1.57079633+-0.08726646 - zwidth = 5.0+110*sin(zangle) - global measure_width = (floor(zwidth*10))/10-9.2 - if slave: - slave_measure_width=measure_width - else: - master_measure_width=measure_width - end - return grip_detected - end + sleep(0.016) + timeout = 0 + while get_digital_in(9) == True: + timeout=timeout+1 + sleep(0.008) + if timeout > 20: + break + end + end + timeout = 0 + timeout_limit = 750000 + while get_digital_in(9) == False: + zscale = (get_analog_in(2)-0.026)/2.9760034 + zangle = zscale*1.57079633+-0.08726646 + zwidth = 5.0+110*sin(zangle) + measure_depth = cos(zangle)*finger_length + compensation_depth = (measure_depth - start_depth) + target_pose = pose_trans(start_pose,p[0,0,-compensation_depth,0,0,0]) + if timeout > timeout_limit: + break + end + timeout=timeout+1 + servoj(get_inverse_kin(target_pose),0,0,0.008,0.01,2000) + if point_dist(target_pose, get_forward_kin()) > 0.005: + popup("Lower grasping force or max width",title="RG-lag threshold exceeded", warning=False, error=False, blocking=False) + end + end + nspeed = norm(get_actual_tcp_speed()) + while nspeed > 0.001: + servoj(get_inverse_kin(target_pose),0,0,0.008,0.01,2000) + nspeed = norm(get_actual_tcp_speed()) + end + stopj(2) + end + if depth_compensation==False: + timeout = 0 + timeout_count=20*0.008/0.008 + while get_digital_in(9) == True: + timeout = timeout+1 + sync() + if timeout > timeout_count: + break + end + end + timeout = 0 + timeout_limit = 750000 + while get_digital_in(9) == False: + timeout = timeout+1 + sync() + if timeout > timeout_limit: + break + end + end + end + sleep(0.024) + if set_payload: + if slave: + if get_analog_in(3) < 2: + zslam=0 + else: + zslam=payload + end + else: + if get_digital_in(8) == False: + zmasm=0 + else: + zmasm=payload + end + end + zload=zmasm+zslam+zsysm + set_payload(zload,[(zsysx*zsysm+zmasx*zmasm+zslax*zslam)/zload,(zsysy*zsysm+zmasy*zmasm+zslay*zslam)/zload,(zsysz*zsysm+zmasz*zmasm+zslaz*zslam)/zload]) + end + master_grip_detected=False + master_lost_grip=False + slave_grip_detected=False + slave_lost_grip=False + if True == get_digital_in(8): + master_grip_detected=True + end + if get_analog_in(3)>2: + slave_grip_detected=True + end + grip_detected=False + lost_grip=False + if True == get_digital_in(8): + grip_detected=True + end + zscale = (get_analog_in(2)-0.026)/2.9760034 + zangle = zscale*1.57079633+-0.08726646 + zwidth = 5.0+110*sin(zangle) + global measure_width = (floor(zwidth*10))/10-9.2 + if slave: + slave_measure_width=measure_width + else: + master_measure_width=measure_width + end + return grip_detected + end set_tool_voltage(24) set_tcp(p[0,0,0.18659,0,-0,0]) """ @@ -260,14 +260,14 @@ def close_gripper(self, target_width=-10, target_force=40, payload=0.5, set_payl self.robot.send_program(urscript()) time.sleep(wait) - @property.getter - def width(self): - zscale = (self.robot.get_analog_in(2) - 0.026) / 2.9760034 - zangle = zscale * 1.57079633 + -0.08726646 - zwidth = 5.0 + 110 * math.sin(zangle) - measure_width = (math.floor(zwidth * 10)) / 10 - 9.2 - return measure_width + @property + def width(self): + zscale = (self.robot.get_analog_in(2) - 0.026) / 2.9760034 + zangle = zscale * 1.57079633 + -0.08726646 + zwidth = 5.0 + 110 * math.sin(zangle) + measure_width = (math.floor(zwidth * 10)) / 10 - 9.2 + return measure_width - @property.getter - def object_gripped(self): - return self.robot.get_digital_in(16) + @property + def object_gripped(self): + return self.robot.get_digital_in(16) > 0