def display(self):
if self.state == None: return
glEnable(GL_LIGHTING)
self.statelock.acquire()
for i, d in enumerate(self.state):
Twidget = d['widgetTransform']
#T = (so3.mul(so3.inv(deviceToBaxterBaseTransform[0]),Twidget[0]),Twidget[1])
T = (so3.mul(Twidget[0],
deviceToBaxterBaseTransform[0]), Twidget[1])
# T = Twidget
width = 0.05 * d['rotationScale']
length = 0.1 * d['positionScale']
gldraw.xform_widget(T, length, width)
self.statelock.release()
if self.robot:
#draw robot
qcmd = self.qcmdGetter.get()
if qcmd:
self.robot.setConfig(qcmd)
self.robot.drawGL(True)
else:
self.robot.drawGL(True)
qdest = self.qdestGetter.get()
if qdest:
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])
self.robot.setConfig(qdest)
self.robot.drawGL(False)
glDisable(GL_BLEND)
glDisable(GL_LIGHTING)
def drawStuff(self):
glDisable(GL_LIGHTING)
gldraw.xform_widget(self.Tvacuum, 0.1, 0.01)
gldraw.xform_widget(self.Tcamera, 0.1, 0.01)
glPointSize(5.0)
glColor3f(0.0, 0.0, 1.0)
glBegin(GL_POINTS)
for point in self.points1[::25]:
glVertex3f(point[0], point[1], point[2])
glEnd()
glColor3f(1.0, 0.0, 0.0)
glBegin(GL_POINTS)
for point in self.points2[::25]:
glVertex3f(point[0], point[1], point[2])
glEnd()
glPointSize(20.0)
glColor3f(0.0, 0.8, 0.0)
glBegin(GL_POINTS)
glVertex3f(self.object_com[0], self.object_com[1], self.object_com[2])
glEnd()
glPointSize(5.0)
glColor3f(1.0, 0.0, 0.0)
glBegin(GL_POINTS)
for i in range(self.vacuumPc.getPointCloud().numPoints()):
point = self.vacuumPc.getPointCloud().getPoint(i)
glVertex3f(point[0], point[1], point[2])
glEnd()
def display(self):
#Put your display handler here
robot = motion.robot
q = robot.getKlamptSensedPosition()
self.world.robot(0).setConfig(q)
self.world.drawGL()
#draw commanded configuration
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])
q = robot.getKlamptCommandedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL(False)
glDisable(GL_BLEND)
#draw commanded pose
gldraw.xform_widget((self.driveRot, self.drivePos),
0.1,
0.02,
fancy=True)
#draw trace
glDisable(GL_LIGHTING)
glColor3f(1, 0.5, 0)
for arm in self.trace_sensed:
glBegin(GL_LINE_STRIP)
for T in self.trace_sensed[arm]:
glVertex3f(*T[1])
glEnd()
glEnable(GL_LIGHTING)
return
def drawRoadmap(self):
V,E = planner.roadmap
positions = []
gldraw.xform_widget(self.simworld.robot(0).link(0).getTransform(), 0.1, 0.015, lighting=False, fancy=True)
# print len(V)
for v in V:
qcmd = self.planworld.robot(0).getConfig()
self.planworld.robot(0).setConfig(qcmd)
linkNum = 0
R = self.planworld.robot(0).link(linkNum).getTransform()[0]
t = self.planworld.robot(0).link(linkNum).getTransform()[1]
# loc = self.planworld.robot(0).link(54).getTransform()[1]
positions.append(t)
# remove this line later (slows down the visualizer)
gldraw.xform_widget([R,t], 0.015, 0.002, lighting=False, fancy=True)
glColor3f(0.1,0.1,0.1)
glLineWidth(0.1)
glBegin(GL_LINES)
for (i,j) in E:
glVertex3f(*positions[i])
glVertex3f(*positions[j])
glEnd()
def display(self):
#Put your display handler here
robot = motion.robot
q = robot.getKlamptSensedPosition()
self.world.robot(0).setConfig(q)
self.world.drawGL()
#draw commanded configuration
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])
q = robot.getKlamptCommandedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL(False)
glDisable(GL_BLEND)
#draw commanded pose
gldraw.xform_widget((self.driveRot,self.drivePos),0.1,0.02,fancy=True)
#draw trace
glDisable(GL_LIGHTING)
glColor3f(1,0.5,0)
for arm in self.trace_sensed:
glBegin(GL_LINE_STRIP)
for T in self.trace_sensed[arm]:
glVertex3f(*T[1])
glEnd()
glEnable(GL_LIGHTING)
return
def display(self):
self.sim.updateWorld()
self.simWorld.drawGL()
self.controller.drawGL()
glEnable(GL_LIGHTING)
gldraw.xform_widget(self.sensors['blobdetector'].Tsensor,
0.1,
0.01,
fancy=True)
def display(self):
if self.world:
self.world.drawGL()
glPointSize(max(1, self.point_size))
for point_list in self.point_lists:
glCallList(point_list)
for xform in self.xforms:
gldraw.xform_widget(xform, 0.1, 0.01)
def display(self):
global currentTask, currentTaskLock
glEnable(GL_LIGHTING)
currentTaskLock.acquire()
task = currentTask
if task != None:
task = currentTask.copy()
currentTaskLock.release()
if task != None:
if task['type'] == 'multi_ik':
for ikgoal in task['components']:
T = (so3.from_moment(ikgoal['endRotation']),
ikgoal['endPosition'])
width = 0.02
length = 0.2
gldraw.xform_widget(T, length, width)
def display(self):
self.world.drawGL()
if self.draw_frames:
left_camera_link = self.world.robot(0).link(LEFT_CAMERA_LINK_NAME)
right_camera_link = self.world.robot(0).link(RIGHT_CAMERA_LINK_NAME)
left_gripper_link = self.world.robot(0).link(LEFT_GRIPPER_LINK_NAME)
right_gripper_link = self.world.robot(0).link(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)
self.world = self.master.drawStuff()
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 display(self):
self.world.drawGL()
for xform in self.xforms:
gldraw.xform_widget(xform, 0.1, 0.01)
def display(self):
if self.knowledge_base:
# update the world
if self.knowledge_base.shelf_xform:
# load the shelf once a transform is available
if not self.shelf:
self.shelf = self.world.loadRigidObject("klampt_models/north_shelf/shelf_with_bins.obj")
logger.info("spawned shelf model")
self.shelf.setTransform(*self.knowledge_base.shelf_xform)
if self.knowledge_base.order_bin_xform:
# load the order bin once a transform is available
if not self.order_bin:
self.order_bin = self.world.loadRigidObject("klampt_models/apc_bin/apc_bin.obj")
logger.info("spawned order bin model")
self.order_bin.setTransform(*self.knowledge_base.order_bin_xform)
# spawn/update objects
for (name, xform) in self.knowledge_base.object_xforms.items():
# load the object once a transform is availale
if name not in self.objects:
body = self.world.loadRigidObject("klampt_models/items/{0}/{0}.obj".format(name))
logger.info("spawned {} model".format(name))
# load the point cloud
if name in self.knowledge_base.object_clouds:
self.n += 1
display_list = glGenLists(self.n)
# compile the display list
glNewList(display_list, GL_COMPILE)
glDisable(GL_LIGHTING)
glBegin(GL_POINTS)
points = self.knowledge_base.object_clouds[name]
for point in points:
if len(point) == 2:
xyz = point[0]
rgb = point[1]
else:
xyz = point[:3]
if len(point) == 4:
rgb = point[3]
elif len(point) > 3:
rgb = point[3:6]
else:
rgb = None
if rgb is not None:
glColor3f(*map(lambda x: x / 255.0, rgb))
else:
glColor3f(*colors[i % len(colors)])
glVertex3f(*xyz[:3])
glEnd()
glEndList()
logging.debug("compiled {} points for {}".format(len(points), name))
else:
display_list = None
self.objects[name] = {"body": body, "display_list": display_list}
# self.objects[name]['body'].setTransform(*xform)
# delete objects
for (name, props) in self.objects.items():
if name not in self.knowledge_base.object_xforms:
# remove the object
# XXX: cannot actually delete object... so move it far away... very far away
props["body"].setTransform(so3.identity(), [1e3, 1e3, 1e3])
del self.objects[name]
# update the robot state
if self.robot_state:
try:
self.robot.setConfig(self.robot_state.sensed_config)
except TypeError as e:
logger.error("error visualizing config: {}".format(self.robot_state.sensed_config))
logger.error(traceback.format_exc())
sys.exit(-1)
self.world.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])
if self.robot_state:
qi = self.robot.getConfig()
self.robot.setConfig(self.robot_state.commanded_config)
self.robot.drawGL(False)
self.robot.setConfig(qi)
glDisable(GL_BLEND)
# draw the point clouds
glPointSize(2)
for (name, props) in self.objects.items():
if "display_list" in props:
glCallList(props["display_list"])
# draw gripper link transforms
for name in ["left_gripper", "right_gripper"]:
gldraw.xform_widget(self.robot.getLink(name).getTransform(), 0.1, 0.01)
# draw axis-aligned axes for reference
gldraw.xform_widget((so3.identity(), [0, 0, 1]), 0.1, 0.01)
# draw local origins of objects
if self.knowledge_base:
for xform in self.knowledge_base.object_xforms.values():
gldraw.xform_widget(xform, 0.1, 0.01)
def display(self):
self.sim.updateWorld()
self.simWorld.drawGL()
self.controller.drawGL()
glEnable(GL_LIGHTING)
gldraw.xform_widget(self.sensors['blobdetector'].Tsensor,0.1,0.01,fancy=True)
def display(self):
#draw the world
self.sim.updateWorld()
self.simworld.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.low_level_controller.getCommandedConfig()
r.setConfig(q)
r.drawGL(False)
glDisable(GL_BLEND)
self.drawContactForces()
# self.drawRoadmap()
# add commands to queue if automatically restarted
global automatic
if automatic >= 1:
automatic += 1
if automatic == 10:
self.command_queue.put('l')
self.command_queue.put('x')
self.command_queue.put('t')
automatic = -1
# print self.sim.inContact(i,j)
# print self.simworld.getName(i),"-",self.simworld.getName(j), f
# t = self.sim.mean
# printf("%s - %s: force %g %g %g\n",world.GetName(i).c_str(),world.GetName(j)).c_str(),f.x,f.y,f.z,t.x,t.y,t.z);
# print contact.simContactMap(self.sim)
# qSim = self.simworld.robot(0).getConfig()
# qReal = self.low_level_controller.getSensedConfig()
# diff = vectorops.distance(qSim, qReal)
# print diff
# print self.sim.getActualTorques(0)[1]
# print self.sim.getContactForces(1,2)
# print self.simworld.robot(0).getID()
# print self.simworld.rigidObject(0).getID()
# self.sim.enableContactFeedbackAll()
# print max(abs(self.sim.getJointForces(self.simworld.robot(0).link(9))[3:6]))
# print max(abs(self.sim.getJointForces(self.simworld.robot(0).link(14))[3:6]))
# print max(abs(self.sim.getJointForces(self.simworld.robot(0).link(18))[3:6]))
# print self.sim.getJointForces(self.simworld.robot(0).link(9))
if self.showFrames:
# world frame
gldraw.xform_widget(se3.identity(), 0.15, 0.005, lighting=True, fancy=True)
# robot frame
gldraw.xform_widget(self.simworld.robot(0).link(6).getTransform(), 0.15, 0.005, lighting=True, fancy=True)
# object frame
gldraw.xform_widget(self.simworld.rigidObject(0).getTransform(), 0.15, 0.005, lighting = True, fancy = True)
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])
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
