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