def snapToPoint(self,x,y): clickPt = Geometry2D.Point(x,y) min_distance = self.getSnapRange() for snapPt in self.snapPoints(): if Geometry2D.distance(clickPt,snapPt) <= min_distance: return (snapPt.x(),snapPt.y()) return (x,y)
def getLine(self):
origin = self.getOrigin()
pt1 = Geometry2D.Point(origin.x(),
origin.y() - self.getSliderHeight() / 2)
pt2 = Geometry2D.Point(pt1.x() + self.getSliderWidth(), pt1.y())
line = Geometry2D.LineSegment(pt1, pt2)
return CVLineSegment(Colors.BLACK, 1, line)
def initBasic(self): #Line Drawing Members self.startPt = Geometry2D.Point(0,0) self.endPt = Geometry2D.Point(0,0) #Poly Drawing Members self.newPoly = [] self.drawColor = Colors.GREEN self.lineColor = Colors.WHITE
def getShape(self): ((buttonWidth,buttonHeight),baseline) = cv.GetTextSize(self.getText(),self.getFont()) origin = self.getBottomLeft() xMin = origin.x()-4 xMax = origin.x() + buttonWidth yMin = origin.y() - buttonHeight yMax = origin.y()+4 return Geometry2D.Polygon(Geometry2D.Point(xMin,yMin),Geometry2D.Point(xMin,yMax),Geometry2D.Point(xMax,yMax),Geometry2D.Point(xMax,yMin))
def center_and_bound(self,points,bound):
avgx = sum([pt.x() for pt in points]) / len(points)
avgy = sum([pt.y() for pt in points]) / len(points)
centered_pts = [Geometry2D.Point(pt.x()-avgx,pt.y()-avgy) for pt in points]
scale = max([max(fabs(pt.x()),fabs(pt.y())) for pt in centered_pts])
self.scale_factor = bound/(2.3*scale)
self.x_offset = bound/2 - avgx*bound/(2.3*scale)
self.y_offset = bound/2 - avgy*bound/(2.3*scale)
return [Geometry2D.Point(pt.x()*bound/(2.3*scale)+bound/2,pt.y()*bound/(2.3*scale)+bound/2) for pt in centered_pts]
def setHeuristic(searchNode2):
#print "start", searchNode2
allFoldList = list(fold_sequence)
#print allFoldList
completedFoldList = []
searchNode = searchNode2
for poly in searchNode.get_polys():
gui.addPropCVShape(poly)
#raw_input()
gui.clearProposed()
for fold in allFoldList:
'''
print "\n\n\n\n\n\ New SearchNode"
child2 = simulateDrag(searchNode,10, '-x', True)
if child2:
print "\n\n\n\n polys of drag \n\n\n"
for poly in child2.get_polys():
gui.addPropCVShape(poly)
print poly
print "In drag: drawing grippers"
for g in child2.gripPts:
gui.drawGripper(g)
raw_input()
gui.clearProposed()
'''
child, gripPts, endPts = simulateFold(searchNode,fold,transFold = fold,isHeuristic= True)
#print "Child in set heuristic" , child, gripPts, endPts
if child:
completedFoldList.append(fold)
fold.gripPoints = gripPts
fold.endPoints = endPts
clothConfig[tuple(getTupOfCompletedFolds(completedFoldList))] = child.get_polys()
#completedFoldList.append(fold)
#compString = getStringOfList(completedFoldList)
#clothConfig[compString] = child.get_polys()
for poly in child.get_polys():
gui.addPropCVShape(poly)
#raw_input()
gui.clearProposed()
#print "Child in set heuristic" , child, gripPts, endPts
maxDistance = float(max(Geometry2D.ptMagnitude(Geometry2D.ptDiff(pt1, pt2)) for pt1, pt2 in zip(gripPts, endPts)))
#print "Current GripPoint"
h = 3 + (((maxDistance/util.scale_factor)/0.25))
fold.setCost(h)
searchNode = child
else:
fold.setCost(float("inf"))
return False
gui.UPDATE_GRAPHICS = False
return True
def poly_handler(self,stamped_poly):
self.poly_frame = stamped_poly.header.frame_id
self.z_offset = stamped_poly.z_offset
points = [Geometry2D.Point(point.x,point.y) for point in stamped_poly.vertices]
vertices = self.center_and_bound(points,500)
poly = Geometry2D.Polygon(*vertices)
self.poly_cache = poly
cvPoly = CVPolygon(Colors.GREEN,self.gui.front(),poly)
self.gui.clearShapes()
self.gui.addCVShape(cvPoly)
self.handle_automatic_folds(vertices)
def drawToImage(self,img,imgtype): circle = self.shape center = circle.center() if (imgtype == 'temp'): center = Geometry2D.translatePt(center, 500,0) elif(imgtype == 'sel'): center = Geometry2D.translatePt(center, 1000,0) if self.filled(): return cv.Circle(img, center.toTuple(),int(circle.radius()),self.getDrawColor(),-1) else: return cv.Circle(img,center.toTuple(),int(circle.radius()),self.getDrawColor(),1)
def initExtended(self):
self.lineColor = Colors.BLACK
self.foldline_pts = []
self.foldline = None
self.has_foldline = False
clearShapesButton = CVButton(text="CLEAR",bottomLeft=Geometry2D.Point(50,100), onClick=self.clearAll)
self.addOverlay(clearShapesButton)
saveModelButton = CVButton(text="SAVE MODEL",bottomLeft=Geometry2D.Point(150,100), onClick = self.saveModel)
self.addOverlay(saveModelButton)
closeButton = CVButton(text="CLOSE",bottomLeft=Geometry2D.Point(350,100), onClick = self.close)
self.addOverlay(closeButton)
def getBar(self): (valMin,valMax) = self.getRange() value = self.getValue() pct = (value - valMin) / (valMax - valMin) origin = self.getOrigin() barWidth = 5.0 pt1 = Geometry2D.Point(origin.x()-barWidth/2 + pct*self.getSliderWidth(), origin.y()) pt2 = Geometry2D.Point(pt1.x(),pt1.y() - self.getSliderHeight()) pt3 = Geometry2D.Point(pt1.x()+barWidth,pt2.y()) pt4 = Geometry2D.Point(pt3.x(),pt1.y()) bar = Geometry2D.Polygon(pt1,pt2,pt3,pt4) return CVPolygon(Colors.invertCV(self.getColor()),1,bar)
def snapToLines(self, pt, lines):
if len(lines) == 0:
return pt
snapLine = min(
[Geometry2D.ptSegmentDisplacement(pt, line) for line in lines],
key=lambda displ: displ.length())
return snapLine.end()
def handleEvents(self,event,x,y,flags,param): self.clearTemp() #Check buttons first onButton = False for button in self.getOverlay(): if button.getShape().contains(Geometry2D.Point(x,y)): onButton = True if not button in self.last_mouseover_overlay: self.last_mouseover_overlay.append(button) button.onMouseOn(event,x,y,flags,param) button.onMouse(event,x,y,flags,param) else: if button in self.last_mouseover_overlay: self.last_mouseover_overlay.remove(button) button.onMouseOff(event,x,y,flags,param) if onButton: return thread.start_new_thread(self.onMouseLocked,(event,x,y,flags,param)) if(self.shapeListeners()): for cvShape in self.getShapes(): if cvShape in self.getHighlighted(x,y): if not cvShape in self.last_mouseover: self.last_mouseover.append(cvShape) cvShape.onMouseOn(event,x,y,flags,param) cvShape.onMouse(event,x,y,flags,param) else: if cvShape in self.last_mouseover: self.last_mouseover.remove(cvShape) cvShape.onMouseOff(event,x,y,flags,param) return
def getModelPantsSkel(self):
#Parameters: mid_center,top_center,mid_left,left_leg_center,left_leg_left
width = Geometry2D.distance(self.left_leg_left,self.left_leg_right)
return Models.Model_Pants_Skel_New(True,
self.mid_center.toTuple(), self.top_center.toTuple(), self.mid_left.toTuple(),
self.left_leg_center.toTuple(),self.top_left.toTuple(),width
)
def getModelTee(self):
left_sleeve_width = Geometry2D.distance(self.sleeve_top,self.sleeve_node)*2
return Models.Model_Tee_Skel_No_Skew(True,
self.spine_bottom.toTuple(), self.spine_top.toTuple(),
self.collar.toTuple(), self.shoulder_joint.toTuple(),
self.shoulder_top.toTuple(), self.sleeve_node.toTuple(),self.bottom_left.toTuple(),left_sleeve_width
)
def checkFeasibleDrag(searchNode,gripPts,gripPoints3D,dist,direction):
(canDrag, costDrag) = robot.feasible_drag(gripPoints3D,dist,direction,searchNode.robotPosition)
if canDrag and not (costDrag == float("inf")):
dragHist = list(searchNode.dragHistory)
dragHist.append((direction, dist))
drag_x_new, drag_y_new = getDragDistance(dragHist)
newPolys = gui.translatePolys(searchNode.get_polys(), drag_x_new, drag_y_new)
endPts = [Geometry2D.movePt(pt,direction,dist) for pt in gripPts]
"""
for poly in newPolys:
gui.addPropCVShape(poly)
print "Poly After Drag"
raw_input()
"""
isValid = gui.checkValidity(newPolys)
if isValid:
#print "Current Drag" , dist, "Direction", direction, "is Valid"
child = SearchState(polys = searchNode.get_polys(),robotPosition = searchNode.robotPosition,
g = searchNode.get_g()+costDrag,
action = Action(actionType = "drag",dragDirection = direction,
dragDistance = dist, gripPoints = gripPts,
endPoints = endPts),
parent=searchNode,depth = searchNode.get_depth()+1,
availableFolds = list(searchNode.availableFolds),
completedFolds = list(searchNode.completedFolds),
dragHistory = dragHist)
child.h = getHeuristic(child)
child.action.gripPoints = gripPts
return child
return False
def getModelPantsSkel(self):
#Parameters: mid_center,top_center,mid_left,left_leg_center,left_leg_left
width = Geometry2D.distance(self.left_leg_left, self.left_leg_right)
return Models.Model_Pants_Skel_New(True, self.mid_center.toTuple(),
self.top_center.toTuple(),
self.mid_left.toTuple(),
self.left_leg_center.toTuple(),
self.top_left.toTuple(), width)
def getModelSkel(self):
left_sleeve_width = Geometry2D.distance(self.sleeve_top,self.sleeve_node)*2
return Models.Model_Shirt_Skel_Less_Restricted(True,
self.spine_bottom.toTuple(), self.spine_top.toTuple(),
self.collar.toTuple(), self.shoulder_joint.toTuple(),
self.shoulder_top.toTuple(),self.sleeve_node.toTuple(),self.bottom_left.toTuple(),
left_sleeve_width
)
def getModelSkel(self):
left_sleeve_width = Geometry2D.distance(self.sleeve_top,
self.sleeve_node) * 2
return Models.Model_Shirt_Skel_Less_Restricted(
True, self.spine_bottom.toTuple(), self.spine_top.toTuple(),
self.collar.toTuple(), self.shoulder_joint.toTuple(),
self.shoulder_top.toTuple(), self.sleeve_node.toTuple(),
self.bottom_left.toTuple(), left_sleeve_width)
def __init__(self,
origin,
valueGetter,
color,
displayCondition=lambda: True):
self.valueGetter = valueGetter
self.displayCondition = displayCondition
circle = Geometry2D.Circle(origin, 1)
CVShape.__init__(self, color=color, height=1, shape=circle)
def initExtended(self):
self.lineColor = Colors.BLACK
if TYPE == SYMM:
self.has_symmline = False
self.symmline = None
self.symmpts = []
if TYPE == SWEATER_SKEL or TYPE == TEE_SKEL:
self.mode = 0
self.spine_bottom = None
self.spine_top = None
self.collar = None
self.spine_arm_junction = None
self.shoulder_joint = None
self.shoulder_top = None
self.sleeve_node = None
self.sleeve_top = None
if TYPE == PANTS_SKEL:
self.mode = 0
self.mid_center = None
self.top_center = None
self.mid_left = None
self.left_leg_bottom = None
self.left_leg_left = None
if TYPE == SOCK_SKEL:
self.mode = 0
self.ankle_center = None
self.ankle_joint = None
self.toe_center = None
self.toe_top = None
self.sock_width = None
clearShapesButton = CVButton(text="CLEAR",
bottomLeft=Geometry2D.Point(50, 100),
onClick=self.clearAll)
self.addOverlay(clearShapesButton)
saveModelButton = CVButton(text="SAVE MODEL",
bottomLeft=Geometry2D.Point(150, 100),
onClick=self.saveModel)
self.addOverlay(saveModelButton)
closeButton = CVButton(text="CLOSE",
bottomLeft=Geometry2D.Point(350, 100),
onClick=self.close)
self.addOverlay(closeButton)
def getModelTee(self):
left_sleeve_width = Geometry2D.distance(self.sleeve_top,
self.sleeve_node) * 2
return Models.Model_Tee_Skel_No_Skew(True, self.spine_bottom.toTuple(),
self.spine_top.toTuple(),
self.collar.toTuple(),
self.shoulder_joint.toTuple(),
self.shoulder_top.toTuple(),
self.sleeve_node.toTuple(),
self.bottom_left.toTuple(),
left_sleeve_width)
def convert_from_world_frame(self,pt3D):
now = rospy.Time.now()
self.listener.waitForTransform(self.poly_frame,pt3D.header.frame_id,now,rospy.Duration(20.0))
newpt = self.listener.transformPoint(self.poly_frame,pt3D)
x = newpt.point.y * -1
y = newpt.point.x * -1
x *= self.scale_factor
y *= self.scale_factor
x += self.x_offset
y += self.y_offset
return Geometry2D.Point(x,y)
def setHeuristic(searchNode):
print "start", searchNode
allFoldList = list(fold_sequence)
print allFoldList
searchNode = searchNode
for fold in allFoldList:
# if gui.legalBlueFold(fold,searchNode.get_polys()):
child, gripPts, endPts = simulateFold(searchNode,fold,transFold = fold,isHeuristic= True)
foldActivePoints[fold] = (gripPts, endPts)
#print "Child in set heuristic" , child, gripPts, endPts
if child:
maxDistance = float(max(Geometry2D.ptMagnitude(Geometry2D.ptDiff(pt1, pt2)) for pt1, pt2 in zip(gripPts, endPts)))
print "Current GripPoint"
h = 3 + (((maxDistance/util.scale_factor)/0.25))*4
fold.setCost(h)
searchNode = child
else:
fold.setCost(float("inf"))
return False
return True
def polyDrawer(self,event,x,y,flags,param): if event == cv.CV_EVENT_LBUTTONDOWN: print "Left button: Point x:%f, y:%f"%(x,y) self.newPoly.append(Geometry2D.Point(x,y)) elif event == cv.CV_EVENT_RBUTTONDOWN: print "Right button" poly = Geometry2D.Polygon(*self.newPoly) cvPoly = CVPolygon(self.getDrawColor(),self.front(),poly) self.addCVShape(cvPoly) self.newPoly = [] elif len(self.newPoly) > 1: startPt = self.newPoly[-1] print "StartPoint", startPt endPt = Geometry2D.Point(x,y) print "End Point" line = Geometry2D.LineSegment(startPt,endPt) cvLine = CVLineSegment(self.lineColor,self.tempFront(),line) #self.addTempCVShape(cvLine) for i, pt in enumerate(self.newPoly): self.highlightPt(pt) if i > 0: startPt = self.newPoly[i-1] line = Geometry2D.LineSegment(startPt,pt) cvLine = CVLineSegment(self.lineColor,self.tempFront(),line) self.addTempCVShape(cvLine) elif i == len(self.newPoly)-1: endPt = self.newPoly[0] line = Geometry2D.LineSegment(pt,endPt) cvLine = CVLineSegment(self.lineColor,self.tempFront(), line) self.addTempCVShape(cvLine)
def symmPolyDrawer(self, event, x, y, flags, param):
if event == cv.CV_EVENT_LBUTTONDOWN:
self.newPoly.append(Geometry2D.Point(x, y))
elif event == cv.CV_EVENT_RBUTTONDOWN:
print "MADE IT"
backwards = list(self.newPoly)
backwards.reverse()
backwards = [pt.toTuple() for pt in backwards]
for pt in backwards:
print "Looking at pt"
newpt = Vector2D.mirror_pt(pt, self.symmline)
print newpt
self.newPoly.append(Geometry2D.Point(newpt[0], newpt[1]))
print "Added pt"
print len(self.newPoly)
poly = Geometry2D.Polygon(*self.newPoly)
cvPoly = CVPolygon(self.getDrawColor(), self.front(), poly)
self.addCVShape(cvPoly)
self.newPoly = []
elif len(self.newPoly) > 0:
startPt = self.newPoly[-1]
endPt = Geometry2D.Point(x, y)
line = Geometry2D.LineSegment(startPt, endPt)
cvLine = CVLineSegment(self.lineColor, self.tempFront(), line)
self.addTempCVShape(cvLine)
for i, pt in enumerate(self.newPoly):
self.highlightPt(pt)
if i > 0:
startPt = self.newPoly[i - 1]
line = Geometry2D.LineSegment(startPt, pt)
cvLine = CVLineSegment(self.lineColor, self.tempFront(), line)
self.addTempCVShape(cvLine)
def foldDrawer(self,event,x,y,flags,param):
if event==cv.CV_EVENT_LBUTTONUP:
self.foldline_pts.append((x,y))
print "ADDED PT"
cv.Circle(self.background,(x,y),3,cv.CV_RGB(255,0,0),-1)
if len(self.foldline_pts) >= 2:
self.foldline = Vector2D.make_ln_from_pts(self.foldline_pts[0],self.foldline_pts[1])
ln_start = Vector2D.intercept(self.foldline,Vector2D.horiz_ln(y=0))
ln_end = Vector2D.intercept(self.foldline,Vector2D.horiz_ln(y=self.background.height))
cv.Line(self.background,(int(ln_start[0]),int(ln_start[1])),(int(ln_end[0]),int(ln_end[1])),cv.CV_RGB(0,0,0))
self.has_foldline = True
elif len(self.foldline_pts) > 0:
self.addTempCVShape(CVLineSegment(cv.CV_RGB(255,255,255),2,Geometry2D.LineSegment(Geometry2D.Point(self.foldline_pts[0][0],self.foldline_pts[0][1]),Geometry2D.Point(x,y))))
def polyDrawer(self, event, x, y, flags, param):
if event == cv.CV_EVENT_LBUTTONDOWN:
self.newPoly.append(Geometry2D.Point(x, y))
elif event == cv.CV_EVENT_RBUTTONDOWN:
poly = Geometry2D.Polygon(*self.newPoly)
cvPoly = CVPolygon(self.getDrawColor(), self.front(), poly)
self.addCVShape(cvPoly)
self.newPoly = []
elif len(self.newPoly) > 0:
startPt = self.newPoly[-1]
endPt = Geometry2D.Point(x, y)
line = Geometry2D.LineSegment(startPt, endPt)
cvLine = CVLineSegment(self.lineColor, self.tempFront(), line)
self.addTempCVShape(cvLine)
for i, pt in enumerate(self.newPoly):
self.highlightPt(pt)
if i > 0:
startPt = self.newPoly[i - 1]
line = Geometry2D.LineSegment(startPt, pt)
cvLine = CVLineSegment(self.lineColor, self.tempFront(), line)
self.addTempCVShape(cvLine)
def isRedFold(parentNode, gripPts):
if(parentNode != None and parentNode.action != "None" and parentNode.action !=None and parentNode.action.get_actionType() == 'drag'):
isRedFold = True
for gripPt in gripPts:
isPtMapped = False
for endPt in parentNode.action.get_endPoints():
if(Geometry2D.distanceSquared(gripPt, endPt) < 25):
isPtMapped = True
#if not gripPt in parentNode.action.get_endPoints():
if not isPtMapped:
isRedFold = False
if isRedFold:
print "red Fold returning True"
#raw_input()
return True
return False
def lineDrawer(self,event,x,y,flags,param): if event == cv.CV_EVENT_LBUTTONDOWN: self.startPt = Geometry2D.Point(x,y) elif event == cv.CV_EVENT_LBUTTONUP: self.endPt = Geometry2D.Point(x,y) line = Geometry2D.DirectedLineSegment(self.startPt,self.endPt) self.addCVShape(CVDirectedLineSegment(cv.RGB(255,255,255),10,line,2)) elif cv.CV_EVENT_FLAG_LBUTTON == flags-32: line = Geometry2D.DirectedLineSegment(self.startPt,Geometry2D.Point(x,y)) self.addTempCVShape(CVDirectedLineSegment(cv.RGB(150,150,150),10,line,2)) elif event == cv.CV_EVENT_RBUTTONDOWN: self.startPt = Geometry2D.Point(x,y) elif event == cv.CV_EVENT_RBUTTONUP: self.endPt = Geometry2D.Point(x,y) line = Geometry2D.DirectedLineSegment(self.startPt,self.endPt) self.addCVShape(CVDirectedLineSegment(cv.RGB(0,0,255),10,line,2)) return
def sockSkelDrawer(self, event, x, y, flags, param):
if self.mode == 0:
new_pt = Geometry2D.Point(x, y)
self.ankle_center = new_pt
if event == cv.CV_EVENT_LBUTTONUP:
self.permanentHighlightPt(self.ankle_center)
self.mode += 1
else:
self.highlightPt(self.ankle_center)
elif self.mode == 1:
new_pt = Geometry2D.Point(x, y)
self.ankle_joint = new_pt
if event == cv.CV_EVENT_LBUTTONUP:
self.permanentHighlightPt(self.ankle_joint)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.ankle_center, self.ankle_joint))
self.mode += 1
else:
self.highlightPt(self.ankle_joint)
self.highlightSegment(Geometry2D.LineSegment(self.ankle_center, self.ankle_joint))
elif self.mode == 2:
new_pt = Geometry2D.Point(x, y)
self.toe_center = new_pt
if event == cv.CV_EVENT_LBUTTONUP:
self.permanentHighlightPt(self.toe_center)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.ankle_joint, self.toe_center))
self.mode += 1
else:
self.highlightPt(self.toe_center)
self.highlightSegment(Geometry2D.LineSegment(self.ankle_joint, self.toe_center))
elif self.mode == 3:
new_pt = Geometry2D.Point(x, y)
self.toe_top = new_pt
self.sock_width = 2 * Geometry2D.distance(self.toe_top, self.toe_center)
if event == cv.CV_EVENT_LBUTTONUP:
self.permanentHighlightPt(self.toe_top)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.toe_center, self.toe_top))
self.mode += 1
else:
self.highlightPt(self.toe_top)
self.highlightSegment(Geometry2D.LineSegment(self.toe_center, self.toe_top))
def skelDrawer(self, event, x, y, flags, param):
if self.mode == 0:
# Draw spine_bottom
print "Select midle of a spine_bottom"
if event == cv.CV_EVENT_LBUTTONUP:
self.spine_bottom = Geometry2D.Point(x, y)
self.permanentHighlightPt(self.spine_bottom)
self.mode += 1
else:
pass
elif self.mode == 1:
print "Select bottom point of a collar"
if event == cv.CV_EVENT_LBUTTONUP:
self.spine_top = Geometry2D.Point(x, y)
self.permanentHighlightPt(self.spine_top)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.mode += 1
else:
self.highlightSegment(Geometry2D.LineSegment(self.spine_bottom, Geometry2D.Point(x, y)))
elif self.mode == 2:
# Draw collar
print "Select left upper point of a collar"
if event == cv.CV_EVENT_LBUTTONUP:
self.collar = Geometry2D.Point(x, y)
self.virtual_collar = Geometry2D.mirrorPt(
self.collar, Geometry2D.LineSegment(self.spine_bottom, self.spine_top)
)
self.permanentHighlightPt(self.collar)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.spine_top, self.collar))
self.permanentHighlightPt(self.virtual_collar)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.spine_top, self.virtual_collar))
self.mode += 1
else:
temp_collar = Geometry2D.Point(x, y)
virtual_collar = Geometry2D.mirrorPt(
temp_collar, Geometry2D.LineSegment(self.spine_bottom, self.spine_top)
)
self.highlightPt(temp_collar)
self.highlightSegment(Geometry2D.LineSegment(self.spine_top, temp_collar))
self.highlightPt(virtual_collar)
self.highlightSegment(Geometry2D.LineSegment(self.spine_top, virtual_collar))
elif self.mode == 3:
# Draw shoulder
print "Select point between a left shoulder and a left armpit"
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.shoulder_joint = new_pt
self.virtual_shoulder_joint = Geometry2D.mirrorPt(
self.shoulder_joint, Geometry2D.LineSegment(self.spine_bottom, self.spine_top)
)
self.permanentHighlightPt(self.shoulder_joint)
self.permanentHighlightPt(self.virtual_shoulder_joint)
seg = Geometry2D.LineSegment(self.shoulder_joint, self.virtual_shoulder_joint)
self.permanentHighlightSegment(seg)
self.bottom_left = Geometry2D.Point(
self.spine_bottom.x() - 0.5 * seg.dx(), self.spine_bottom.y() - 0.5 * seg.dy()
)
self.bottom_right = Geometry2D.Point(
self.spine_bottom.x() + 0.5 * seg.dx(), self.spine_bottom.y() + 0.5 * seg.dy()
)
bottom_seg = Geometry2D.LineSegment(self.bottom_left, self.bottom_right)
self.permanentHighlightPt(self.bottom_left)
self.permanentHighlightPt(self.bottom_right)
self.permanentHighlightSegment(bottom_seg)
self.mode += 1
else:
temp_shoulder_joint = new_pt
virtual_shoulder_joint = Geometry2D.mirrorPt(
temp_shoulder_joint, Geometry2D.LineSegment(self.spine_bottom, self.spine_top)
)
self.highlightPt(temp_shoulder_joint)
self.highlightPt(virtual_shoulder_joint)
seg = Geometry2D.LineSegment(temp_shoulder_joint, virtual_shoulder_joint)
self.highlightSegment(seg)
bottom_left = Geometry2D.Point(
self.spine_bottom.x() - 0.5 * seg.dx(), self.spine_bottom.y() - 0.5 * seg.dy()
)
bottom_right = Geometry2D.Point(
self.spine_bottom.x() + 0.5 * seg.dx(), self.spine_bottom.y() + 0.5 * seg.dy()
)
bottom_seg = Geometry2D.LineSegment(bottom_left, bottom_right)
self.highlightPt(bottom_left)
self.highlightPt(bottom_right)
self.highlightSegment(bottom_seg)
elif self.mode == 4:
# Draw shoulder
print "Select point in a left shoulder"
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.shoulder_top = new_pt
seg = Geometry2D.LineSegment(self.shoulder_top, self.shoulder_joint)
self.armpit = seg.extrapolate(2.0)
self.virtual_shoulder_top = Geometry2D.mirrorPt(
self.shoulder_top, Geometry2D.LineSegment(self.spine_bottom, self.spine_top)
)
self.virtual_armpit = Geometry2D.mirrorPt(
self.armpit, Geometry2D.LineSegment(self.spine_bottom, self.spine_top)
)
self.permanentHighlightPt(self.shoulder_top)
self.permanentHighlightPt(self.armpit)
self.permanentHighlightPt(self.virtual_shoulder_top)
self.permanentHighlightPt(self.virtual_armpit)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.armpit, self.shoulder_top))
self.permanentHighlightSegment(Geometry2D.LineSegment(self.virtual_armpit, self.virtual_shoulder_top))
self.mode += 1
else:
shoulder_top = new_pt
seg = Geometry2D.LineSegment(shoulder_top, self.shoulder_joint)
armpit = seg.extrapolate(2.0)
virtual_shoulder_top = Geometry2D.mirrorPt(
shoulder_top, Geometry2D.LineSegment(self.spine_bottom, self.spine_top)
)
virtual_armpit = Geometry2D.mirrorPt(armpit, Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.highlightPt(shoulder_top)
self.highlightPt(armpit)
self.highlightPt(virtual_shoulder_top)
self.highlightPt(virtual_armpit)
self.highlightSegment(Geometry2D.LineSegment(armpit, shoulder_top))
self.highlightSegment(Geometry2D.LineSegment(virtual_armpit, virtual_shoulder_top))
elif self.mode == 5:
# Draw sleeve
print "Select midle point at the end of a left sleeve"
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.sleeve_node = new_pt
self.virtual_sleeve_node = Geometry2D.mirrorPt(
self.sleeve_node, Geometry2D.LineSegment(self.spine_bottom, self.spine_top)
)
self.permanentHighlightPt(self.sleeve_node)
self.permanentHighlightPt(self.virtual_sleeve_node)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.shoulder_joint, self.sleeve_node))
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.virtual_shoulder_joint, self.virtual_sleeve_node)
)
self.mode += 1
else:
temp_sleeve_node = new_pt
virtual_sleeve_node = Geometry2D.mirrorPt(
temp_sleeve_node, Geometry2D.LineSegment(self.spine_bottom, self.spine_top)
)
self.highlightPt(temp_sleeve_node)
self.highlightPt(virtual_sleeve_node)
self.highlightSegment(Geometry2D.LineSegment(self.shoulder_joint, temp_sleeve_node))
self.highlightSegment(Geometry2D.LineSegment(self.virtual_shoulder_joint, virtual_sleeve_node))
elif self.mode == 6:
# Draw shoulder
print "Select upper end point at the end of a left sleeve"
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.sleeve_top = new_pt
seg = Geometry2D.LineSegment(self.sleeve_top, self.sleeve_node)
self.sleeve_bottom = seg.extrapolate(2.0)
self.virtual_sleeve_top = Geometry2D.mirrorPt(
self.sleeve_top, Geometry2D.LineSegment(self.spine_bottom, self.spine_top)
)
self.virtual_sleeve_bottom = Geometry2D.mirrorPt(
self.sleeve_bottom, Geometry2D.LineSegment(self.spine_bottom, self.spine_top)
)
self.permanentHighlightPt(self.sleeve_top)
self.permanentHighlightPt(self.sleeve_bottom)
self.permanentHighlightPt(self.virtual_sleeve_top)
self.permanentHighlightPt(self.virtual_sleeve_bottom)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.sleeve_bottom, self.sleeve_top))
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.virtual_sleeve_bottom, self.virtual_sleeve_top)
)
# self.addCVShape(CVPolygon(cv.CV_RGB(0,0,0),self.front(),
# Geometry2D.Polygon( self.bottom_left,self.armpit,self.sleeve_bottom,self.sleeve_top,self.shoulder_top,self.collar,
# self.virtual_collar,self.virtual_shoulder_top,self.virtual_sleeve_top,self.virtual_sleeve_bottom,self.virtual_armpit,self.bottom_right)))
self.mode += 1
else:
sleeve_top = new_pt
seg = Geometry2D.LineSegment(sleeve_top, self.sleeve_node)
sleeve_bottom = seg.extrapolate(2.0)
virtual_sleeve_top = Geometry2D.mirrorPt(
sleeve_top, Geometry2D.LineSegment(self.spine_bottom, self.spine_top)
)
virtual_sleeve_bottom = Geometry2D.mirrorPt(
sleeve_bottom, Geometry2D.LineSegment(self.spine_bottom, self.spine_top)
)
self.highlightPt(sleeve_top)
self.highlightPt(sleeve_bottom)
self.highlightPt(virtual_sleeve_top)
self.highlightPt(virtual_sleeve_bottom)
self.highlightSegment(Geometry2D.LineSegment(sleeve_bottom, sleeve_top))
self.highlightSegment(Geometry2D.LineSegment(virtual_sleeve_bottom, virtual_sleeve_top))
def correct_foldpoints(self,state,scoot_prev = 0,child=None):
"""
human in the loop correction
"""
action = state.action
self.gui.clearProposed()
# draw gripper_point
#drag_x, drag_y = state.getDragDistance()
#newPolys = self.gui.translatePolys(state.parent.get_polys(), drag_x, drag_y)
if not EXECUTE_FLAG:
drag_x,drag_y = state.parent.getDragDistance()
newPolys = self.gui.translatePolys(state.parent.get_polys(), drag_x, drag_y)
for poly in newPolys:
self.gui.addPropCVShape(poly)
gripPts_new = []
endPts_new = []
print "previous scoot ",scoot_prev
#print "CLICK GRIP-POINT"
i = 0
for gripPt_old, endPt_old in zip(action.get_gripPoints(), action.get_endPoints()):
if action.get_actionType() == "fold" and action.get_foldType() == "red":
self.gui.drawGripper(endPt_old)
self.gui.highlightPt(endPt_old)
print "old endpoint",endPt_old
else:
self.gui.drawGripper(gripPt_old)
self.gui.highlightPt(gripPt_old)
print "old grippoint",gripPt_old,"old endpoint",endPt_old
print "ClICK Corresponding Point and hit enter or Hit p to proceed with old grippoint"
if (raw_input() == 'p'):
self.corrected_gripPoint_latest = gripPt_old
else:
'''
while not self.corrected_gripPoint_latest:
print self.corrected_gripPoint_latest
print "Click correspondiong grippoint and hit enter or hit p to proceed with old grippoint"
raw_input()
'''
if self.corrected_gripPoint_latest == None:
print 'Click corrected grip point then press enter'
raw_input()
#stamped_poly = rospy.wait_for_message('/poly_maker_output', PolyStamped)
self.corrected_gripPoint_latest.xval += scoot_prev
gripPts_new.append(deepcopy(self.corrected_gripPoint_latest))
#ptMove = Geometry2D.ptDiff(deepcopy(self.corrected_gripPoint_latest), gripPt_old)
if action.get_actionType() == "fold" and action.get_foldType() == "red":
ptMove = Geometry2D.ptDiff(deepcopy(self.corrected_gripPoint_latest), endPt_old)
ptMove.yval = 0
else:
ptMove = Geometry2D.ptDiff(deepcopy(self.corrected_gripPoint_latest), gripPt_old)
child_action = child.action if child else None
if action.get_actionType() == "drag" and child_action and child_action.get_actionType() == "fold" and child_action.get_foldType() == "red":
child_gripPt_old,child_endPt_old = child_action.get_gripPoints()[i],child_action.get_endPoints()[i]
print "child: old gripPt",child_gripPt_old,"child: old endpoint",child_endPt_old
child_gripPt_old.translate(ptMove.x()-scoot_prev,ptMove.y())
child_endPt_old.translate(ptMove.x()-scoot_prev,ptMove.y())
print "child: new gripPt",child_gripPt_old,"child: new endpoint",child_endPt_old
print "error", ptMove.x(), ptMove.y()
self.corrected_gripPoint_latest = None
endPt_new = Geometry2D.translatePt(endPt_old, ptMove.x(), ptMove.y())
print "old grippoint",gripPt_old,"old endpoint",endPt_old
print "new grippoint",gripPts_new[-1], "new endpoint",endPt_new
endPts_new.append(endPt_new)
i +=1
print "correction done"
return gripPts_new, endPts_new
def snapToLines(self,pt,lines): if len(lines) == 0: return pt snapLine = min([Geometry2D.ptSegmentDisplacement(pt,line) for line in lines],key = lambda displ: displ.length()) return snapLine.end()
def highlightPt(self,pt): return self.addTempCVShape(CVCircle(Colors.WHITE,self.tempFront(),Geometry2D.Circle(pt,3)))
def permanentHighlightPt(self,pt):
self.addCVShape(CVCircle(cv.CV_RGB(0,0,0),self.front(),Geometry2D.Circle(pt,3)))
def sockSkelDrawer(self, event, x, y, flags, param):
if self.mode == 0:
new_pt = Geometry2D.Point(x, y)
self.ankle_center = new_pt
if event == cv.CV_EVENT_LBUTTONUP:
self.permanentHighlightPt(self.ankle_center)
self.mode += 1
else:
self.highlightPt(self.ankle_center)
elif self.mode == 1:
new_pt = Geometry2D.Point(x, y)
self.ankle_joint = new_pt
if event == cv.CV_EVENT_LBUTTONUP:
self.permanentHighlightPt(self.ankle_joint)
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.ankle_center,
self.ankle_joint))
self.mode += 1
else:
self.highlightPt(self.ankle_joint)
self.highlightSegment(
Geometry2D.LineSegment(self.ankle_center,
self.ankle_joint))
elif self.mode == 2:
new_pt = Geometry2D.Point(x, y)
self.toe_center = new_pt
if event == cv.CV_EVENT_LBUTTONUP:
self.permanentHighlightPt(self.toe_center)
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.ankle_joint, self.toe_center))
self.mode += 1
else:
self.highlightPt(self.toe_center)
self.highlightSegment(
Geometry2D.LineSegment(self.ankle_joint, self.toe_center))
elif self.mode == 3:
new_pt = Geometry2D.Point(x, y)
self.toe_top = new_pt
self.sock_width = 2 * Geometry2D.distance(self.toe_top,
self.toe_center)
if event == cv.CV_EVENT_LBUTTONUP:
self.permanentHighlightPt(self.toe_top)
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.toe_center, self.toe_top))
self.mode += 1
else:
self.highlightPt(self.toe_top)
self.highlightSegment(
Geometry2D.LineSegment(self.toe_center, self.toe_top))
def pantsSkelDrawer(self, event, x, y, flags, param):
if self.mode == 0:
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.mid_center = new_pt
self.permanentHighlightPt(self.mid_center)
self.mode += 1
elif self.mode == 1:
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.top_center = new_pt
self.crotch = Geometry2D.LineSegment(
self.mid_center, self.top_center).extrapolate(-1)
self.permanentHighlightPt(self.top_center)
self.permanentHighlightPt(self.crotch)
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.crotch, self.top_center))
self.mode += 1
else:
top_center = new_pt
crotch = Geometry2D.LineSegment(self.mid_center,
top_center).extrapolate(-1)
self.highlightPt(top_center)
self.highlightPt(crotch)
self.highlightSegment(
Geometry2D.LineSegment(crotch, top_center))
elif self.mode == 2:
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.mid_left = new_pt
self.mid_right = Geometry2D.mirrorPt(
self.mid_left,
Geometry2D.LineSegment(self.mid_center, self.top_center))
self.permanentHighlightPt(self.mid_left)
self.permanentHighlightPt(self.mid_right)
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.mid_left, self.mid_right))
self.mode += 1
else:
mid_left = new_pt
mid_right = Geometry2D.mirrorPt(
mid_left,
Geometry2D.LineSegment(self.mid_center, self.top_center))
self.highlightPt(mid_left)
self.highlightPt(mid_right)
self.highlightSegment(
Geometry2D.LineSegment(mid_left, mid_right))
elif self.mode == 3:
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.left_leg_center = new_pt
self.right_leg_center = Geometry2D.mirrorPt(
self.left_leg_center,
Geometry2D.LineSegment(self.mid_center, self.top_center))
self.permanentHighlightPt(self.left_leg_center)
self.permanentHighlightPt(self.right_leg_center)
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.left_leg_center,
self.mid_left))
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.right_leg_center,
self.mid_right))
self.mode += 1
else:
left_leg_center = new_pt
right_leg_center = Geometry2D.mirrorPt(
left_leg_center,
Geometry2D.LineSegment(self.mid_center, self.top_center))
self.highlightPt(left_leg_center)
self.highlightPt(right_leg_center)
self.highlightSegment(
Geometry2D.LineSegment(left_leg_center, self.mid_left))
self.highlightSegment(
Geometry2D.LineSegment(right_leg_center, self.mid_right))
elif self.mode == 4:
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.left_leg_left = new_pt
self.left_leg_right = Geometry2D.LineSegment(
self.left_leg_center, self.left_leg_left).extrapolate(-1)
self.right_leg_left = Geometry2D.mirrorPt(
self.left_leg_right,
Geometry2D.LineSegment(self.mid_center, self.top_center))
self.right_leg_right = Geometry2D.mirrorPt(
self.left_leg_left,
Geometry2D.LineSegment(self.mid_center, self.top_center))
self.permanentHighlightPt(self.left_leg_left)
self.permanentHighlightPt(self.left_leg_right)
self.permanentHighlightPt(self.right_leg_left)
self.permanentHighlightPt(self.right_leg_right)
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.left_leg_left,
self.left_leg_right))
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.right_leg_left,
self.right_leg_right))
top_displ = Geometry2D.ptDiff(self.top_center, self.mid_center)
left_displ = Geometry2D.ptDiff(self.left_leg_left,
self.left_leg_center)
self.top_left = Geometry2D.ptSum(
Geometry2D.ptSum(self.mid_left, top_displ), left_displ)
self.top_right = Geometry2D.mirrorPt(
self.top_left,
Geometry2D.LineSegment(self.mid_center, self.top_center))
self.permanentHighlightPt(self.top_left)
self.permanentHighlightPt(self.top_right)
self.mode += 1
else:
left_leg_left = new_pt
left_leg_right = Geometry2D.LineSegment(
self.left_leg_center, left_leg_left).extrapolate(-1)
right_leg_left = Geometry2D.mirrorPt(
left_leg_right,
Geometry2D.LineSegment(self.mid_center, self.top_center))
right_leg_right = Geometry2D.mirrorPt(
left_leg_left,
Geometry2D.LineSegment(self.mid_center, self.top_center))
self.highlightPt(left_leg_left)
self.highlightPt(left_leg_right)
self.highlightPt(right_leg_left)
self.highlightPt(right_leg_right)
self.highlightSegment(
Geometry2D.LineSegment(left_leg_left, left_leg_right))
self.highlightSegment(
Geometry2D.LineSegment(right_leg_left, right_leg_right))
top_displ = Geometry2D.ptDiff(self.top_center, self.mid_center)
left_displ = Geometry2D.ptDiff(left_leg_left,
self.left_leg_center)
top_left = Geometry2D.ptSum(
Geometry2D.ptSum(self.mid_left, top_displ), left_displ)
top_right = Geometry2D.mirrorPt(
top_left,
Geometry2D.LineSegment(self.mid_center, self.top_center))
self.highlightPt(top_left)
self.highlightPt(top_right)
def pantsSkelDrawer(self, event, x, y, flags, param):
if self.mode == 0:
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.mid_center = new_pt
self.permanentHighlightPt(self.mid_center)
self.mode += 1
elif self.mode == 1:
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.top_center = new_pt
self.crotch = Geometry2D.LineSegment(self.mid_center, self.top_center).extrapolate(-1)
self.permanentHighlightPt(self.top_center)
self.permanentHighlightPt(self.crotch)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.crotch, self.top_center))
self.mode += 1
else:
top_center = new_pt
crotch = Geometry2D.LineSegment(self.mid_center, top_center).extrapolate(-1)
self.highlightPt(top_center)
self.highlightPt(crotch)
self.highlightSegment(Geometry2D.LineSegment(crotch, top_center))
elif self.mode == 2:
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.mid_left = new_pt
self.mid_right = Geometry2D.mirrorPt(
self.mid_left, Geometry2D.LineSegment(self.mid_center, self.top_center)
)
self.permanentHighlightPt(self.mid_left)
self.permanentHighlightPt(self.mid_right)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.mid_left, self.mid_right))
self.mode += 1
else:
mid_left = new_pt
mid_right = Geometry2D.mirrorPt(mid_left, Geometry2D.LineSegment(self.mid_center, self.top_center))
self.highlightPt(mid_left)
self.highlightPt(mid_right)
self.highlightSegment(Geometry2D.LineSegment(mid_left, mid_right))
elif self.mode == 3:
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.left_leg_center = new_pt
self.right_leg_center = Geometry2D.mirrorPt(
self.left_leg_center, Geometry2D.LineSegment(self.mid_center, self.top_center)
)
self.permanentHighlightPt(self.left_leg_center)
self.permanentHighlightPt(self.right_leg_center)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.left_leg_center, self.mid_left))
self.permanentHighlightSegment(Geometry2D.LineSegment(self.right_leg_center, self.mid_right))
self.mode += 1
else:
left_leg_center = new_pt
right_leg_center = Geometry2D.mirrorPt(
left_leg_center, Geometry2D.LineSegment(self.mid_center, self.top_center)
)
self.highlightPt(left_leg_center)
self.highlightPt(right_leg_center)
self.highlightSegment(Geometry2D.LineSegment(left_leg_center, self.mid_left))
self.highlightSegment(Geometry2D.LineSegment(right_leg_center, self.mid_right))
elif self.mode == 4:
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.left_leg_left = new_pt
self.left_leg_right = Geometry2D.LineSegment(self.left_leg_center, self.left_leg_left).extrapolate(-1)
self.right_leg_left = Geometry2D.mirrorPt(
self.left_leg_right, Geometry2D.LineSegment(self.mid_center, self.top_center)
)
self.right_leg_right = Geometry2D.mirrorPt(
self.left_leg_left, Geometry2D.LineSegment(self.mid_center, self.top_center)
)
self.permanentHighlightPt(self.left_leg_left)
self.permanentHighlightPt(self.left_leg_right)
self.permanentHighlightPt(self.right_leg_left)
self.permanentHighlightPt(self.right_leg_right)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.left_leg_left, self.left_leg_right))
self.permanentHighlightSegment(Geometry2D.LineSegment(self.right_leg_left, self.right_leg_right))
top_displ = Geometry2D.ptDiff(self.top_center, self.mid_center)
left_displ = Geometry2D.ptDiff(self.left_leg_left, self.left_leg_center)
self.top_left = Geometry2D.ptSum(Geometry2D.ptSum(self.mid_left, top_displ), left_displ)
self.top_right = Geometry2D.mirrorPt(
self.top_left, Geometry2D.LineSegment(self.mid_center, self.top_center)
)
self.permanentHighlightPt(self.top_left)
self.permanentHighlightPt(self.top_right)
self.mode += 1
else:
left_leg_left = new_pt
left_leg_right = Geometry2D.LineSegment(self.left_leg_center, left_leg_left).extrapolate(-1)
right_leg_left = Geometry2D.mirrorPt(
left_leg_right, Geometry2D.LineSegment(self.mid_center, self.top_center)
)
right_leg_right = Geometry2D.mirrorPt(
left_leg_left, Geometry2D.LineSegment(self.mid_center, self.top_center)
)
self.highlightPt(left_leg_left)
self.highlightPt(left_leg_right)
self.highlightPt(right_leg_left)
self.highlightPt(right_leg_right)
self.highlightSegment(Geometry2D.LineSegment(left_leg_left, left_leg_right))
self.highlightSegment(Geometry2D.LineSegment(right_leg_left, right_leg_right))
top_displ = Geometry2D.ptDiff(self.top_center, self.mid_center)
left_displ = Geometry2D.ptDiff(left_leg_left, self.left_leg_center)
top_left = Geometry2D.ptSum(Geometry2D.ptSum(self.mid_left, top_displ), left_displ)
top_right = Geometry2D.mirrorPt(top_left, Geometry2D.LineSegment(self.mid_center, self.top_center))
self.highlightPt(top_left)
self.highlightPt(top_right)
self.count = self.count + 1
return adjusted_foldline
def adjustFoldline(self,intended):
#intended.expand(10)
start = self.convert_to_world_frame(intended.start())
end = self.convert_to_world_frame(intended.end())
try:
srv = rospy.ServiceProxy('adjust_fold',AdjustFold)
resp = srv(start,end)
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
return False
new_start = self.convert_from_world_frame(resp.start)
new_end = self.convert_from_world_frame(resp.end)
newfold = Geometry2D.DirectedLineSegment(new_start,new_end)
newfold.expand(0.1)
return newfold
def send_traj(self,fold_traj):
try:
srv = rospy.ServiceProxy('execute_fold',ExecuteFold)
resp = srv(ExecuteFoldRequest(fold_traj=fold_traj))
return True
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
return False
def main(args):
rospy.init_node("poly_gui_bridge")
pg = PolyGUIBridge()
def received_fold(self,foldline,active_vertices,red):
pre_fold_world_frame = []
start_fold_world_frame = []
quarter_fold_world_frame = []
weight_fold_world_frame = []
mid_fold_world_frame = []
end_fold_world_frame = []
tilts = []
if self.mode == "pants" and self.count == 1:
if len(active_vertices) > 1:
active_vertices.remove(min(active_vertices,key=lambda v: v.x))
#Active vertices will be sorted by order of pickup
for active_vert in sorted(active_vertices,key=lambda pt: -1 * Geometry2D.ptLineDisplacement(pt,foldline).length()):
displ = Geometry2D.ptLineDisplacement(active_vert,foldline)
if displ.length() == 0:
start_fold = self.convert_to_world_frame(active_vert)
else:
start_fold = self.convert_to_world_frame(active_vert)
in_amt = 0.015
start_fold = self.convert_to_world_frame(displ.extrapolate(in_amt*self.scale_factor/displ.length()))
start_fold.point.z -= 0.00
start_fold_world_frame.append(start_fold)
pre_fold = self.convert_to_world_frame(displ.extrapolate(-0.015*self.scale_factor/displ.length()))
pre_fold.point.z += 0.04
pre_fold_world_frame.append(pre_fold)
quarter_fold = self.convert_to_world_frame(displ.extrapolate(1.0/4.0))
quarter_fold.point.z += displ.length()/(4*abs(self.scale_factor))
quarter_fold_world_frame.append(quarter_fold)
weight_fold = self.convert_to_world_frame(displ.extrapolate(1.0/2.0))
weight_fold.point.z += displ.length()/(2*abs(self.scale_factor))
weight_fold_world_frame.append(weight_fold)
mid_fold = self.convert_to_world_frame(displ.end())
mid_fold.point.z += displ.length()/abs(self.scale_factor)
mid_fold_world_frame.append(mid_fold)
end_fold = self.convert_to_world_frame(displ.extrapolate(2.00 - 0.01*self.scale_factor/displ.length()))#Was 2.00
end_fold.point.z += 0.03 #was 0.035
#if self.count == 0 or self.count == 3:
# end_fold = mid_fold
end_fold_world_frame.append(end_fold)
now = rospy.Time.now()
self.listener.waitForTransform("base_footprint",mid_fold.header.frame_id,now,rospy.Duration(10.0))
mid_fold_base = self.listener.transformPoint("base_footprint",mid_fold)
start_fold_base = self.listener.transformPoint("base_footprint",start_fold)
dx = mid_fold_base.point.x - start_fold_base.point.x
dy = mid_fold_base.point.y - start_fold_base.point.y
tilt = arctan(dy /(dx+0.00001))
if dx < 0 and not red:
tilt += pi
elif dx > 0 and red:
tilt += pi
tilt = (tilt + pi) % (2*pi) - pi
tilts.append(tilt)
fold_traj = FoldTraj()
fold_traj.approach_points = pre_fold_world_frame
fold_traj.grip_points = start_fold_world_frame
fold_traj.quarter_points = quarter_fold_world_frame
fold_traj.weight_points = weight_fold_world_frame
fold_traj.vertical_points = mid_fold_world_frame
fold_traj.goal_points = end_fold_world_frame
fold_traj.tilts = tilts
fold_traj.red = red
fold_traj.smooth_center = self.convert_to_world_frame(foldline.center())
fold_traj.smooth_edges = [self.convert_to_world_frame(pt) for pt in sorted(foldline.pts(),key=lambda pt: pt.x())]
if foldline.length() < 0.14:
fold_traj.ignore_smooth = True
success = self.send_traj(fold_traj)
if ADJUST_FOLDLINE:
adjusted_foldline = self.adjustFoldline(intended=foldline)
else:
adjusted_foldline = foldline
def skelDrawer(self, event, x, y, flags, param):
if self.mode == 0:
#Draw spine_bottom
print "Select midle of a spine_bottom"
if event == cv.CV_EVENT_LBUTTONUP:
self.spine_bottom = Geometry2D.Point(x, y)
self.permanentHighlightPt(self.spine_bottom)
self.mode += 1
else:
pass
elif self.mode == 1:
print "Select bottom point of a collar"
if event == cv.CV_EVENT_LBUTTONUP:
self.spine_top = Geometry2D.Point(x, y)
self.permanentHighlightPt(self.spine_top)
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.mode += 1
else:
self.highlightSegment(
Geometry2D.LineSegment(self.spine_bottom,
Geometry2D.Point(x, y)))
elif self.mode == 2:
#Draw collar
print "Select left upper point of a collar"
if event == cv.CV_EVENT_LBUTTONUP:
self.collar = Geometry2D.Point(x, y)
self.virtual_collar = Geometry2D.mirrorPt(
self.collar,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.permanentHighlightPt(self.collar)
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.spine_top, self.collar))
self.permanentHighlightPt(self.virtual_collar)
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.spine_top,
self.virtual_collar))
self.mode += 1
else:
temp_collar = Geometry2D.Point(x, y)
virtual_collar = Geometry2D.mirrorPt(
temp_collar,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.highlightPt(temp_collar)
self.highlightSegment(
Geometry2D.LineSegment(self.spine_top, temp_collar))
self.highlightPt(virtual_collar)
self.highlightSegment(
Geometry2D.LineSegment(self.spine_top, virtual_collar))
elif self.mode == 3:
#Draw shoulder
print "Select point between a left shoulder and a left armpit"
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.shoulder_joint = new_pt
self.virtual_shoulder_joint = Geometry2D.mirrorPt(
self.shoulder_joint,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.permanentHighlightPt(self.shoulder_joint)
self.permanentHighlightPt(self.virtual_shoulder_joint)
seg = Geometry2D.LineSegment(self.shoulder_joint,
self.virtual_shoulder_joint)
self.permanentHighlightSegment(seg)
self.bottom_left = Geometry2D.Point(
self.spine_bottom.x() - 0.5 * seg.dx(),
self.spine_bottom.y() - 0.5 * seg.dy())
self.bottom_right = Geometry2D.Point(
self.spine_bottom.x() + 0.5 * seg.dx(),
self.spine_bottom.y() + 0.5 * seg.dy())
bottom_seg = Geometry2D.LineSegment(self.bottom_left,
self.bottom_right)
self.permanentHighlightPt(self.bottom_left)
self.permanentHighlightPt(self.bottom_right)
self.permanentHighlightSegment(bottom_seg)
self.mode += 1
else:
temp_shoulder_joint = new_pt
virtual_shoulder_joint = Geometry2D.mirrorPt(
temp_shoulder_joint,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.highlightPt(temp_shoulder_joint)
self.highlightPt(virtual_shoulder_joint)
seg = Geometry2D.LineSegment(temp_shoulder_joint,
virtual_shoulder_joint)
self.highlightSegment(seg)
bottom_left = Geometry2D.Point(
self.spine_bottom.x() - 0.5 * seg.dx(),
self.spine_bottom.y() - 0.5 * seg.dy())
bottom_right = Geometry2D.Point(
self.spine_bottom.x() + 0.5 * seg.dx(),
self.spine_bottom.y() + 0.5 * seg.dy())
bottom_seg = Geometry2D.LineSegment(bottom_left, bottom_right)
self.highlightPt(bottom_left)
self.highlightPt(bottom_right)
self.highlightSegment(bottom_seg)
elif self.mode == 4:
#Draw shoulder
print "Select point in a left shoulder"
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.shoulder_top = new_pt
seg = Geometry2D.LineSegment(self.shoulder_top,
self.shoulder_joint)
self.armpit = seg.extrapolate(2.0)
self.virtual_shoulder_top = Geometry2D.mirrorPt(
self.shoulder_top,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.virtual_armpit = Geometry2D.mirrorPt(
self.armpit,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.permanentHighlightPt(self.shoulder_top)
self.permanentHighlightPt(self.armpit)
self.permanentHighlightPt(self.virtual_shoulder_top)
self.permanentHighlightPt(self.virtual_armpit)
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.armpit, self.shoulder_top))
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.virtual_armpit,
self.virtual_shoulder_top))
self.mode += 1
else:
shoulder_top = new_pt
seg = Geometry2D.LineSegment(shoulder_top, self.shoulder_joint)
armpit = seg.extrapolate(2.0)
virtual_shoulder_top = Geometry2D.mirrorPt(
shoulder_top,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
virtual_armpit = Geometry2D.mirrorPt(
armpit,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.highlightPt(shoulder_top)
self.highlightPt(armpit)
self.highlightPt(virtual_shoulder_top)
self.highlightPt(virtual_armpit)
self.highlightSegment(
Geometry2D.LineSegment(armpit, shoulder_top))
self.highlightSegment(
Geometry2D.LineSegment(virtual_armpit,
virtual_shoulder_top))
elif self.mode == 5:
#Draw sleeve
print "Select midle point at the end of a left sleeve"
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.sleeve_node = new_pt
self.virtual_sleeve_node = Geometry2D.mirrorPt(
self.sleeve_node,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.permanentHighlightPt(self.sleeve_node)
self.permanentHighlightPt(self.virtual_sleeve_node)
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.shoulder_joint,
self.sleeve_node))
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.virtual_shoulder_joint,
self.virtual_sleeve_node))
self.mode += 1
else:
temp_sleeve_node = new_pt
virtual_sleeve_node = Geometry2D.mirrorPt(
temp_sleeve_node,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.highlightPt(temp_sleeve_node)
self.highlightPt(virtual_sleeve_node)
self.highlightSegment(
Geometry2D.LineSegment(self.shoulder_joint,
temp_sleeve_node))
self.highlightSegment(
Geometry2D.LineSegment(self.virtual_shoulder_joint,
virtual_sleeve_node))
elif self.mode == 6:
#Draw shoulder
print "Select upper end point at the end of a left sleeve"
new_pt = Geometry2D.Point(x, y)
if event == cv.CV_EVENT_LBUTTONUP:
self.sleeve_top = new_pt
seg = Geometry2D.LineSegment(self.sleeve_top, self.sleeve_node)
self.sleeve_bottom = seg.extrapolate(2.0)
self.virtual_sleeve_top = Geometry2D.mirrorPt(
self.sleeve_top,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.virtual_sleeve_bottom = Geometry2D.mirrorPt(
self.sleeve_bottom,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.permanentHighlightPt(self.sleeve_top)
self.permanentHighlightPt(self.sleeve_bottom)
self.permanentHighlightPt(self.virtual_sleeve_top)
self.permanentHighlightPt(self.virtual_sleeve_bottom)
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.sleeve_bottom,
self.sleeve_top))
self.permanentHighlightSegment(
Geometry2D.LineSegment(self.virtual_sleeve_bottom,
self.virtual_sleeve_top))
#self.addCVShape(CVPolygon(cv.CV_RGB(0,0,0),self.front(),
# Geometry2D.Polygon( self.bottom_left,self.armpit,self.sleeve_bottom,self.sleeve_top,self.shoulder_top,self.collar,
# self.virtual_collar,self.virtual_shoulder_top,self.virtual_sleeve_top,self.virtual_sleeve_bottom,self.virtual_armpit,self.bottom_right)))
self.mode += 1
else:
sleeve_top = new_pt
seg = Geometry2D.LineSegment(sleeve_top, self.sleeve_node)
sleeve_bottom = seg.extrapolate(2.0)
virtual_sleeve_top = Geometry2D.mirrorPt(
sleeve_top,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
virtual_sleeve_bottom = Geometry2D.mirrorPt(
sleeve_bottom,
Geometry2D.LineSegment(self.spine_bottom, self.spine_top))
self.highlightPt(sleeve_top)
self.highlightPt(sleeve_bottom)
self.highlightPt(virtual_sleeve_top)
self.highlightPt(virtual_sleeve_bottom)
self.highlightSegment(
Geometry2D.LineSegment(sleeve_bottom, sleeve_top))
self.highlightSegment(
Geometry2D.LineSegment(virtual_sleeve_bottom,
virtual_sleeve_top))
def getShape(self): pt1 = self.getOrigin() pt2 = Geometry2D.Point(pt1.x(),pt1.y()-self.getSliderHeight()) pt3 = Geometry2D.Point(pt2.x()+self.getSliderWidth(),pt2.y()) pt4 = Geometry2D.Point(pt3.x(),pt1.y()) return Geometry2D.Polygon(pt1,pt2,pt3,pt4)
def skelDrawer(self,event,x,y,flags,param):
if self.mode==0:
#Draw spine_bottom
if event==cv.CV_EVENT_LBUTTONUP:
self.permanentHighlightPt(self.toe_center)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.ankle_joint,self.toe_center))
self.mode += 1
else:
self.highlightPoint(self.toe_center)
self.highlightSegment(Geometry2D.LineSegment(self.ankle_joint,self.toe_center))
self.spine_bottom = Geometry2D.Point(x,y)
self.permanentHighlightPt(self.spine_bottom)
self.mode += 1
elif self.mode==1:
if event==cv.CV_EVENT_LBUTTONUP:
self.spine_top = Geometry2D.Point(x,y)
self.permanentHighlightPt(self.spine_top)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
self.mode += 1
else:
self.highlightSegment(Geometry2D.LineSegment(self.spine_bottom,Geometry2D.Point(x,y)))
elif self.mode==2:
#Draw collar
if event==cv.CV_EVENT_LBUTTONUP:
self.collar = Geometry2D.Point(x,y)
self.virtual_collar = Geometry2D.mirrorPt(self.collar,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
self.permanentHighlightPt(self.collar)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.spine_top,self.collar))
self.permanentHighlightPt(self.virtual_collar)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.spine_top,self.virtual_collar))
self.mode += 1
else:
temp_collar = Geometry2D.Point(x,y)
virtual_collar = Geometry2D.mirrorPt(temp_collar,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
self.highlightPt(temp_collar)
self.highlightSegment(Geometry2D.LineSegment(self.spine_top,temp_collar))
self.highlightPt(virtual_collar)
self.highlightSegment(Geometry2D.LineSegment(self.spine_top,virtual_collar))
elif self.mode==3:
#Draw shoulder
new_pt = Geometry2D.Point(x,y)
if event==cv.CV_EVENT_LBUTTONUP:
self.shoulder_joint = new_pt
self.virtual_shoulder_joint = Geometry2D.mirrorPt(self.shoulder_joint,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
self.permanentHighlightPt(self.shoulder_joint)
self.permanentHighlightPt(self.virtual_shoulder_joint)
seg = Geometry2D.LineSegment(self.shoulder_joint,self.virtual_shoulder_joint)
self.permanentHighlightSegment(seg)
self.bottom_left = Geometry2D.Point(self.spine_bottom.x()-0.5*seg.dx(),self.spine_bottom.y()-0.5*seg.dy())
self.bottom_right = Geometry2D.Point(self.spine_bottom.x()+0.5*seg.dx(),self.spine_bottom.y()+0.5*seg.dy())
bottom_seg = Geometry2D.LineSegment(self.bottom_left,self.bottom_right)
self.permanentHighlightPt(self.bottom_left)
self.permanentHighlightPt(self.bottom_right)
self.permanentHighlightSegment(bottom_seg)
self.mode += 1
else:
temp_shoulder_joint = new_pt
virtual_shoulder_joint = Geometry2D.mirrorPt(temp_shoulder_joint,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
self.highlightPt(temp_shoulder_joint)
self.highlightPt(virtual_shoulder_joint)
seg = Geometry2D.LineSegment(temp_shoulder_joint,virtual_shoulder_joint)
self.highlightSegment(seg)
bottom_left = Geometry2D.Point(self.spine_bottom.x()-0.5*seg.dx(),self.spine_bottom.y()-0.5*seg.dy())
bottom_right = Geometry2D.Point(self.spine_bottom.x()+0.5*seg.dx(),self.spine_bottom.y()+0.5*seg.dy())
bottom_seg = Geometry2D.LineSegment(bottom_left,bottom_right)
self.highlightPt(bottom_left)
self.highlightPt(bottom_right)
self.highlightSegment(bottom_seg)
elif self.mode==4:
new_pt = Geometry2D.Point(x,y)
if event==cv.CV_EVENT_LBUTTONUP:
self.shoulder_top = new_pt
seg = Geometry2D.LineSegment(self.shoulder_top,self.shoulder_joint)
self.armpit = seg.extrapolate(2.0)
self.virtual_shoulder_top = Geometry2D.mirrorPt(self.shoulder_top,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
self.virtual_armpit = Geometry2D.mirrorPt(self.armpit,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
self.permanentHighlightPt(self.shoulder_top)
self.permanentHighlightPt(self.armpit)
self.permanentHighlightPt(self.virtual_shoulder_top)
self.permanentHighlightPt(self.virtual_armpit)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.armpit,self.shoulder_top))
self.permanentHighlightSegment(Geometry2D.LineSegment(self.virtual_armpit,self.virtual_shoulder_top))
self.mode+=1
else:
shoulder_top = new_pt
seg = Geometry2D.LineSegment(shoulder_top,self.shoulder_joint)
armpit = seg.extrapolate(2.0)
virtual_shoulder_top = Geometry2D.mirrorPt(shoulder_top,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
virtual_armpit = Geometry2D.mirrorPt(armpit,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
self.highlightPt(shoulder_top)
self.highlightPt(armpit)
self.highlightPt(virtual_shoulder_top)
self.highlightPt(virtual_armpit)
self.highlightSegment(Geometry2D.LineSegment(armpit,shoulder_top))
self.highlightSegment(Geometry2D.LineSegment(virtual_armpit,virtual_shoulder_top))
elif self.mode==5:
new_pt = Geometry2D.Point(x,y)
if event==cv.CV_EVENT_LBUTTONUP:
self.sleeve_node = new_pt
self.virtual_sleeve_node = Geometry2D.mirrorPt(self.sleeve_node,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
self.permanentHighlightPt(self.sleeve_node)
self.permanentHighlightPt(self.virtual_sleeve_node)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.shoulder_joint,self.sleeve_node))
self.permanentHighlightSegment(Geometry2D.LineSegment(self.virtual_shoulder_joint,self.virtual_sleeve_node))
self.mode += 1
else:
temp_sleeve_node = new_pt
virtual_sleeve_node = Geometry2D.mirrorPt(temp_sleeve_node,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
self.highlightPt(temp_sleeve_node)
self.highlightPt(virtual_sleeve_node)
self.highlightSegment(Geometry2D.LineSegment(self.shoulder_joint,temp_sleeve_node))
self.highlightSegment(Geometry2D.LineSegment(self.virtual_shoulder_joint,virtual_sleeve_node))
elif self.mode==6:
new_pt = Geometry2D.Point(x,y)
if event==cv.CV_EVENT_LBUTTONUP:
self.sleeve_top = new_pt
seg = Geometry2D.LineSegment(self.sleeve_top,self.sleeve_node)
self.sleeve_bottom = seg.extrapolate(2.0)
self.virtual_sleeve_top = Geometry2D.mirrorPt(self.sleeve_top,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
self.virtual_sleeve_bottom = Geometry2D.mirrorPt(self.sleeve_bottom,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
self.permanentHighlightPt(self.sleeve_top)
self.permanentHighlightPt(self.sleeve_bottom)
self.permanentHighlightPt(self.virtual_sleeve_top)
self.permanentHighlightPt(self.virtual_sleeve_bottom)
self.permanentHighlightSegment(Geometry2D.LineSegment(self.sleeve_bottom,self.sleeve_top))
self.permanentHighlightSegment(Geometry2D.LineSegment(self.virtual_sleeve_bottom,self.virtual_sleeve_top))
#self.addCVShape(CVPolygon(cv.CV_RGB(0,0,0),self.front(),
# Geometry2D.Polygon( self.bottom_left,self.armpit,self.sleeve_bottom,self.sleeve_top,self.shoulder_top,self.collar,
# self.virtual_collar,self.virtual_shoulder_top,self.virtual_sleeve_top,self.virtual_sleeve_bottom,self.virtual_armpit,self.bottom_right)))
self.mode+=1
else:
sleeve_top = new_pt
seg = Geometry2D.LineSegment(sleeve_top,self.sleeve_node)
sleeve_bottom = seg.extrapolate(2.0)
virtual_sleeve_top = Geometry2D.mirrorPt(sleeve_top,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
virtual_sleeve_bottom = Geometry2D.mirrorPt(sleeve_bottom,Geometry2D.LineSegment(self.spine_bottom,self.spine_top))
self.highlightPt(sleeve_top)
self.highlightPt(sleeve_bottom)
self.highlightPt(virtual_sleeve_top)
self.highlightPt(virtual_sleeve_bottom)
self.highlightSegment(Geometry2D.LineSegment(sleeve_bottom,sleeve_top))
self.highlightSegment(Geometry2D.LineSegment(virtual_sleeve_bottom,virtual_sleeve_top))
def received_fold(self,foldline,active_vertices,red,fold_type):
pre_fold_world_frame = []
start_fold_world_frame = []
quarter_fold_world_frame = []
weight_fold_world_frame = []
mid_fold_world_frame = []
end_fold_world_frame = []
tilts = []
if self.mode == "pants" and self.count == 1:
if len(active_vertices) > 1:
active_vertices.remove(min(active_vertices,key=lambda v: v.x))
if fold_type == 'hang':
for active_vert in sorted(active_vertices,key=lambda pt: -1 * Geometry2D.ptLineDisplacement(pt,foldline).length()):
displ = Geometry2D.ptLineDisplacement(active_vert,foldline)
in_amt = 0.015
start_fold = self.convert_to_world_frame(displ.extrapolate(in_amt*self.scale_factor/displ.length()))
start_fold.point.z -= 0.00
start_fold_world_frame.append(start_fold)
pre_fold = self.convert_to_world_frame(displ.extrapolate(-0.015*self.scale_factor/displ.length()))
pre_fold.point.z += 0.04
pre_fold_world_frame.append(pre_fold)
quarter_fold = pre_fold
quarter_fold_world_frame.append(quarter_fold)
weight_fold = pre_fold
weight_fold_world_frame.append(weight_fold)
mid_fold = pre_fold
mid_fold_world_frame.append(mid_fold)
end_fold = self.convert_to_world_frame(displ.extrapolate(-2.00 + 0.01*self.scale_factor/displ.length()))
end_fold.point.z += 0.03
end_fold_world_frame.append(end_fold)
now = rospy.Time.now()
self.listener.waitForTransform("base_footprint",mid_fold.header.frame_id,now,rospy.Duration(10.0))
mid_fold_base = self.listener.transformPoint("base_footprint",mid_fold)
start_fold_base = self.listener.transformPoint("base_footprint",start_fold)
dx = mid_fold_base.point.x - start_fold_base.point.x
dy = mid_fold_base.point.y - start_fold_base.point.y
tilt = arctan(dy /(dx+0.00001))
if dx < 0 and not red:
tilt += pi
elif dx > 0 and red:
tilt += pi
tilt = (tilt + pi) % (2*pi) - pi
tilts.append(tilt)
else:
#Active vertices will be sorted by order of pickup
for active_vert in sorted(active_vertices,key=lambda pt: -1 * Geometry2D.ptLineDisplacement(pt,foldline).length()):
displ = Geometry2D.ptLineDisplacement(active_vert,foldline)
if displ.length() == 0:
start_fold = self.convert_to_world_frame(active_vert)
else:
start_fold = self.convert_to_world_frame(active_vert)
in_amt = 0.015
start_fold = self.convert_to_world_frame(displ.extrapolate(in_amt*self.scale_factor/displ.length()))
start_fold.point.z -= 0.00
start_fold_world_frame.append(start_fold)
pre_fold = self.convert_to_world_frame(displ.extrapolate(-0.015*self.scale_factor/displ.length()))
pre_fold.point.z += 0.04
pre_fold_world_frame.append(pre_fold)
quarter_fold = self.convert_to_world_frame(displ.extrapolate(1.0/4.0))
quarter_fold.point.z += displ.length()/(4*abs(self.scale_factor))
quarter_fold_world_frame.append(quarter_fold)
weight_fold = self.convert_to_world_frame(displ.extrapolate(1.0/2.0))
weight_fold.point.z += displ.length()/(2*abs(self.scale_factor))
weight_fold_world_frame.append(weight_fold)
mid_fold = self.convert_to_world_frame(displ.end())
mid_fold.point.z += displ.length()/abs(self.scale_factor)
mid_fold_world_frame.append(mid_fold)
end_fold = self.convert_to_world_frame(displ.extrapolate(2.00 - 0.01*self.scale_factor/displ.length()))#Was 2.00
end_fold.point.z += 0.03 #was 0.035
#if self.count == 0 or self.count == 3:
# end_fold = mid_fold
end_fold_world_frame.append(end_fold)
now = rospy.Time.now()
self.listener.waitForTransform("base_footprint",mid_fold.header.frame_id,now,rospy.Duration(10.0))
mid_fold_base = self.listener.transformPoint("base_footprint",mid_fold)
start_fold_base = self.listener.transformPoint("base_footprint",start_fold)
dx = mid_fold_base.point.x - start_fold_base.point.x
dy = mid_fold_base.point.y - start_fold_base.point.y
tilt = arctan(dy /(dx+0.00001))
if dx < 0 and not red:
tilt += pi
elif dx > 0 and red:
tilt += pi
tilt = (tilt + pi) % (2*pi) - pi
tilts.append(tilt)
fold_traj = FoldTraj()
fold_traj.approach_points = pre_fold_world_frame
fold_traj.grip_points = start_fold_world_frame
fold_traj.quarter_points = quarter_fold_world_frame
fold_traj.weight_points = weight_fold_world_frame
fold_traj.vertical_points = mid_fold_world_frame
fold_traj.goal_points = end_fold_world_frame
fold_traj.tilts = tilts
fold_traj.red = red
fold_traj.smooth_center = self.convert_to_world_frame(foldline.center())
fold_traj.smooth_edges = [self.convert_to_world_frame(pt) for pt in sorted(foldline.pts(),key=lambda pt: pt.x())]
if foldline.length() < 0.14:
fold_traj.ignore_smooth = True
success = self.send_traj(fold_traj)
if ADJUST_FOLDLINE:
adjusted_foldline = self.adjustFoldline(intended=foldline)
else:
adjusted_foldline = foldline
self.count = self.count + 1
return adjusted_foldline
def getHighlighted(self,x,y): return [cvShape for cvShape in self.getShapes() if cvShape.getShape().contains(Geometry2D.Point(x,y))]
