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' ] ]
#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'
def execute(self, plan, sleep):
'''
Run the plan to completion or until an error occurs. This method blocks
until the plan execution is finished.
sleep = method to call to perform a wait
Returns True if the plan was run successfully.
Returns False if an error occurred.
'''
# visualize all the paths starting with the robot's sensed config
paths = [ p[1] for p in plan if p[0] in [ 'path', 'fast_path' ] ]
if len(paths) > 0:
visualization.debug_plan(self.knowledge_base, sum(paths, [ self.knowledge_base.robot_state.sensed_config ]))
for (type, command, waitBefore, waitAfter) in plan:
try:
sleep(waitBefore)
if(type == 'path'):
#visualization.debug_plan(self.knowledge_base, command)
logger.debug('Sending a plan')
self.robot.setMilestone(command[0])
for q in command[1:]:
# while(self.robot.isMoving()):
# sleep(.1)
self.robot.appendMilestone(q)
while(self.robot.isMoving()):
sleep(.1)
# print 'waiting', self.robot.isMoving()
elif(type == 'fast_path'):
visualization.debug_plan(self.knowledge_base, command)
logger.debug('sending a fast path')
self.robot.setConfig(command[0])
for q in command:
self.robot.appendConfig(q)
while(self.robot.isMoving()):
sleep(.1)
elif(type == 'cartesian_drive'):
limb = command['limb']
error_threshold = command['error_threshold'] if 'error_threshold' in command else float('0.05')
velocity = command['velocity']
duration = command['duration']
angular_velocity = command['angularVelocity'] if 'angular_velocity' in command else [0,0,0]
print "Drive",velocity,"for time",duration
self.robot.setCartesianVelocityCommand(limb,velocity,angular_velocity)
t0 = time.time()
while(time.time() - t0 < duration):
#TODO: monitor errors
sleep(0.01)
self.robot.setCartesianVelocityCommand(limb,[0]*3,[0]*3)
elif(type.endswith('gripper')):
logger.debug('Sending a gripper command')
if (type.startswith('left')):
pass
self.robot.commandGripper('left', command)
else:
pass
# self.robot.commandGripper('right', command)
else:
logger.error('You messed up! Not a path or gripper command')
raise Exception
sleep(waitAfter)
except RuntimeError as e:
print e
return False
return True
