diff --git a/README.md b/README.md index 985e328..805c6f9 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,3 @@ - urx is a python library to control the robots from [Universal Robots](https://www.universal-robots.com/). It is published under the LGPL license and comes with absolutely no guarantee. It is meant as an easy to use module for pick and place operations, although it has been used for welding and other sensor based applications that do not require high control frequency. @@ -93,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.gripper import Robotiq_Two_Finger_Gripper + +if __name__ == '__main__': + rob = urx.Robot("192.168.0.100") + robotiqgrip = Robotiq_Two_Finger_Gripper() + + 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() + + rob.send_program(robotiqgrip.ret_program_to_run()) + + 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.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper +from urx.gripper import OnRobotGripperRG2 if __name__ == '__main__': - rob = urx.Robot("192.168.0.100") - robotiqgrip = Robotiq_Two_Finger_Gripper() + rob = urx.Robot("192.168.0.100") + gripper = OnRobotGripperRG2() - 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") : + gripper.close_gripper() + if(sys.argv[1] == "open") : + gripper.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() ``` + +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..ea67716 100644 --- a/urx/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 @@ -152,17 +153,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) @@ -270,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/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 diff --git a/urx/urrobot.py b/urx/urrobot.py index 84e078a..f955c44 100644 --- a/urx/urrobot.py +++ b/urx/urrobot.py @@ -5,6 +5,8 @@ """ import logging +import numbers +import collections from urx import urrtmon from urx import ursecmon @@ -351,19 +353,30 @@ 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 movejs(self, joint_positions_list, acc=0.01, vel=0.01, radius=0.01, + wait=True, threshold=None): + """ + Concatenate several movej commands and applies a blending radius + joint_positions_list is a list of joint_positions. + This method is usefull since any new command from python + to robot make the robot stop + """ + return URRobot.movexs(self, "movej", joint_positions_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): + 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 pose_list is a list of pose. This method is usefull since any new command from python to robot make the robot stop """ - return self.movexs("movel", pose_list, acc, vel, radius, wait, threshold=threshold) + return self.movexs("movel", pose_list, acc, vel, radius, + wait, threshold=threshold) - def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, threshold=None): + def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, + wait=True, threshold=None): """ Concatenate several movex commands and applies a blending radius pose_list is a list of pose. @@ -373,16 +386,46 @@ def movexs(self, command, pose_list, acc=0.01, vel=0.01, radius=0.01, wait=True, header = "def myProg():\n" end = "end\n" prog = header + # Check if 'vel' is a single number or a sequence. + if isinstance(vel, numbers.Number): + # Make 'vel' a sequence + vel = len(pose_list) * [vel] + elif not isinstance(vel, collections.Sequence): + raise RobotException( + 'movexs: "vel" must be a single number or a sequence!') + # Check for adequate number of speeds + if len(vel) != len(pose_list): + raise RobotException( + 'movexs: "vel" must be a number or a list ' + + 'of numbers the same length as "pose_list"!') + # Check if 'radius' is a single number. + if isinstance(radius, numbers.Number): + # Make 'radius' a sequence + radius = len(pose_list) * [radius] + elif not isinstance(radius, collections.Sequence): + raise RobotException( + 'movexs: "radius" must be a single number or a sequence!') + # Ensure that last pose a stopping pose. + radius[-1] = 0.0 + # Require adequate number of radii. + if len(radius) != len(pose_list): + raise RobotException( + 'movexs: "radius" must be a number or a list ' + + 'of numbers the same length as "pose_list"!') + prefix = '' + if command in ['movel', 'movec']: + prefix = 'p' for idx, pose in enumerate(pose_list): - if idx == (len(pose_list) - 1): - radius = 0 - prog += self._format_move(command, pose, acc, vel, radius, prefix="p") + "\n" + prog += self._format_move(command, pose, acc, + vel[idx], radius[idx], + prefix=prefix) + "\n" prog += end self.send_program(prog) if wait: - self._wait_for_move(target=pose_list[-1], threshold=threshold) - if command == "movej": - return self.getj() + 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) return self.getl() def stopl(self, acc=0.5): diff --git a/urx/urrtmon.py b/urx/urrtmon.py index 3ff825b..9c08133 100644 --- a/urx/urrtmon.py +++ b/urx/urrtmon.py @@ -88,6 +88,16 @@ def q_actual(self, wait=False, timestamp=False): return self._qActual getActual = q_actual + def qd_actual(self, wait=False, timestamp=False): + """ Get the actual joint velocity vector.""" + if wait: + self.wait() + with self._dataAccess: + if timestamp: + return self._timestamp, self._qdActual + else: + return self._qdActual + def q_target(self, wait=False, timestamp=False): """ Get the target joint position vector.""" if wait: @@ -164,6 +174,7 @@ def __recv_rt_data(self): self._ctrlTimestamp - self._last_ctrl_ts) self._last_ctrl_ts = self._ctrlTimestamp self._qActual = np.array(unp[31:37]) + self._qdActual = np.array(unp[37:43]) self._qTarget = np.array(unp[1:7]) self._tcp_force = np.array(unp[67:73]) self._tcp = np.array(unp[73:79]) 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): """