def tps_rpm_multi(source_clouds, targ_clouds, *args,**kw):
"""
Given multiple source clouds and corresponding target clouds, solve for global transformation that matches them.
Right now, we just concatenate them all. Eventually, we'll do something more sophisticated, e.g. initially match each
source cloud to each target cloud, then use those correspondences to initialize tps-rpm on concatenated clouds.
"""
x_nd = np.concatenate([np.asarray(cloud).reshape(-1,3) for cloud in source_clouds], 0)
y_md = np.concatenate([np.asarray(cloud).reshape(-1,3) for cloud in targ_clouds], 0)
from image_proc.clouds import voxel_downsample
x_nd = voxel_downsample(x_nd,.02)
y_md = voxel_downsample(y_md,.02)
return tps_rpm(x_nd, y_md, *args,**kw)
def plot_original_and_warped_demo(best_demo, warped_demo, traj):
arms_used = best_demo["arms_used"].value
if arms_used in "lb":
pose_array = conversions.array_to_pose_array(asarray(best_demo["l_gripper_xyzs"]), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,0,0,1),ns = "make_verb_traj_service"))
pose_array = conversions.array_to_pose_array(asarray(warped_demo["l_gripper_xyzs"]), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,1,0,1),ns = "make_verb_traj_service"))
if arms_used in "rb":
pose_array = conversions.array_to_pose_array(asarray(best_demo["r_gripper_xyzs"]), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,0,0,1),ns = "make_verb_traj_service"))
pose_array = conversions.array_to_pose_array(asarray(warped_demo["r_gripper_xyzs"]), 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,1,0,1),ns = "make_verb_traj_service"))
Globals.handles.extend(Globals.rviz.draw_trajectory(traj.gripper0_poses, traj.gripper0_angles, ns = "make_verb_traj_service_grippers"))
if arms_used == 'b':
Globals.handles.extend(Globals.rviz.draw_trajectory(traj.gripper1_poses, traj.gripper1_angles, ns = "make_verb_traj_service_grippers"))
for (clouds,rgba) in [(sorted_values(best_demo["object_clouds"]),(1,0,0,.5)),(sorted_values(warped_demo["object_clouds"]),(0,1,0,.5))]:
cloud = []
for subcloud in clouds:
cloud.extend(np.asarray(subcloud).reshape(-1,3))
cloud = np.array(cloud)
cloud = voxel_downsample(cloud, .02)
pose_array = conversions.array_to_pose_array(cloud, 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = rgba,width=.01,type=Marker.CUBE_LIST))
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 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 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 voxel_downsample(xyz, s, return_inds=False):
xyz = xyz.reshape(-1, 3)
xyz = xyz[np.isfinite(xyz[:, 0])]
pts = []
keys = set([])
for (i, pt) in enumerate(xyz):
x, y, z = pt
key = (int(x // s), int(y // s), int(z // s))
if key not in keys:
keys.add(key)
pts.append(pt)
return np.array(pts)
X = voxel_downsample(ctrl_pts, 0.02)
y = dists = ssd.cdist(X, good_pts).min(axis=1) ** 2
# X = array([
# (10,0),
# (3,4),
# (4,3),
# (5,0),
# (4,-3),
# (3,-4)])
# y = array([
# 25,
# 0,0,0,0,0])
from os.path import join, basename, splitext
from glob import glob
from point_clouds.features import cloud_shape_context
from image_proc.clouds import voxel_downsample
import numpy as np
DATA_DIR = '/home/joschu/Data/rope'
for fname in glob(join(DATA_DIR,"*.txt")):
if "feats" in fname: continue
print fname
xyz = np.loadtxt(fname)
if "sim" in fname: xyz = np.dot(xyz, np.diag([1,-1,1]))
else: xyz = voxel_downsample(xyz, .03)
features = np.c_[xyz, cloud_shape_context(xyz, 2)]
feat_fname = splitext(fname)[0] + ".feats.txt"
np.savetxt(feat_fname, features)
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 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
from utils.colorize import colorize
import h5py
import matplotlib.pyplot as plt
h5file = h5py.File(osp.join(osp.dirname(lfd.__file__), "data/knot_segments.h5"),"r")
seg_names = h5file.keys()
results = []
dists = []
ropes = []
for i in xrange(6):
rope = np.squeeze(h5file[seg_names[i]]["cloud_xyz"])
if args.downsample:
rope_ds,ds_inds = voxel_downsample(rope,.03,return_inds = True)
dist = recognition.calc_geodesic_distances_downsampled(rope, rope_ds, ds_inds)
dists.append(dist)
ropes.append(rope_ds)
else:
dist = recognition.calc_geodesic_distances(rope)
dists.append(dist)
ropes.append(rope)
for i0 in xrange(6):
rope0 = ropes[i0]
n0 = len(rope0)
#pairs = zip(xrange(n0), np.random.randint(0,n0,n0))
dists0 = dists[i0]
for i1 in xrange(6):