def calc_match_score(xyz0, xyz1, dists0 = None, dists1 = None, plotting = False):
"""
calculate similarity between xyz0 and xyz1 using geodesic distances
"""
f,info = registration.tps_rpm(xyz0, xyz1, plotting=plotting,reg_init=1,reg_final=.1,n_iter=21, verbose=False, return_full=True)
partners = info["corr_nm"].argmax(axis=1)
starts, ends = np.meshgrid(partners, partners)
reses = [.05, .07, .09]
nres = len(reses)
targ_dist_mats, src_dist_mats = [],[]
for i in xrange(nres):
dists0 = calc_geodesic_distances(xyz0, reses[i])
dists1 = calc_geodesic_distances(xyz1, reses[i])
dists_targ = dists1[starts, ends]
dists0_normed = dists0 / np.median(dists0)
dists_targ_normed = dists_targ / np.median(dists1)
src_dist_mats.append(dists0_normed)
targ_dist_mats.append(dists_targ_normed)
distmat = np.empty((nres, nres))
for i in xrange(nres):
for j in xrange(nres):
distmat[i,j] = np.abs(src_dist_mats[i] - targ_dist_mats[j]).mean()
print "dist at res:", distmat, distmat.min()
return distmat.min()
def test_icp():
matobj = sio.loadmat(osp.join(osp.dirname(lfd.__file__), "matlab", "pointset_pair.mat"))
points0 = matobj["xy1"][:-1]
points1 = matobj["xy2"][:-1]
tps = registration.tps_rpm(points0, points1, plotting = True)
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 match_and_calc_shape_context(xyz_demo_ds,
xyz_new_ds,
hists_demo=None,
hists_new=None,
normalize_costs=False,
return_tuple=False):
def compare_hists(h1, h2):
assert h2.shape == h1.shape
# (single terms of) chi-squared test statistic
return 0.5 / h1.shape[0] * ((h2 - h1)**2 / (h2 + h1 + 1))
f = registration.tps_rpm(xyz_demo_ds,
xyz_new_ds,
plotting=False,
reg_init=1,
reg_final=.1,
n_iter=21,
verbose=False)
partners = f.corr.argmax(axis=1)
if hists_demo is None:
hists_demo = calc_shape_context_hists(xyz_demo_ds)
if hists_new is None:
hists_new = calc_shape_context_hists(xyz_new_ds[partners])
costs = compare_hists(hists_demo, hists_new).sum(axis=1).sum(axis=1)
if normalize_costs:
print max(costs)
m = max(0.5, max(costs))
if m != 0: costs /= m
if return_tuple:
return f, partners, hists_demo, hists_new, costs
return costs
def fit_warp(args, cloud1, cloud2, set_warp_params=None):
def read_warp_params(params):
return ([float(p.strip()) for p in set_warp_params.strip().split()])
if args.warp_type == 'nonrigid':
f = registration.tps_rpm(cloud1,
cloud2,
plotting=False,
reg_init=1,
reg_final=.1,
n_iter=21,
verbose=False)
elif args.warp_type == 'rigid3d':
f = registration.Rigid3d()
if set_warp_params is not None:
f.set_params(read_warp_params(set_warp_params))
else:
f.fit(cloud1, cloud2)
elif args.warp_type == 'translation3d':
f = registration.Translation3d()
if set_warp_params is not None:
f.set_params(read_warp_params(set_warp_params))
else:
f.fit(cloud1, cloud2)
else:
raise NotImplementedError
return f
def calc_match_score(xyz0, xyz1, dists0 = None, dists1 = None, plotting = False):
"""
calculate similarity between xyz0 and xyz1 using geodesic distances
"""
f,info = registration.tps_rpm(xyz0, xyz1, plotting=plotting,reg_init=1,reg_final=.1,n_iter=21, verbose=False, return_full=True)
partners = info["corr_nm"].argmax(axis=1)
starts, ends = np.meshgrid(partners, partners)
reses = [.05, .07, .09]
nres = len(reses)
targ_dist_mats, src_dist_mats = [],[]
for i in xrange(nres):
dists0 = calc_geodesic_distances(xyz0, reses[i])
dists1 = calc_geodesic_distances(xyz1, reses[i])
dists_targ = dists1[starts, ends]
dists0_normed = dists0 / np.median(dists0)
dists_targ_normed = dists_targ / np.median(dists1)
src_dist_mats.append(dists0_normed)
targ_dist_mats.append(dists_targ_normed)
distmat = np.empty((nres, nres))
for i in xrange(nres):
for j in xrange(nres):
distmat[i,j] = np.abs(src_dist_mats[i] - targ_dist_mats[j]).mean()
print "dist at res:", distmat, distmat.min()
return distmat.min()
def test_registration_3d():
#if __name__ == "__main__":
import rospy, itertools, glob
from jds_utils.colorize import colorize
if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True)
data_dir = "/home/joschu/Data/rope1"
files = sorted(glob.glob(osp.join(data_dir,"*.txt")))
distmat1 = np.zeros((len(files), len(files)))
distmat2 = np.zeros((len(files), len(files)))
for (i0, i1) in itertools.combinations(xrange(12),2):
print colorize("comparing %s to %s"%(files[i0], files[i1]),'red',bold=True)
rope0 = np.loadtxt(osp.join(data_dir,files[i0]))
rope1 = np.loadtxt(osp.join(data_dir,files[i1]))
f = registration.tps_rpm(rope0, rope1, plotting=1,reg_init=100,reg_final=1,n_iter=51, verbose=False)
##distmat1[i0, i1] = f.cost
#distmat2[i0, i1] = f.corr_sum
break
plt.figure(1)
plt.imshow(distmat1)
plt.title("distances")
plt.figure(2)
plt.imshow(distmat2)
plt.title("corr_sums")
np.savez("cross_registration_results", distmat = distmat1, names = files)
def calc_match_score(xyz0, xyz1, dists0 = None, dists1 = None):
f = registration.tps_rpm(xyz0, xyz1, plotting=False,reg_init=1,reg_final=.1,n_iter=21, verbose=False)
corr = f.corr
partners = corr.argmax(axis=1)
if dists0 is None: dists0 = calc_geodesic_distances(xyz0)
if dists1 is None: dists1 = calc_geodesic_distances(xyz1)
starts, ends = np.meshgrid(partners, partners)
dists_targ = dists1[starts, ends]
return np.abs(dists0 - dists_targ).sum()/dists0.size
def get_warping_transform(from_cloud, to_cloud, transform_type="tps"):
if transform_type == "tps":
return registration.tps_rpm(from_cloud, to_cloud, plotting=2, reg_init=2, reg_final=.5, n_iter=10, verbose=False)
elif transform_type == "tps_zrot":
return registration.tps_rpm_zrot(from_cloud, to_cloud, plotting=2, reg_init=2, reg_final=.5, n_iter=10, verbose=False)
elif transform_type == "translation2d":
warp = registration.Translation2d()
warp.fit(from_cloud, to_cloud)
return warp
elif transform_type == "rigid2d":
warp = registration.Rigid2d()
warp.fit(from_cloud, to_cloud)
return warp
else:
raise Exception("transform type %s is not yet implemented" % transform_type)
def test_multi_fitting():
library = trajectory_library.TrajectoryLibrary(osp.join(osp.dirname(lfd.__file__),"data/lm.h5"),"read")
drawn_points = np.load(osp.join(osp.dirname(lfd.__file__),"data/should_be_demo01.txt.npy"))
best_cost = np.inf
best_f = None
best_name = None
for (seg_name,trajectory) in library.root["segments"].items():
#f = registration.ThinPlateSpline()
#f.fit(trajectory["rope"][0],
#drawn_points, 1e-2,1e-2)
f = registration.tps_rpm(np.asarray(trajectory["rope"][0][::-1]), drawn_points, plotting = False)
print "seg_name: %s. cost: %s"%(seg_name, f.cost)
if f.cost < best_cost:
best_cost = f.cost
best_f = f
best_name = seg_name
seg_name = best_name
print seg_name
def test_cups():
import rospy, itertools, glob
from jds_utils.colorize import colorize
from jds_image_proc.pcd_io import load_xyzrgb
if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True)
data_dir = "/home/joschu/Data/cups"
xyz1, rgb1 = load_xyzrgb(osp.join(data_dir,"cups1.pcd"))
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
xyz1 = preproc(xyz1)
#from mayavi import mlab
#mlab.points3d(*xyz1.T,color=(1,1,1),scale_factor=.01)
xyz2, rgb2 = load_xyzrgb(osp.join(data_dir,"cups4.pcd"))
xyz2 = preproc(xyz2)
f = registration.tps_rpm(3*xyz1, 3*xyz2, plotting=4,reg_init=1,reg_final=.05,n_iter=200, verbose=False)
def fit_warp(args, cloud1, cloud2, set_warp_params=None):
def read_warp_params(params):
return ([float(p.strip()) for p in set_warp_params.strip().split()])
if args.warp_type == 'nonrigid':
f = registration.tps_rpm(cloud1, cloud2, plotting=False,reg_init=1,reg_final=.1,n_iter=21, verbose=False)
elif args.warp_type == 'rigid3d':
f = registration.Rigid3d()
if set_warp_params is not None:
f.set_params(read_warp_params(set_warp_params))
else:
f.fit(cloud1, cloud2)
elif args.warp_type == 'translation3d':
f = registration.Translation3d()
if set_warp_params is not None:
f.set_params(read_warp_params(set_warp_params))
else:
f.fit(cloud1, cloud2)
else:
raise NotImplementedError
return f
def match_and_calc_shape_context(xyz_demo_ds, xyz_new_ds, hists_demo=None, hists_new=None, normalize_costs=False, return_tuple=False):
def compare_hists(h1, h2):
assert h2.shape == h1.shape
# (single terms of) chi-squared test statistic
return 0.5/h1.shape[0] * ((h2 - h1)**2 / (h2 + h1 + 1))
f = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting=False,reg_init=1,reg_final=.1,n_iter=21, verbose=False)
partners = f.corr.argmax(axis=1)
if hists_demo is None:
hists_demo = calc_shape_context_hists(xyz_demo_ds)
if hists_new is None:
hists_new = calc_shape_context_hists(xyz_new_ds[partners])
costs = compare_hists(hists_demo, hists_new).sum(axis=1).sum(axis=1)
if normalize_costs:
print max(costs)
m = max(0.5, max(costs))
if m != 0: costs /= m
if return_tuple:
return f, partners, hists_demo, hists_new, costs
return costs
plt.imshow(distmat1)
plt.title("distances")
plt.figure(2)
plt.imshow(distmat2)
plt.title("corr_sums")
np.savez("cross_registration_results", distmat = distmat1, names = files)
if __name__ == "__main__":
import rospy, itertools, glob
from utils.colorize import colorize
from image_proc.pcd_io import load_xyzrgb
if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True)
data_dir = "/home/joschu/Data/cups"
xyz1, rgb1 = load_xyzrgb(osp.join(data_dir,"cups1.pcd"))
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
xyz1 = preproc(xyz1)
#from mayavi import mlab
#mlab.points3d(*xyz1.T,color=(1,1,1),scale_factor=.01)
xyz2, rgb2 = load_xyzrgb(osp.join(data_dir,"cups4.pcd"))
xyz2 = preproc(xyz2)
f = registration.tps_rpm(3*xyz1, 3*xyz2, plotting=4,reg_init=1,reg_final=.05,n_iter=200, verbose=False)
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"
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)
if args.human_select_demo:
raise NotImplementedError
seg_name = trajectory_library.interactive_select_demo(demos)
best_demo = demos[seg_name]
pts0, _ = best_demo["cloud_xyz_ds"]
pts1, _ = downsample(xyz_new)
self.f = registration.tps_rpm(pts0,
pts1,
plotting=4,
reg_init=1,
reg_final=args.reg_final,
n_iter=40)
else:
if args.count_steps:
candidate_demo_names = self.count2segnames[Globals.stage]
else:
candidate_demo_names = demos.keys()
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]
_, best_name = min(costs_names)
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)
xyz_demo_ds = best_demo["cloud_xyz_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)
else:
self.f = registration.tps_rpm(xyz_demo_ds,
xyz_new_ds,
plotting=20,
reg_init=1,
reg_final=.01,
n_iter=n_iter,
verbose=False) #, interactive=True)
np.savez('registration_data',
xyz_demo_ds=xyz_demo_ds,
xyz_new_ds=xyz_new_ds)
# 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
Globals.pr2.update_rave()
trajectory = {}
# calculate joint trajectory using IK
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)
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 "failure"
trajectory["%s_arm" % lr] = arm_traj
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)
# 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'))
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"
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"
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 get_tps_transform(from_cloud, to_cloud):
#return registration.tps_rpm(from_cloud, to_cloud)
return registration.tps_rpm(from_cloud, to_cloud, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False)
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
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)
if args.human_select_demo:
raise NotImplementedError
seg_name = trajectory_library.interactive_select_demo(demos)
best_demo = demos[seg_name]
pts0,_ = best_demo["cloud_xyz_ds"]
pts1,_ = downsample(xyz_new)
self.f = registration.tps_rpm(pts0, pts1, plotting = 4, reg_init=1,reg_final=args.reg_final,n_iter=40)
else:
if args.count_steps: candidate_demo_names = self.count2segnames[Globals.stage]
else: candidate_demo_names = demos.keys()
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]
_, best_name = min(costs_names)
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)
xyz_demo_ds = best_demo["cloud_xyz_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)
else:
self.f = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting = 20, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False)#, interactive=True)
np.savez('registration_data', xyz_demo_ds=xyz_demo_ds, xyz_new_ds=xyz_new_ds)
# 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
Globals.pr2.update_rave()
trajectory = {}
# calculate joint trajectory using IK
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)
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 "failure"
trajectory["%s_arm"%lr] = arm_traj
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
)
# 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'
))
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"
if __name__ == "__main__":
import rospy, itertools, glob
from jds_utils.colorize import colorize
if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True)
data_dir = "/home/joschu/Data/rope1"
files = sorted(glob.glob(osp.join(data_dir,"*.txt")))
distmat1 = np.zeros((len(files), len(files)))
distmat2 = np.zeros((len(files), len(files)))
for (i0, i1) in itertools.combinations(xrange(12),2):
print colorize("comparing %s to %s"%(files[i0], files[i1]),'red',bold=True)
rope0 = np.loadtxt(osp.join(data_dir,files[i0]))
rope1 = np.loadtxt(osp.join(data_dir,files[i1]))
f = registration.tps_rpm(rope0, rope1, plotting=1,reg_init=100,reg_final=1,n_iter=51, verbose=False)
##distmat1[i0, i1] = f.cost
#distmat2[i0, i1] = f.corr_sum
break
plt.figure(1)
plt.imshow(distmat1)
plt.title("distances")
plt.figure(2)
plt.imshow(distmat2)
plt.title("corr_sums")
np.savez("cross_registration_results", distmat = distmat1, names = files)
#if __name__ == "__main__":
def test_cups():
dists = ssd.cdist(np.array([z0src, z1src]), x)
wts = np.exp(-dists/.01)
wts = wts / wts.sum(axis=0)[None,:]
return wts[0][:,None] * local_trans(x, z0src, z0tgt, theta0) + \
wts[1][:,None] * local_trans(x, z1src, z1tgt, theta1)
soln = so.fmin_cg(cost, np.r_[z0tgt, 0, z1tgt, 0],callback=plotter.callback)
def warp_fn(x, params):
s_z0tgt = params[:2]
s_theta0 = params[2]
s_z1tgt = params[3:5]
s_theta1 = params[5]
return warp(x, s_z0tgt, s_z1tgt, s_theta0, s_theta1)
plt.figure(2)
tps_rpm(uvs_src, uvs_tgt, n_iter = 100, reg_init=1, reg_final = .1, plotting=4, rad_init=.01, rad_final = .001, f_init = lambda x: warp_fn(x, soln))
#plt.plot(mug0src[:,1], mug0src[:,0],'cx')
#plt.plot(mug0tgt[:,1], mug0tgt[:,0],'cx')
show()
wts = wts / wts.sum(axis=0)[None, :]
return wts[0][:,None] * local_trans(x, z0src, z0tgt, theta0) + \
wts[1][:,None] * local_trans(x, z1src, z1tgt, theta1)
soln = so.fmin_cg(cost, np.r_[z0tgt, 0, z1tgt, 0], callback=plotter.callback)
def warp_fn(x, params):
s_z0tgt = params[:2]
s_theta0 = params[2]
s_z1tgt = params[3:5]
s_theta1 = params[5]
return warp(x, s_z0tgt, s_z1tgt, s_theta0, s_theta1)
plt.figure(2)
tps_rpm(uvs_src,
uvs_tgt,
n_iter=100,
reg_init=1,
reg_final=.1,
plotting=4,
rad_init=.01,
rad_final=.001,
f_init=lambda x: warp_fn(x, soln))
#plt.plot(mug0src[:,1], mug0src[:,0],'cx')
#plt.plot(mug0tgt[:,1], mug0tgt[:,0],'cx')
show()
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
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}
