pshell_config/mxsc
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Browse Source
This commit is contained in:
gac-S_Changer
2018-04-23 08:18:18 +02:00
parent e48acd85b9
commit cdb602b7c7
20 changed files with 854 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
script/calibration/BinarySearchXZ.py → script/calibration/BinarySearchYZ.py
View File

@@ -15,15 +15,15 @@ move_to_laser()
robot.set_motors_enabled(True)
current_x = robot_x.getPosition()
current_y = robot_y.getPosition()
current_z = robot_z.getPosition()
r = bsearch([robot_x, robot_z], laser_distance,[RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [STEP_SIZE,STEP_SIZE], relative = True, maximum=True, strategy = STRATEGY, latency = LATENCY, title = "Binary Search XZ")
r = bsearch([robot_y, robot_z], laser_distance,[RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [STEP_SIZE,STEP_SIZE], relative = True, maximum=True, strategy = STRATEGY, latency = LATENCY, title = "Binary Search YZ")
print r.print()
opt_x, opt_z= r.getOptimalPosition()
offset_x, offset_z = opt_x - current_x, opt_z - current_z
opt_y, opt_z= r.getOptimalPosition()
offset_y, offset_z = opt_y - current_y, opt_z - current_z
print "offset_x: ", offset_x, " offset_z: ", offset_z
print "offset_y: ", offset_y, " offset_z: ", offset_z

13
script/calibration/ScanRZ.py
View File

@@ -1,9 +1,13 @@
import plotutils
from mathutils import fit_gaussian, Gaussian
RANGE = [-20.0,20.0]
STEP = 1.0
RANGE = [180.0,-180.0]
STEP = 5.0
LATENCY = 0.005
RELATIVE = False
robot.set_tool(TOOL_DEFAULT)
robot.enable()
move_to_laser()
@@ -13,8 +17,9 @@ 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")
robot.set_joint_motors_enabled(True)
ret = lscan(robot_rz, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = RELATIVE, range = "auto", title = "Scan2")

37
script/calibration/ScanX.py
View File

@@ -1,37 +0,0 @@
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

81
script/calibration/ScanY.py Normal file
View File

@@ -0,0 +1,81 @@
import plotutils
from mathutils import fit_gaussian, Gaussian
d = robot.get_distance_to_pnt("pLaser")
if d<0:
raise Exception ("Error calculating distance to laser: " + str(d))
if d>20:
raise Exception ("Should be near the laser position to perform the scan")
RANGE = [-1.5, 1.5]
STEP = 0.02
Z_OFFSET = 0 #-1.0
LATENCY = 0.025
BORDER_SIZE = 0.15
robot.enable()
robot.set_motors_enabled(True)
current_positon = robot_y.getPosition()
robot_z.moveRel(Z_OFFSET)
robot.setPolling(25)
try:
ret = lscan(robot_y, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True)
finally:
robot.setPolling(DEFAULT_ROBOT_POLLING)
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")
remove = int(max(BORDER_SIZE, STEP) / STEP)
_range = [first_index+remove, last_index-remove]
if _range[1] <= _range[0]:
raise Exception("invalid range: " + str(_range))
center_index = int((_range[0] + _range[1])/2)
center_positon = ret.getPositions(0)[center_index]
y = ret.getReadable(0)[_range[0] : _range[1]]
x = ret.getPositions(0)[_range[0]: _range[1]]
#x = enforce_monotonic(x)
#(normalization, mean_val, sigma) = fit_gaussian([-v for v in y], x)
closest_y = x[y.indexOf(min(y))]
closest_x = y[y.indexOf(min(y))]
if closest_y is None or closest_y <= ret.getPositions(0)[first_index] or closest_y >= ret.getPositions(0)[last_index]:
raise Exception("Invalid Fit")
center_offset = center_positon-closest_y
#center_offset = current_positon-closest_y
p=get_plots()[0]
p.addMarker(closest_y, p.AxisId.X, str(closest_y), Color.GREEN)
robot.set_motors_enabled(True)
robot_y.move(closest_y)

24
script/calibration/ScanXZ.py → script/calibration/ScanYZ.py
View File

@@ -21,24 +21,24 @@ STEP_SIZE = 0.1
robot.enable()
move_to_laser()
step_x = STEP_SIZE
step_y = STEP_SIZE
step_z = STEP_SIZE
range_x = [RANGE[0], RANGE[1]]
range_y = [RANGE[0], RANGE[1]]
range_z = [RANGE[0], RANGE[1]]
robot.set_motors_enabled(True)
current_x = robot_x.getPosition()
current_y = robot_y.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, title = "Scan XY")
print "Current pos y,z" , current_y, ",", current_z
ret = ascan([robot_y, robot_z], ue.readable, [range_y[0], range_z[0]], [range_y[1], range_z[1]], [step_y,step_z], latency = LATENCY, relative = True , zigzag=False, title = "Scan XY")
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)
xdata= frange(range_y[0], range_y[1], step_y , False, True)
p = plot(integ, title = "Fit", xdata=xdata)[0]
@@ -49,18 +49,18 @@ try:
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)
xdata= frange(range_y[0], range_y[1], step_y/100.0 , False, True)
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
raise Exception("Invalid Y detection")
y_offset = mean_val
center_y = current_y + y_offset
print "X offset = ", x_offset
print "Y offset = ", y_offset
robot_x.move(center_x)
robot_y.move(center_y)
if SINGLE_PASS:
z_scan_data = data[max_x_index]
else:

