コード例 #1
0
ファイル: registration.py プロジェクト: benkehoe/python
def tps_rpm_multi(source_clouds, targ_clouds, *args,**kw):
    """
    Given multiple source clouds and corresponding target clouds, solve for global transformation that matches them.
    Right now, we just concatenate them all. Eventually, we'll do something more sophisticated, e.g. initially match each
    source cloud to each target cloud, then use those correspondences to initialize tps-rpm on concatenated clouds.    
    """
    x_nd = np.concatenate([np.asarray(cloud).reshape(-1,3) for cloud in source_clouds], 0)
    y_md = np.concatenate([np.asarray(cloud).reshape(-1,3) for cloud in targ_clouds], 0)
    from image_proc.clouds import voxel_downsample
    x_nd = voxel_downsample(x_nd,.02)
    y_md = voxel_downsample(y_md,.02)
    return tps_rpm(x_nd, y_md, *args,**kw)
コード例 #2
0
def plot_original_and_warped_demo(best_demo, warped_demo, traj):
    arms_used = best_demo["arms_used"].value

    if arms_used in "lb":
        pose_array = conversions.array_to_pose_array(asarray(best_demo["l_gripper_xyzs"]), 'base_footprint')
        Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,0,0,1),ns = "make_verb_traj_service"))
        pose_array = conversions.array_to_pose_array(asarray(warped_demo["l_gripper_xyzs"]), 'base_footprint')
        Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,1,0,1),ns = "make_verb_traj_service"))
        
    if arms_used in "rb":
        pose_array = conversions.array_to_pose_array(asarray(best_demo["r_gripper_xyzs"]), 'base_footprint')
        Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,0,0,1),ns = "make_verb_traj_service"))
        pose_array = conversions.array_to_pose_array(asarray(warped_demo["r_gripper_xyzs"]), 'base_footprint')
        Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,1,0,1),ns = "make_verb_traj_service"))

    Globals.handles.extend(Globals.rviz.draw_trajectory(traj.gripper0_poses, traj.gripper0_angles, ns = "make_verb_traj_service_grippers"))
    if arms_used == 'b':
        Globals.handles.extend(Globals.rviz.draw_trajectory(traj.gripper1_poses, traj.gripper1_angles, ns = "make_verb_traj_service_grippers"))
        

    for (clouds,rgba) in [(sorted_values(best_demo["object_clouds"]),(1,0,0,.5)),(sorted_values(warped_demo["object_clouds"]),(0,1,0,.5))]:

        cloud = []
        for subcloud in clouds:
            cloud.extend(np.asarray(subcloud).reshape(-1,3))
        cloud = np.array(cloud)
        
        cloud = voxel_downsample(cloud, .02)
        pose_array = conversions.array_to_pose_array(cloud, 'base_footprint')
        Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = rgba,width=.01,type=Marker.CUBE_LIST))
コード例 #3
0
ファイル: test_registration.py プロジェクト: benkehoe/python
def test_registration_rope_images():
    
    library = trajectory_library.TrajectoryLibrary("lm.h5","read")
    


    plt.ion()
    segment_group = library.root["segments"]
    
    n_segs = len(segment_group.keys())
    n_rows = int(round(np.sqrt(n_segs)))
    n_cols = int(np.ceil(n_segs / n_rows))    
    print n_rows, n_cols
    
    sim_ropes = []
    for (i,(name, data)) in enumerate(segment_group.items()):
        plt.subplot(n_rows, n_cols, i+1)
        if "object_points" in segment_group:
            points_n3 = data["object_points"][0]
        else:
            print "rope field is deprecated. use object points"
            points_n3 = data["rope"][0]
        plt.plot(points_n3[:,1], points_n3[:,0],'.')
        plt.title(name)
    
        sim_ropes.append(points_n3)
        
    i=int(sys.argv[1])
    rope = np.dot(np.squeeze(np.loadtxt("/home/joschu/Data/rope/rope%i.txt"%i)),np.diag([1,-1,-1]))    
    rope = voxel_downsample(rope,.03)
    
    #f = registration.tps_rpm
    tps = registration.tps_rpm(rope[:,:2], sim_ropes[i][:,:2],plotting=10,reg_init=1,reg_final=.025,n_iter=200)
コード例 #4
0
ファイル: test_registration.py プロジェクト: benkehoe/python
 def preproc(xyz1):
     xyz1 = xyz1.reshape(-1,3)
     xyz1 = np.dot(xyz1, np.diag([1,-1,-1]))
     xyz1 = xyz1[(xyz1[:,2] > .02) & (xyz1[:,2] < .2) 
                 & (np.abs(xyz1[:,0]) < .15) & (np.abs(xyz1[:,1]) < .3)]
     xyz1 = voxel_downsample(xyz1, .015)
     return xyz1
