script/calibration/ScanRZ.py

@@ -0,0 +1,21 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+RANGE = [-20.0,20.0]
+STEP = 1.0
+LATENCY = 0.005
+
+robot.enable()
+move_to_laser()
+
+
+
+
+
+
+robot.set_motors_enabled(True)
+ret = lscan(robot_rz, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True, range = "auto")
+
+
+
+

script/calibration/ScanX.py

@@ -0,0 +1,37 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+robot.enable()
+move_to_laser()
+
+
+RANGE = [-1.5, 1.5]
+STEP = 0.025
+Z_OFFSET = -1.0
+LATENCY = 0.005
+
+robot.set_motors_enabled(True)
+current_positon = robot_x.getPosition()
+robot_z.moveRel(Z_OFFSET)
+ret = lscan(robot_x, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True)
+
+d = ret.getReadable(0)
+
+first_index = -1
+last_index = -1
+for i in range(len(d)):
+    if not math.isnan(d[i]):
+        if first_index<0:
+            first_index = i  
+        last_index = i  
+
+if first_index == -1 or last_index < first_index:
+    raise Exception("Invalid X detection")
+
+
+center_index = (first_index + last_index)/2
+center_positon = ret.getPositions(0)[center_index]
+center_offset = center_positon-current_positon
+
+
+print "X offset = ", center_offset

script/calibration/ScanXZ.py

@@ -0,0 +1,79 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+SINGLE_PASS = False
+if SINGLE_PASS:
+    STEP_SIZE = 0.5
+else:
+    STEP_SIZE = 1.0
+RANGE = [-5.0, 5.0]
+LATENCY = 0.005
+
+robot.enable()
+move_to_laser()
+
+step_x = STEP_SIZE
+step_z = STEP_SIZE
+range_x = [RANGE[0], RANGE[1]]
+range_z = [RANGE[0], RANGE[1]]
+
+robot.set_motors_enabled(True)
+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)
+data = ret.getData(0)[0]
+#plot(Convert.transpose(data), title="Data")
+
+integ = []
+for x in data: integ.append(sum(   [ (0.0 if (math.isnan(y)) else y)  for y  in x]))
+
+xdata= frange(range_x[0], range_x[1], step_x , False, True)
+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)
+gaussian = Gaussian(normalization, mean_val, sigma)
+plot_function(p, gaussian, "Fit", xdata, show_points = False, show_lines = True, color = Color.BLUE)
+
+#So 
+if abs(mean_val - max_x) > 1.0:
+    raise Exception("Invalid X detection")
+x_offset = mean_val    
+center_x = current_x + x_offset
+
+print "X offset = ", x_offset
+
+
+if SINGLE_PASS:
+    z_scan_data = data[max_x_index]        
+else:
+    robot_x.move(center_x)
+    step_z = step_z/4.0
+    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]
+    
+max_z_index= z_scan_data.index(max(z_scan_data))    
+last_z_index = 0
+for i in range(len(z_scan_data)):
+    if not math.isnan(z_scan_data[i]):
+        last_z_index = i    
+#Shape is cone: z is inceraseing. For proper detection last Z must be furthest    
+if abs(max_z_index - last_z_index) * step_z > 1.0: 
+    raise Exception("Invalid Z detection")
+
+if SINGLE_PASS:
+    max_z = ret.getPositions(1)[len(data[0]) * max_x_index + last_z_index]    
+else:    
+    max_z = ret2.getPositions(0)[last_z_index]    
+
+z_offset = max_z-current_z
+
+print "Z offset = ", z_offset
+
+
+#Updating tool:
+#update_tool(None, x_offset=x_offset, z_offset=z_offset)

script/calibration/ToolCalibration.py

@@ -0,0 +1,42 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+
+robot.enable()
+move_to_laser()
+robot.set_motors_enabled(True)
+
+
+initial_pos = robot.get_cartesian_pos()
+
+
+
+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")
+
+
+
+
+
+
+#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