18
script/calibration/ToolCalibration.py
View File

@@ -15,17 +15,17 @@ move_to_laser()
robot.align()
run("calibration/ScanXZ")
run("calibration/ScanYZ")
robot.set_motors_enabled(True)
first_x = robot_x.take()
first_y = robot_y.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")
raise Exception("Invalid YZ scan values in first scan")
robot.set_joint_motors_enabled(True)
@@ -36,11 +36,11 @@ else:
robot.set_motors_enabled(True)
run("calibration/ScanXZ")
run("calibration/ScanYZ")
robot.set_motors_enabled(True)
second_x = robot_x.take()
second_y = robot_y.take()
second_z = robot_z.take()
second_y = ue.take()
second_j6 = robot_j6.take()
@@ -51,10 +51,10 @@ if second_y is None:
#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)
t=robot.get_tool_trsf(TOOL_DEFAULT)
t[0]=xoff
t[1]=-yoff
robot.set_tool_trsf(t, TOOL_DEFAULT)

75
script/calibration/ToolCalibration2.py Normal file
View File

@@ -0,0 +1,75 @@
import plotutils
from mathutils import fit_gaussian, Gaussian
robot.assert_tool(TOOL_CALIBRATION)
robot.enable()
robot.set_motors_enabled(True)
robot.set_joint_motors_enabled(True)
move_to_laser()
initial_pos = robot.get_cartesian_pos()
robot.enable()
move_to_laser()
#robot.align()
run("calibration/ScanY")
pos1 = robot.get_cartesian_pos()
x1, y1 = closest_x, closest_y
print "Closest 1: ", [x1, y1]
print "Position 1: ", pos1
pj6 = robot_j6.position
if pj6>0:
robot_j6.move(pj6 - 180.0)
else:
robot_j6.move(pj6 + 180.0)
run("calibration/ScanY")
x2, y2 = closest_x, closest_y
print "Closest 2: ", [x2, y2]
off_x = x1 - x2
#robot.set_motors_enabled(True)
#robot_x.moveRel(off_x, -1)
#For composing cannot use tcp_p, need another auxiliary point. tcp_t is also destroyed.
robot.set_pnt(robot.get_cartesian_pos(), "pTemp")
robot.set_trsf([off_x, 0,0,0,0,0])
c=robot.compose("pTemp", "fTable", "tcp_t" )
robot.set_pnt(c, "pTemp")
robot.movel("pTemp", TOOL_CALIBRATION, DESC_SCAN, sync=True)
pos2 = robot.get_cartesian_pos()
print pos2
print "Position 2: ", pos2
#Updates the tool
xoff = (pos2[0]-pos1[0])/2
yoff = (pos2[1]-pos1[1])/2
print "Offset: ", [xoff, yoff]
t=robot.get_tool_trsf(TOOL_DEFAULT)
t[0]=-xoff
t[1]=yoff
robot.set_tool_trsf(t, TOOL_DEFAULT)

35
script/devices/Hexiposi.py
View File

