kb.object_xforms['safety_works_safety_glasses'] = se3.identity()
kb.object_clouds['safety_works_safety_glasses'] = pcd.read(open('/tmp/safety_works_safety_glasses_bin_G.pcd'))[1]
kb.object_xforms['stanley_66_052'] = se3.identity()
kb.object_clouds['stanley_66_052'] = pcd.read(open('/tmp/stanley_66_052_bin_F.pcd'))[1]
p = PlanningInterface(kb)
p.planMoveToInitialPose()
kb.target_bin = 'bin_F'
plan = p.planGraspObjectInBin('bin_F','stanley_66_052')
plan = plan[0]
paths = [ p1[1] for p1 in plan if p1[0] in [ 'path', 'fast_path' ] ]
visualization.debug_plan(kb, sum(paths, [ kb.robot_state.sensed_config ]))
p.robot.setConfig(paths[-1][-1])
kb.robot_state.commanded_config = paths[-1][-1]
plan = p.planMoveObjectToOrderBin('right')[0]
paths = [ p1[1] for p1 in plan if p1[0] in [ 'path', 'fast_path' ] ]
visualization.debug_plan(kb, sum(paths, [ kb.robot_state.commanded_config ]))
#plan = p.failsafeplan('bin_G','safety_works_safety_glasses')[0]
#paths = [ p1[1] for p1 in plan if p1[0] in [ 'path', 'fast_path' ] ]