コード例 #5
0
ファイル: tie_knot.py プロジェクト: benkehoe/python
def downsample(xyz):
    if DS_METHOD == "voxel":        
        xyz_ds, ds_inds = voxel_downsample(xyz, DS_LENGTH, return_inds = True)
    elif DS_METHOD == "hull":
        xyz = np.squeeze(xyz)
        _, inds = get_concave_hull(xyz[:,0:2],.05)
        xyz_ds = xyz[inds]
        ds_inds = [[i] for i in inds]    
    return xyz_ds, ds_inds
コード例 #6
0
def test_registration_rope_images():
    
    library = trajectory_library.TrajectoryLibrary("lm.h5","read")
    


    plt.ion()
    segment_group = library.root["segments"]
    
    n_segs = len(segment_group.keys())
    n_rows = int(round(np.sqrt(n_segs)))
    n_cols = int(np.ceil(n_segs / n_rows))    
    print n_rows, n_cols
    
    sim_ropes = []
    for (i,(name, data)) in enumerate(segment_group.items()):
        plt.subplot(n_rows, n_cols, i+1)
        if "object_points" in segment_group:
            points_n3 = data["object_points"][0]
        else:
            print "rope field is deprecated. use object points"
            points_n3 = data["rope"][0]
        plt.plot(points_n3[:,1], points_n3[:,0],'.')
        plt.title(name)
    
        sim_ropes.append(points_n3)
        
    
    #win_name = "get points"
    #cv2.namedWindow(win_name)
    #im = np.zeros((100,100), 'uint8')
    #im.fill(150)
    #xys = np.array(roi.get_polyline(im, win_name),float)/100
    #curve = curves.unif_resample(xys,100,tol=.1)
    print np.squeeze(np.loadtxt("/home/joschu/Data/rope/rope2.txt")).shape
    rope = np.dot(np.squeeze(np.loadtxt("/home/joschu/Data/rope/rope1.txt")),np.diag([1,-1,-1]))    
    rope = voxel_downsample(rope,.03)
    
    reg = registration_matlab.NonrigidRegistration(display=True)
    reg.fit_transformation_icp(rope[:,:2], sim_ropes[3][:,:2])    
