script/calibration/ScanXZ.py

@@ -3,14 +3,19 @@ from mathutils import fit_gaussian, Gaussian
 
 SINGLE_PASS = False
 if SINGLE_PASS:
-    STEP_SIZE = 0.5
+    STEP_SIZE = 0.2
 else:
     STEP_SIZE = 1.0
+STEP_Z_FINAL = 0.1
+    
 RANGE = [-5.0, 5.0]
 LATENCY = 0.005
 
+Z_FINAL_OFFSET = 0.0
+
+
 robot.enable()
-move_to_laser()
+#move_to_laser()
 
 step_x = STEP_SIZE
 step_z = STEP_SIZE
@@ -22,7 +27,7 @@ current_x = robot_x.getPosition()
 current_z = robot_z.getPosition()
 
 print "Current pos x,z" , current_x, ",", current_z
-ret = ascan([robot_x, robot_z], ue.readable, [range_x[0], range_z[0]], [range_x[1], range_z[1]], [step_x,step_z], latency = LATENCY, relative = True , zigzag=False)
+ret = ascan([robot_x, robot_z], ue.readable, [range_x[0], range_z[0]], [range_x[1], range_z[1]], [step_x,step_z], latency = LATENCY, relative = True , zigzag=False, title = "2d Scan")
 data = ret.getData(0)[0]
 #plot(Convert.transpose(data), title="Data")
 
@@ -35,8 +40,12 @@ p = plot(integ, title = "Fit", xdata=xdata)[0]
 
 max_x_index = integ.index(max(integ))
 max_x = xdata[max_x_index]
-(normalization, mean_val, sigma) = fit_gaussian(integ, xdata)
+try:
+    (normalization, mean_val, sigma) = fit_gaussian(integ, xdata)
+except:
+    raise Exception("Invalid Fit")    
 gaussian = Gaussian(normalization, mean_val, sigma)
+xdata= frange(range_x[0], range_x[1], step_x/100.0 , False, True)
 plot_function(p, gaussian, "Fit", xdata, show_points = False, show_lines = True, color = Color.BLUE)
 
 #So 
@@ -47,12 +56,11 @@ center_x = current_x + x_offset
 
 print "X offset = ", x_offset
 
-
+robot_x.move(center_x)
 if SINGLE_PASS:
     z_scan_data = data[max_x_index]        
-else:
-    robot_x.move(center_x)
-    step_z = step_z/4.0
+else:    
+    step_z = STEP_Z_FINAL
     ret2 = lscan(robot_z, ue.readable, range_z[0], range_z[1], step_z, latency = LATENCY, relative = True , zigzag=False)
     z_scan_data = ret2.getData(0)[0]
     
@@ -70,7 +78,9 @@ if SINGLE_PASS:
 else:    
     max_z = ret2.getPositions(0)[last_z_index]    
 
-z_offset = max_z-current_z
+z_offset = max_z - current_z + Z_FINAL_OFFSET
+
+robot_z.move(max_z + Z_FINAL_OFFSET)
 
 print "Z offset = ", z_offset
 

script/calibration/ToolCalibration.py

@@ -2,41 +2,59 @@ import plotutils
 from mathutils import fit_gaussian, Gaussian
 
 
-robot.enable()
-move_to_laser()
+robot.assert_tool(TOOL_CALIBRATION)
 robot.set_motors_enabled(True)
+robot.set_joint_motors_enabled(True)
 
 
 initial_pos = robot.get_cartesian_pos()
 
+robot.enable()
+move_to_laser()
+
+robot.align()  
 
 
 run("calibration/ScanXZ")
 
 robot.set_motors_enabled(True)
-robot_x.moveRel(x_offset)
-robot_z.moveRel(z_offset)
 
 
-initial_x = robot_x.take()
-initial_z = robot_z.take()
-initial_y = ue.take()
-if initial_y is None:
-    raise Exception("Invalid XZ scan values")
+first_x = robot_x.take()
+first_z = robot_z.take()
+first_y = ue.take()
+first_j6 = robot_j6.take()
+if first_y is None:
+    raise Exception("Invalid XZ scan values in first scan")
+
+
+robot.set_joint_motors_enabled(True)
+if first_j6>0:
+    robot_j6.moveRel(-180.0, -1)
+else:
+    robot_j6.moveRel(180.0, -1)    
+
+    
+robot.set_motors_enabled(True)
+run("calibration/ScanXZ")
+
+robot.set_motors_enabled(True)
+
+second_x = robot_x.take()
+second_z = robot_z.take()
+second_y = ue.take()
+second_j6 = robot_j6.take()
+if second_y is None:
+    raise Exception("Invalid XZ scan values in first scan")
+
+
+#Updates the tool
+xoff = (first_x - second_x)/2
+yoff = (first_y - second_y)/2
+robot.get_tool_trsf(robot.tool)
+t[0]=xoff     #TODO: Why signal?
+t[1]=-yoff    #TODO: Why signal?
+robot.set_tool_trsf(t, robot.tool)
 
 
 
