def display(self):
#you may run auxiliary openGL calls, if you wish to visually debug
self.world.robot(0).setConfig(self.controller.config)
self.world.drawGL()
global ground_truth_items
#show bin boxes
if self.draw_bins:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0,1])
for b in apc.bin_bounds.values():
draw_oriented_box(self.controller.knowledge.shelf_xform,b[0],b[1])
for b in apc.bin_names:
c = self.controller.knowledge.bin_front_center(b)
if c:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0.5,1])
r = 0.01
gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r])
c = self.controller.knowledge.bin_vantage_point(b)
if c:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0.5,1,0.5,1])
r = 0.01
gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r])
#show object state
for i in ground_truth_items:
if i.xform == None:
continue
#if perceived, draw in solid color
if self.controller.knowledge.bin_contents[i.bin_name]!=None and i in self.controller.knowledge.bin_contents[i.bin_name]:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,0.5,0,1])
draw_oriented_box(i.xform,i.info.bmin,i.info.bmax)
else:
#otherwise, draw in wireframe
glDisable(GL_LIGHTING)
glColor3f(1,0.5,0)
draw_oriented_wire_box(i.xform,i.info.bmin,i.info.bmax)
glEnable(GL_LIGHTING)
if self.draw_grasps:
#draw grasps, if available
g = self.controller.knowledge.grasp_xforms(i)
if g:
for xform in g:
gldraw.xform_widget(xform,0.05,0.005)
#show gripper and camera frames
if self.draw_gripper_and_camera:
left_camera_link = self.world.robot(0).getLink(left_camera_link_name)
right_camera_link = self.world.robot(0).getLink(right_camera_link_name)
left_gripper_link = self.world.robot(0).getLink(left_gripper_link_name)
right_gripper_link = self.world.robot(0).getLink(right_gripper_link_name)
gldraw.xform_widget(left_camera_link.getTransform(),0.1,0.01)
gldraw.xform_widget(right_camera_link.getTransform(),0.1,0.01)
gldraw.xform_widget(se3.mul(left_gripper_link.getTransform(),left_gripper_center_xform),0.05,0.005)
gldraw.xform_widget(se3.mul(right_gripper_link.getTransform(),right_gripper_center_xform),0.05,0.005)
#draw order box
glDisable(GL_LIGHTING)
glColor3f(1,0,0)
draw_oriented_wire_box(order_bin_xform,order_bin_bounds[0],order_bin_bounds[1])
glEnable(GL_LIGHTING)
return
def display(self):
#you may run auxiliary openGL calls, if you wish to visually debug
#draw the world
self.sim.updateWorld()
self.simworld.drawGL()
#if you're doing question 1, this will draw the shelf and floor
if self.simworld.numTerrains()==0:
for i in range(self.planworld.numTerrains()):
self.planworld.terrain(i).drawGL()
#draw commanded configurations
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5])
# only 1 robot in this case, but still use for-loop for generality
for i in xrange(self.simworld.numRobots()):
r = self.simworld.robot(i)
#q = self.sim.controller(i).getCommandedConfig()
q = self.low_level_controller.getCommandedConfig()
r.setConfig(q)
r.drawGL(False)
glDisable(GL_BLEND)
#show bin boxes
if self.draw_bins:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0,1])
for b in apc.bin_bounds.values():
draw_oriented_box(self.picking_controller.knowledge.shelf_xform,b[0],b[1])
for b in apc.bin_names:
c = self.picking_controller.knowledge.bin_front_center(b)
if c:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0.5,1])
r = 0.01
gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r])
c = self.picking_controller.knowledge.bin_vantage_point(b)
if c:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0.5,1,0.5,1])
r = 0.01
gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r])
#show object state
for i in ground_truth_items:
if i.xform == None:
continue
if i.bin_name == 'order_bin':
continue
#if perceived, draw in solid color
if self.picking_controller.knowledge.bin_contents[i.bin_name]!=None and i in self.picking_controller.knowledge.bin_contents[i.bin_name]:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,0.5,0,1])
draw_oriented_box(i.xform,i.info.bmin,i.info.bmax)
else:
#otherwise, draw in wireframe
glDisable(GL_LIGHTING)
glColor3f(1,0.5,0)
draw_oriented_wire_box(i.xform,i.info.bmin,i.info.bmax)
glEnable(GL_LIGHTING)
if self.draw_grasps:
#draw grasps, if available
g = self.picking_controller.knowledge.grasp_xforms(i)
if g:
for grasp,xform in g:
gldraw.xform_widget(xform,0.05,0.005,fancy=False)
# Draws the object held on gripper
obj,limb,grasp = self.picking_controller.held_object,self.picking_controller.active_limb,self.picking_controller.active_grasp
if obj != None:
if limb == 'left':
gripper_xform = self.simworld.robot(0).link(left_gripper_link_name).getTransform()
else:
gripper_xform = self.simworld.robot(0).link(right_gripper_link_name).getTransform()
objxform = se3.mul(gripper_xform,se3.mul(left_gripper_center_xform,se3.inv(grasp.grasp_xform)))
glDisable(GL_LIGHTING)
glColor3f(1,1,1)
draw_oriented_wire_box(objxform,obj.info.bmin,obj.info.bmax)
glEnable(GL_LIGHTING)
#show gripper and camera frames
if self.draw_gripper_and_camera:
left_camera_link = self.simworld.robot(0).link(left_camera_link_name)
right_camera_link = self.simworld.robot(0).link(right_camera_link_name)
left_gripper_link = self.simworld.robot(0).link(left_gripper_link_name)
right_gripper_link = self.simworld.robot(0).link(right_gripper_link_name)
gldraw.xform_widget(left_camera_link.getTransform(),0.1,0.01,fancy=False)
gldraw.xform_widget(right_camera_link.getTransform(),0.1,0.01,fancy=False)
gldraw.xform_widget(se3.mul(left_gripper_link.getTransform(),left_gripper_center_xform),0.05,0.005,fancy=False)
gldraw.xform_widget(se3.mul(right_gripper_link.getTransform(),right_gripper_center_xform),0.05,0.005,fancy=False)
# Show world frame and shelf frame
gldraw.xform_widget(ground_truth_shelf_xform, 0.1, 0.015, lighting=False, fancy=True)
gldraw.xform_widget(se3.identity(), 0.2, 0.037, lighting=False, fancy=True)
#draw order box
glDisable(GL_LIGHTING)
glColor3f(1,0,0)
draw_oriented_wire_box(order_bin_xform,order_bin_bounds[0],order_bin_bounds[1])
glEnable(GL_LIGHTING)
return
def draw_oriented_box(xform,bmin,bmax):
"""Helper: draws an oriented box"""
draw_xformed(xform,lambda:gldraw.box(bmin,bmax))
def display(self):
#you may run auxiliary openGL calls, if you wish to visually debug
#draw the world
self.sim.updateWorld()
self.simworld.drawGL()
#if you're doing question 1, this will draw the shelf and floor
if self.simworld.numTerrains()==0:
for i in range(self.planworld.numTerrains()):
self.planworld.terrain(i).drawGL()
#draw commanded configurations
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5])
for i in xrange(self.simworld.numRobots()):
r = self.simworld.robot(i)
q = self.sim.getController(i).getCommandedConfig()
r.setConfig(q)
r.drawGL(False)
glDisable(GL_BLEND)
global ground_truth_items
#show bin boxes
if self.draw_bins:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0,1])
for b in apc.bin_bounds.values():
draw_oriented_box(self.picking_controller.knowledge.shelf_xform,b[0],b[1])
for b in apc.bin_names:
c = self.picking_controller.knowledge.bin_front_center(b)
if c:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,1,0.5,1])
r = 0.01
gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r])
c = self.picking_controller.knowledge.bin_vantage_point(b)
if c:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0.5,1,0.5,1])
r = 0.01
gldraw.box([c[0]-r,c[1]-r,c[2]-r],[c[0]+r,c[1]+r,c[2]+r])
#show object state
for i in ground_truth_items:
if i.xform == None:
continue
#if perceived, draw in solid color
if self.picking_controller.knowledge.bin_contents[i.bin_name]!=None and i in self.picking_controller.knowledge.bin_contents[i.bin_name]:
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,0.5,0,1])
draw_oriented_box(i.xform,i.info.bmin,i.info.bmax)
else:
#otherwise, draw in wireframe
glDisable(GL_LIGHTING)
glColor3f(1,0.5,0)
draw_oriented_wire_box(i.xform,i.info.bmin,i.info.bmax)
glEnable(GL_LIGHTING)
if self.draw_grasps:
#draw grasps, if available
g = self.picking_controller.knowledge.grasp_xforms(i)
if g:
for xform in g:
gldraw.xform_widget(xform,0.05,0.005)
#show gripper and camera frames
if self.draw_gripper_and_camera:
left_camera_link = self.simworld.robot(0).getLink(left_camera_link_name)
right_camera_link = self.simworld.robot(0).getLink(right_camera_link_name)
left_gripper_link = self.simworld.robot(0).getLink(left_gripper_link_name)
right_gripper_link = self.simworld.robot(0).getLink(right_gripper_link_name)
gldraw.xform_widget(left_camera_link.getTransform(),0.1,0.01)
gldraw.xform_widget(right_camera_link.getTransform(),0.1,0.01)
gldraw.xform_widget(se3.mul(left_gripper_link.getTransform(),left_gripper_center_xform),0.05,0.005)
gldraw.xform_widget(se3.mul(right_gripper_link.getTransform(),right_gripper_center_xform),0.05,0.005)
#draw order box
glDisable(GL_LIGHTING)
glColor3f(1,0,0)
draw_oriented_wire_box(order_bin_xform,order_bin_bounds[0],order_bin_bounds[1])
glEnable(GL_LIGHTING)
return
