Ejemplo n.º 1
0
def simulate_demo_traj(sim_env, new_xyz, seg_info, full_trajs, ignore_infeasibility=True, animate=False, interactive=False):
    sim_util.reset_arms_to_side(sim_env)
    
    old_xyz = np.squeeze(seg_info["cloud_xyz"])
    old_xyz = clouds.downsample(old_xyz, DS_SIZE)
    new_xyz = clouds.downsample(new_xyz, DS_SIZE)
    
    handles = []
    if animate:
        handles.append(sim_env.env.plot3(old_xyz,5, (1,0,0)))
        handles.append(sim_env.env.plot3(new_xyz,5, (0,0,1)))

    miniseg_starts, miniseg_ends, _ = sim_util.split_trajectory_by_gripper(seg_info, thresh = GRIPPER_ANGLE_THRESHOLD)    
    success = True
    feasible = True
    misgrasp = False
    print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends

    for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):      
        if i_miniseg >= len(full_trajs): break           

        full_traj = full_trajs[i_miniseg]

        for lr in 'lr':
            gripper_open = sim_util.binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start], GRIPPER_ANGLE_THRESHOLD)
            prev_gripper_open = sim_util.binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start-1], GRIPPER_ANGLE_THRESHOLD) if i_start != 0 else False
            if not sim_util.set_gripper_maybesim(sim_env, lr, gripper_open, prev_gripper_open):
                redprint("Grab %s failed" % lr)
                misgrasp = True
                success = False

        if not success: break

        if len(full_traj[0]) > 0:
            if not eval_util.traj_is_safe(sim_env, full_traj, COLLISION_DIST_THRESHOLD):
                redprint("Trajectory not feasible")
                feasible = False
            if feasible or ignore_infeasibility:
                success &= sim_util.sim_full_traj_maybesim(sim_env, full_traj, animate=animate, interactive=interactive)
            else:
                success = False

        if not success: break

    sim_env.sim.settle(animate=animate)
    sim_env.sim.release_rope('l')
    sim_env.sim.release_rope('r')
    sim_util.reset_arms_to_side(sim_env)
    if animate:
        sim_env.viewer.Step()
    
    return success, feasible, misgrasp, full_trajs
