def __init__(self): self.count = 0 self.listener = tf.TransformListener() self.mode = rospy.get_param('~mode',"default") self.gui = FoldingGUI(name="poly_gui_bridge") self.gui.setFoldCallback(self.received_fold) self.gui.setGripperLimit(2) self.traj_pub = rospy.Publisher("trajectory_output",FoldTraj) self.scale_factor = self.x_offset = self.y_offset = self.poly_frame = False self.poly_points = [] now = time.localtime() self.logfile = open('/tmp/folding_%04d-%02d-%02d__%02d-%02d-%02d.log'%(now.tm_year,now.tm_mon,now.tm_mday,now.tm_hour,now.tm_min,now.tm_sec),'w') self.poly_sub = rospy.Subscriber("input",PolyStamped,self.poly_handler) self.start_time = rospy.Time.now()
def __init__(self): util.listener = tf.TransformListener() rospy.sleep(3) self.robot = Robot.Robot() #self.robot.robotposition = "table_front_left" self.gui = FoldingGUI(name="Geometry_Based_Folding") self.mode = util.mode #self.table_preset() self.poly_sub = rospy.Subscriber("input",PolyStamped,self.poly_handler) self.stereo_sub = rospy.Subscriber("stereo_points_3d",PointStamped,self.stereo_handler) #self.scale_factor = self.x_offset = self.y_offset = self.poly_frame = False util.scale_factor = 5.0/0.0254 self.poly_points = [] self.corrected_gripPoint_latest = None now = time.localtime() self.logfile = open('/tmp/folding_%04d-%02d-%02d__%02d-%02d-%02d.log'%(now.tm_year,now.tm_mon,now.tm_mday,now.tm_hour,now.tm_min,now.tm_sec),'w') self.start_time = rospy.Time.now() rospy.loginfo("READY TO GO") self.makePolyFns = [self.gui.makeSmallTowel, self.gui.makeBigTowel, self.gui.makePants,\ self.gui.makeShirt, self.gui.makeLongSleeveShirt];
class PolyGUIBridge(): def __init__(self): self.count = 0 self.listener = tf.TransformListener() self.mode = rospy.get_param('~mode',"default") self.gui = FoldingGUI(name="poly_gui_bridge") self.gui.setFoldCallback(self.received_fold) self.gui.setGripperLimit(2) self.traj_pub = rospy.Publisher("trajectory_output",FoldTraj) self.scale_factor = self.x_offset = self.y_offset = self.poly_frame = False self.poly_points = [] now = time.localtime() self.logfile = open('/tmp/folding_%04d-%02d-%02d__%02d-%02d-%02d.log'%(now.tm_year,now.tm_mon,now.tm_mday,now.tm_hour,now.tm_min,now.tm_sec),'w') self.poly_sub = rospy.Subscriber("input",PolyStamped,self.poly_handler) self.start_time = rospy.Time.now() #Receives a stream of polygon vertices and updates the poly appropriately def poly_handler(self,stamped_poly): rospy.loginfo("received a point") 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) #Waits til it has received enough, then folds the article sketched def handle_automatic_folds(self,vertices): if len(vertices) == 10 and self.mode == "shirt": self.start_logging() self.gui.foldShirt_v3() self.stop_logging() elif len(vertices) == 10 and self.mode == "tee": self.start_logging() self.gui.foldTeeNoSleeve() self.stop_logging() elif len(vertices) == 7 and self.mode == "pants": self.start_logging() self.gui.foldPants_v2() self.stop_logging() elif len(vertices) == 4 and self.mode == "towel": self.start_logging() self.gui.foldTowelThirds() self.stop_logging() #Logging def start_logging(self): self.start_time = rospy.Time.now() msg = "Starting to execute fold of type %s"%self.mode rospy.loginfo(msg) self.logfile.write("%s\n"%msg) def stop_logging(self): self.end_time = rospy.Time.now() dur = self.end_time - self.start_time msg = "Finished %s. Duration: %d.%d seconds"%(self.mode,dur.secs,dur.nsecs) rospy.loginfo(msg) self.logfile.write("%s\n"%msg) # Centers the polygon and scales it appropriately, so it will fit in the window 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] # Convert a 2D vertex to a 3D world point def convert_to_world_frame(self,pt2D): newx = pt2D.x() - self.x_offset newy = pt2D.y() - self.y_offset newx /= self.scale_factor newy /= self.scale_factor newPt = PointStamped() newPt.header.stamp = rospy.Time.now() newPt.header.frame_id = self.poly_frame newPt.point.x = -1*newy newPt.point.y = -1*newx newPt.point.z = self.z_offset return newPt # Project a 3D world point onto the 2D window 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) #Called by the GUI once a fold has been drawn. Tells me to start folding. 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 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
class PolyGUIBridge(): def __init__(self): self.count = 0 self.listener = tf.TransformListener() self.mode = rospy.get_param('~mode',"default") self.gui = FoldingGUI(name="poly_gui_bridge") self.gui.setFoldCallback(self.received_fold) self.gui.setGripperLimit(2) self.traj_pub = rospy.Publisher("trajectory_output",FoldTraj) self.scale_factor = self.x_offset = self.y_offset = self.poly_frame = False self.poly_points = [] now = time.localtime() self.logfile = open('/tmp/folding_%04d-%02d-%02d__%02d-%02d-%02d.log'%(now.tm_year,now.tm_mon,now.tm_mday,now.tm_hour,now.tm_min,now.tm_sec),'w') self.poly_sub = rospy.Subscriber("input",PolyStamped,self.poly_handler) self.start_time = rospy.Time.now() #Receives a stream of polygon vertices and updates the poly appropriately 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) #Waits til it has received enough, then folds the article sketched def handle_automatic_folds(self,vertices): if len(vertices) == 10 and self.mode == "shirt": self.start_logging() self.gui.foldShirt_v3() self.stop_logging() elif len(vertices) == 10 and self.mode == "tee": self.start_logging() self.gui.foldTeeNoSleeve() self.stop_logging() elif len(vertices) == 7 and self.mode == "pants": self.start_logging() self.gui.foldPants_v2() self.stop_logging() elif len(vertices) == 4 and self.mode == "towel": self.start_logging() self.gui.foldTowelThirds() self.stop_logging() #Logging def start_logging(self): self.start_time = rospy.Time.now() msg = "Starting to execute fold of type %s"%self.mode rospy.loginfo(msg) self.logfile.write("%s\n"%msg) def stop_logging(self): self.end_time = rospy.Time.now() dur = self.end_time - self.start_time msg = "Finished %s. Duration: %d.%d seconds"%(self.mode,dur.secs,dur.nsecs) rospy.loginfo(msg) self.logfile.write("%s\n"%msg) # Centers the polygon and scales it appropriately, so it will fit in the window 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] # Convert a 2D vertex to a 3D world point def convert_to_world_frame(self,pt2D): newx = pt2D.x() - self.x_offset newy = pt2D.y() - self.y_offset newx /= self.scale_factor newy /= self.scale_factor newPt = PointStamped() newPt.header.stamp = rospy.Time.now() newPt.header.frame_id = self.poly_frame newPt.point.x = -1*newy newPt.point.y = -1*newx newPt.point.z = self.z_offset return newPt # Project a 3D world point onto the 2D window 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) #Called by the GUI once a fold has been drawn. Tells me to start folding. 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 self.count = self.count + 1 return adjusted_foldline
class FoldingMain(): def __init__(self): util.listener = tf.TransformListener() rospy.sleep(3) self.robot = Robot.Robot() #self.robot.robotposition = "table_front_left" self.gui = FoldingGUI(name="Geometry_Based_Folding") self.mode = util.mode #self.table_preset() self.poly_sub = rospy.Subscriber("input",PolyStamped,self.poly_handler) self.stereo_sub = rospy.Subscriber("stereo_points_3d",PointStamped,self.stereo_handler) #self.scale_factor = self.x_offset = self.y_offset = self.poly_frame = False util.scale_factor = 5.0/0.0254 self.poly_points = [] self.corrected_gripPoint_latest = None now = time.localtime() self.logfile = open('/tmp/folding_%04d-%02d-%02d__%02d-%02d-%02d.log'%(now.tm_year,now.tm_mon,now.tm_mday,now.tm_hour,now.tm_min,now.tm_sec),'w') self.start_time = rospy.Time.now() rospy.loginfo("READY TO GO") self.makePolyFns = [self.gui.makeSmallTowel, self.gui.makeBigTowel, self.gui.makePants,\ self.gui.makeShirt, self.gui.makeLongSleeveShirt]; """ # test version of poly handler def poly_handler(self,stamped_poly): if uil.BUSY == True: return rospy.loginfo("RECEIVED A POINT") points = stamped_poly.vertices #[Geometry2D.Point(point.x,point.y) for point in stamped_poly.vertices] vertices = [util.convert_from_world_frame(point) for point in points] #vertices = [util.convert_from_world_frame(point) for point in points] poly = Geometry2D.Polygon(*vertices) self.poly_cache = poly cvPoly = CVPolygon(Colors.GREEN,self.gui.front(self.gui.shapes),poly) self.gui.clearShapes() self.gui.addCVShape(cvPoly) vertices = self.gui.getPolys()[0].getShape().vertices() print "#vertices = ",len(vertices) if len(vertices) >= 2: self.robot.arms_test(vertices[0],vertices[1]) """ #Receives a stream of polygon vertices and updates the poly appropriately def getModel(self,bl): if(util.mode == 'tee'): return self.gui.makeBerkeleyProjectTee(bl) elif (util.mode =='bigTowel'): return self.gui.makeBigTowel(bl) elif (util.mode == 'towel'): return self.gui.makeSmallRedTowel(bl) elif (util.mode == 'shirt'): return self.gui.makeLongSleeveShirt(bl) elif (util.mode == 'skirt'): return self.gui.makeSkirt(bl) elif (util.mode == 'tie'): return self.gui.makeTie(bl) elif (util.mode == 'scarf'): return self.gui.makeScarf(bl) elif (util.mode == 'pants'): return self.gui.makePants(bl) elif (util.mode == 'vest'): return self.gui.makeVest(bl) else: return False def poly_handler(self,stamped_poly): rospy.loginfo("RECEIVED A POINT") if(util.BUSY == True): return #self.robot.arms_test() points = stamped_poly.vertices #[Geometry2D.Point(point.x,point.y) for point in stamped_poly.vertices] vertices = [util.convert_from_world_frame(point) for point in points] """ for pt in vertices: print pt """ if SIM_FLAG: if os.environ['UTIL_MODE']: util.mode = os.environ['UTIL_MODE'] self.mode = util.mode print "PRINTING UTIL MODE", util.mode tbl =Geometry2D.Point(151.979, 405.130237) tbf = Geometry2D.Point(224.808244, 488.925433) tbr = Geometry2D.Point(350.632802, 468.785788) self.table_detector([tbl,tbf,tbr]) bl = bottomLeftCorners[self.mode] bl = Geometry2D.Point(bl[0], bl[1]) poly = Geometry2D.Polygon(*self.getModel(bl)) #(*vertices) #poly = Geometry2D.Polygon(*self.gui.makePants(vertices[0])) #bl = Geometry2D.Point(200,470) #poly = Geometry2D.Polygon(*self.makePolyFns[self.article_ind](bl)) #(*vertices) #poly = Geometry2D.Polygon(*self.gui.makePants(vertices[0])) self.poly_cache = poly cvPoly = CVPolygon(Colors.PALEGREEN,self.gui.front(self.gui.shapes),poly) self.gui.clearShapes() self.gui.addCVShape(cvPoly) self.handle_automatic_folds(self.gui.getPolys()[0].getShape().vertices()) return # Edited for simulation # the first 6 define the table edge if TABLE_FLAG: #print vertices vertices = vertices[3:] else: self.table_detector(vertices) return if len(vertices) == 0: return #self.robot.arms_test() #poly = Geometry2D.Polygon(*self.gui.makeVest(vertices[0])) #(*vertices) #poly = Geometry2D.Polygon(*vertices) #poly = Geometry2D.Polygon(*self.gui.makeBerkeleyProjectTee(Geometry2D.Point(199.512588,368.866753))) if os.environ['UTIL_MODE']: util.mode = os.environ['UTIL_MODE'] self.mode = util.mode print "Current Mode is", util.mode poly = Geometry2D.Polygon(*self.getModel(vertices[0])) #poly = Geometry2D.Polygon(*self.gui.makeBigTowel(vertices[0])) #poly = Geometry2D.Polygon(*self.gui.makeBlackWillowTee(vertices[0])) #poly = Geometry2D.Polygon(*self.gui.makeSmallRedTowel(vertices[0])) #poly = Geometry2D.Polygon(*self.gui.makePants(vertices[0])) #poly = Geometry2D.Polygon(*self.gui.makeShirt(vertices[0])) self.poly_cache = poly cvPoly = CVPolygon(Colors.GREEN,self.gui.front(self.gui.shapes),poly) self.gui.clearShapes() self.gui.addCVShape(cvPoly) self.handle_automatic_folds(self.gui.getPolys()[0].getShape().vertices()) def stereo_handler(self,stamped_point): rospy.loginfo("STEREO POINT RECEIVED") if not util.BUSY: return else: point = Point2D(-stamped_point.point.y,-stamped_point.point.x) point = util.convert_from_world_frame(point) self.corrected_gripPoint_latest = point print "converted to",point.x(),point.y() # waits for 6 points and sets table def table_detector(self,vertices): global TABLE_FLAG #print "table_detector, numvertices=",vertices if len(vertices) == 3: print "table vertices" for vertex in vertices: print vertex.x(),vertex.y() TABLE_FLAG = True self.gui.createTable(vertices) self.gui.drawAllTables() return True return False def table_preset(self): left = Geometry2D.Point3d(Geometry2D.Point(152.979421441,357.344998331),0,None) center = Geometry2D.Point3d(Geometry2D.Point(256.599412286,419.188921359),0,None) right = Geometry2D.Point3d(Geometry2D.Point(344.034873648,354.753115415),0,None) vertices = [left,center,right] self.gui.createTable(vertices) self.gui.drawAllTables() return True #Waits til it has received enough, then folds the article sketched def handle_automatic_folds(self,vertices): print "self.mode ==" , self.mode if len(vertices) == 10 and self.mode == "shirt": self.start_logging() self.gui.foldShirt_v3() util.BUSY=True solution = FoldingSearch.FoldingSearch(self.gui,self.robot,self.gui.startpoly) self.robot.print_costs() self.stop_logging() self.stop_logging() elif len(vertices) == 10 and self.mode == "tee": if EXECUTE_FLAG: print "calling execute_tee_actions" util.BUSY = True actions = get_execute_BerkeleyProjectTee_actions_2() self.execute_actions(actions) sys.exit(0) #self.start_logging() self.gui.foldTeeNoSleeve() util.BUSY=True solution = FoldingSearch.FoldingSearch(self.gui,self.robot,self.gui.startpoly) self.robot.print_costs() print "Brett:: Hit a key to make me fold!" raw_input() self.execute_actions(solution) #actions = get_execute_tee_actions() #self.execute_actions(actions) return #self.start_logging() self.gui.foldTeeNoSleeve() solution = FoldingSearch.FoldingSearch(self.gui,self.robot,self.gui.startpoly) self.robot.print_costs() #self.stop_logging() elif len(vertices) == 7 and self.mode == "pants": self.start_logging() self.gui.foldPants_v2() util.BUSY=True solution = FoldingSearch.FoldingSearch(self.gui,self.robot,self.gui.startpoly) self.robot.print_costs() print "Brett:: Hit a key to make me fold!" raw_input() self.execute_actions(solution) self.stop_logging() elif len(vertices)== 4 and self.mode =="skirt": self.start_logging() self.gui.foldSkirt() util.BUSY = True solution = FoldingSearch.FoldingSearch(self.gui,self.robot,self.gui.startpoly) self.robot.print_costs() self.stop_logging() print "Brett:: Hit a key to make me fold!" raw_input() self.execute_actions(solution) elif len(vertices) == 9 and self.mode =="vest": self.start_logging() self.gui.foldVest() solution = FoldingSearch.FoldingSearch(self.gui,self.robot,self.gui.startpoly) self.robot.print_costs() self.stop_logging() elif len(vertices) == 6 and self.mode == "tie": self.start_logging() self.gui.foldTie() util.BUSY = True solution = FoldingSearch.FoldingSearch(self.gui,self.robot,self.gui.startpoly) self.robot.print_costs() self.stop_logging() print "Brett:: Hit a key to make me fold!" raw_input() self.execute_actions(solution) elif len(vertices) == 4 and (self.mode == 'scarf'): util.BUSY = True self.gui.foldScarf() solution = FoldingSearch.FoldingSearch(self.gui,self.robot,self.gui.startpoly) self.robot.print_costs() print "Brett:: Hit a key to make me fold!" raw_input() self.stop_logging() elif len(vertices) == 4 and (self.mode == "towel" or self.mode == "bigTowel"): util.BUSY = True #self.start_logging() """ # arms test stuff gripPts = vertices[0:2] endPts = vertices[2:] gripPts,endPts = self.gui.convertGripPts(gripPts,endPts) #gripPts = [self.robot.convert_to_robot_frame(util.convert_to_world_frame(gripPt),self.robot.robotposition) for gripPt in gripPts] #endPts = [self.robot.convert_to_robot_frame(util.convert_to_world_frame(endPt),self.robot.robotposition) for endPt in endPts] #vertices = [util.convert_to_world_frame(vertex) for vertex in vertices] #vertices = [(self.robot.convert_to_robot_frame(vertex,"table_front")) for vertex in vertices] #self.robot.arms_test(None,'l') self.robot.arms_test(endPts[1],endPts[0]) return """ # --------------------------------- self.gui.foldTowelHalves() util.BUSY=True solution = FoldingSearch.FoldingSearch(self.gui,self.robot,self.gui.startpoly) self.robot.print_costs() print "Brett:: Hit a key to make me fold!" raw_input() self.execute_actions(solution) #self.stop_logging() raw_input("Ended, New Article") return # Centers the polygon and scales it appropriately, so it will fit in the window 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 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 execute_actions(self,states): """ now execute the actions returned by the search """ i = 1 scoot_prev = 0 if not EXECUTE_FLAG or self.mode != 'tee': states=states[1:] for state in states: action = state.action print "\n\n\n\n\naction is ",action if (i < len(states)): child = states[i] if (action.get_actionType() == "fold" or action.get_actionType() == "drag"): gripPts3d,endPts3d = self.correct_foldpoints(state,scoot_prev,child) else: gripPts3d,endPts3d = (action.get_gripPoints(),action.get_endPoints()) # transform points to current frame of robot gripPts3d, endPts3d = self.gui.convertGripPts(gripPts3d,endPts3d) if action.get_actionType() in ("drag"): d = action.get_dragDistance()/util.scale_factor if action.get_actionType() == "fold": #endPts = [self.robot.convert_to_robot_frame(util.convert_to_world_frame(endPt),self.robot.robotposition) for endPt in endPts3d] color_current = action.get_foldType() # find type of next action if (i < len(states) and states[i].action.get_actionType()=="fold"): color_next = states[i].action.get_foldType() gripPts_next = states[i].action.get_gripPoints() endPts_next = states[i].action.get_endPoints() gripPts_next,endPts_next = self.gui.convertGripPts(gripPts_next,endPts_next) else: color_next = "blue" gripPts_next = None endPts_next = None # figure out starting scoot amount for next fold print "color_next",color_next if action.get_actionType() == "fold": (SUCCESS,scoot_prev) = self.robot.execute_fold(gripPts3d,endPts3d,color_current,color_next,scoot_prev) print "Fold successfully completed. Now at", scoot_prev elif action.get_actionType() == "drag": (SUCCESS,scoot_prev) = self.robot.execute_drag(gripPts3d,d,action.get_dragDirection(),color_next, gripPts_next, endPts_next,scoot_prev) print "Drag successfully completed. Now at", scoot_prev elif action.get_actionType() == "move": (SUCCESS,scoot_prev) = self.robot.execute_move(action.get_moveDestination(),scoot_prev) print "Move successfully completed. Now at", scoot_prev if not SUCCESS: rospy.loginfo("Failure to execute %s",action.get_actionType()) raw_input(" hit a key for next ACTION") i+=1 def start_logging(self): self.start_time = rospy.Time.now() msg = "Starting to execute fold of type %s"%self.mode rospy.loginfo(msg) self.logfile.write("%s\n"%msg) def stop_logging(self): self.end_time = rospy.Time.now() dur = self.end_time - self.start_time msg = "Finished %s. Duration: %d.%d seconds"%(self.mode,dur.secs,dur.nsecs) rospy.loginfo(msg) self.logfile.write("%s\n"%msg)