@@ -4,15 +4,17 @@ import json
class Hexiposi(DiscretePositionerBase):
def __init__(self, name, url):
DiscretePositionerBase.__init__(self, name, ["1","2","3","4","5","6"])
DiscretePositionerBase.__init__(self, name, ["A","B","C","D","E","F"])
if not url.startswith("http://"):
url = "http://" + url
if not url.endswith("/"):
url = url + "/"
self.url = url
self.setState(State.Ready)
self.val = self.doReadReadback()
self.url = url
def doInitialize(self):
super(Hexiposi, self).doInitialize()
self.val = self.doReadReadback()
def get_response(self, response):
if (response.status_code!=200):
raise Exception (response.text)
@@ -28,16 +30,15 @@ class Hexiposi(DiscretePositionerBase):
self.pos = self.status["position"]
self.moving = self.status["moving"]
self.offset = self.status["offset"]
self.dpos = self.status["discretePosition"]
if self.dpos == 1: self.rback = 1
elif self.dpos == 2: self.rback = 2
elif self.dpos == 4: self.rback = 3
elif self.dpos == 8: self.rback = 4
elif self.dpos == 16: self.rback = 5
elif self.dpos == 32: self.rback = 6
else: self.rback = None
self.rbackstr = self.UNKNOWN_POSITION if ((self.rback is None) or (self.homed==False)) else str(self.rback)
self.dpos = self.status["discretePosition"]
if (self.homed==False): self.rback = self.UNKNOWN_POSITION
elif self.dpos == 1: self.rback = "A"
elif self.dpos == 2: self.rback = "B"
elif self.dpos == 4: self.rback = "C"
elif self.dpos == 8: self.rback = "D"
elif self.dpos == 16: self.rback = "E"
elif self.dpos == 32: self.rback = "F"
else: self.rback = self.UNKNOWN_POSITION
return self.status
def move_pos(self, pos):
@@ -65,10 +66,12 @@ class Hexiposi(DiscretePositionerBase):
def doReadReadback(self):
self.get_status()
return self.rbackstr
return self.rback
def doWrite(self, val):
val = int(val)
val = ord(val) - ord('A') +1
if val<1 or val>6:
raise Exception("Invalid value: " + str(val))
moving = val != self.val
self.val = val
self.move_pos(self.val)

4
script/devices/RobotMotors.py
View File

@@ -21,7 +21,7 @@ class RobotCartesianMotor (PositionerBase):
def doWrite(self, value):
if self.robot.cartesian_destination is not None:
print "Move " + ROBOT_MOTORS[self.index] + " to " + str(value)
#print "Move " + ROBOT_MOTORS[self.index] + " to " + str(value)
self.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)
@@ -49,7 +49,7 @@ class RobotJointMotor (PositionerBase):
return self.setpoint
def doWrite(self, value):
print "Move " + ROBOT_JOINT_MOTORS[self.index] + " to " + str(value)
#print "Move " + ROBOT_JOINT_MOTORS[self.index] + " to " + str(value)
self.setpoint = value
joint = self.robot.herej()
joint[self.index] = value

16
script/devices/RobotSC.py
View File

@@ -8,16 +8,21 @@ DESC_SCAN = "mScan"
DESC_DEFAULT = DESC_FAST
DEFAULT_ROBOT_POLLING = 500
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)
RobotTCP.__init__(self, name, server, timeout, retries)
self.setPolling(DEFAULT_ROBOT_POLLING)
def mount(self, puck, sample):
return self.execute('mount',segment, puck, sample)
@@ -66,13 +71,6 @@ if simulation:
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_motors_enabled(True)
robot.set_joint_motors_enabled(True)
#robot.set_monitor_speed(20)

2
script/devices/RobotTCP.py
View File

@@ -368,7 +368,7 @@ class RobotTCP(TcpDevice, Stoppable):
def distance_p(self, pnt1, pnt2):
return self.eval_float("distance(" + pnt1 + ", " + pnt2 + ")")
def compose(self, pnt, frame, trsf):
def compose(self, pnt, frame = FRAME_DEFAULT, trsf = "tcp_t"):
return self.eval_pnt("compose(" + pnt + ", " + frame + ", " + trsf + ")")
def here(self, tool, frame):

71
script/imgproc/CameraCalibration.py Normal file
View File

