def execute_dual_arm_trajectory(pr2, lquat, lpos, lgrip, left_used, rquat, rpos, rgrip, right_used, rope): rviz.draw_curve(lpos, RVIZ_LPOS, width=.005) rviz.draw_curve(rpos, RVIZ_RPOS, width=.005) ok = yes_or_no("trajectory ok?") if not ok: print "canceling trajectory" return for i in xrange(len(lquat)): if left_used: brett.larm.set_cart_target(normalize(quatswap(lquat[i])), lpos[i], "/base_footprint") if lgrip[i] < .03: lgrip[i] = 0 brett.lgrip.set_angle_goal(lgrip[i]) if right_used: brett.rarm.set_cart_target(normalize(quatswap(rquat[i])), rpos[i], "/base_footprint") if rgrip[i] < .03: rgrip[i] = 0 brett.rgrip.set_angle_goal(rgrip[i]) rviz.draw_curve(rope[i], RVIZ_ORIG_ROPE, width=.01, rgba = (1,0,0,1)) sleep(.04)
def mkdir_ask(path,make_path=False): if os.path.exists(path): consent = yes_or_no("%s already exists. Delete it?"%path) if consent: shutil.rmtree(path) else: raise IOError if make_path: os.makedirs(path) else: os.mkdir(path)