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 callback(msg):
s = msg.data
print "message:",colorize(s, "blue", bold=True)
action_arg = s.split()
assert len(action_arg) == 2
action, arg = action_arg
if action == "relax":
assert action_arg[0] == "relax"
pr2 = PR2.create()
pr2.rgrip.open()
pr2.lgrip.open()
pr2.rarm.goto_posture('side')
pr2.larm.goto_posture('side')
elif action == "tie":
assert arg == "figure8_knot" or arg == "overhand_knot"
call_and_print("execute_task.py --task=%s --count_steps"%arg)
elif action == "teach":
print colorize("which arms will be used?", "red", bold=True)
arms_used = raw_input()
call_and_print("teach_verb.py %s %s %s"%(arg, "object", arms_used))
elif action == "execute":
call_and_print("exec_one_demo_traj.py --verb=%s"%arg)
def make_verb_library_multi(data_dir, verb_lib):
from lfd import multi_item_verbs
link_names = ["r_gripper_tool_frame", "l_gripper_tool_frame"]
verb_data_accessor = multi_item_verbs.VerbDataAccessor()
for (verb_name,
verb_info) in verb_data_accessor.get_all_demo_info().items():
print colorize("processing demo: %s" % verb_name, "red")
for stage_num, stage_name in enumerate(verb_info["stages"]):
bag_file_name = "bags/%s.bag" % (stage_name)
seg_file_name = "images/%s.seg.h5" % (stage_name)
bag = rosbag.Bag(osp.join(data_dir, bag_file_name))
segs = bag_proc.create_segment_without_look(bag, link_names)
if len(segs) > 1: print "warning: more than one segment found"
kinematics_data = segs[0]
stage_data = copy(kinematics_data)
seg_file = h5py.File(osp.join(data_dir, seg_file_name), "r")
bag_proc.dict_to_hdf(verb_lib, stage_data, stage_name)
seg_file.copy("/", verb_lib[stage_name], "object_cloud")
# is the following needed, since it is stored in the yaml file?
verb_lib[stage_name]["arms_used"] = verb_info["arms_used"][
stage_num]
def execute_series(pipeline, dry_run=False):
nodedict = pipeline.graph.node
ordered_progs = [
node for node in nx.topological_sort(pipeline.graph)
if nodedict[node]["type"] == "program"
]
for prog in ordered_progs:
command = nodedict[prog]["command"]
item2status = pipeline.get_all_status()
if pipeline.products_already_made(prog, item2status):
print "next:", colorize("skipping %s" % prog, "red")
logging.info("skipping %s", prog)
continue
print colorize(command, "red")
logging.info(command)
raw_input("press enter to continue")
if not dry_run:
child = subprocess.Popen(command.split(), env=pipeline.env)
try:
interrupted = False
while child.poll() is None:
sleep(.1)
except KeyboardInterrupt:
interrupted = True
if not interrupted and child.returncode != 0:
raise subprocess.CalledProcessError(child.returncode, command)
def callback(msg):
s = msg.data
print "message:", colorize(s, "blue", bold=True)
action_arg = s.split()
assert len(action_arg) == 2
action, arg = action_arg
if action == "relax":
assert action_arg[0] == "relax"
pr2 = PR2.create()
pr2.rgrip.open()
pr2.lgrip.open()
pr2.rarm.goto_posture('side')
pr2.larm.goto_posture('side')
elif action == "tie":
assert arg == "figure8_knot" or arg == "overhand_knot"
call_and_print("execute_task.py --task=%s --count_steps" % arg)
elif action == "teach":
print colorize("which arms will be used?", "red", bold=True)
arms_used = raw_input()
call_and_print("teach_verb.py %s %s %s" % (arg, "object", arms_used))
elif action == "execute":
call_and_print("exec_one_demo_traj.py --verb=%s" % arg)
def record_trajectory(dry_run, name_for_motion):
raw_input("Press enter when ready to record")
print colorize("now, human, demonstrate the next action for %s on the robot" % (name_for_motion), color="red", bold=True)
call_and_print("rosrun pr2_controller_manager pr2_controller_manager stop r_arm_controller l_arm_controller")
try:
old_tmp_bags = glob("tmpname*.bag")
if not dry_run:
for bag in old_tmp_bags:
os.remove(bag)
call_and_print("rosbag record /tf /joint_states /joy -o tmpname")
except KeyboardInterrupt:
pass
call_and_print("rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller")
def do_single(demo_name, stage_num, prev_demo_index, verb_data_accessor, prev_and_cur_pc2):
if stage_num == 0:
do_stage(demo_name, stage_num, None, None, prev_and_cur_pc2[1], verb_data_accessor)
else:
prev_stage_num = stage_num - 1
prev_demo_name = "%s%i" % (demo_base_name, prev_demo_index)
prev_stage_info = verb_data_accessor.get_stage_info(prev_demo_name, prev_stage_num)
call_and_print("rosrun pr2_controller_manager pr2_controller_manager stop r_arm_controller l_arm_controller")
print colorize("do the stage involving the %s" % (prev_stage_info.item), color="red", bold=True)
yes_or_no("type y to continue")
call_and_print("rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller")
do_stage(demo_name, stage_num, prev_stage_info, prev_and_cur_pc2[0], prev_and_cur_pc2[1], verb_data_accessor)
def do_single(demo_base_name, demo_index, stage_num, prev_demo_index):
if stage_num == 0:
do_stage(demo_base_name, demo_index, stage_num, None, None)
else:
prev_stage_num = stage_num - 1
prev_demo_name = demo_base_name + str(prev_demo_index)
prev_stage_info = verb_data_accessor.get_stage_info(prev_demo_name, prev_stage_num)
prev_exp_pc = do_segmentation(prev_stage_info.item)
call_and_print("rosrun pr2_controller_manager pr2_controller_manager stop r_arm_controller l_arm_controller")
print colorize("do the stage involving the %s" % (prev_stage_info.item), color="red", bold=True)
yes_or_no("type y to continue")
call_and_print("rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller")
do_stage(demo_base_name, demo_index, stage_num, prev_stage_info, np.array([prev_exp_pc]))
def make_verb_library_single():
for (verb_name, verb_info) in verbs.get_all_demo_info().items():
print colorize("processing demo: %s"%verb_name, "red")
bag = rosbag.Bag(osp.join(data_dir, verb_info["bag_file"]))
segs = bag_proc.create_segment_without_look(bag, link_names)
if len(segs) > 1: print "warning: more than one segment found"
kinematics_data = segs[0]
verb_data = copy(kinematics_data)
seg_file = h5py.File(osp.join(data_dir,verb_info["seg_file"]),"r")
bag_proc.dict_to_hdf(verb_lib, verb_data, verb_name)
seg_file.copy("/", verb_lib[verb_name],"object_clouds")
verb_lib[verb_name]["arms_used"] = verb_info["arms_used"]
def transform_points(xyz, listener, to_frame, from_frame,n_tries=10):
#listener.waitForTransform(to_frame, from_frame, rospy.Time.now(), rospy.Duration(1))
for i in xrange(n_tries):
try:
trans, rot = listener.lookupTransform(to_frame, from_frame, rospy.Time(0))
break
except Exception:
print "tf exception:"
print colorize.colorize(traceback.format_exc(),"yellow")
rospy.sleep(.1)
time.sleep(.05)
if i == n_tries-1: raise Exception("f**k tf")
hmat = conversions.trans_rot_to_hmat(trans,rot)
xyz1 = np.c_[xyz.reshape(-1,3), np.ones((xyz.size/3,1))]
xyz1_tf = np.dot(xyz1, hmat.T)
xyz_tf = xyz1_tf[:,:3].reshape(xyz.shape)
return xyz_tf
def make_verb_library_single(data_dir, verb_lib):
from lfd import verbs
link_names = ["r_gripper_tool_frame", "l_gripper_tool_frame"]
for (verb_name, verb_info) in verbs.get_all_demo_info().items():
print colorize("processing demo: %s" % verb_name, "red")
bag = rosbag.Bag(osp.join(data_dir, verb_info["bag_file"]))
segs = bag_proc.create_segment_without_look(bag, link_names)
if len(segs) > 1: print "warning: more than one segment found"
kinematics_data = segs[0]
verb_data = copy(kinematics_data)
seg_file = h5py.File(osp.join(data_dir, verb_info["seg_file"]), "r")
bag_proc.dict_to_hdf(verb_lib, verb_data, verb_name)
seg_file.copy("/", verb_lib[verb_name], "object_clouds")
verb_lib[verb_name]["arms_used"] = verb_info["arms_used"]
def transform_points(xyz, listener, to_frame, from_frame, n_tries=10):
#listener.waitForTransform(to_frame, from_frame, rospy.Time.now(), rospy.Duration(1))
for i in xrange(n_tries):
try:
trans, rot = listener.lookupTransform(to_frame, from_frame,
rospy.Time(0))
break
except Exception:
print "tf exception:"
print colorize.colorize(traceback.format_exc(), "yellow")
rospy.sleep(.1)
time.sleep(.05)
if i == n_tries - 1: raise Exception("f**k tf")
hmat = conversions.trans_rot_to_hmat(trans, rot)
xyz1 = np.c_[xyz.reshape(-1, 3), np.ones((xyz.size / 3, 1))]
xyz1_tf = np.dot(xyz1, hmat.T)
xyz_tf = xyz1_tf[:, :3].reshape(xyz.shape)
return xyz_tf
def make_verb_library_multi():
for (verb_name, verb_info) in multi_item_verbs.get_all_demo_info().items():
print colorize("processing demo: %s"%verb_name, "red")
for stage_num, stage_name in enumerate(verb_info["stages"]):
bag_file_name = "bags/%s.bag" % (stage_name)
seg_file_name = "images/%s.seg.h5" % (stage_name)
bag = rosbag.Bag(osp.join(data_dir, bag_file_name))
segs = bag_proc.create_segment_without_look(bag, link_names)
if len(segs) > 1: print "warning: more than one segment found"
kinematics_data = segs[0]
stage_data = copy(kinematics_data)
seg_file = h5py.File(osp.join(data_dir, seg_file_name), "r")
bag_proc.dict_to_hdf(verb_lib, stage_data, stage_name)
seg_file.copy("/", verb_lib[stage_name], "object_clouds")
# is the following needed, since it is stored in the yaml file?
verb_lib[stage_name]["arms_used"] = verb_info["arms_used"][stage_num]
def execute_parallel(pipeline, lifetime, max_lag, noclean=False):
nodedict = pipeline.graph.node
target_topic = pipeline.get_target_topic()
prog2child = {}
remaining_progs = pipeline.get_programs()
#plot = PipelinePlot(pipeline)
try:
while True:
item2status = pipeline.get_all_status()
item2done = pipeline.get_all_doneness(item2status)
remaining_progs = [
prog for prog in remaining_progs
if not pipeline.products_already_made(prog, item2done)
or pipeline.makes_topic(prog)
]
for prog in remaining_progs:
if pipeline.ready_to_run(prog, item2status):
remaining_progs.remove(prog)
command = nodedict[prog]["command"]
print colorize(command, "red")
logging.info(command)
child = subprocess.Popen(command.split(), env=pipeline.env)
prog2child[prog] = child
for (prog, child) in prog2child.items():
if child.poll() is not None and child.returncode != 0:
print colorize("%s failed" % prog, "red")
logging.error("%s failed", prog)
for child in prog2child.values():
if child.poll() is None: child.terminate()
return
if not noclean: pipeline.cleanup_all(lifetime)
pipeline.throttle(target_topic, item2status, max_lag)
#plot.draw()
sleep(.1)
except Exception:
traceback.print_exc()
for child in prog2child.values():
if child.poll() is None: child.terminate()
return
def execute_series(pipeline, dry_run=False):
nodedict = pipeline.graph.node
ordered_progs = [node for node in nx.topological_sort(pipeline.graph) if nodedict[node]["type"] == "program"]
for prog in ordered_progs:
command = nodedict[prog]["command"]
item2status = pipeline.get_all_status()
if pipeline.products_already_made(prog, item2status):
print "next:", colorize("skipping %s"%prog, "red")
logging.info("skipping %s",prog)
continue
print colorize(command,"red")
logging.info(command)
raw_input("press enter to continue")
if not dry_run:
child = subprocess.Popen(command.split(), env=pipeline.env)
try:
interrupted=False
while child.poll() is None: sleep(.1)
except KeyboardInterrupt:
interrupted=True
if not interrupted and child.returncode != 0:
raise subprocess.CalledProcessError(child.returncode, command)
def record_demonstration_for_motion(name_for_motion, item_name):
# get the point cloud for the first item
os.chdir(data_dir + "/images")
call_and_print("get_point_cloud.py %s" % (name_for_motion))
print colorize("now, select the %s object" % (item_name), color="red", bold=True)
# two point clouds need to be stored, so give first point cloud a suffix of '-1'
call_and_print("manually_segment_point_cloud.py %s.npz --objs=%s" % (name_for_motion, item_name))
yes_or_no("Ready to continue?")
# do the demonstration for the first motion
print colorize("now, human, demonstrate the next action for %s on the robot" % (name_for_motion), color="red", bold=True)
os.chdir(data_dir + "/bags")
call_and_print("rosrun pr2_controller_manager pr2_controller_manager stop r_arm_controller l_arm_controller")
# record the demonstration for the first motion
try:
old_tmp_bags = glob("tmpname*.bag")
if not args.dry_run:
for bag in old_tmp_bags:
os.remove(bag)
call_and_print("rosbag record /tf /joint_states /joy -o tmpname")
except KeyboardInterrupt:
pass
# rename bag file that was just created
n_tries = 20
if not args.dry_run:
success = False
for i in xrange(n_tries):
try:
bagname = glob("tmpname*.bag")[0]
os.rename(bagname, "%s.bag" % (name_for_motion))
success = True
except IndexError:
time.sleep(.1)
if not success: raise Exception("couldn't get bag file")
call_and_print("rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller")
def execute_parallel(pipeline, lifetime, max_lag, noclean = False):
nodedict = pipeline.graph.node
target_topic = pipeline.get_target_topic()
prog2child = {}
remaining_progs = pipeline.get_programs()
#plot = PipelinePlot(pipeline)
try:
while True:
item2status = pipeline.get_all_status()
item2done = pipeline.get_all_doneness(item2status)
remaining_progs = [prog for prog in remaining_progs if not pipeline.products_already_made(prog, item2done) or pipeline.makes_topic(prog)]
for prog in remaining_progs:
if pipeline.ready_to_run(prog, item2status):
remaining_progs.remove(prog)
command = nodedict[prog]["command"]
print colorize(command, "red")
logging.info(command)
child = subprocess.Popen(command.split(), env=pipeline.env)
prog2child[prog] = child
for (prog, child) in prog2child.items():
if child.poll() is not None and child.returncode != 0:
print colorize("%s failed"%prog, "red")
logging.error("%s failed",prog)
for child in prog2child.values():
if child.poll() is None: child.terminate()
return
if not noclean: pipeline.cleanup_all(lifetime)
pipeline.throttle(target_topic, item2status, max_lag)
#plot.draw()
sleep(.1)
except Exception:
traceback.print_exc()
for child in prog2child.values():
if child.poll() is None: child.terminate()
return
def test_all(stop=False):
nPass,nFail = 0,0
for (name,func) in TEST_FUNCS.items():
print colorize("function: %s"%name,"green")
try:
t_start = time()
func()
t_elapsed = time() - t_start
print colorize("PASSED (%.3f sec)"%t_elapsed,"blue")
nPass += 1
except Exception:
traceback.print_exc(file=sys.stdout)
if stop: raise
print colorize("FAILED","red")
nFail += 1
print "%i passed, %i failed"%(nPass,nFail)
def report((x, msg)):
if x:
print colorize("Pass", "green")
else:
print colorize("Fail: %s" % (msg), "red")
args = parser.parse_args()
verb = args.verb
target = args.target
arms_used = args.arms_used
from lfd import verbs
all_demo_info = verbs.get_all_demo_info()
for i in xrange(1000):
demo_name = "%s-%s%i" % (verb, target, i)
if demo_name not in all_demo_info:
break
data_dir = osp.join(osp.dirname(lfd.__file__), "data")
os.chdir(data_dir + "/images")
call_and_print("get_point_cloud.py %s" % demo_name)
print colorize("now, select the target object", color="red", bold=True)
call_and_print("manually_segment_point_cloud.py %s.npz --objs=%s" %
(demo_name, target))
print colorize("now, human, demonstrate the action on the robot",
color="red",
bold=True)
os.chdir(data_dir + "/bags")
call_and_print(
"rosrun pr2_controller_manager pr2_controller_manager stop r_arm_controller l_arm_controller"
)
try:
old_tmp_bags = glob("tmpname*.bag")
if not args.dry_run:
def call_and_print(cmd, color='green'):
print colorize(cmd, color, bold=True)
if not args.dry_run: subprocess.check_call(cmd, shell=True)
def call_and_print(cmd, color='green'):
print colorize(cmd, color, bold=True)
subprocess.check_call(cmd, shell=True)
#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 tps_nr_fit_enhanced(x_na, y_ng, bend_coef, nr_ma, nr_coef, method="newton", plotting=0):
N,D = x_na.shape
M = nr_ma.shape[0]
E = N + 4*M
F = E - M
Q = N + 3*M - 4
s = .1 # tetrahedron sidelength (meters)
u = 1/(2*np.sqrt(2))
tetra_pts = []
for pt in nr_ma:
tetra_pts.append(s*np.r_[-.5, 0, -u]+pt)
tetra_pts.append(s*np.r_[+.5, 0, -u]+pt)
tetra_pts.append(s*np.r_[0, -.5, +u]+pt)
tetra_pts.append(s*np.r_[0, +.5, +u]+pt)
x_ea = np.r_[x_na, tetra_pts]
badsub_ex = np.c_[x_ea, np.ones((E,1)), np.r_[np.zeros((N,M)), np.repeat(np.eye(M), 4, axis=0)]]
lin_ag, trans_g, w_ng = tps_fit2(x_na, y_ng, bend_coef, 1e-3)
w_eg = np.r_[w_ng, np.zeros((4*M, D))]
assert badsub_ex.shape[0] >= badsub_ex.shape[1]
_u,_s,_vh = np.linalg.svd(badsub_ex, full_matrices=True)
assert badsub_ex.shape[1] == _s.size
N_eq = _u[:,badsub_ex.shape[1]:] # null of data
assert N_eq.shape == (E,Q)
assert E == N + 4*M
assert F == Q + 4
# e is number of kernels
# q is number of nonrigid dofs
# f is total number of dofs
K_ee = ssd.squareform(ssd.pdist(x_ea))
K_ne = K_ee[:N, :]
Q_nf = np.c_[x_na, np.ones((N,1)),K_ne.dot(N_eq)]
QQ_ff = np.dot(Q_nf.T, Q_nf)
Bend_ff = np.zeros((F,F))
Bend_ff[4:, 4:] = - N_eq.T.dot(K_ee.dot(N_eq)) # K_qq
assert Q_nf.shape == (N, F)
assert w_eg.shape == (E, D)
n_iter=40
for i in xrange(n_iter):
if plotting and i%plotting==0:
import lfd.registration as lr
lr.Globals.setup()
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, x_na, y_ng, res=.008)
X_fg = np.r_[lin_ag,
trans_g[None,:],
N_eq.T.dot(w_eg)]
res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval_general(lin_ag, trans_g, w_eg, x_ea, y_ng, nr_ma, bend_coef, nr_coef, return_tuple=True)
if VERBOSE: print colorize("iteration %i, cost %.3e"%(i, fval), 'red'),
if VERBOSE: print "= %.3e (res) + %.3e (bend) + %.3e (nr)"%(res_cost, bend_cost, nr_cost)
Jl_zcg, Jt_zg, Jw_zeg = tps_nr_grad(nr_ma, lin_ag, trans_g, w_eg, x_ea, return_tuple=True)
nrerr_z = tps_nr_err(nr_ma, lin_ag, trans_g, w_eg, x_ea)
fullstep_fg = np.empty((F,D))
for g in xrange(D):
J_zf = np.c_[Jl_zcg[:,g::D], Jt_zg[:,g::D], Jw_zeg[:,g::D].dot(N_eq)]
JJ_ff = np.dot(J_zf.T, J_zf)
A_ff = nr_coef*JJ_ff + QQ_ff + bend_coef*Bend_ff
X0 = X_fg[:,g]
B_f = nr_coef*np.dot(J_zf.T, np.dot(J_zf, X0) - nrerr_z) + Q_nf.T.dot(y_ng[:,g])
fullstep_fg[:,g] = np.linalg.solve(A_ff,B_f) - X_fg[:,g]
cost_improved = False
for stepsize in 3.**np.arange(0,-10,-1):
cand_X_fg = X_fg + fullstep_fg*stepsize
cand_lin_ag, cand_trans_g, cand_w_eg = cand_X_fg[:D], cand_X_fg[D], N_eq.dot(cand_X_fg[D+1:])
fval_cand = tps_nr_cost_eval_general(cand_lin_ag, cand_trans_g, cand_w_eg, x_ea, y_ng, nr_ma, bend_coef, nr_coef, return_tuple=False)
if VERBOSE: print "stepsize: %.1g, fval: %.3e"%(stepsize, fval_cand)
if fval_cand < fval:
cost_improved = True
break
if not cost_improved:
if VERBOSE: print "couldn't improve objective"
break
lin_ag = cand_lin_ag
trans_g = cand_trans_g
w_eg = cand_w_eg
return lin_ag, trans_g, w_eg, x_ea
def call_and_print(cmd,color='green'):
print colorize(cmd, color, bold=True)
subprocess.check_call(cmd, shell=True)
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):
rope1 = ropes[i1]
dists1 = dists[i1]
n1 = len(rope1)
print colorize("comparing %s to %s" % (seg_names[i0], seg_names[i1]),
"red")
cost = recognition.calc_match_score(rope0, rope1, dists0, dists1)
results.append((i0, i1, cost))
print i0, i1, cost
distmat = np.zeros((6, 6))
for (i0, i1, cost) in results:
distmat[i0, i1] = cost
distmat[xrange(6), xrange(6)] = np.nan
print distmat.argmin(axis=0)
a, b = np.meshgrid([0, 3, 1, 4, 2, 5], [0, 3, 1, 4, 2, 5])
distmat_rearr = distmat[a, b]
distmat_rearr[range(6), range(6)] = np.nan
plt.imshow(distmat_rearr, interpolation='nearest', cmap='gray')
def get_point_cloud(name_for_motion, item_name):
call_and_print("get_point_cloud.py %s" % (name_for_motion))
print colorize("now, select the %s object" % (item_name), color="red", bold=True)
# two point clouds need to be stored, so give first point cloud a suffix of '-1'
call_and_print("manually_segment_point_cloud.py %s.npz --objs=%s --do_filtering --plotting" % \
(name_for_motion, item_name))
def tps_nr_fit(x_na, y_ng, bend_coef, nr_ma, nr_coef, method="newton"):
N, D = x_na.shape
lin_ag, trans_g, w_ng = tps_fit2(x_na, y_ng, bend_coef, 1e-3)
#return lin_ag, trans_g, w_ng
##for testing that it takes one step when nonrigidity cost is zero:
#lin_ag, trans_g, w_ng = tps_fit(x_na, y_ng, bend_coef, 0)
#res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, nr_ma, bend_coef, nr_coef, return_tuple=True)
#print "CORRECT COST, res,bend,nr,total = %.3e, %.3e, %.3e, %.3e"%(res_cost, bend_cost, nr_cost, fval)
#lin_ag += np.random.randn(*lin_ag.shape)*5
#res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, nr_ma, bend_coef, nr_coef, return_tuple=True)
#print "NOISE ADDED COST, res,bend,nr,total = %.ef, %.3e, %.3e, %.3e"%(res_cost, bend_cost, nr_cost, fval)
_u, _s, _vh = np.linalg.svd(np.c_[x_na, np.ones((N, 1))],
full_matrices=True)
N_nq = _u[:, 4:] # null of data
#w_ng = N_nq.dot(N_nq.T.dot(w_ng))
K_nn = ssd.squareform(ssd.pdist(x_na))
Q_nn = np.c_[x_na, np.ones((N, 1)), K_nn.dot(N_nq)]
QQ_nn = np.dot(Q_nn.T, Q_nn)
Bend_nn = np.zeros((N, N))
Bend_nn[4:, 4:] = -N_nq.T.dot(K_nn.dot(N_nq))
n_iter = 60
for i in xrange(n_iter):
X_ng = np.r_[lin_ag, trans_g[None, :], N_nq.T.dot(w_ng)]
res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(
lin_ag,
trans_g,
w_ng,
x_na,
y_ng,
nr_ma,
bend_coef,
nr_coef,
return_tuple=True)
if VERBOSE:
print colorize("iteration %i, cost %.3e" % (i, fval), 'red'),
if VERBOSE:
print "= %.3e (res) + %.3e (bend) + %.3e (nr)" % (
res_cost, bend_cost, nr_cost)
Jl_zcg, Jt_zg, Jw_zng = tps_nr_grad(nr_ma,
lin_ag,
trans_g,
w_ng,
x_na,
return_tuple=True)
nrerr_z = tps_nr_err(nr_ma, lin_ag, trans_g, w_ng, x_na)
if method == "newton":
fullstep_ng = np.empty((N, D))
for g in xrange(D):
J_zn = np.c_[Jl_zcg[:, g::D], Jt_zg[:, g::D],
Jw_zng[:, g::D].dot(N_nq)]
JJ_nn = np.dot(J_zn.T, J_zn)
A = nr_coef * JJ_nn + QQ_nn + bend_coef * Bend_nn
X0 = X_ng[:, g]
B = nr_coef * np.dot(J_zn.T,
np.dot(J_zn, X0) - nrerr_z) + Q_nn.T.dot(
y_ng[:, g])
fullstep_ng[:, g] = np.linalg.solve(A, B) - X_ng[:, g]
elif method == "gradient":
def eval_partial(cand_X_ng):
cand_X_ng = cand_X_ng.reshape(-1, 3)
cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[
D], N_nq.dot(cand_X_ng[D + 1:])
fval_cand = tps_nr_cost_eval(cand_lin_ag, cand_trans_g,
cand_w_ng, x_na, y_ng, nr_ma,
bend_coef, nr_coef)
return fval_cand
def eval_partial2(cand_X_ng):
return ((Q_nn.dot(X_ng) - y_ng)**2).sum()
def eval_partial3(cand_X_ng):
cand_X_ng = cand_X_ng.reshape(-1, 3)
cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[
D], N_nq.dot(cand_X_ng[D + 1:])
return ((y_ng - tps_eval(x_na, cand_lin_ag, cand_trans_g,
cand_w_ng, x_na))**2).sum()
grad_ng = np.empty((N, D))
for g in xrange(D - 1, -1, -1):
Jnr_zn = np.c_[Jl_zcg[:, g::D], Jt_zg[:, g::D],
Jw_zng[:, g::D].dot(N_nq)]
grad_ng[:,g] = 2 * nr_coef * nrerr_z.dot(Jnr_zn) \
+ 2 * Q_nn.T.dot(Q_nn.dot(X_ng[:,g]) - y_ng[:,g]) \
+ 2 * bend_coef * Bend_nn.dot(X_ng[:,g])
#assert np.allclose(eval_partial2(X_ng), eval_partial3(X_ng))
#assert np.allclose(eval_partial(X_ng), eval_partial2(X_ng))
#grad0_ng = ndt.Gradient(eval_partial)(X_ng.flatten()).reshape(-1,3)
fullstep_ng = -grad_ng
#assert np.allclose(grad0_ng, grad_ng)
cost_improved = False
for stepsize in 3.**np.arange(0, -10, -1):
cand_X_ng = X_ng + fullstep_ng * stepsize
cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[
D], N_nq.dot(cand_X_ng[D + 1:])
fval_cand = tps_nr_cost_eval(cand_lin_ag, cand_trans_g, cand_w_ng,
x_na, y_ng, nr_ma, bend_coef, nr_coef)
if VERBOSE:
print "stepsize: %.1g, fval: %.3e" % (stepsize, fval_cand)
if fval_cand < fval:
cost_improved = True
break
if not cost_improved:
if VERBOSE: print "couldn't improve objective"
break
lin_ag = cand_lin_ag
trans_g = cand_trans_g
w_ng = cand_w_ng
return lin_ag, trans_g, w_ng
def report((x, msg)):
if x:
print colorize("Pass", "green")
else:
print colorize("Fail: %s" % (msg), "red")
def tps_nr_fit_enhanced(x_na,
y_ng,
bend_coef,
nr_ma,
nr_coef,
method="newton",
plotting=0):
N, D = x_na.shape
M = nr_ma.shape[0]
E = N + 4 * M
F = E - M
Q = N + 3 * M - 4
s = .1 # tetrahedron sidelength (meters)
u = 1 / (2 * np.sqrt(2))
tetra_pts = []
for pt in nr_ma:
tetra_pts.append(s * np.r_[-.5, 0, -u] + pt)
tetra_pts.append(s * np.r_[+.5, 0, -u] + pt)
tetra_pts.append(s * np.r_[0, -.5, +u] + pt)
tetra_pts.append(s * np.r_[0, +.5, +u] + pt)
x_ea = np.r_[x_na, tetra_pts]
badsub_ex = np.c_[x_ea,
np.ones((E, 1)), np.r_[np.zeros((N, M)),
np.repeat(np.eye(M), 4, axis=0)]]
lin_ag, trans_g, w_ng = tps_fit2(x_na, y_ng, bend_coef, 1e-3)
w_eg = np.r_[w_ng, np.zeros((4 * M, D))]
assert badsub_ex.shape[0] >= badsub_ex.shape[1]
_u, _s, _vh = np.linalg.svd(badsub_ex, full_matrices=True)
assert badsub_ex.shape[1] == _s.size
N_eq = _u[:, badsub_ex.shape[1]:] # null of data
assert N_eq.shape == (E, Q)
assert E == N + 4 * M
assert F == Q + 4
# e is number of kernels
# q is number of nonrigid dofs
# f is total number of dofs
K_ee = ssd.squareform(ssd.pdist(x_ea))
K_ne = K_ee[:N, :]
Q_nf = np.c_[x_na, np.ones((N, 1)), K_ne.dot(N_eq)]
QQ_ff = np.dot(Q_nf.T, Q_nf)
Bend_ff = np.zeros((F, F))
Bend_ff[4:, 4:] = -N_eq.T.dot(K_ee.dot(N_eq)) # K_qq
assert Q_nf.shape == (N, F)
assert w_eg.shape == (E, D)
n_iter = 40
for i in xrange(n_iter):
if plotting and i % plotting == 0:
import lfd.registration as lr
lr.Globals.setup()
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, x_na, y_ng, res=.008)
X_fg = np.r_[lin_ag, trans_g[None, :], N_eq.T.dot(w_eg)]
res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval_general(
lin_ag,
trans_g,
w_eg,
x_ea,
y_ng,
nr_ma,
bend_coef,
nr_coef,
return_tuple=True)
if VERBOSE:
print colorize("iteration %i, cost %.3e" % (i, fval), 'red'),
if VERBOSE:
print "= %.3e (res) + %.3e (bend) + %.3e (nr)" % (
res_cost, bend_cost, nr_cost)
Jl_zcg, Jt_zg, Jw_zeg = tps_nr_grad(nr_ma,
lin_ag,
trans_g,
w_eg,
x_ea,
return_tuple=True)
nrerr_z = tps_nr_err(nr_ma, lin_ag, trans_g, w_eg, x_ea)
fullstep_fg = np.empty((F, D))
for g in xrange(D):
J_zf = np.c_[Jl_zcg[:, g::D], Jt_zg[:, g::D],
Jw_zeg[:, g::D].dot(N_eq)]
JJ_ff = np.dot(J_zf.T, J_zf)
A_ff = nr_coef * JJ_ff + QQ_ff + bend_coef * Bend_ff
X0 = X_fg[:, g]
B_f = nr_coef * np.dot(J_zf.T,
np.dot(J_zf, X0) - nrerr_z) + Q_nf.T.dot(
y_ng[:, g])
fullstep_fg[:, g] = np.linalg.solve(A_ff, B_f) - X_fg[:, g]
cost_improved = False
for stepsize in 3.**np.arange(0, -10, -1):
cand_X_fg = X_fg + fullstep_fg * stepsize
cand_lin_ag, cand_trans_g, cand_w_eg = cand_X_fg[:D], cand_X_fg[
D], N_eq.dot(cand_X_fg[D + 1:])
fval_cand = tps_nr_cost_eval_general(cand_lin_ag,
cand_trans_g,
cand_w_eg,
x_ea,
y_ng,
nr_ma,
bend_coef,
nr_coef,
return_tuple=False)
if VERBOSE:
print "stepsize: %.1g, fval: %.3e" % (stepsize, fval_cand)
if fval_cand < fval:
cost_improved = True
break
if not cost_improved:
if VERBOSE: print "couldn't improve objective"
break
lin_ag = cand_lin_ag
trans_g = cand_trans_g
w_eg = cand_w_eg
return lin_ag, trans_g, w_eg, x_ea
def tps_nr_fit(x_na, y_ng, bend_coef, nr_ma, nr_coef, method="newton"):
N,D = x_na.shape
lin_ag, trans_g, w_ng = tps_fit2(x_na, y_ng, bend_coef, 1e-3)
#return lin_ag, trans_g, w_ng
##for testing that it takes one step when nonrigidity cost is zero:
#lin_ag, trans_g, w_ng = tps_fit(x_na, y_ng, bend_coef, 0)
#res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, nr_ma, bend_coef, nr_coef, return_tuple=True)
#print "CORRECT COST, res,bend,nr,total = %.3e, %.3e, %.3e, %.3e"%(res_cost, bend_cost, nr_cost, fval)
#lin_ag += np.random.randn(*lin_ag.shape)*5
#res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, nr_ma, bend_coef, nr_coef, return_tuple=True)
#print "NOISE ADDED COST, res,bend,nr,total = %.ef, %.3e, %.3e, %.3e"%(res_cost, bend_cost, nr_cost, fval)
_u,_s,_vh = np.linalg.svd(np.c_[x_na, np.ones((N,1))], full_matrices=True)
N_nq = _u[:,4:] # null of data
#w_ng = N_nq.dot(N_nq.T.dot(w_ng))
K_nn = ssd.squareform(ssd.pdist(x_na))
Q_nn = np.c_[x_na, np.ones((N,1)),K_nn.dot(N_nq)]
QQ_nn = np.dot(Q_nn.T, Q_nn)
Bend_nn = np.zeros((N,N))
Bend_nn[4:, 4:] = - N_nq.T.dot(K_nn.dot(N_nq))
n_iter=60
for i in xrange(n_iter):
X_ng = np.r_[lin_ag, trans_g[None,:], N_nq.T.dot(w_ng)]
res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, y_ng, nr_ma, bend_coef, nr_coef, return_tuple=True)
if VERBOSE: print colorize("iteration %i, cost %.3e"%(i, fval), 'red'),
if VERBOSE: print "= %.3e (res) + %.3e (bend) + %.3e (nr)"%(res_cost, bend_cost, nr_cost)
Jl_zcg, Jt_zg, Jw_zng = tps_nr_grad(nr_ma, lin_ag, trans_g, w_ng, x_na, return_tuple=True)
nrerr_z = tps_nr_err(nr_ma, lin_ag, trans_g, w_ng, x_na)
if method == "newton":
fullstep_ng = np.empty((N,D))
for g in xrange(D):
J_zn = np.c_[Jl_zcg[:,g::D], Jt_zg[:,g::D], Jw_zng[:,g::D].dot(N_nq)]
JJ_nn = np.dot(J_zn.T, J_zn)
A = nr_coef*JJ_nn + QQ_nn + bend_coef*Bend_nn
X0 = X_ng[:,g]
B = nr_coef*np.dot(J_zn.T, np.dot(J_zn, X0) - nrerr_z) + Q_nn.T.dot(y_ng[:,g])
fullstep_ng[:,g] = np.linalg.solve(A,B) - X_ng[:,g]
elif method == "gradient":
def eval_partial(cand_X_ng):
cand_X_ng = cand_X_ng.reshape(-1,3)
cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:])
fval_cand = tps_nr_cost_eval(cand_lin_ag, cand_trans_g, cand_w_ng, x_na, y_ng, nr_ma, bend_coef, nr_coef)
return fval_cand
def eval_partial2(cand_X_ng):
return ((Q_nn.dot(X_ng) - y_ng)**2).sum()
def eval_partial3(cand_X_ng):
cand_X_ng = cand_X_ng.reshape(-1,3)
cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:])
return ((y_ng - tps_eval(x_na, cand_lin_ag, cand_trans_g, cand_w_ng, x_na))**2).sum()
grad_ng = np.empty((N,D))
for g in xrange(D-1,-1,-1):
Jnr_zn = np.c_[Jl_zcg[:,g::D], Jt_zg[:,g::D], Jw_zng[:,g::D].dot(N_nq)]
grad_ng[:,g] = 2 * nr_coef * nrerr_z.dot(Jnr_zn) \
+ 2 * Q_nn.T.dot(Q_nn.dot(X_ng[:,g]) - y_ng[:,g]) \
+ 2 * bend_coef * Bend_nn.dot(X_ng[:,g])
#assert np.allclose(eval_partial2(X_ng), eval_partial3(X_ng))
#assert np.allclose(eval_partial(X_ng), eval_partial2(X_ng))
#grad0_ng = ndt.Gradient(eval_partial)(X_ng.flatten()).reshape(-1,3)
fullstep_ng = -grad_ng
#assert np.allclose(grad0_ng, grad_ng)
cost_improved = False
for stepsize in 3.**np.arange(0,-10,-1):
cand_X_ng = X_ng + fullstep_ng*stepsize
cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:])
fval_cand = tps_nr_cost_eval(cand_lin_ag, cand_trans_g, cand_w_ng, x_na, y_ng, nr_ma, bend_coef, nr_coef)
if VERBOSE: print "stepsize: %.1g, fval: %.3e"%(stepsize, fval_cand)
if fval_cand < fval:
cost_improved = True
break
if not cost_improved:
if VERBOSE: print "couldn't improve objective"
break
lin_ag = cand_lin_ag
trans_g = cand_trans_g
w_ng = cand_w_ng
return lin_ag, trans_g, w_ng
#!/usr/bin/env python
import sys
from os.path import exists
from time import sleep
import subprocess
from jds_utils.colorize import colorize
command = sys.argv[1]
dependencies = sys.argv[2:]
while True:
if all(exists(file) for file in dependencies):
print colorize(command, 'red')
subprocess.check_call(command, shell=True)
break
else:
sleep(.01)
#!/usr/bin/env python
import sys
from os.path import exists
from time import sleep
import subprocess
from jds_utils.colorize import colorize
command = sys.argv[1]
dependencies = sys.argv[2:]
while True:
if all(exists(file) for file in dependencies):
print colorize(command,'red')
subprocess.check_call(command,shell=True)
break
else:
sleep(.01)
labelGetter = comm.FileGetter(args.label,"bmp",comm.SingleChannelImageMessage)
ropeInitPub = comm.FilePublisher(args.out,"txt")
prev_id = -1
while True:
latest_id = getLastInd(args.label)
print "latest id", latest_id
if latest_id == prev_id:
sleep(.1)
continue
else:
prev_id = latest_id
timePrevStart = time()
xyz,bgr = pcdGetter.recv_id(latest_id).data
label = labelGetter.recv_id(latest_id).data
if label is None: raise Exception("could not read label file")
try:
xyzs,labels = initialize_rope(label,xyz, bgr,plotting=args.plotting)
ropeInitPub.send(RopeInitMessage(data=(xyzs, labels)))
sleep(max(args.pause - (time() - timePrevStart),0))
except Exception:
print colorize("exception occurred in rope init:","red")
traceback.print_exc()
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):
rope1 = ropes[i1]
dists1 = dists[i1]
n1 = len(rope1)
print colorize("comparing %s to %s" % (seg_names[i0], seg_names[i1]), "red")
cost = recognition.calc_match_score(rope0, rope1, dists0, dists1, plotting=1)
results.append((i0, i1, cost))
print i0, i1, cost
distmat = np.zeros((6, 6))
for (i0, i1, cost) in results:
distmat[i0, i1] = cost
distmat[xrange(6), xrange(6)] = np.nan
print distmat.argmin(axis=0)
a, b = np.meshgrid([0, 3, 1, 4, 2, 5], [0, 3, 1, 4, 2, 5])
distmat_rearr = distmat[a, b]
distmat_rearr[range(6), range(6)] = np.nan
plt.imshow(distmat_rearr, interpolation="nearest", cmap="gray")
verb = args.verb
target = args.target
arms_used = args.arms_used
from lfd import verbs
all_demo_info = verbs.get_all_demo_info()
for i in xrange(1000):
demo_name = "%s-%s%i"%(verb, target, i)
if demo_name not in all_demo_info:
break
data_dir = osp.join(osp.dirname(lfd.__file__), "data")
os.chdir(data_dir + "/images")
call_and_print("get_point_cloud.py %s"%demo_name)
print colorize("now, select the target object", color="red", bold=True)
call_and_print("manually_segment_point_cloud.py %s.npz --objs=%s"%(demo_name, target))
print colorize("now, human, demonstrate the action on the robot", color="red", bold=True)
os.chdir(data_dir + "/bags")
call_and_print("rosrun pr2_controller_manager pr2_controller_manager stop r_arm_controller l_arm_controller")
try:
old_tmp_bags = glob("tmpname*.bag")
if not args.dry_run:
for bag in old_tmp_bags:
os.remove(bag)
call_and_print("rosbag record /tf /joint_states /joy -o tmpname")
except KeyboardInterrupt:
pass
def call_and_print(cmd,color='green'):
print colorize(cmd, color, bold=True)
if not args.dry_run: subprocess.check_call(cmd, shell=True)
pcdGetter = comm.FileGetter(args.pcd, "pcd", comm.PointCloudMessage)
labelGetter = comm.FileGetter(args.label, "bmp",
comm.SingleChannelImageMessage)
ropeInitPub = comm.FilePublisher(args.out, "txt")
prev_id = -1
while True:
latest_id = getLastInd(args.label)
print "latest id", latest_id
if latest_id == prev_id:
sleep(.1)
continue
else:
prev_id = latest_id
timePrevStart = time()
xyz, bgr = pcdGetter.recv_id(latest_id).data
label = labelGetter.recv_id(latest_id).data
if label is None: raise Exception("could not read label file")
try:
xyzs, labels = initialize_rope(label, xyz, bgr, plotting=args.plotting)
ropeInitPub.send(RopeInitMessage(data=(xyzs, labels)))
sleep(max(args.pause - (time() - timePrevStart), 0))
except Exception:
print colorize("exception occurred in rope init:", "red")
traceback.print_exc()