@@ -0,0 +1,73 @@
+
+import ch.psi.pshell.device.PositionerConfig as PositionerConfig
+
+
+
+ROBOT_MOTORS = ["x" , "y", "z", "rx", "ry", "rz"]
+
+
+class RobotCartesianMotor (PositionerBase):
+    def __init__(self, robot, index):
+         PositionerBase.__init__(self, robot.name + "_" + ROBOT_MOTORS[index], PositionerConfig())
+         self.index = index
+         self.robot = robot
+        
+    #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):
+        return float("nan") if self.robot.cartesian_destination is None else float(robot.cartesian_destination[self.index])
+
+    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
+        
+    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])
+
+
+
+
+ROBOT_JOINT_MOTORS = ["j1" , "j2", "j3", "j4", "j5", "j6"]
+
+
+class RobotJointMotor (PositionerBase):
+    def __init__(self, robot, index):
+         PositionerBase.__init__(self, robot.name + "_" + ROBOT_JOINT_MOTORS[index], PositionerConfig())
+         self.index = index
+         self.robot = robot         
+            
+    def doInitialize(self):        
+        self.setpoint = self.doReadReadback()
+        self.setCache(self.doRead(), None)        
+                  
+    def doRead(self):        
+        return self.setpoint
+
+    def doWrite(self, value):                
+        print "Move " + ROBOT_JOINT_MOTORS[self.index] + " to " + str(value)
+        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
+
+    
+    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

@@ -1,9 +1,25 @@
+TOOL_CALIBRATION = "tCalib"
+TOOL_SUNA= "tSuna"
+TOOL_DEFAULT= TOOL_SUNA
+
+DESC_FAST = "mFast"
+DESC_SLOW = "mSlow"
+DESC_SCAN = "mScan"
+DESC_DEFAULT = DESC_FAST
+
+
 run("devices/RobotTCP")
 
-simulation = True
+
+simulation = False
+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
+        
     def mount(self, puck, sample):
         return self.execute('mount',segment, puck, sample)   
 
@@ -22,10 +38,11 @@ class RobotSC(RobotTCP):
         RobotTCP.doUpdate(self)
         global simulation
         if not simulation:
-            if self.state != State.Offline:
-                self.get_joint_forces()
-                for dev in [jf1, jf2, jf3, jf4,jf5, jf6, jfc]: 
-                    dev.update()        
+            if joint_forces:
+                if self.state != State.Offline:
+                    self.get_joint_forces()
+                    for dev in [jf1, jf2, jf3, jf4,jf5, jf6, jfc]: 
+                        dev.update()        
         #print time.time() -start
 
     def start_task(self, program, *args,  **kwargs):
@@ -35,20 +52,30 @@ class RobotSC(RobotTCP):
     def stop_task(self):
         RobotTCP.stop_task(self)
         #TODO: Restore safe position            
+
+    def set_remote_mode(self):
+        robot.set_profile("remote")
                 
+    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:
-    add_device(RobotSC("robot","129.129.126.92:1000"),force = True)
-#add_device(RobotSC("robot","129.129.126.74:1000"),force = True)
+    #add_device(RobotSC("robot","129.129.126.92:1000"),force = True)
+    add_device(RobotSC("robot","129.129.110.99:1000"),force = True)
 else:
-    add_device(RobotSC("robot", "129.129.126.100:1000"), force = True) 
+    add_device(RobotSC("robot", "129.129.110.100:1000"), force = True) 
 
 robot.high_level_tasks = ["mount", "firstmount"]
-robot.setPolling(20)
+robot.setPolling(500)
 #robot.set_monitor_speed(20)
 
 
@@ -79,10 +106,15 @@ class jfc(ReadonlyRegisterBase):
             return  float('NaN') 
         return (robot.joint_forces[1]+74)/4  + (robot.joint_forces[2]+30)/4 + (robot.joint_forces[4]-0.8)/0.2
 
