def get_demo_to_exp_tool_transform(verb_data_accessor, tool_stage_info, exp_tool_cloud, transform_type): tool_stage_data = verb_data_accessor.get_demo_stage_data(tool_stage_info.stage_name) demo_tool_cloud = tool_stage_data["object_cloud"][tool_stage_info.item]["xyz"] demo_tool_cloud_down = voxel_downsample(demo_tool_cloud, .02) exp_tool_cloud_down = voxel_downsample(exp_tool_cloud, .02) demo_to_exp_tool_transform = get_warping_transform(demo_tool_cloud_down, exp_tool_cloud_down, transform_type) return demo_to_exp_tool_transform
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)
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
def check_that_nr_fit_runs(): from jds_image_proc.clouds import voxel_downsample #from brett2.ros_utils import RvizWrapper #import lfd.registration as lr ##import lfd.warping as lw #if rospy.get_name() == "/unnamed": #rospy.init_node("test_rigidity", disable_signals=True) #from time import sleep #sleep(1) #rviz = RvizWrapper.create() pts0 = np.loadtxt("../test/rope_control_points.txt") pts1 = np.loadtxt("../test/bad_rope.txt") pts_rigid = voxel_downsample(pts0[:10], .02) #lr.Globals.setup() np.seterr(all='ignore') np.set_printoptions(suppress=True) lin_ag, trans_g, w_eg, x_ea = tps.tps_nr_fit_enhanced(pts0, pts1, 0.01, pts_rigid, 0.001, method="newton", plotting=0) #lin_ag2, trans_g2, w_ng2 = tps_fit(pts0, pts1, .01, .01) #assert np.allclose(w_ng, w_ng2) def eval_partial(x_ma): return tps_eval(x_ma, lin_ag, trans_g, w_eg, x_ea) #lr.plot_orig_and_warped_clouds(eval_partial, pts0, pts1, res=.008) #handles = lw.draw_grid(rviz, eval_partial, pts0.min(axis=0), pts0.max(axis=0), 'base_footprint') grads = tps.tps_grad(pts_rigid, lin_ag, trans_g, w_eg, x_ea)
def plot_original_and_warped_demo_and_spec_pt(best_demo, warped_demo, spec_pt_traj, warped_spec_pt_traj, traj): arms_used = best_demo["arms_used"] if arms_used in "lb": pose_array = conversions.array_to_pose_array(asarray(best_demo["l_gripper_tool_frame"]["position"]), 'base_footprint', asarray(best_demo["l_gripper_tool_frame"]["orientation"])) Globals.handles.append(Globals.rviz.draw_traj_points(pose_array, rgba = (1,0,0,1),ns = "make_verb_traj_service")) pose_array = conversions.array_to_pose_array(asarray(warped_demo["l_gripper_tool_frame"]["position"]), 'base_footprint', asarray(warped_demo["l_gripper_tool_frame"]["orientation"])) Globals.handles.append(Globals.rviz.draw_traj_points(pose_array, rgba = (0,1,0,1),ns = "make_verb_traj_service")) Globals.handles.extend(Globals.rviz.draw_trajectory(traj.l_gripper_poses, traj.l_gripper_angles, ns = "make_verb_traj_service_grippers")) if arms_used in "rb": pose_array = conversions.array_to_pose_array(asarray(best_demo["r_gripper_tool_frame"]["position"]), 'base_footprint', asarray(best_demo["r_gripper_tool_frame"]["orientation"])) Globals.handles.append(Globals.rviz.draw_traj_points(pose_array, rgba = (1,0,0,1),ns = "make_verb_traj_service")) pose_array = conversions.array_to_pose_array(asarray(warped_demo["r_gripper_tool_frame"]["position"]), 'base_footprint', asarray(warped_demo["r_gripper_tool_frame"]["orientation"])) Globals.handles.append(Globals.rviz.draw_traj_points(pose_array, rgba = (0,1,0,1),ns = "make_verb_traj_service")) Globals.handles.extend(Globals.rviz.draw_trajectory(traj.r_gripper_poses, traj.r_gripper_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["xyz"]).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))
def check_that_nr_fit_runs(): from jds_image_proc.clouds import voxel_downsample #from brett2.ros_utils import RvizWrapper #import lfd.registration as lr ##import lfd.warping as lw #if rospy.get_name() == "/unnamed": #rospy.init_node("test_rigidity", disable_signals=True) #from time import sleep #sleep(1) #rviz = RvizWrapper.create() pts0 = np.loadtxt("../test/rope_control_points.txt") pts1 = np.loadtxt("../test/bad_rope.txt") pts_rigid = voxel_downsample(pts0[:10], .02) #lr.Globals.setup() np.seterr(all='ignore') np.set_printoptions(suppress=True) lin_ag, trans_g, w_eg, x_ea = tps.tps_nr_fit_enhanced(pts0, pts1, 0.01, pts_rigid, 0.001, method="newton",plotting=1) #lin_ag2, trans_g2, w_ng2 = tps_fit(pts0, pts1, .01, .01) #assert np.allclose(w_ng, w_ng2) def eval_partial(x_ma): return tps_eval(x_ma, lin_ag, trans_g, w_eg, x_ea) #lr.plot_orig_and_warped_clouds(eval_partial, pts0, pts1, res=.008) #handles = lw.draw_grid(rviz, eval_partial, pts0.min(axis=0), pts0.max(axis=0), 'base_footprint') grads = tps.tps_grad(pts_rigid, lin_ag, trans_g, w_eg, x_ea)
def save_simple_pc(demo_dir, demo_name, stage_num, file_name): verb_data_accessor = multi_item_verbs.VerbDataAccessor(test_info_dir="test/multi_item/empty_move_data/%s"%demo_dir) stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num) stage_data = verb_data_accessor.get_demo_stage_data(stage_info.stage_name) stage_pc = stage_data["object_cloud"][stage_info.item]["xyz"] pc_down = voxel_downsample(stage_pc, .02) os.chdir(osp.join(osp.dirname(__file__), "test_pcs")) np.savetxt("%s.pc" % (stage_info.item if file_name is None else file_name), pc_down)
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
def setup_obj_rave(obj_cloud_xyz, obj_name): rave_env = Globals.pr2.env pc_down = voxel_downsample(obj_cloud_xyz, 0.02) body = rave.RaveCreateKinBody(rave_env, "") body.SetName(obj_name) # might want to change this name later delaunay = scipy.spatial.Delaunay(pc_down) body.InitFromTrimesh(rave.TriMesh(delaunay.points, delaunay.convex_hull), True) rave_env.Add(body) return body
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
def setup_obj_rave(obj_cloud_xyz, obj_name): rave_env = Globals.pr2.env pc_down = voxel_downsample(obj_cloud_xyz, .02) body = rave.RaveCreateKinBody(rave_env, '') body.SetName(obj_name) #might want to change this name later delaunay = scipy.spatial.Delaunay(pc_down) body.InitFromTrimesh(rave.TriMesh(delaunay.points, delaunay.convex_hull), True) rave_env.Add(body) return body
def save_simple_pc(demo_dir, demo_name, stage_num, file_name): verb_data_accessor = multi_item_verbs.VerbDataAccessor( test_info_dir="test/multi_item/empty_move_data/%s" % demo_dir) stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num) stage_data = verb_data_accessor.get_demo_stage_data(stage_info.stage_name) stage_pc = stage_data["object_cloud"][stage_info.item]["xyz"] pc_down = voxel_downsample(stage_pc, .02) os.chdir(osp.join(osp.dirname(__file__), "test_pcs")) np.savetxt("%s.pc" % (stage_info.item if file_name is None else file_name), pc_down)
def filter_pc2(cloud_pc2): cloud_xyz = (pc2xyzrgb(cloud_pc2)[0]).reshape(-1,3) cloud_xyz_down = voxel_downsample(cloud_xyz, .02) graph = ri.points_to_graph(cloud_xyz_down, .03) cc = ri.largest_connected_component(graph) good_xyzs = np.array([graph.node[node_id]["xyz"] for node_id in cc.nodes()]) pose_array = juc.array_to_pose_array(good_xyzs, "base_footprint") Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,1,0,1), type=Marker.CUBE_LIST, width=.001, ns="segmentation")) raw_input("press enter when done looking") del Globals.handles[:] return xyz2pc(good_xyzs, cloud_pc2.header.frame_id)
def test_plate(): import rospy, itertools, glob from jds_utils.colorize import colorize from jds_image_proc.pcd_io import load_xyzrgb import h5py if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True) data_dir = "/home/joschu/python/lfd/data" f = h5py.File(osp.join(data_dir, "images/pickup-plate0.seg.h5"),"r") xyz1, rgb1 = np.asarray(f["plate"]["xyz"]), np.asarray(f["plate"]["rgb"]) xyz1 = voxel_downsample(xyz1, .02) from numpy import sin, cos, pi xyz_target = xyz1.mean(axis=0)[None,:] + 1.5*np.dot(xyz1 - xyz1.mean(axis=0)[None,:], np.array([[cos(pi/3), sin(pi/3), 0], [-sin(pi/3), cos(pi/3), 0], [0, 0, 1]])) f = registration.tps_rpm_zrot(xyz1, xyz_target, plotting=200,reg_init=2,reg_final=.5,n_iter=8, verbose=False)
def test_zrot1(): #def test_plate(): import rospy, itertools, glob from jds_utils.colorize import colorize from jds_image_proc.pcd_io import load_xyzrgb import h5py if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True) data_dir = "/home/joschu/python/lfd/data" f = h5py.File(osp.join(data_dir, "images/pickup-plate0.seg.h5"),"r") xyz1, rgb1 = np.asarray(f["plate"]["xyz"]), np.asarray(f["plate"]["rgb"]) xyz1 = voxel_downsample(xyz1, .02) from numpy import sin, cos, pi print "true angle", pi/3 xyz_target = xyz1.mean(axis=0)[None,:] + 1.5*np.dot(xyz1 - xyz1.mean(axis=0)[None,:], np.array([[cos(pi/3), sin(pi/3), 0], [-sin(pi/3), cos(pi/3), 0], [0, 0, 1]])) f = registration.tps_rpm_zrot(xyz1, xyz_target, plotting=1,reg_init=2,reg_final=.5,n_iter=8, verbose=False, cost_per_radian=2)
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])
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])
def save_pcs(clouds_pc2, save_file): os.chdir(osp.join(osp.dirname(__file__), "exp_pcs")) for i, cloud_pc2 in enumerate(clouds_pc2): np.savetxt("%s.pc%i" % (save_file, i), voxel_downsample(pc2xyzrgb(cloud_pc2)[0], 0.02))
from os.path import join, basename, splitext from glob import glob from point_clouds.features import cloud_shape_context from jds_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)
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, .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])
def downsample(xyz): from jds_image_proc.clouds import voxel_downsample DS_LENGTH = .025 xyz_ds, ds_inds = voxel_downsample(xyz, DS_LENGTH, return_inds = True) return xyz_ds, ds_inds
def make_traj(req): """ Generate a trajectory using warping See MakeTrajectory service description (TODO) should be able to specify a specific demo """ assert isinstance(req, MakeTrajectoryRequest) new_object_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds] scene_info = "PLACEHOLDER" best_demo_name, best_demo_info = verbs.get_closest_demo( req.verb, scene_info) best_demo_data = verbs.get_demo_data(best_demo_name) transform_type = "tps" old_object_clouds = [ best_demo_data["object_clouds"][obj_name]["xyz"] for obj_name in best_demo_data["object_clouds"].keys() ] if len(old_object_clouds) > 1: raise Exception("i don't know what to do with multiple object clouds") x_nd = voxel_downsample(old_object_clouds[0], .02) y_md = voxel_downsample(new_object_clouds[0], .02) if transform_type == "tps": #warp = registration.tps_rpm_zrot(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False) warp = registration.tps_rpm(x_nd, y_md, plotting=2, reg_init=2, reg_final=.05, n_iter=10, verbose=False) 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) l_offset, r_offset = np.zeros(3), np.zeros(3) #if "r_tool" in best_demo_info: #r_offset = asarray(tool_info[this_verb_info["r_tool"]]["translation"]) #if "l_tool" in best_demo_info: #l_offset = asarray(tool_info[this_verb_info["l_tool"]]["translation"]) arms_used = best_demo_info["arms_used"] warped_demo_data = warping.transform_verb_demo(warp, best_demo_data) resp = MakeTrajectoryResponse() traj = resp.traj traj.arms_used = arms_used if arms_used in "lb": traj.l_gripper_poses.poses = juc.xyzs_quats_to_poses( warped_demo_data["l_gripper_tool_frame"]["position"], warped_demo_data["l_gripper_tool_frame"]["orientation"]) traj.l_gripper_angles = warped_demo_data["l_gripper_joint"] traj.l_gripper_poses.header.frame_id = req.object_clouds[ 0].header.frame_id if "l_tool" in best_demo_info: traj.l_gripper_angles *= 0 if arms_used in "rb": traj.r_gripper_poses.poses = juc.xyzs_quats_to_poses( warped_demo_data["r_gripper_tool_frame"]["position"], warped_demo_data["r_gripper_tool_frame"]["orientation"]) traj.r_gripper_angles = warped_demo_data["r_gripper_joint"] traj.r_gripper_poses.header.frame_id = req.object_clouds[ 0].header.frame_id if "r_tool" in best_demo_info: traj.r_gripper_angles *= 0 Globals.handles = [] plot_original_and_warped_demo(best_demo_data, warped_demo_data, traj) pose_array = conversions.array_to_pose_array(y_md, 'base_footprint') Globals.handles.append( Globals.rviz.draw_curve(pose_array, rgba=(0, 0, 1, 1), width=.01, type=Marker.CUBE_LIST)) return resp
return np.any(np.all((pt[None,:] == arr), axis=1)) def get_good_inds(orig_xyz, good_xyz): good_inds = [] for i, xyz in enumerate(orig_xyz): if array_contains(good_xyzs, xyz): good_inds.append(i) return good_inds for object_name in object_names: pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc)).cloud_out xyz, rgb = ros_utils.pc2xyzrgb(pc_sel) xyz = xyz.reshape(-1,3) rgb = rgb.reshape(-1,3) if args.do_filtering: xyz_down = voxel_downsample(xyz, .02) graph = ri.points_to_graph(xyz_down, .03) cc = ri.largest_connected_component(graph) good_xyzs = np.array([graph.node[node_id]["xyz"] for node_id in cc.nodes()]) good_inds = get_good_inds(xyz, good_xyzs) good_rgbs = rgb[good_inds] else: good_xyzs, good_rgbs = xyz, rgb outfile.create_group(object_name) outfile[object_name]["xyz"] = good_xyzs #outfile[object_name]["rgb"] = good_rgbs if args.plotting: rviz = ros_utils.RvizWrapper.create() rospy.sleep(.5)
def get_demo_to_exp_target_transform(demo_target_cloud, exp_target_cloud, transform_type): demo_target_cloud_down = voxel_downsample(demo_target_cloud, .02) exp_target_cloud_down = voxel_downsample(exp_target_cloud, .02) demo_to_exp_target_transform = get_warping_transform(demo_target_cloud_down, exp_target_cloud_down, transform_type) return demo_to_exp_target_transform
def make_traj(req): """ Generate a trajectory using warping See MakeTrajectory service description (TODO) should be able to specify a specific demo """ assert isinstance(req, MakeTrajectoryRequest) new_object_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds] scene_info = "PLACEHOLDER" best_demo_name, best_demo_info = verbs.get_closest_demo(req.verb, scene_info) best_demo_data = verbs.get_demo_data(best_demo_name) transform_type = "tps" old_object_clouds = [best_demo_data["object_clouds"][obj_name]["xyz"] for obj_name in best_demo_data["object_clouds"].keys()] if len(old_object_clouds) > 1: raise Exception("i don't know what to do with multiple object clouds") x_nd = voxel_downsample(old_object_clouds[0],.02) y_md = voxel_downsample(new_object_clouds[0],.02) if transform_type == "tps": #warp = registration.tps_rpm_zrot(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False) warp = registration.tps_rpm(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False) 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) l_offset,r_offset = np.zeros(3), np.zeros(3) #if "r_tool" in best_demo_info: #r_offset = asarray(tool_info[this_verb_info["r_tool"]]["translation"]) #if "l_tool" in best_demo_info: #l_offset = asarray(tool_info[this_verb_info["l_tool"]]["translation"]) arms_used = best_demo_info["arms_used"] warped_demo_data = warping.transform_verb_demo(warp, best_demo_data) resp = MakeTrajectoryResponse() traj = resp.traj traj.arms_used = arms_used if arms_used in "lb": traj.l_gripper_poses.poses = xyzs_quats_to_poses(warped_demo_data["l_gripper_tool_frame"]["position"], warped_demo_data["l_gripper_tool_frame"]["orientation"]) traj.l_gripper_angles = warped_demo_data["l_gripper_joint"] traj.l_gripper_poses.header.frame_id = req.object_clouds[0].header.frame_id if "l_tool" in best_demo_info: traj.l_gripper_angles *= 0 if arms_used in "rb": traj.r_gripper_poses.poses = xyzs_quats_to_poses(warped_demo_data["r_gripper_tool_frame"]["position"], warped_demo_data["r_gripper_tool_frame"]["orientation"]) traj.r_gripper_angles = warped_demo_data["r_gripper_joint"] traj.r_gripper_poses.header.frame_id = req.object_clouds[0].header.frame_id if "r_tool" in best_demo_info: traj.r_gripper_angles *= 0 Globals.handles = [] plot_original_and_warped_demo(best_demo_data, warped_demo_data, traj) pose_array = conversions.array_to_pose_array(y_md, 'base_footprint') Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,0,1,1),width=.01,type=Marker.CUBE_LIST)) return resp
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 """ 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_old(xyz_new,xyz_new_ds, ds_inds) ELOG.log('SelectTrajectory', 'xyz_new', xyz_new) ELOG.log('SelectTrajectory', 'xyz_new_ds', xyz_new_ds) ELOG.log('SelectTrajectory', 'dists_new', dists_new) if args.count_steps: candidate_demo_names = self.count2segnames[Globals.stage] else: candidate_demo_names = demos.keys() global last_selected_segment print 'Last selected segment:', last_selected_segment if args.human_select_demo: best_name = None while best_name not in demos: print 'Select demo from', demos.keys() best_name = raw_input('> ') else: from joblib import parallel costs_names = parallel.Parallel(n_jobs=-2)(parallel.delayed(calc_seg_cost)(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names) #costs_names = [calc_seg_cost(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names] #costs_names = [calc_seg_cost(seg_name) for seg_name in candidate_demo_names] ELOG.log('SelectTrajectory', 'costs_names', costs_names) _, best_name = min(costs_names) ELOG.log('SelectTrajectory', 'best_name', best_name) best_demo = demos[best_name] if best_demo["done"]: rospy.loginfo("best demo was a 'done' state") return "done" best_demo = demos[best_name] rospy.loginfo("best segment name: %s", best_name) last_selected_segment = best_name xyz_demo_ds = best_demo["cloud_xyz_ds"] ELOG.log('SelectTrajectory', 'xyz_demo_ds', xyz_demo_ds) if args.test: n_iter = 21 else: n_iter = 101 if args.use_rigid: self.f = registration.Translation2d() self.f.fit(xyz_demo_ds, xyz_new_ds) ELOG.log('SelectTrajectory', 'f', self.f) else: self.f, info = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting = 20, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False, return_full=True)#, interactive=True) ELOG.log('SelectTrajectory', 'f', self.f) ELOG.log('SelectTrajectory', 'f_info', info) if args.use_nr: rospy.loginfo('Using nonrigidity costs') from lfd import tps import scipy.spatial.distance as ssd pts_grip = [] for lr in "lr": if best_demo["arms_used"] in ["b", lr]: pts_grip.extend(best_demo["%s_gripper_tool_frame"%lr]["position"]) pts_grip = np.array(pts_grip) dist_to_rope = ssd.cdist(pts_grip, xyz_demo_ds).min(axis=1) pts_grip_near_rope = pts_grip[dist_to_rope < .04,:] pts_rigid = voxel_downsample(pts_grip_near_rope, .01) self.f.lin_ag, self.f.trans_g, self.f.w_ng, self.f.x_na = tps.tps_nr_fit_enhanced(info["x_Nd"], info["targ_Nd"], 0.01, pts_rigid, 0.001, method="newton", plotting=5) # print 'correspondences', self.f.corr_nm #################### Generate new trajectory ################## #### Plot original and warped point clouds ####### # orig_pose_array = conversions.array_to_pose_array(np.squeeze(best_demo["cloud_xyz_ds"]), "base_footprint") # warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(np.squeeze(best_demo["cloud_xyz_ds"])), "base_footprint") # Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST)) # Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST)) #### Plot grid ######## mins = np.squeeze(best_demo["cloud_xyz"]).min(axis=0) maxes = np.squeeze(best_demo["cloud_xyz"]).max(axis=0) mins[2] -= .1 maxes[2] += .1 grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points, mins, maxes, 'base_footprint') Globals.handles.append(grid_handle) #### Actually generate the trajectory ########### warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo) # if yes_or_no('dump warped demo?'): # import pickle # fname = '/tmp/warped_demo_' + str(np.random.randint(9999999999)) + '.pkl' # with open(fname, 'w') as f: # pickle.dump(warped_demo, f) # print 'saved to', fname ELOG.log('SelectTrajectory', 'warped_demo', warped_demo) def make_traj(warped_demo, inds=None, xyz_offset=0, feas_check_only=False): traj = {} total_feas_inds = 0 total_inds = 0 for lr in "lr": leftright = {"l":"left","r":"right"}[lr] if best_demo["arms_used"] in [lr, "b"]: if args.hard_table: clipinplace(warped_demo["l_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf) clipinplace(warped_demo["r_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf) pos = warped_demo["%s_gripper_tool_frame"%lr]["position"] + xyz_offset ori = warped_demo["%s_gripper_tool_frame"%lr]["orientation"] if inds is not None: pos, ori = pos[inds], ori[inds] if feas_check_only: feas_inds = lfd_traj.compute_feas_inds( pos, ori, Globals.pr2.robot.GetManipulator("%sarm"%leftright), "%s_gripper_tool_frame"%lr, check_collisions=True) traj["%s_arm_feas_inds"%lr] = feas_inds else: arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search( pos, ori, Globals.pr2.robot.GetManipulator("%sarm"%leftright), "%s_gripper_tool_frame"%lr, check_collisions=True) traj["%s_arm"%lr] = arm_traj traj["%s_arm_feas_inds"%lr] = feas_inds total_feas_inds += len(feas_inds) total_inds += len(pos) rospy.loginfo("%s arm: %i of %i points feasible", leftright, len(feas_inds), len(pos)) return traj, total_feas_inds, total_inds # Check if we need to move the base for reachability base_offset = np.array([0, 0, 0]) if args.use_base: # First figure out how much we need to move the base to maximize feasible points OFFSET = 0.1 XYZ_OFFSETS = np.array([[0., 0., 0.], [-OFFSET, 0, 0], [OFFSET, 0, 0], [0, -OFFSET, 0], [0, OFFSET, 0]]) inds_to_check = lfd_traj.where_near_rope(best_demo, xyz_demo_ds, add_other_points=30) need_to_move_base = False best_feas_inds, best_xyz_offset = -1, None for xyz_offset in XYZ_OFFSETS: _, n_feas_inds, n_total_inds = make_traj(warped_demo, inds=inds_to_check, xyz_offset=xyz_offset, feas_check_only=True) rospy.loginfo('Cloud offset %s has feas inds %d', str(xyz_offset), n_feas_inds) if n_feas_inds > best_feas_inds: best_feas_inds, best_xyz_offset = n_feas_inds, xyz_offset if n_feas_inds >= 0.99*n_total_inds: break if np.linalg.norm(best_xyz_offset) > 0.01: need_to_move_base = True base_offset = -best_xyz_offset rospy.loginfo('Best base offset: %s, with %d feas inds', str(base_offset), best_feas_inds) # Move the base if need_to_move_base: rospy.loginfo('Will move base.') userdata.base_offset = base_offset return 'move_base' else: rospy.loginfo('Will not move base.') Globals.pr2.update_rave() # calculate joint trajectory using IK trajectory = make_traj(warped_demo)[0] # fill in gripper/grab stuff for lr in "lr": leftright = {"l":"left","r":"right"}[lr] if best_demo["arms_used"] in [lr, "b"]: if len(trajectory["%s_arm_feas_inds"%lr]) == 0: return "failure" trajectory["%s_grab"%lr] = best_demo["%s_gripper_joint"%lr] < .07 trajectory["%s_gripper"%lr] = warped_demo["%s_gripper_joint"%lr] trajectory["%s_gripper"%lr][trajectory["%s_grab"%lr]] = 0 # smooth any discontinuities in the arm traj for lr in "lr": leftright = {"l":"left","r":"right"}[lr] if best_demo["arms_used"] in [lr, "b"]: trajectory["%s_arm"%lr], discont_times, n_steps = lfd_traj.smooth_disconts( trajectory["%s_arm"%lr], Globals.pr2.env, Globals.pr2.robot.GetManipulator("%sarm"%leftright), "%s_gripper_tool_frame"%lr, ignore_inds=[1] # ignore the 0--1 discontinuity, which is usually just moving from rest to the traj starting pose ) # after smoothing the arm traj, we need to fill in all other trajectories (in both arms) other_lr = 'r' if lr == 'l' else 'l' if best_demo["arms_used"] in [other_lr, "b"]: trajectory["%s_arm"%other_lr] = lfd_traj.fill_stationary(trajectory["%s_arm"%other_lr], discont_times, n_steps) for tmp_lr in 'lr': if best_demo["arms_used"] in [tmp_lr, "b"]: trajectory["%s_grab"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_grab"%tmp_lr], discont_times, n_steps) trajectory["%s_gripper"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_gripper"%tmp_lr], discont_times, n_steps) trajectory["%s_gripper"%tmp_lr][trajectory["%s_grab"%tmp_lr]] = 0 # plotting for lr in "lr": leftright = {"l":"left","r":"right"}[lr] if best_demo["arms_used"] in [lr, "b"]: # plot warped trajectory Globals.handles.append(Globals.rviz.draw_curve( conversions.array_to_pose_array( alternate(warped_demo["%s_gripper_l_finger_tip_link"%lr]["position"], warped_demo["%s_gripper_r_finger_tip_link"%lr]["position"]), "base_footprint" ), width=.001, rgba = (1,0,1,.4), type=Marker.LINE_LIST, ns='warped_finger_traj' )) # plot original trajectory Globals.handles.append(Globals.rviz.draw_curve( conversions.array_to_pose_array( alternate(best_demo["%s_gripper_l_finger_tip_link"%lr]["position"], best_demo["%s_gripper_r_finger_tip_link"%lr]["position"]), "base_footprint" ), width=.001, rgba = (0,1,1,.4), type=Marker.LINE_LIST, ns='demo_finger_traj' )) ELOG.log('SelectTrajectory', 'trajectory', trajectory) userdata.trajectory = trajectory if args.prompt_before_motion: consent = yes_or_no("trajectory ok?") else: consent = True if consent: return "not_done" else: return "failure"
from jds_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]
def make_traj_multi_stage_do_work(current_stage_info, cur_exp_clouds, clouds_frame_id, stage_num, prev_stage_info, prev_exp_clouds, verb_data_accessor, to_gripper_frame_func=None): arms_used = current_stage_info.arms_used verb_stage_data = verb_data_accessor.get_demo_data(current_stage_info.stage_name) if stage_num == 0: # don't do any extra transformation for the first stage prev_demo_to_exp_grip_transform_lin_rigid = np.identity(4) # no special point translation for first stage since no tool yet special_point_translation = np.identity(4) elif stage_num > 0: # make sure that the tool stage only uses one arm (the one with the tool) assert arms_used in ['r', 'l'] prev_stage_data = verb_data_accessor.get_demo_data(prev_stage_info.stage_name) prev_demo_pc = prev_stage_data["object_clouds"][prev_stage_info.item]["xyz"] prev_exp_pc = prev_exp_clouds[0] prev_demo_pc_down = voxel_downsample(prev_demo_pc, .02) prev_exp_pc_down = voxel_downsample(prev_exp_pc, .02) gripper_data_key = "%s_gripper_tool_frame" % (arms_used) # transform point cloud in base frame to gripper frame # assume right hand has the tool for now # use the last pose of the gripper in the stage to figure out the point cloud of the tool in the gripper frame when the tool was grabbed prev_demo_gripper_pos = prev_stage_data[gripper_data_key]["position"][-1] prev_demo_gripper_orien = prev_stage_data[gripper_data_key]["orientation"][-1] prev_demo_gripper_to_base_transform = juc.trans_rot_to_hmat(prev_demo_gripper_pos, prev_demo_gripper_orien) prev_demo_base_to_gripper_transform = np.linalg.inv(prev_demo_gripper_to_base_transform) prev_demo_pc_in_gripper_frame = np.array([apply_transform(prev_demo_base_to_gripper_transform, point) for point in prev_demo_pc_down]) # get the new point cloud in the new gripper frame # prev_exp_pc_in_gripper_frame = [apply_transform(prev_exp_base_to_gripper_transform, point) for point in prev_exp_pc_down] if to_gripper_frame_func is None: prev_exp_pc_in_gripper_frame = to_gripper_frame_tf_listener(prev_exp_pc_down, gripper_data_key) else: prev_exp_pc_in_gripper_frame = to_gripper_frame_func(prev_exp_pc_down, gripper_data_key) # get the transformation from the new point cloud to the old point cloud for the previous stage prev_demo_to_exp_grip_transform = get_tps_transform(prev_demo_pc_in_gripper_frame, prev_exp_pc_in_gripper_frame) # transforms gripper trajectory point into special point trajectory point if prev_stage_info.special_point is None: # if there is no special point, linearize at origin prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.zeros(3)) # don't do a special point translation if there is no specified special point special_point_translation = np.identity(4) else: prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.array(prev_stage_info.special_point)) # translation from gripper pose in world frame to special point pose in world frame special_point_translation = jut.translation_matrix(np.array(prev_stage_info.special_point)) if arms_used != 'b': arms_used_list = [arms_used] else: arms_used_list = ['r', 'l'] warped_stage_data = group_to_dict(verb_stage_data) # deep copy it resp = MakeTrajectoryResponse() traj = resp.traj traj.arms_used = arms_used for arm in arms_used_list: gripper_data_key = "%s_gripper_tool_frame" % (arm) # find the special point trajectory before the target transformation cur_demo_gripper_traj_xyzs = verb_stage_data[gripper_data_key]["position"] cur_demo_gripper_traj_oriens = verb_stage_data[gripper_data_key]["orientation"] cur_demo_gripper_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(cur_demo_gripper_traj_xyzs, cur_demo_gripper_traj_oriens)] cur_mid_spec_pt_traj_mats = [np.dot(gripper_mat, special_point_translation) for gripper_mat in cur_demo_gripper_traj_mats] # find the transformation from the new special point to the gripper frame cur_exp_inv_special_point_transformation = np.linalg.inv(np.dot(prev_demo_to_exp_grip_transform_lin_rigid, special_point_translation)) # save the demo special point traj for plotting plot_spec_pt_traj = [] for gripper_mat in cur_demo_gripper_traj_mats: spec_pt_xyz, spec_pt_orien = juc.hmat_to_trans_rot(np.dot(gripper_mat, special_point_translation)) plot_spec_pt_traj.append(spec_pt_xyz) print 'grip transform:' print prev_demo_to_exp_grip_transform_lin_rigid print 'special point translation:' print special_point_translation print 'inverse special point translation:' print cur_exp_inv_special_point_transformation # find the target transformation for the experiment scene demo_object_clouds = [verb_stage_data["object_clouds"][obj_name]["xyz"] for obj_name in verb_stage_data["object_clouds"].keys()] if len(demo_object_clouds) > 1: raise Exception("i don't know what to do with multiple object clouds") x_nd = voxel_downsample(demo_object_clouds[0], .02) y_md = voxel_downsample(cur_exp_clouds[0], .02) # transformation from old target object to new target object in world frame cur_demo_to_exp_transform = get_tps_transform(x_nd, y_md) # apply the target warping transformation to the special point trajectory cur_mid_spec_pt_traj_xyzs, cur_mid_spec_pt_traj_oriens = [], [] for cur_mid_spec_pt_traj_mat in cur_mid_spec_pt_traj_mats: cur_mid_spec_pt_traj_xyz, cur_mid_spec_pt_traj_orien = juc.hmat_to_trans_rot(cur_mid_spec_pt_traj_mat) cur_mid_spec_pt_traj_xyzs.append(cur_mid_spec_pt_traj_xyz) cur_mid_spec_pt_traj_oriens.append(juc.quat2mat(cur_mid_spec_pt_traj_orien)) cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens = cur_demo_to_exp_transform.transform_frames(np.array(cur_mid_spec_pt_traj_xyzs), np.array(cur_mid_spec_pt_traj_oriens)) plot_warped_spec_pt_traj = cur_exp_spec_pt_traj_xyzs #save the special point traj for plotting cur_exp_spec_pt_traj_mats = [juc.trans_rot_to_hmat(cur_exp_spec_pt_traj_xyz, mat2quat(cur_exp_spec_pt_traj_orien)) for cur_exp_spec_pt_traj_xyz, cur_exp_spec_pt_traj_orien in zip(cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens)] # transform the warped special point trajectory back to a gripper trajectory in the experiment cur_exp_gripper_traj_mats = [np.dot(spec_pt_mat, cur_exp_inv_special_point_transformation) for spec_pt_mat in cur_exp_spec_pt_traj_mats] warped_stage_data[gripper_data_key]["position"] = [] warped_stage_data[gripper_data_key]["orientation"] = [] for exp_traj_mat in cur_exp_gripper_traj_mats: warped_pos, warped_orien = juc.hmat_to_trans_rot(exp_traj_mat) warped_stage_data[gripper_data_key]["position"].append(warped_pos) warped_stage_data[gripper_data_key]["orientation"].append(warped_orien) if arm == 'r': traj.r_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"]) print "poses: ", len(traj.r_gripper_poses.poses) traj.r_gripper_angles = warped_stage_data["r_gripper_joint"] traj.r_gripper_poses.header.frame_id = clouds_frame_id elif arm == 'l': traj.l_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"]) print "poses: ", len(traj.l_gripper_poses.poses) traj.l_gripper_angles = warped_stage_data["l_gripper_joint"] traj.l_gripper_poses.header.frame_id = clouds_frame_id Globals.handles = [] plot_original_and_warped_demo_and_spec_pt(verb_stage_data, warped_stage_data, plot_spec_pt_traj, plot_warped_spec_pt_traj, traj) pose_array = conversions.array_to_pose_array(y_md, 'base_footprint') Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,0,1,1),width=.01,type=Marker.CUBE_LIST)) return resp
if 1: if rospy.get_name() == "/unnamed": rospy.init_node("test_rigidity", disable_signals=True) from time import sleep sleep(1) rviz = RvizWrapper.create() np.random.seed(1) if 1: pts0 = np.loadtxt("rope_control_points.txt") pts1 = np.loadtxt("bad_rope.txt") pts_rigid = voxel_downsample(pts0, .02) #pts_rigid += np.random.randn(*pts_rigid.shape) gN = len(pts0) gM = len(pts_rigid) #pts0 = pts0 + np.random.randn(*pts0.shape)*.1 #pts1 = pts1 + np.random.randn(*pts1.shape)*.1 if 0: gN = 100 gM = 20 pts0 = np.random.randn(gN,3) pts1 = np.random.randn(gN,3) from lfd.tps import *
from jds_utils.colorize import colorize if 1: if rospy.get_name() == "/unnamed": rospy.init_node("test_rigidity", disable_signals=True) from time import sleep sleep(1) rviz = RvizWrapper.create() np.random.seed(1) if 1: pts0 = np.loadtxt("rope_control_points.txt") pts1 = np.loadtxt("bad_rope.txt") pts_rigid = voxel_downsample(pts0, .02) #pts_rigid += np.random.randn(*pts_rigid.shape) gN = len(pts0) gM = len(pts_rigid) #pts0 = pts0 + np.random.randn(*pts0.shape)*.1 #pts1 = pts1 + np.random.randn(*pts1.shape)*.1 if 0: gN = 100 gM = 20 pts0 = np.random.randn(gN, 3) pts1 = np.random.randn(gN, 3) from lfd.tps import *
from os.path import join, basename, splitext from glob import glob from point_clouds.features import cloud_shape_context from jds_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)
def plot_original_and_warped_demo(best_demo, warped_demo, traj): arms_used = best_demo["arms_used"] if arms_used in "lb": pose_array = conversions.array_to_pose_array( asarray(best_demo["l_gripper_tool_frame"]["position"]), '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_tool_frame"]["position"]), '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_tool_frame"]["position"]), '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_tool_frame"]["position"]), '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.l_gripper_poses, traj.l_gripper_angles, ns="make_verb_traj_service_grippers")) if arms_used == 'b': Globals.handles.extend( Globals.rviz.draw_trajectory(traj.r_gripper_poses, traj.r_gripper_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["xyz"]).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))
def get_good_inds(orig_xyz, good_xyz): good_inds = [] for i, xyz in enumerate(orig_xyz): if array_contains(good_xyzs, xyz): good_inds.append(i) return good_inds for object_name in object_names: pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in=pc)).cloud_out xyz, rgb = ros_utils.pc2xyzrgb(pc_sel) xyz = xyz.reshape(-1, 3) rgb = rgb.reshape(-1, 3) if args.do_filtering: xyz_down = voxel_downsample(xyz, .02) graph = ri.points_to_graph(xyz_down, .03) cc = ri.largest_connected_component(graph) good_xyzs = np.array( [graph.node[node_id]["xyz"] for node_id in cc.nodes()]) good_inds = get_good_inds(xyz, good_xyzs) good_rgbs = rgb[good_inds] else: good_xyzs, good_rgbs = xyz, rgb outfile.create_group(object_name) outfile[object_name]["xyz"] = good_xyzs #outfile[object_name]["rgb"] = good_rgbs if args.plotting: rviz = ros_utils.RvizWrapper.create()
if rospy.get_name() == "/unnamed": rospy.init_node("blah", disable_signals=True) 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, 0.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):
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, .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([
def select_trajectory(points, curr_robot_joint_vals, curr_step): """ - 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 """ xyz_new = np.squeeze(np.asarray(points)) #if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025) xyz_new_ds, ds_inds = downsample(xyz_new) # xyz_new_ds, ds_inds = xyz_new.reshape(-1,3), np.arange(0, len(xyz_new)).reshape(-1, 1) dists_new = recognition.calc_geodesic_distances_downsampled_old(xyz_new,xyz_new_ds, ds_inds) candidate_demo_names = Globals.demos.keys() #from joblib import parallel #costs_names = parallel.Parallel(n_jobs = 4)(parallel.delayed(calc_seg_cost)(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names) costs_names = [calc_seg_cost(seg_name, xyz_new_ds, dists_new) for seg_name in sorted(candidate_demo_names)] _, best_name = min(costs_names) print "choices: ", candidate_demo_names best_name = None while best_name not in Globals.demos: best_name = raw_input("type name of trajectory you want to use\n") rospy.loginfo('costs_names %s', costs_names) #matcher = recognition.CombinedNNMatcher(recognition.DataSet.LoadFromDict(Globals.demos), [recognition.GeodesicDistMatcher, recognition.ShapeContextMatcher], [1, 0.1]) #best_name, best_cost = matcher.match(xyz_new) best_demo = Globals.demos[best_name] if best_demo["done"]: rospy.loginfo("best demo was a 'done' state") return {'status': 'success'} rospy.loginfo("best segment name: %s", best_name) xyz_demo_ds = best_demo["cloud_xyz_ds"] # print 'arms used', best_demo['arms_used'] # overlap_ctl_pts = [] # grabbing_pts = [] # for lr in 'lr': # # look at points around gripper when grabbing # grabbing = map(bool, list(best_demo["%s_gripper_joint"%lr] < .07)) # grabbing_pts.extend([p for i, p in enumerate(best_demo["%s_gripper_l_finger_tip_link"%lr]["position"]) if grabbing[i] and (i == 0 or not grabbing[i-1])]) # grabbing_pts.extend([p for i, p in enumerate(best_demo["%s_gripper_r_finger_tip_link"%lr]["position"]) if grabbing[i] and (i == 0 or not grabbing[i-1])]) # overlap_ctl_pts = [p for p in xyz_demo_ds if any(np.linalg.norm(p - g) < 0.1 for g in grabbing_pts)] # overlap_ctl_pts = xyz_demo_ds #rviz_draw_points(overlap_ctl_pts,rgba=(1,1,1,1),type=Marker.CUBE_LIST) # rviz_draw_points(grabbing_pts,rgba=(.5,.5,.5,1),type=Marker.CUBE_LIST) n_iter = 101 #warping_map = registration.tps_rpm_with_overlap_control(xyz_demo_ds, xyz_new_ds, overlap_ctl_pts, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False, plotting=20) warping_map,info = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False, plotting=20,return_full=True) from lfd import tps import scipy.spatial.distance as ssd f = warping_map pts_grip = [] for lr in "lr": if best_demo["arms_used"] in ["b", lr]: pts_grip.extend(best_demo["%s_gripper_tool_frame"%lr]["position"]) pts_grip = np.array(pts_grip) dist_to_rope = ssd.cdist(pts_grip, xyz_demo_ds).min(axis=1) pts_grip_near_rope = pts_grip[dist_to_rope < .04,:] pts_rigid = voxel_downsample(pts_grip_near_rope, .01) Globals.handles = [] registration.Globals.handles = [] f.lin_ag, f.trans_g, f.w_ng, f.x_na = tps.tps_nr_fit_enhanced(info["x_Nd"], info["targ_Nd"], 0.01, pts_rigid, 0.001, method="newton", plotting=5) #if plotting: #plot_orig_and_warped_clouds(f.transform_points, x_nd, y_md) #targ_pose_array = conversions.array_to_pose_array(targ_Nd, 'base_footprint') #Globals.handles.append(Globals.rviz.draw_curve(targ_pose_array,rgba=(1,1,0,1),type=Marker.CUBE_LIST)) #raw_input('Press enter to continue:') #################### Generate new trajectory ################## #### Plot original and warped point clouds ####### # orig_pose_array = conversions.array_to_pose_array(np.squeeze(best_demo["cloud_xyz_ds"]), "base_footprint") # warped_pose_array = conversions.array_to_pose_array(warping_map.transform_points(np.squeeze(best_demo["cloud_xyz_ds"])), "base_footprint") # Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST, ns='demo_cloud')) # Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST, ns='warped_cloud')) #### Plot grid ######## mins = np.squeeze(best_demo["cloud_xyz"]).min(axis=0) maxes = np.squeeze(best_demo["cloud_xyz"]).max(axis=0) mins[2] -= .1 maxes[2] += .1 grid_handle = warping.draw_grid(Globals.rviz, warping_map.transform_points, mins, maxes, 'base_footprint') Globals.handles.append(grid_handle) #### Actually generate the trajectory ########### warped_demo = warping.transform_demo_with_fingertips(warping_map, best_demo) Globals.pr2.update_rave_without_ros(curr_robot_joint_vals) trajectory = {} trajectory['seg_name'] = best_name trajectory['demo'] = best_demo if 'tracked_states' in best_demo: trajectory['orig_tracked_states'] = best_demo['tracked_states'] trajectory['tracked_states'], Globals.offset_trans = warping.transform_tracked_states(warping_map, best_demo, Globals.offset_trans) steps = 0 for lr in "lr": leftright = {"l":"left","r":"right"}[lr] if best_demo["arms_used"] in [lr, "b"]: #if args.hard_table: # clipinplace(warped_demo["l_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf) # clipinplace(warped_demo["r_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf) rospy.loginfo("calculating joint trajectory...") #arm_traj, feas_inds = lfd_traj.make_joint_traj( # warped_demo["%s_gripper_tool_frame"%lr]["position"], # warped_demo["%s_gripper_tool_frame"%lr]["orientation"], # best_demo["%sarm"%leftright], # Globals.pr2.robot.GetManipulator("%sarm"%leftright), # "base_footprint","%s_gripper_tool_frame"%lr, # 1+2+16 #) arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search( warped_demo["%s_gripper_tool_frame"%lr]["position"], warped_demo["%s_gripper_tool_frame"%lr]["orientation"], Globals.pr2.robot.GetManipulator("%sarm"%leftright), "%s_gripper_tool_frame"%lr, check_collisions=True ) if len(feas_inds) == 0: return {'status': "failure"} trajectory["%s_arm"%lr] = arm_traj trajectory["%s_steps"%lr] = steps = len(arm_traj) rospy.loginfo("%s arm: %i of %i points feasible", leftright, len(feas_inds), len(arm_traj)) trajectory["%s_grab"%lr] = map(bool, list(best_demo["%s_gripper_joint"%lr] < .02)) trajectory["%s_gripper"%lr] = warped_demo["%s_gripper_joint"%lr] trajectory["%s_gripper"%lr][trajectory["%s_grab"%lr]] = 0 # plot warped trajectory Globals.handles.append(Globals.rviz.draw_curve( conversions.array_to_pose_array( alternate(warped_demo["%s_gripper_l_finger_tip_link"%lr]["position"], warped_demo["%s_gripper_r_finger_tip_link"%lr]["position"]), "base_footprint" ), width=.001, rgba = (1,0,1,.4), type=Marker.LINE_LIST, ns='warped_finger_traj' )) # plot original trajectory Globals.handles.append(Globals.rviz.draw_curve( conversions.array_to_pose_array( alternate(best_demo["%s_gripper_l_finger_tip_link"%lr]["position"], best_demo["%s_gripper_r_finger_tip_link"%lr]["position"]), "base_footprint" ), width=.001, rgba = (0,1,1,.4), type=Marker.LINE_LIST, ns='demo_finger_traj' )) assert 'l_steps' not in trajectory or steps == trajectory['l_steps'] assert 'r_steps' not in trajectory or steps == trajectory['r_steps'] trajectory['steps'] = steps #raw_input('Press enter to continue:') return {'status': 'not_done', 'trajectory': trajectory}