Ejemplo n.º 1
0
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)
Ejemplo n.º 2
0
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)