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' ] ]