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 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()