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))]