-
-
-
-#update_tool(None, x_offset=x_offset, z_offset=z_offset)
-
-current_x = robot_x.getPosition()
-current_y = robot_x.getPosition()
-current_z = robot_z.getPosition()
-
-
-
-#Updating tool:
-#update_tool(None, x_offset=x_offset, z_offset=z_offset)
-

script/devices/RobotMotors.py

@@ -14,7 +14,6 @@ class RobotCartesianMotor (PositionerBase):
         
     #ATTENTION: Always initialize cartesian motors before scanning (or call robot.set_motors_enabled(True))
     def doInitialize(self):
-        self.setCache(None, None)
         self.setCache(self.doRead(), None)
                   
     def doRead(self):
@@ -23,16 +22,11 @@ class RobotCartesianMotor (PositionerBase):
     def doWrite(self, value):        
         if self.robot.cartesian_destination is not None:
             print "Move " + ROBOT_MOTORS[self.index] + " to " + str(value)
-            self.robot.updating = True
-            try:
-                robot.cartesian_destination[self.index] = float(value)  
-                self.robot.set_pnt(robot.cartesian_destination , "tcp_p")        
-                self.robot.movel("tcp_p", self.robot.tool , DESC_SCAN)        
-            except:
-                self.robot.updating = False
+            robot.cartesian_destination[self.index] = float(value)  
+            self.robot.set_pnt(robot.cartesian_destination , "tcp_p")        
+            self.robot.movel("tcp_p", self.robot.tool , DESC_SCAN)        
         
     def doReadReadback(self):
-        #return float(self.robot.get_cartesian_pos(self._get_tool(),self.frame)[self.index])
         return float("nan") if self.robot.cartesian_pos is None else float(self.robot.cartesian_pos[self.index])
 
 
@@ -59,13 +53,8 @@ class RobotJointMotor (PositionerBase):
         self.setpoint = value
         joint = self.robot.herej()
         joint[self.index] = value
-        self.robot.updating = True
-        try:
-            self.robot.set_jnt(joint, name="tcp_j")
-            self.robot.movej("tcp_j", self.robot.tool , DESC_SCAN)       
-        finally:
-            self.robot.updating = False
-
+        self.robot.set_jnt(joint, name="tcp_j")
+        self.robot.movej("tcp_j", self.robot.tool , DESC_SCAN)       
     
     def doReadReadback(self):
         return self.robot.herej()[self.index] if self.robot.joint_pos is None else float(self.robot.joint_pos[self.index])

script/devices/RobotSC.py

@@ -11,14 +11,13 @@ DESC_DEFAULT = DESC_FAST
 run("devices/RobotTCP")
 
 
-simulation = False
+simulation = True
 joint_forces = False
 
 
 class RobotSC(RobotTCP):    
     def __init__(self, name, server, timeout = 1000, retries = 1):
-        RobotTCP.__init__(self, name, server, timeout, retries) 
-        self.tool = TOOL_CALIBRATION
+        RobotTCP.__init__(self, name, server, timeout, retries)         
         
     def mount(self, puck, sample):
         return self.execute('mount',segment, puck, sample)   
@@ -58,14 +57,7 @@ class RobotSC(RobotTCP):
                 
     def set_local(self):
         robot.set_profile("default")
-            
-    def set_tool(self,tool):
-        if tool != self.tool:
-            self.tool  = tool
-            for dev in (robot_x, robot_y, robot_z, robot_rx, robot_ry, robot_rz):
-                dev.initialize()
-                dev.update()
-    
+                
         
 
 if simulation:
@@ -75,7 +67,9 @@ else:
     add_device(RobotSC("robot", "129.129.110.100:1000"), force = True) 
 
 robot.high_level_tasks = ["mount", "firstmount"]
+robot.set_tool(TOOL_CALIBRATION)
 robot.setPolling(500)
+
 #robot.set_monitor_speed(20)
 
 

script/devices/RobotTCP.py

@@ -1,6 +1,7 @@
 import threading
 
 FRAME_DEFAULT = "world"
+FLANGE = "flange"
 
 
 run("devices/RobotMotors")