Ejemplo n.º 2
0
def simulate_demo_traj(sim_env,
                       new_xyz,
                       seg_info,
                       full_trajs,
                       ignore_infeasibility=True,
                       animate=False,
                       interactive=False):
    sim_util.reset_arms_to_side(sim_env)

    old_xyz = np.squeeze(seg_info["cloud_xyz"])
    old_xyz = clouds.downsample(old_xyz, DS_SIZE)
    new_xyz = clouds.downsample(new_xyz, DS_SIZE)

    handles = []
    if animate:
        handles.append(sim_env.env.plot3(old_xyz, 5, (1, 0, 0)))
        handles.append(sim_env.env.plot3(new_xyz, 5, (0, 0, 1)))

    miniseg_starts, miniseg_ends, _ = sim_util.split_trajectory_by_gripper(
        seg_info, thresh=GRIPPER_ANGLE_THRESHOLD)
    success = True
    feasible = True
    misgrasp = False
    print colorize.colorize("mini segments:",
                            "red"), miniseg_starts, miniseg_ends

    for (i_miniseg, (i_start,
                     i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
        if i_miniseg >= len(full_trajs): break

        full_traj = full_trajs[i_miniseg]

        for lr in 'lr':
            gripper_open = sim_util.binarize_gripper(
                seg_info["%s_gripper_joint" % lr][i_start],
                GRIPPER_ANGLE_THRESHOLD)
            prev_gripper_open = sim_util.binarize_gripper(
                seg_info["%s_gripper_joint" % lr][i_start - 1],
                GRIPPER_ANGLE_THRESHOLD) if i_start != 0 else False
            if not sim_util.set_gripper_maybesim(sim_env, lr, gripper_open,
                                                 prev_gripper_open):
                redprint("Grab %s failed" % lr)
                misgrasp = True
                success = False

        if not success: break

        if len(full_traj[0]) > 0:
            if not eval_util.traj_is_safe(sim_env, full_traj,
                                          COLLISION_DIST_THRESHOLD):
                redprint("Trajectory not feasible")
                feasible = False
            if feasible or ignore_infeasibility:
                success &= sim_util.sim_full_traj_maybesim(
                    sim_env,
                    full_traj,
                    animate=animate,
                    interactive=interactive)
            else:
                success = False

        if not success: break

    sim_env.sim.settle(animate=animate)
    sim_env.sim.release_rope('l')
    sim_env.sim.release_rope('r')
    sim_util.reset_arms_to_side(sim_env)
    if animate:
        sim_env.viewer.Step()

    return success, feasible, misgrasp, full_trajs
Ejemplo n.º 3
0
def compute_trans_traj(sim_env,
                       new_xyz,
                       seg_info,
                       ignore_infeasibility=True,
                       animate=False,
                       interactive=False):
    sim_util.reset_arms_to_side(sim_env)

    redprint("Generating end-effector trajectory")

    old_xyz = np.squeeze(seg_info["cloud_xyz"])
    old_xyz = clouds.downsample(old_xyz, DS_SIZE)
    new_xyz = clouds.downsample(new_xyz, DS_SIZE)

    link_names = ["%s_gripper_tool_frame" % lr for lr in ('lr')]
    hmat_list = [(lr, seg_info[ln]['hmat'])
                 for lr, ln in zip('lr', link_names)]
    if GlobalVars.gripper_weighting:
        interest_pts = get_closing_pts(seg_info)
    else:
        interest_pts = None
    lr2eetraj, _, old_xyz_warped = warp_hmats_tfm(old_xyz, new_xyz, hmat_list,
                                                  interest_pts)

    handles = []
    if animate:
        handles.append(sim_env.env.plot3(old_xyz, 5, (1, 0, 0)))
        handles.append(sim_env.env.plot3(new_xyz, 5, (0, 0, 1)))
        handles.append(sim_env.env.plot3(old_xyz_warped, 5, (0, 1, 0)))

    miniseg_starts, miniseg_ends, _ = sim_util.split_trajectory_by_gripper(
        seg_info, thresh=GRIPPER_ANGLE_THRESHOLD)
    success = True
    feasible = True
    misgrasp = False
    print colorize.colorize("mini segments:",
                            "red"), miniseg_starts, miniseg_ends
    full_trajs = []
    prev_vals = {lr: None for lr in 'lr'}

    for (i_miniseg, (i_start,
                     i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):

        ################################
        redprint("Generating joint trajectory for part %i" % (i_miniseg))

        # figure out how we're gonna resample stuff

        # Use inverse kinematics to get trajectory for initializing TrajOpt,
        # since demonstrations library does not contain joint angle data for
        # left and right arms
        ee_hmats = {}
        for lr in 'lr':
            ee_link_name = "%s_gripper_tool_frame" % lr
            # TODO: Change # of timesteps for resampling?
            ee_hmats[ee_link_name] = resampling.interp_hmats(
                np.arange(i_end + 1 - i_start), np.arange(i_end + 1 - i_start),
                lr2eetraj[lr][i_start:i_end + 1])
        lr2oldtraj = get_old_joint_traj_ik(sim_env, ee_hmats, prev_vals,
                                           i_start, i_end)

        #lr2oldtraj = {}
        #for lr in 'lr':
        #    manip_name = {"l":"leftarm", "r":"rightarm"}[lr]
        #    old_joint_traj = asarray(seg_info[manip_name][i_start:i_end+1])
        #    #print (old_joint_traj[1:] - old_joint_traj[:-1]).ptp(axis=0), i_start, i_end
        #    if sim_util.arm_moved(old_joint_traj):
        #        lr2oldtraj[lr] = old_joint_traj

        if len(lr2oldtraj) > 0:
            old_total_traj = np.concatenate(lr2oldtraj.values(), 1)
            JOINT_LENGTH_PER_STEP = .1
            _, timesteps_rs = sim_util.unif_resample(old_total_traj,
                                                     JOINT_LENGTH_PER_STEP)
        ####

        ### Generate fullbody traj
        bodypart2traj = {}

        for (lr, old_joint_traj) in lr2oldtraj.items():

            manip_name = {"l": "leftarm", "r": "rightarm"}[lr]

            old_joint_traj_rs = mu.interp2d(timesteps_rs,
                                            np.arange(len(old_joint_traj)),
                                            old_joint_traj)

            ee_link_name = "%s_gripper_tool_frame" % lr
            new_ee_traj = lr2eetraj[lr][i_start:i_end + 1]
            new_ee_traj_rs = resampling.interp_hmats(
                timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
            print "planning trajectory following"
            new_joint_traj, pose_errs = planning.plan_follow_traj(
                sim_env.robot, manip_name, sim_env.robot.GetLink(ee_link_name),
                new_ee_traj_rs, old_joint_traj_rs)
            prev_vals[lr] = new_joint_traj[-1]

            part_name = {"l": "larm", "r": "rarm"}[lr]
            bodypart2traj[part_name] = new_joint_traj
            ################################
            redprint("Executing joint trajectory for part %i using arms '%s'" %
                     (i_miniseg, bodypart2traj.keys()))
        full_traj = sim_util.getFullTraj(sim_env, bodypart2traj)
        full_trajs.append(full_traj)

        for lr in 'lr':
            gripper_open = sim_util.binarize_gripper(
                seg_info["%s_gripper_joint" % lr][i_start],
                GRIPPER_ANGLE_THRESHOLD)
            prev_gripper_open = sim_util.binarize_gripper(
                seg_info["%s_gripper_joint" % lr][i_start - 1],
                GRIPPER_ANGLE_THRESHOLD) if i_start != 0 else False
            if not sim_util.set_gripper_maybesim(sim_env, lr, gripper_open,
                                                 prev_gripper_open):
                redprint("Grab %s failed" % lr)
                misgrasp = True
                success = False

        if not success: break

        if len(full_traj[0]) > 0:
            if not eval_util.traj_is_safe(sim_env, full_traj,
                                          COLLISION_DIST_THRESHOLD):
                redprint("Trajectory not feasible")
                feasible = False
            if feasible or ignore_infeasibility:
                success &= sim_util.sim_full_traj_maybesim(
                    sim_env,
                    full_traj,
                    animate=animate,
                    interactive=interactive)
            else:
                success = False

        if not success: break

    sim_env.sim.settle(animate=animate)
    sim_env.sim.release_rope('l')
    sim_env.sim.release_rope('r')
    sim_util.reset_arms_to_side(sim_env)
    if animate:
        sim_env.viewer.Step()
    return success, feasible, misgrasp, full_trajs
Ejemplo n.º 4
0
def compute_trans_traj(sim_env, new_xyz, seg_info, ignore_infeasibility=True, animate=False, interactive=False):
    sim_util.reset_arms_to_side(sim_env)
    
    redprint("Generating end-effector trajectory")    
    
    old_xyz = np.squeeze(seg_info["cloud_xyz"])
    old_xyz = clouds.downsample(old_xyz, DS_SIZE)
    new_xyz = clouds.downsample(new_xyz, DS_SIZE)
        
    link_names = ["%s_gripper_tool_frame"%lr for lr in ('lr')]
    hmat_list = [(lr, seg_info[ln]['hmat']) for lr, ln in zip('lr', link_names)]
    if GlobalVars.gripper_weighting:
        interest_pts = get_closing_pts(seg_info)
    else:
        interest_pts = None
    lr2eetraj, _, old_xyz_warped = warp_hmats_tfm(old_xyz, new_xyz, hmat_list, interest_pts)

    handles = []
    if animate:
        handles.append(sim_env.env.plot3(old_xyz,5, (1,0,0)))
        handles.append(sim_env.env.plot3(new_xyz,5, (0,0,1)))
        handles.append(sim_env.env.plot3(old_xyz_warped,5, (0,1,0)))

    miniseg_starts, miniseg_ends, _ = sim_util.split_trajectory_by_gripper(seg_info, thresh = GRIPPER_ANGLE_THRESHOLD)    
    success = True
    feasible = True
    misgrasp = False
    print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends
    full_trajs = []
    prev_vals = {lr:None for lr in 'lr'}

    for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):            

        ################################    
        redprint("Generating joint trajectory for part %i"%(i_miniseg))

        # figure out how we're gonna resample stuff


        # Use inverse kinematics to get trajectory for initializing TrajOpt,
        # since demonstrations library does not contain joint angle data for
        # left and right arms
        ee_hmats = {}
        for lr in 'lr':
            ee_link_name = "%s_gripper_tool_frame"%lr
            # TODO: Change # of timesteps for resampling?
            ee_hmats[ee_link_name] = resampling.interp_hmats(np.arange(i_end+1-i_start), np.arange(i_end+1-i_start), lr2eetraj[lr][i_start:i_end+1])
        lr2oldtraj = get_old_joint_traj_ik(sim_env, ee_hmats, prev_vals, i_start, i_end)

        #lr2oldtraj = {}
        #for lr in 'lr':
        #    manip_name = {"l":"leftarm", "r":"rightarm"}[lr]                 
        #    old_joint_traj = asarray(seg_info[manip_name][i_start:i_end+1])
        #    #print (old_joint_traj[1:] - old_joint_traj[:-1]).ptp(axis=0), i_start, i_end
        #    if sim_util.arm_moved(old_joint_traj):       
        #        lr2oldtraj[lr] = old_joint_traj   

        if len(lr2oldtraj) > 0:
            old_total_traj = np.concatenate(lr2oldtraj.values(), 1)
            JOINT_LENGTH_PER_STEP = .1
            _, timesteps_rs = sim_util.unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP)
        ####

        ### Generate fullbody traj
        bodypart2traj = {}

        for (lr,old_joint_traj) in lr2oldtraj.items():
            
            manip_name = {"l":"leftarm", "r":"rightarm"}[lr]
            
            old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj)
            
            ee_link_name = "%s_gripper_tool_frame"%lr
            new_ee_traj = lr2eetraj[lr][i_start:i_end+1]          
            new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
            print "planning trajectory following"
            new_joint_traj, pose_errs = planning.plan_follow_traj(sim_env.robot, manip_name,
                                                       sim_env.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs)
            prev_vals[lr] = new_joint_traj[-1]

            part_name = {"l":"larm", "r":"rarm"}[lr]
            bodypart2traj[part_name] = new_joint_traj
            ################################    
            redprint("Executing joint trajectory for part %i using arms '%s'"%(i_miniseg, bodypart2traj.keys()))
        full_traj = sim_util.getFullTraj(sim_env, bodypart2traj)
        full_trajs.append(full_traj)

        for lr in 'lr':
            gripper_open = sim_util.binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start], GRIPPER_ANGLE_THRESHOLD)
            prev_gripper_open = sim_util.binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start-1], GRIPPER_ANGLE_THRESHOLD) if i_start != 0 else False
            if not sim_util.set_gripper_maybesim(sim_env, lr, gripper_open, prev_gripper_open):
                redprint("Grab %s failed" % lr)
                misgrasp = True
                success = False

        if not success: break

        if len(full_traj[0]) > 0:
            if not eval_util.traj_is_safe(sim_env, full_traj, COLLISION_DIST_THRESHOLD):
                redprint("Trajectory not feasible")
                feasible = False
            if feasible or ignore_infeasibility:
                success &= sim_util.sim_full_traj_maybesim(sim_env, full_traj, animate=animate, interactive=interactive)
            else:
                success = False

        if not success: break

    sim_env.sim.settle(animate=animate)
    sim_env.sim.release_rope('l')
    sim_env.sim.release_rope('r')
    sim_util.reset_arms_to_side(sim_env)
    if animate:
        sim_env.viewer.Step()
    return success, feasible, misgrasp, full_trajs