Exemplo 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
Exemplo 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
Exemplo 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
Exemplo 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
def compute_trans_traj(sim_env, new_xyz, seg_info, ignore_infeasibility=True, animate=False, interactive=False):
    redprint("Generating end-effector trajectory")    
    
    
    old_xyz = np.squeeze(seg_info["cloud_xyz"])
    old_xyz = clouds.downsample(old_xyz, DS_SIZE)
    l1 = len(old_xyz)
    new_xyz = clouds.downsample(new_xyz, DS_SIZE)
    l2 = len(new_xyz)
    print l1, l2
    
            
    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, f = warp_hmats_tfm(old_xyz, new_xyz, hmat_list, interest_pts)


    handles = []
    if animate:
        handles.extend(plotting_openrave.draw_grid(sim_env.env, f.transform_points, old_xyz.min(axis=0)-np.r_[0,0,.1], old_xyz.max(axis=0)+np.r_[0,0,.1], xres = .1, yres = .1, zres = .04))
        handles.append(sim_env.env.plot3(old_xyz,5, (1,0,0))) # red: demonstration point cloud
        handles.append(sim_env.env.plot3(new_xyz,5, (0,0,1))) # blue: rope nodes
        handles.append(sim_env.env.plot3(old_xyz_warped,5, (0,1,0))) # green: warped point cloud from demonstration
        
        mapped_pts = []
        for i in range(len(old_xyz)):
            mapped_pts.append(old_xyz[i])
            mapped_pts.append(old_xyz_warped[i])
        handles.append(sim_env.env.drawlinelist(np.array(mapped_pts), 1, [0.1,0.1,1]))
        
    for lr in 'lr':
        handles.append(sim_env.env.drawlinestrip(lr2eetraj[lr][:,:3,3], 2, (0,1,0,1)))
        
    for k, hmats in hmat_list:
        hmats_tfm = np.asarray([GlobalVars.init_tfm.dot(h) for h in hmats])
        handles.append(sim_env.env.drawlinestrip(hmats_tfm[:,:3,3], 2, (1,0,0,1)))
        

    miniseg_starts, miniseg_ends, lr_open = 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
    miniseg_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))


        ### adaptive resampling based on xyz in end_effector
        end_trans_trajs = np.zeros([i_end+1-i_start, 6])

        for lr in 'lr':
            for i in xrange(i_start,i_end+1):
                if lr == 'l':
                    end_trans_trajs[i-i_start, :3] = lr2eetraj[lr][i][:3,3]
                else:
                    end_trans_trajs[i-i_start, 3:] = lr2eetraj[lr][i][:3,3]

        if True:
            adaptive_times, end_trans_trajs = resampling.adaptive_resample2(end_trans_trajs, 0.005)
        else:
            adaptive_times = range(len(end_trans_trajs))
            

        miniseg_traj = {}
        for lr in 'lr':
            #ee_hmats = resampling.interp_hmats(np.arange(i_end+1-i_start), np.arange(i_end+1-i_start), lr2eetraj[lr][i_start:i_end+1])
            ee_hmats = resampling.interp_hmats(adaptive_times, np.arange(i_end+1-i_start), lr2eetraj[lr][i_start:i_end+1])
            
            # the interpolation above will then the velocity of the trajectory (since there are fewer waypoints). Resampling again to make sure 
            # the trajectory has the same number of waypoints as before.
            ee_hmats = resampling.interp_hmats(np.arange(i_end+1-i_start), adaptive_times, ee_hmats)
            
            # if arm_moved(ee_hmats, floating=True):
            if True:
                miniseg_traj[lr] = ee_hmats
                

        
        safe_drop = {'l': True, 'r': True}
        for lr in 'lr':
            next_gripper_open = lr_open[lr][i_miniseg+1] if i_miniseg < len(miniseg_starts) - 1 else False
            gripper_open = lr_open[lr][i_miniseg] 
            
            if next_gripper_open and not gripper_open:
                tfm = miniseg_traj[lr][-1]
                if tfm[2,3] > GlobalVars.table_height + 0.01:
                    safe_drop[lr] = False
                    
        #safe_drop = {'l': True, 'r': True}             
                
        if not (safe_drop['l'] and safe_drop['r']):
            for lr in 'lr':
                
                if not safe_drop[lr]:
                    tfm = miniseg_traj[lr][-1]
                    for i in range(1, 8):
                        safe_drop_tfm = tfm
                        safe_drop_tfm[2,3] = tfm[2,3] - i / 10. * (tfm[2,3] - GlobalVars.table_height - 0.01)
                        miniseg_traj[lr].append(safe_drop_tfm)
                else:
                    for i in range(1, 8):
                        miniseg_traj[lr].append(miniseg_traj[lr][-1])
                     
                
        miniseg_trajs.append(miniseg_traj)
        

        for lr in 'lr':
            hmats = np.asarray(miniseg_traj[lr])
            handles.append(sim_env.env.drawlinestrip(hmats[:,:3,3], 2, (0,0,1,1)))
            
        redprint("Executing joint trajectory for part %i using arms '%s'"%(i_miniseg, miniseg_traj.keys()))
          
        
        for lr in 'lr':
            gripper_open = lr_open[lr][i_miniseg]
            prev_gripper_open = lr_open[lr][i_miniseg-1] if i_miniseg != 0 else False
            if not sim_util.set_gripper_maybesim(sim_env, lr, gripper_open, prev_gripper_open, floating=True):
                redprint("Grab %s failed"%lr)
                success = False

        if not success: break
        
        if len(miniseg_traj) > 0:
            success &= sim_util.exec_traj_sim(sim_env, miniseg_traj, animate=animate)

        if not success: break

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