@@ -0,0 +1,71 @@
import ch.psi.pshell.device.Camera as Camera
import ch.psi.pshell.imaging.RendererMode as RendererMode
from ch.psi.pshell.imaging.Overlays import *
"""
img.camera.setColorMode(Camera.ColorMode.Mono)
img.camera.setDataType(Camera.DataType.UInt8)
img.camera.setGrabMode(Camera.GrabMode.Continuous)
img.camera.setTriggerMode(Camera.TriggerMode.Fixed_Rate)
img.camera.setExposure(50.00)
img.camera.setAcquirePeriod(200.00)
img.camera.setGain(0.0)
img.config.rotationCrop=True
"""
#img.camera.setROI(200, 0,1200,1200)
img.camera.setROI(0, 0,1600,1200)
img.config.rotation=0
img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight =0,0,-1,-1
img.config.setCalibration(None)
img.camera.stop()
img.camera.start()
#img.camera.setROI(300, 200,1000,1000)
#img.config.rotation=17
#img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight = 50,50,900,900
p = show_panel(img)
p.setMode(RendererMode.Fit)
ov_text = Text(Pen(java.awt.Color.GREEN.darker()), "", java.awt.Font("Verdana", java.awt.Font.PLAIN, 24), java.awt.Point(20,20))
ov_text.setFixed(True)
p.addOverlay(ov_text)
try:
ov_text.update("Click on upper reference...")
p1 = p.waitClick(60000)
print p1
ov_text.update("Click on left reference...")
p2 = p.waitClick(60000)
print p2
ov_text.update("Click on right reference...")
p3 = p.waitClick(60000)
print p3
x, y, z = p1.x+p1.y*1j, p2.x+p2.y*1j, p3.x+p3.y*1j
w = z-x
w /= y-x
c = (x-y)*(w-abs(w)**2)/2j/w.imag-x
cx, cy, r = -c.real, -c.imag, abs(c+x)
a = math.degrees(math.atan((cx-p1.x)/(p1.y-cy)))
print cx, cy, r, a
#img.camera.setROI(int((1600-cx)/2),int((1200-cy)/2),1000,1000)
img.camera.setROI(int(cx-r),int(cy-r),int(2*r),int(2*r))
img.config.rotation=-a
#remove rotation border
d=int(r/11)
img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight =d,d, int(2*r-2*d), int(2*r-2*d)
#img.config.setCalibration(None)
img.camera.stop()
img.camera.start()
finally:
p.removeOverlay(ov_text)

49
script/local.py
View File

@@ -37,20 +37,47 @@ run("tools/Math")
# Device initialization
###################################################################################################
import ch.psi.pshell.device.Camera as Camera
img.camera.setColorMode(Camera.ColorMode.Mono)
img.camera.setDataType(Camera.DataType.UInt8)
img.camera.setGrabMode(Camera.GrabMode.Continuous)
img.camera.setTriggerMode(Camera.TriggerMode.Fixed_Rate)
img.camera.setExposure(50.00)
img.camera.setAcquirePeriod(200.00)
img.camera.setGain(0.0)
try:
release_local_safety.write(False)
release_psys_safety.write(False)
except:
print >> sys.stderr, traceback.format_exc()
try:
hexiposi.polling=500
except:
print >> sys.stderr, traceback.format_exc()
release_local_safety.write(False)
release_psys_safety.write(False)
try:
robot.setPolling(DEFAULT_ROBOT_POLLING)
robot.high_level_tasks = ["mount", "firstmount"]
robot.set_tool(TOOL_CALIBRATION)
robot.set_motors_enabled(True)
robot.set_joint_motors_enabled(True)
except:
print >> sys.stderr, traceback.format_exc()
try:
import ch.psi.pshell.device.Camera as Camera
img.camera.setColorMode(Camera.ColorMode.Mono)
img.camera.setDataType(Camera.DataType.UInt8)
img.camera.setGrabMode(Camera.GrabMode.Continuous)
img.camera.setTriggerMode(Camera.TriggerMode.Fixed_Rate)
img.camera.setExposure(50.00)
img.camera.setAcquirePeriod(200.00)
img.camera.setGain(0.0)
#img.camera.setROI(200, 0,1200,1200)
img.camera.setROI(300, 200,1000,1000)
img.config.rotation=17
img.config.rotationCrop=True
img.config.roiX,img.config.roiY, img.config.roiWidth,img.config.roiHeight = 50,50,900,900
img.camera.stop()
img.camera.start()
except:
print >> sys.stderr, traceback.format_exc()
hexiposi.polling=500
###################################################################################################

10
script/tools/Math.py
View File

@@ -57,4 +57,12 @@ def fit(ydata, xdata = None, draw_plot = True):
if draw_plot:
p.addMarker(max_x, None, "Max="+str(round(max_x,4)), Color.GRAY)
#print "Invalid gaussian fit: " + str(mean)
return (None, None, None)
return (None, None, None)
def enforce_monotonic(x):
epsilon = 1e-8
for i in range(len(x)-1):
if x[i+1]<=x[i]:
x[i+1] = x[i]+ epsilon
return x