def solution(ray):
_, iksolList, indexlist1 = coating.WorkspaceOnPose(pN, 0, [ray],robot,ikmodel,facevector,theta,coatingdistancetolerance)
print 'solution: iksolList - ',array(iksolList).shape
_, AlliksolList, _ = coating.AllExtraCoating2([ray],indexlist1,coatingdistance,numberofangles,tolerance,ikmodel,facevector,coatingdistancetolerance)
print 'solution: AlliksolList - ',array(AlliksolList).shape
if iksolList: return iksolList
elif AlliksolList: return AlliksolList
else: return False
def HasSol(ray):
reachableRays, iksolList, indexlist1 = coating.WorkspaceOnPose(
pN, 0, [ray], robot, ikmodel, facevector, theta,
coatingdistancetolerance)
AllreachableRayschableRays, AlliksolList, indexlist2 = coating.AllExtraCoating2(
[ray], indexlist1, coatingdistance, numberofangles, tolerance, ikmodel,
facevector, coatingdistancetolerance)
if AlliksolList: return True
else: return False
def fullSolution(ray):
angles = arange(0,tolerance,0.1)
numberofangles = 10
for angle in angles:
angle=1.0*pi*angle/180
Rv3tol = coating.RotationCalc2([0,0,1],angle)
p = dot(facevector,transpose(Rv3tol))
k = 1.0*2*pi/numberofangles
for i in range(0,numberofangles):
alfa = k*i
Rv1alfa = coating.RotationCalc2(facevector,alfa)
tempP = dot(p,transpose(Rv1alfa))
_, iksolList, _ = coating.WorkspaceOnPose(pN, 0, [ray],robot,ikmodel,tempP,theta,coatingdistancetolerance)
if iksolList:
return iksolList
return []
return indexAll
discover = []
for dy in y:
dy_time = time.time()
for position in range(0,len(gbase_position)):
position_time = time.time()
real_index = indexList[~indexAll]
pN = copy.copy(gbase_position[position])
pN[1]+=dy
normal = [-1,0,0]
pN = numpy.concatenate((pN,normal))
reachableRays, iksolList, indexlist1 = coating.WorkspaceOnPose(pN, 0, gapproachrays[~indexAll],robot,ikmodel,facevector,theta,coatingdistancetolerance)
indexBlackList.append(real_index[indexlist1])
AllreachableRays, AlliksolList, indexlist2 = coating.AllExtraCoating2(gapproachrays[~indexAll],indexlist1,coatingdistance,numberofangles,tolerance,ikmodel,facevector,coatingdistancetolerance)
indexBlueList.append(real_index[indexlist2])
try:
coatedrays = coating.IndexToPoints(gapproachrays[~indexAll],indexlist1|indexlist2)
except: break
indexAll = completeIndex(indexAll,real_index,indexlist1|indexlist2)
indexBlack = completeIndex(indexBlack,real_index,indexlist1)
indexBlue = completeIndex(indexBlue,real_index,indexlist2)
discover.append([dy,position,len(coatedrays)])
savetxt(str(24)+'.csv',array(discover),fmt='%.4f',delimiter=',')
print 'position_time = '+str(time.time()-position_time)
print 'dy_time = '+str(time.time()-dy_time)
savez_compressed('full_blade_test/indexBlackList_0d_pa24_c12_t30_y05.npz', array=indexBlackList)