@@ -27,15 +28,16 @@ class RobotTCP(TcpDevice, Stoppable):
          self.current_task = None
          self.high_level_tasks = []         
          self.cartesian_destination = None  
-         self.cartesian_pos = None  
-         self.joint_pos = None  
+         #self.flange_pos = [None] * 6
+         self.cartesian_pos = [None] * 6
+         self.joint_pos = [None] * 6
          self.cartesian_motors_enabled = False
          self.cartesian_motors = []
          self.joint_motors_enabled = False
          self.joint_motors = []
          self.tool = None
+         #self.tool_trsf = [0.0] * 6
          self.frame = FRAME_DEFAULT
-         self.updating = False
                 
 
     def doInitialize(self):           
@@ -45,8 +47,9 @@ class RobotTCP(TcpDevice, Stoppable):
 
     def set_tool(self,tool):
         self.tool = tool
-        if self.cartesian_motors_enabled:
-            self.cartesian_pos = self.get_cartesian_pos()                
+        #self.tool_trsf = self.get_tool_trsf()
+        if self.cartesian_motors_enabled:            
+            self.update()               
             for m in self.cartesian_motors:
                 m.initialize()
                 m.update()                
@@ -54,7 +57,10 @@ class RobotTCP(TcpDevice, Stoppable):
     
     def get_tool(self):    
         return self.tool
-        
+
+    def assert_tool(self, tool):
+        if self.tool != tool:
+            raise Exception("Invalid tool: " + self.tool)        
          
     def _sendReceive(self, msg_id, msg = "", timeout =  None):  
         tx = self.header if (self.header != None) else  ""
@@ -313,25 +319,44 @@ class RobotTCP(TcpDevice, Stoppable):
             self.joint_forces = None
             raise
 
-    def movej(self, joint_or_point, tool, desc):
-        return self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")")
+    def movej(self, joint_or_point, tool, desc, sync=False):
+        ret = self.eval_int("movej(" + joint_or_point + ", " + tool + ", " + desc +")")
+        if sync:
+            self.wait_end_of_move()        
+        return ret
         
-    def movel(self, point, tool, desc):
-        return self.eval_int("movel(" + point + ", " + tool + ", " + desc +")")
+    def movel(self, point, tool, desc, sync=False):
+        ret = self.eval_int("movel(" + point + ", " + tool + ", " + desc +")")
+        if sync:
+            self.wait_end_of_move()        
+        return ret
 
-    def movec(self, point_interm, point_target, tool, desc):
-        return self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")")
+    def movec(self, point_interm, point_target, tool, desc, sync=False):
+        ret = self.eval_int("movec(" + point_interm + ", " + point_target + ", " + tool + ", " + desc +")")
+        if sync:
+            self.wait_end_of_move()        
+        return ret
 
+    def wait_end_of_move(self):
+        time.sleep(0.05)
+        self.update()  
+        #time.sleep(0.01)
+        #robot.waitCacheChange(-1)              
+        self.waitReady(-1)
+        #robot.waitState(State.Ready, -1)
+            
 
     #Tool
     #This function can timeout since it synchronizes move. 
     #Better check state before otherwise it can freeze the communication
-    def open_tool(self, tool):
+    def open_tool(self, tool=None):
+        if tool is None: tool = self.tool
         return self.evaluate("open(" + tool + " )", timeout=3000)
 
     #This function can timeout since it synchronizes move. Better check state before
     #Better check state before otherwise it can freeze the communication
-    def close_tool(self, tool):
+    def close_tool(self, tool=None):
+        if tool is None: tool = self.tool
         return self.evaluate("close(" + tool + " )", timeout=3000)
 
     #Arm position
@@ -424,19 +449,30 @@ class RobotTCP(TcpDevice, Stoppable):
         try:
             start = time.time()
             #sts = self._sendReceive("EVT").strip().split(self.array_separator)        
-            sts = self.execute("get_status", self.current_task)
+            sts = self.execute("get_status", self.current_task, self.tool, self.frame)
             self._update_working_mode(int(sts[0]), int(sts[1]))
             self.powered = sts[2] == "1"
             self.speed = int(sts[3])
             self.empty = sts[4] == "1"
             self.settled = sts[5] == "1"                                    