-add_device(jf1(), force = True)   
-add_device(jf2(), force = True)   
-add_device(jf3(), force = True)   
-add_device(jf4(), force = True)   
-add_device(jf5(), force = True)   
-add_device(jf6(), force = True)   
-add_device(jfc(), force = True)   
+if joint_forces:
+    add_device(jf1(), force = True)   
+    add_device(jf2(), force = True)   
+    add_device(jf3(), force = True)   
+    add_device(jf4(), force = True)   
+    add_device(jf5(), force = True)   
+    add_device(jf6(), force = True)   
+    add_device(jfc(), force = True)   
+
+
+    
+"""

script/devices/RobotTCP.py

@@ -1,5 +1,10 @@
 import threading
 
+FRAME_DEFAULT = "world"
+
+
+run("devices/RobotMotors")
+
 class RobotTCP(TcpDevice, Stoppable):    
     def __init__(self, name, server, timeout = 1000, retries = 1):
          TcpDevice.__init__(self, name, server) 
@@ -20,11 +25,36 @@ class RobotTCP(TcpDevice, Stoppable):
          self.lock = threading.Lock()
          self.joint_forces = None
          self.current_task = None
-         self.high_level_tasks = []                   
+         self.high_level_tasks = []         
+         self.cartesian_destination = None  
+         self.cartesian_pos = None  
+         self.joint_pos = None  
+         self.cartesian_motors_enabled = False
+         self.cartesian_motors = []
+         self.joint_motors_enabled = False
+         self.joint_motors = []
+         self.tool = None
+         self.frame = FRAME_DEFAULT
+         self.updating = False
+                
 
     def doInitialize(self):           
         super(RobotTCP, self).doInitialize()
         self.reset = True      
+
+
+    def set_tool(self,tool):
+        self.tool = tool
+        if self.cartesian_motors_enabled:
+            self.cartesian_pos = self.get_cartesian_pos()                
+            for m in self.cartesian_motors:
+                m.initialize()
+                m.update()                
+    
+    
+    def get_tool(self):    
+        return self.tool
+        
          
     def _sendReceive(self, msg_id, msg = "", timeout =  None):  
         tx = self.header if (self.header != None) else  ""
@@ -106,13 +136,13 @@ class RobotTCP(TcpDevice, Stoppable):
         for i in range(size): ret.append(float(a[i])); 
         return ret        
     
-    def get_trf(self, name="tcp_t"):
+    def get_trsf(self, name="tcp_t"):
         a = self.execute('get_trf', name)
         ret = []
         for i in range(6): ret.append(float(a[i]))
         return ret        
 
-    def set_tr(self, l, name="tcp_t"):             
+    def set_trsf(self, l, name="tcp_t"):             
         self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}")
 
     def get_jnt(self, name="tcp_j"):
@@ -125,10 +155,26 @@ class RobotTCP(TcpDevice, Stoppable):
         self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}")
 
     def get_pnt(self, name="tcp_p"):
-        a = self.execute('get_pnt', name)
-        ret = []
-        for i in range(6): ret.append(float(a[i]))
-        return ret        
+        #a = self.execute('get_pnt', name)
+        #ret = []
+        #for i in range(6): ret.append(float(a[i]))
+        #return ret        
+        return self.get_trsf(name+".trsf")
+        
+    #trsf = (x,y,z,rx,ry,rz)
+    #TODO: config = (shoulder, elbow, wrist)
+    def set_pnt(self, trsf, name="tcp_p", config=None):
+        self.set_trsf(trsf, name+".trsf")       
+
+    def get_tool_trsf(self, name=None):
+        if name is None: 
+            name = self.tool
+        return self.get_trsf(name+".trsf")
+
+    def set_tool_trsf(self, trsf, name=None):
+        if name is None: 
+            name = self.tool
+        self.set_trsf(trsf, name+".trsf")       
 
     def eval_int(self, cmd):
         self.evaluate("tcp_n=" + cmd)
@@ -152,7 +198,7 @@ class RobotTCP(TcpDevice, Stoppable):
 
     def eval_trf(self, cmd):
         self.evaluate("tcp_t=" + cmd)
-        return self.get_trf()
+        return self.get_trsf()
 
     def eval_pnt(self, cmd):
         self.evaluate("tcp_p=" + cmd)
@@ -165,16 +211,19 @@ class RobotTCP(TcpDevice, Stoppable):
         return self.powered
 
     def enable(self):
-        self.evaluate("enablePower()")
         if not self.is_powered():
-            raise Exception("Cannot enable power")
-
+            self.evaluate("enablePower()")
+            time.sleep(1.0)
+            if not self.is_powered():
+                raise Exception("Cannot enable power")
+    
     #waits for power to be actually cut off
     def disable(self):
         self.evaluate("disablePower()", timeout=5000)
 
     def get_monitor_speed(self):
         self.speed =  self.eval_int("getMonitorSpeed()")
+        return self.speed
 
     def set_monitor_speed(self, speed):
         ret = self.eval_int("setMonitorSpeed(" + str(speed) + ")")
@@ -185,6 +234,10 @@ class RobotTCP(TcpDevice, Stoppable):
     def is_calibrated(self):
         return self.eval_bool("isCalibrated()")
 
+    def save_program(self):
+        ret = self.execute('save',  timeout=5000)        
+        if str(ret) != "0": raise Exception("Error saving program: " + str(ret))
+            
     def _update_working_mode(self, mode, status):
         if mode==1:                 
             self.working_mode = "manual"
@@ -205,7 +258,7 @@ class RobotTCP(TcpDevice, Stoppable):
     def read_working_mode(self):
         try:
             mode =  self.eval_int("workingMode(tcp_a)")          
-            status = int(self.get_var("tcp_a[0]"))            
+            status = int(self.get_var("tcp_a[0]"))                        
             self._update_working_mode(mode, status)
             self._update_state()    
         except:
@@ -232,8 +285,8 @@ class RobotTCP(TcpDevice, Stoppable):
     def resume(self):
         self.evaluate("restartMove()")
 
-    def resetMotion(self):
-        self.evaluate("resetMotion()")
+    def reset_motion(self, joint=None):
+        self.evaluate("resetMotion()" if (joint is None) else ("resetMotion(" + joint + ")"))
 
     def is_empty(self):
         self.empty = self.eval_bool("isEmpty()")
@@ -243,7 +296,7 @@ class RobotTCP(TcpDevice, Stoppable):
     def is_settled(self):
         self.settled = self.eval_bool("isSettled()")
         self._update_state()    
-        return self.powered
+        return self.settled
         
     def get_move_id(self):
         return self.eval_int("getMoveId()")
@@ -289,7 +342,7 @@ class RobotTCP(TcpDevice, Stoppable):
         return self.eval_float("distance(" + trsf1 + ", " + trsf2 + ")")
     
     def distance_p(self, pnt1, pnt2):    
-        return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")")
+        return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")")   
 
     def compose(self, pnt, frame, trsf):    
         return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")")
@@ -297,16 +350,24 @@ class RobotTCP(TcpDevice, Stoppable):
     def here(self, tool, frame):
         return self.eval_pnt("here(" + tool + ", " + frame + ")")
 
-    def joint_to_point(self, tool, frame, joint):
+    def joint_to_point(self, tool, frame, joint="tcp_j"):
         return self.eval_pnt("jointToPoint(" + tool + ", " + frame + ", " + joint +")")
 
-    def point_to_joint(self, tool, initial_joint, point):
+    def point_to_joint(self, tool, initial_joint="tcp_j", point="tcp_p"):
         if self.eval_bool("pointToJoint(" + tool + ", " + initial_joint + ", " + point +", j)"):
             return self.get_jnt()
 
     def position(self, point, frame):
         return self.eval_trf("position(" + point + ", " + frame + ")")
     
+    #Profile
+    def get_profile(self):
+        return self.execute('get_profile', timeout=2000)
+
+    def set_profile(self, name, pwd = ""):
+        self.execute('set_profile', str(name), str(pwd), timeout=5000)
+    
+    
     #Task control
     def task_create(self, program, *args,  **kwargs):        
         program = str(program)
@@ -332,7 +393,8 @@ class RobotTCP(TcpDevice, Stoppable):
 
     #waits for the task to be actually killed
     def task_kill(self, name):
-        self.evaluate('taskKill("' + str(name)+ '")', timeout = 2000)
+        #self.evaluate('taskKill("' + str(name)+ '")', timeout = 5000)
+        self.execute('kill', str(name), timeout=5000)
 
     def get_task_status(self, name):
         code = self.eval_int('taskStatus("' + str(name)+ '")')
@@ -381,14 +443,83 @@ class RobotTCP(TcpDevice, Stoppable):
                            "speed": self.speed,
                            "empty": self.empty,
                            "settled": self.settled,
-                           "task": self.current_task }, None)
-
-            #print time.time() - start                        
+                           "task": self.current_task,
+                           "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()
         except:
             if self.state != State.Offline:
                 print >> sys.stderr, "Update error: " + str(sys.exc_info()[1])
             self.setState(State.Offline)            
 
+    #Cartesian space
+
+    def get_cartesian_pos(self):            
+        return self.eval_pnt("jointToPoint(" + self.tool + ", " + self.frame + ", herej())")
+        #self.set_jnt(self.herej())
+        #return self.joint_to_point(tool, frame)        
+
+    def get_cartesian_destination(self):
+         return self.here(self.tool, self.frame)
+
+    def get_distance_to_pnt(self, name):   
+        #self.here(self.tool, self.frame) #???
+        self.get_cartesian_pos() 
+        return  self.distance_p("tcp_p", name)
+
+    #Cartesian peudo-motors
+    def set_motors_enabled(self, value):
+        if value !=self.cartesian_motors_enabled:
+            self.cartesian_motors_enabled = value
+            if value:    
+                for i in range(6):
+                    self.cartesian_motors.append(RobotCartesianMotor(self, i))
+                    add_device(self.cartesian_motors[i], True)        
+                self.cartesian_destination = self.get_cartesian_destination()
+            else:
+                for m in self.cartesian_motors:
+                    remove_device(m)
+                self.cartesian_motors = []            
+                self.cartesian_destination = None
+        else:
+            if value:
+                self.cartesian_destination = self.get_cartesian_destination()
+                for m in self.cartesian_motors:
+                    m.initialize()                
+        
+
+    #Cartesian peudo-motors
+    def set_joint_motors_enabled(self, value):
+        if value !=self.joint_motors_enabled:
+            self.joint_motors_enabled = value
+            if value:    
+                for i in range(6):
+                    self.joint_motors.append(RobotJointMotor(self, i))
+                    add_device(self.joint_motors[i], True)                        
+            else:
+                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()                
+        
+
     #High-level, exclusive motion task.
     def start_task(self, program, *args,  **kwargs):
         tasks = [t for t in self.high_level_tasks]
@@ -410,8 +541,7 @@ class RobotTCP(TcpDevice, Stoppable):
         self._update_state()
         
     def stop_task(self):
-        #tasks = [t for t in self.high_level_tasks]
-        tasks = []
+        tasks = [t for t in self.high_level_tasks]        
         if (self.current_task is not None) and (not self.current_task in tasks): 
             tasks.append(self.current_task)
         for task in tasks:
@@ -426,7 +556,7 @@ class RobotTCP(TcpDevice, Stoppable):
                  self.current_task = task
                  return task
         return None
-                    
+
 
     def on_event(self,ev):
         pass

script/local.py

@@ -17,6 +17,7 @@ run("devices/RobotSC")
 #run("devices/RobotModbus")
 #run("devices/OneWire")
 
+
 #Raspberry login: usr=pi  pwd=Buntschu
 
 add_device(img.getContrast(), force = True)
@@ -31,6 +32,12 @@ def set_led_range(room_temp = True):
     led_ctrl2.config.save()
 
 
+###################################################################################################
+#  Motion modules
+###################################################################################################
+
+run("motion/tools")
+
 
 ###################################################################################################
 #  Barcode reader
@@ -155,9 +162,9 @@ def fit(ydata, xdata = None, draw_plot = True):
     max_y= max(ydata)    
     index_max = ydata.index(max_y)    
     max_x= xdata[index_max]
-    print "Max index:" + str(index_max),
-    print " x:" + str(max_x),
-    print " y:" + str(max_y)
+    #print "Max index:" + str(index_max),
+    #print " x:" + str(max_x),
+    #print " y:" + str(max_y)
     
     if draw_plot:
         plots = plot([ydata],["data"],[xdata], title="Fit" )
@@ -191,12 +198,12 @@ def fit(ydata, xdata = None, draw_plot = True):
     if abs(mean - xdata[index_max]) < abs((scale_x[0] + scale_x[1])/2):        
         if draw_plot:
             p.addMarker(mean, None, "Mean="+str(round(mean,4)), Color.MAGENTA.darker())
-        print "Mean -> " +  str(mean)
+        #print "Mean -> " +  str(mean)
         return (norm, mean, sigma)
     else:
         if draw_plot:
             p.addMarker(max_x, None, "Max="+str(round(max_x,4)), Color.GRAY)
-        print "Invalid gaussian fit: " +  str(mean)
+        #print "Invalid gaussian fit: " +  str(mean)
         return (None, None, None)
 
 context = get_context()

script/motion/tools.py

@@ -0,0 +1,60 @@
+POSITION_TOLERANCE = 50
+
+def wait_end_of_move():
+    robot.update()
+    while (not robot.settled) or (not robot.empty) or (not robot.isReady()) :
+        time.sleep(0.01)
+
+
+def move_home():        
+    #robot.reset_motion("jHome")
+    robot.movej("pHome", robot.tool , DESC_SCAN)    
+    wait_end_of_move()
+
+def move_to_laser():
+    tool = robot.tool
+    d = robot.get_distance_to_pnt("pLaser")
+    if d<0:
+        raise Exception ("Error calculating distance to laser: " + str(d))
+    if d<POSITION_TOLERANCE:
+        print "FROM LASER"
+        robot.reset_motion()
+        robot.movel("pLaser", robot.tool, DESC_SCAN)
+        wait_end_of_move()
+        return
+    d = robot.get_distance_to_pnt("pLaserAppro")
+    if d<0:
+        raise Exception ("Error calculating distance to laser appro: " + str(d))
+    if d<POSITION_TOLERANCE:
+        print "FROM APPRO"
+        robot.reset_motion()
+        robot.movel("pLaser", robot.tool, DESC_SCAN)
+        wait_end_of_move()
+        return
+    d = robot.get_distance_to_pnt("pHome")
+    if d<0:
+        raise Exception ("Error calculating distance to home: " + str(d))
+    if d<POSITION_TOLERANCE:
+        print "FROM HOME"
+        robot.reset_motion()
+        robot.movel("pLaserAppro", robot.tool, DESC_DEFAULT)
+        robot.movel("pLaser", robot.tool, DESC_SCAN)    
+        wait_end_of_move()
+        return
+    raise Exception ("Must be in home position to start move to laser")
+    
+
+
+
+def update_tool(tool=None, x_offset=0.0, y_offset=0.0, z_offset=0.0):
+    #Updating tool:
+    t=robot.get_tool_trsf(tool)
+    t[0]=t[0] - x_offset
+    t[1]=t[1] - y_offset
+    t[2]=t[2] - z_offset
+    robot.set_tool_trsf(t, tool)
+    print "Updated " + (str(robot.tool) if (tool is None) else tool) + ": " + str(t) 
+    robot.save_program()
+    
+
+

script/test/RobotCartesianScan.py

@@ -0,0 +1,28 @@
+import plotutils
+from mathutils import fit_gaussian, Gaussian
+
+robot.enable()
+move_to_laser()
+
+#robot.reset_motion("jLaser")
+
+#import ch.psi.pshell.device.Timestamp as Timestamp
+#sensor = Timestamp()
+
+#ret = lscan(robot_z, sensor, -50.0, 0.0, 0.5, latency = 0, relative = True)
+#ret = lscan(robot_x, sensor, -50.0, 0.0, 0.5, latency = 0, relative = True)
+
+robot.set_motors_enabled(True)
+ret = lscan(robot_x, ue.readable, -1.5, 1.5, 0.1, latency = 0.01, relative = True)
+
+x,y = ret.getPositions(0), ret.getReadable(0)
+y = [100.0-v for v in y]
+(norm, mea, sigma) = fit(y, xdata=x)
+print "Maximum at " + str(mea)
+
+
+
+robot.set_motors_enabled(True)
+ret = lscan(robot_rz, ue.readable, 0.0, 40.0, 1.0, latency = 0.01, relative = True, range = "auto")
+
+

script/test/TestLaserScan.py

@@ -0,0 +1,12 @@
+robot.set_motors_enabled(True)
+steps_x = 10
+steps_y = 8
+#ret = ascan([robot_x, robot_z], ue.readable, [-10.0, -10.0], [10.0, 10.0], [10,10], latency = 0.01, relative = True , zigzag=True)
+ret = ascan([robot_x, robot_z], ue.readable, [-10.0, -10.0], [10.0, 10.0], [steps_y,steps_x], latency = 0.01, relative = True , zigzag=False)
+data = Convert.transpose(Convert.toBidimensional(to_array(ret.getReadable(0),'d'),steps_x+1,steps_y+1))
+plot(data, title="data")
+
+data = ret.getData(0)
+plot(data, title="data2")
+        
+

script/test/TestRobot.py

@@ -22,7 +22,7 @@ print robot.get_emergency_stop_sts()
 print robot.get_safety_fault_signal()
 print robot.stop()
 print robot.resume()
-print robot.resetMotion()
+print robot.reset_motion()
 print robot.is_empty()
 print robot.is_settled()
 print robot.get_move_id()

script/test/TestRobot2.py

@@ -30,7 +30,7 @@ try:
     step = 11
     robot.resume()
     step = 12
-    robot.resetMotion()
+    robot.reset_motion()
     step = 13
     robot.is_empty()
     step = 14