[WIP] RG2 script

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.

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",
+]

urx/onrobot_rg2_gripper.py → 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)

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):

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])

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):
         """