-            ev = sts[7] if len(sts)>7 else ""            
+            if int(sts[6]) < 0:
+                self.current_task = None
+
+            for i in range(6):
+                self.joint_pos[i] = float(sts[7 + i])
+            for i in range(6):
+                #self.flange_pos[i] = float(sts[13 + i])
+                #self.tool_pos[i] =  (self.flange_pos[i] - self.tool_trsf[i] ) if i<3 else (self.flange_pos[i] + self.tool_trsf[i] )
+                self.cartesian_pos[i] = float(sts[13 + i])
+            
+            ev_index = 19 #7
+            ev = sts[ev_index] if len(sts)>ev_index else ""            
             if len(ev.strip()) >0:
                 self.getLogger().info(ev)
                 self.on_event(ev) 
+
+                
             #if (self.current_task is not None) and (self.get_task_status(self.current_task)[0]<0):
-            if int(sts[6]) < 0:
-                self.current_task = None
             self._update_state()              
             self.reset = False
             self.setCache({"powered": self.powered,
@@ -447,24 +483,22 @@ class RobotTCP(TcpDevice, Stoppable):
                            "mode": self.working_mode,
                            "status": self.status
                            }, None)
-            if not self.updating:
-                if self.cartesian_motors_enabled:
-                    self.cartesian_pos = self.get_cartesian_pos()                
-                    for m in self.cartesian_motors:
-                        #m.update()
-                        m.readback.update()
-                else:
-                    self.cartesian_pos = None                
-                if self.joint_motors_enabled:
-                    self.joint_pos = self.herej()
-                    for m in self.joint_motors:
-                        m.readback.update()
+            if self.cartesian_motors_enabled:
+                for m in self.cartesian_motors:
+                    m.readback.update()
+
+            if self.joint_motors_enabled:
+                for m in self.joint_motors:
+                    m.readback.update()
+                    
         except:
             if self.state != State.Offline:
                 print >> sys.stderr, "Update error: " + str(sys.exc_info()[1])
-            self.setState(State.Offline)            
+                self.setState(State.Offline)            
 
     #Cartesian space
+    def get_flange_pos(self):            
+        return self.eval_pnt("jointToPoint(" +FLANGE + ", " + self.frame + ", herej())")
 
     def get_cartesian_pos(self):            
         return self.eval_pnt("jointToPoint(" + self.tool + ", " + self.frame + ", herej())")
@@ -512,14 +546,23 @@ class RobotTCP(TcpDevice, Stoppable):
                 for m in self.joint_motors:
                     remove_device(m)
                 self.joint_motors = []            
-                self.joint_destination = None
         else:
             if value:
-                self.joint_destination = self.get_joint_destination()
                 for m in self.joint_motors:
                     m.initialize()                
+    #Alignment
+    def align(self, point = None, frame = FRAME_DEFAULT, desc = DESC_FAST):
+        if point is None:
+            self.get_cartesian_pos()
+        else:
+            self.set_pnt(point)        
+        np=self.eval_trf('align(tcp_p.trsf, ' + FRAME_DEFAULT + '.trsf)')
+        self.set_pnt(np)
+        self.movej("tcp_p", self.tool, desc)
+        #TODO: The first command is not executed (but receive a move id)
+        self.movej("tcp_p", self.tool, desc, sync = True)        
+        return np
         
-
     #High-level, exclusive motion task.
     def start_task(self, program, *args,  **kwargs):
         tasks = [t for t in self.high_level_tasks]

script/motion/tools.py

@@ -19,8 +19,7 @@ def move_to_laser():
     if d<POSITION_TOLERANCE:
         print "FROM LASER"
         robot.reset_motion()
-        robot.movel("pLaser", robot.tool, DESC_SCAN)
-        wait_end_of_move()
+        robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
         return
     d = robot.get_distance_to_pnt("pLaserAppro")
     if d<0:
@@ -28,8 +27,7 @@ def move_to_laser():
     if d<POSITION_TOLERANCE:
         print "FROM APPRO"
         robot.reset_motion()
-        robot.movel("pLaser", robot.tool, DESC_SCAN)
-        wait_end_of_move()
+        robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)
         return
     d = robot.get_distance_to_pnt("pHome")
     if d<0:
@@ -38,8 +36,7 @@ def move_to_laser():
         print "FROM HOME"
         robot.reset_motion()
         robot.movel("pLaserAppro", robot.tool, DESC_DEFAULT)
-        robot.movel("pLaser", robot.tool, DESC_SCAN)    
-        wait_end_of_move()
+        robot.movel("pLaser", robot.tool, DESC_SCAN, sync = True)    
         return
     raise Exception ("Must be in home position to start move to laser")
     

script/test/TestAlign.py

@@ -0,0 +1,9 @@
+
+v = 2.0
+#while(true):
+for  i in range(100):
+    v = v * (-1.0)
+    robot_j4.initialize()
+    robot_j4.moveRel(v)    
+    robot.align()  
+