コード例 #7
0
ファイル: tps_shape_3d.py プロジェクト: benkehoe/python
def voxel_downsample(xyz, s, return_inds=False):
    xyz = xyz.reshape(-1, 3)
    xyz = xyz[np.isfinite(xyz[:, 0])]
    pts = []
    keys = set([])
    for (i, pt) in enumerate(xyz):
        x, y, z = pt
        key = (int(x // s), int(y // s), int(z // s))
        if key not in keys:
            keys.add(key)
            pts.append(pt)

    return np.array(pts)


X = voxel_downsample(ctrl_pts, 0.02)

y = dists = ssd.cdist(X, good_pts).min(axis=1) ** 2


# X = array([
# (10,0),
# (3,4),
# (4,3),
# (5,0),
# (4,-3),
# (3,-4)])

# y = array([
# 25,
# 0,0,0,0,0])
コード例 #8
0
from os.path import join, basename, splitext
from glob import glob
from point_clouds.features import cloud_shape_context
from image_proc.clouds import voxel_downsample
import numpy as np

DATA_DIR = '/home/joschu/Data/rope'

for fname in glob(join(DATA_DIR,"*.txt")):
    if "feats" in fname: continue
    print fname
    xyz = np.loadtxt(fname)
    if "sim" in fname: xyz = np.dot(xyz, np.diag([1,-1,1]))
    else: xyz = voxel_downsample(xyz, .03)
    features = np.c_[xyz, cloud_shape_context(xyz, 2)]
    
    feat_fname = splitext(fname)[0] + ".feats.txt"
    np.savetxt(feat_fname, features)
コード例 #9
0
ファイル: tie_knot.py プロジェクト: benkehoe/python
    def execute(self,userdata):
        """
        - lookup closest trajectory from database
        - if it's a terminal state, we're done
        - warp it based on the current rope
        returns: done, not_done, failure
        
        visualization: 
        - show all library states
        - warping visualization from matlab
        """
        xyz_new = np.squeeze(np.asarray(userdata.points))
        if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025)
        
        xyz_new_ds, ds_inds = downsample(xyz_new)
        dists_new = recognition.calc_geodesic_distances_downsampled(xyz_new,xyz_new_ds, ds_inds)
        
        if HUMAN_SELECT_DEMO:
            seg_name = trajectory_library.interactive_select_demo(self.library)
            best_demo = self.library.root[seg_name]         
            pts0,_ = downsample(np.asarray(best_demo["cloud_xyz"]))
            pts1,_ = downsample(xyz_new)
            self.f = registration.tps_rpm(pts0, pts1, 
                                     plotting = 4, reg_init=1,reg_final=.025,n_iter=40)                
        else:
            
            best_f = None
            best_cost = np.inf
            best_name = None
            for (seg_name,candidate_demo) in self.library.root.items():
                xyz_demo = np.squeeze(np.asarray(candidate_demo["cloud_xyz"]))
                if args.obj == "cloth": xyz_demo = voxel_downsample(xyz_demo, .025)
                xyz_demo_ds, ds_inds = downsample(xyz_demo)#voxel_downsample(xyz_demo, DS_LENGTH, return_inds = True)
                dists_demo = recognition.calc_geodesic_distances_downsampled(xyz_demo, xyz_demo_ds, ds_inds)
                cost = recognition.calc_match_score(xyz_new_ds, xyz_demo_ds, dists0 = dists_new, dists1 = dists_demo)
                print "seg_name: %s. cost: %s"%(seg_name, cost)
                if cost < best_cost:
                    best_cost = cost
                    best_name = seg_name

            #if best_name.startswith("done"): return "done"
            best_demo = self.library.root[best_name]
            xyz_demo_ds,_ = downsample(np.asarray(best_demo["cloud_xyz"][0]))
            self.f = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, 
                            plotting = 10, reg_init=1,reg_final=.01,n_iter=200,verbose=True)                

            print "best segment:", best_name

        

        orig_pose_array = conversions.array_to_pose_array(best_demo["cloud_xyz"][0], "base_footprint")
        warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(best_demo["cloud_xyz"][0]), "base_footprint")
        HANDLES.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST))
        HANDLES.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST))

        mins = best_demo["cloud_xyz"][0].min(axis=0)
        maxes = best_demo["cloud_xyz"][0].max(axis=0)
        mins[2] -= .1
        maxes[2] += .1
        grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points, mins, maxes, 'base_footprint')
        HANDLES.append(grid_handle)
        #self.f = fit_tps(demo["rope"][0], userdata.points)
        
        userdata.left_used = left_used = best_demo["arms_used"].value in "lb"
        userdata.right_used = right_used = best_demo["arms_used"].value in "rb"
        print "left_used", left_used
        print "right_used", right_used
        
        warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo, left=left_used, right=right_used)
        trajectory = np.zeros(len(best_demo["times"]), dtype=trajectories.BodyState)                        
        
        Globals.pr2.update_rave()          
        if left_used:            
            l_arm_traj, feas_inds = trajectories.make_joint_traj(warped_demo["l_gripper_xyzs"], warped_demo["l_gripper_quats"], Globals.pr2.robot.GetManipulator("leftarm"),"base_footprint","l_gripper_tool_frame",1+16)            
            if len(feas_inds) == 0: return "failure"
            trajectory["l_arm"] = l_arm_traj
            rospy.loginfo("left arm: %i of %i points feasible", len(feas_inds), len(trajectory))
            trajectory["l_gripper"] = fix_gripper(warped_demo["l_gripper_angles"])
            HANDLES.append(Globals.rviz.draw_curve(
                conversions.array_to_pose_array(
                    alternate(warped_demo["l_gripper_xyzs1"],warped_demo["l_gripper_xyzs2"]), 
                    "base_footprint"), 
                width=.001, rgba = (1,0,1,.4),type=Marker.LINE_LIST))
        if right_used:
            r_arm_traj,feas_inds = trajectories.make_joint_traj(warped_demo["r_gripper_xyzs"], warped_demo["r_gripper_quats"], Globals.pr2.robot.GetManipulator("rightarm"),"base_footprint","r_gripper_tool_frame",1+16)            
            if len(feas_inds) == 0: return "failure"
            trajectory["r_arm"] = r_arm_traj
            rospy.loginfo("right arm: %i of %i points feasible", len(feas_inds), len(trajectory))            
            trajectory["r_gripper"] = fix_gripper(warped_demo["r_gripper_angles"])
            HANDLES.append(Globals.rviz.draw_curve(
                conversions.array_to_pose_array(
                    alternate(warped_demo["l_gripper_xyzs1"],warped_demo["l_gripper_xyzs2"]), 
                    "base_footprint"), 
                width=.001, rgba = (1,0,1,.4),type=Marker.LINE_LIST))
        userdata.trajectory = trajectory
        #userdata.base_xya = (x,y,0)
        # todo: draw pr2        
        # consent = yes_or_no("trajectory ok?")
        consent = True
        if consent: return "not_done"
        else: return "failure"
