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' ] ]
#p.robot.setConfig(paths[-1][-1])
#kb.robot_state.sensed_config = paths[-1][-1]
#visualization.debug_plan(kb, sum(paths, [ kb.robot_state.sensed_config ]))
#kb.grasped_object = 'safety_works_safety_glasses'
#plan = p.planMoveObjectToOrderBin('left')