コード例 #10
0
def callback(req):
    assert isinstance(req, MakeTrajectoryRequest)
    if req.verb not in h5file:
        rospy.logerr("verb %s was not found in library",req.verb)
    
    demo_group = h5file[req.verb]["demos"]
    new_object_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds]
    
    rospy.logwarn("using first demo")
    best_demo_name = demo_group.keys()[0]
    best_demo = demo_group[best_demo_name]
                           
    assert "transform" in verb_info[req.verb]                       
        
    transform_type = verb_info[req.verb]["transform"]
    x_nd = voxel_downsample(asarray(best_demo["object_clouds"].values()[0]),.02)
    y_md = voxel_downsample(new_object_clouds[0],.02)
    
    if transform_type == "tps":
        warp = registration.tps_rpm(x_nd, y_md, plotting = 4, reg_init = 2, reg_final = .1, n_iter = 39)
    elif transform_type == "translation2d":
        warp = registration.Translation2d()
        warp.fit(x_nd, y_md)
    elif transform_type == "rigid2d":
        warp = registration.Rigid2d()
        warp.fit(x_nd, y_md)
    else:
        raise Exception("transform type %s is not yet implemented"%transform_type)        

    this_verb_info = verb_info[req.verb]
    l_offset,r_offset = np.zeros(3), np.zeros(3)
    if "r_tool" in this_verb_info:
        r_offset = asarray(tool_info[this_verb_info["r_tool"]]["translation"])
    if "l_tool" in this_verb_info:
        l_offset = asarray(tool_info[this_verb_info["l_tool"]]["translation"])
    if "r_offset" in this_verb_info:
        r_offset += asarray(this_verb_info["r_offset"])
    if "l_offset" in this_verb_info:
        l_offset += asarray(this_verb_info["l_offset"])



    arms_used = best_demo["arms_used"].value
    warped_demo = warping.transform_demo(warp, best_demo,arms_used in 'lb', arms_used in 'rb',object_clouds=True, l_offset = l_offset, r_offset = r_offset)
    
    

    resp = MakeTrajectoryResponse()
    traj = resp.traj
        
    if arms_used == "l":
        traj.arms_used = "l"
        traj.gripper0_poses.poses = xyzs_quats_to_poses(warped_demo["l_gripper_xyzs"], warped_demo["l_gripper_quats"])
        traj.gripper0_angles = warped_demo["l_gripper_angles"]
        if "l_tool" in this_verb_info: traj.gripper0_angles *= 0
    elif arms_used == "r":
        traj.arms_used = "r"
        traj.gripper0_poses.poses = xyzs_quats_to_poses(warped_demo["r_gripper_xyzs"], warped_demo["r_gripper_quats"])
        traj.gripper0_angles = warped_demo["r_gripper_angles"]
        if "r_tool" in this_verb_info: traj.gripper0_angles *= 0
    elif arms_used == "b":
        traj.arms_used = "b"
        traj.gripper0_poses.poses = xyzs_quats_to_poses(warped_demo["l_gripper_xyzs"], warped_demo["l_gripper_quats"])
        traj.gripper1_poses.poses = xyzs_quats_to_poses(warped_demo["r_gripper_xyzs"], warped_demo["r_gripper_quats"])
        traj.gripper0_angles = warped_demo["l_gripper_angles"]
        traj.gripper1_angles = warped_demo["r_gripper_angles"]

        if "l_tool" in this_verb_info: traj.gripper0_angles *= 0
        if "r_tool" in this_verb_info: traj.gripper1_angles *= 0
        
    traj.gripper0_poses.header.frame_id = req.object_clouds[0].header.frame_id
    traj.gripper1_poses.header.frame_id = req.object_clouds[0].header.frame_id

    Globals.handles = []
    plot_original_and_warped_demo(best_demo, warped_demo, traj)
    return resp
コード例 #11
0
from utils.colorize import colorize
import h5py
import matplotlib.pyplot as plt

h5file = h5py.File(osp.join(osp.dirname(lfd.__file__), "data/knot_segments.h5"),"r")
seg_names = h5file.keys()


results = []

dists = []
ropes = []
for i in xrange(6):
    rope = np.squeeze(h5file[seg_names[i]]["cloud_xyz"])
    if args.downsample:
        rope_ds,ds_inds = voxel_downsample(rope,.03,return_inds = True)
        dist = recognition.calc_geodesic_distances_downsampled(rope, rope_ds, ds_inds)
        dists.append(dist)
        ropes.append(rope_ds)
    else:
        dist = recognition.calc_geodesic_distances(rope)
        dists.append(dist)
        ropes.append(rope)
                                                                    
for i0 in xrange(6):
    rope0 = ropes[i0]
    n0 = len(rope0)
    #pairs = zip(xrange(n0), np.random.randint(0,n0,n0))
    dists0 = dists[i0]

    for i1 in xrange(6):