def test_tpsrpm_objective_monotonicity(self): n_iter = 10 em_iter = 10 reg_factory = TpsRpmRegistrationFactory(n_iter=n_iter, em_iter=em_iter, f_solver_factory=solver.AutoTpsSolverFactory(use_cache=False)) objs = np.zeros((n_iter, em_iter)) def callback(i, i_em, x_nd, y_md, xtarg_nd, wt_n, f, corr_nm, rad): objs[i, i_em] = TpsRpmRegistration.get_objective2(x_nd, y_md, f, corr_nm, rad).sum() reg = reg_factory.register(self.demos.values()[0], self.test_scene_state, callback=callback) print np.diff(objs, axis=1) <= 0 # TODO assert when monotonicity is more robust
def setup_registration_and_trajectory_transferer(args, sim): if args.eval.batch: if args.eval.reg_type == 'rpm': reg_factory = BatchGpuTpsRpmRegistrationFactory(GlobalVars.demos, args.eval.actionfile) elif args.eval.reg_type == 'bij': reg_factory = BatchGpuTpsRpmBijRegistrationFactory(GlobalVars.demos, args.eval.actionfile) else: raise RuntimeError("Invalid reg_type option %s"%args.eval.reg_type) else: if args.eval.reg_type == 'segment': reg_factory = TpsSegmentRegistrationFactory(GlobalVars.demos) elif args.eval.reg_type == 'rpm': reg_factory = TpsRpmRegistrationFactory(GlobalVars.demos) elif args.eval.reg_type == 'bij': reg_factory = TpsRpmBijRegistrationFactory(GlobalVars.demos, actionfile=args.eval.actionfile) else: raise RuntimeError("Invalid reg_type option %s"%args.eval.reg_type) if args.eval.transferopt == 'pose' or args.eval.transferopt == 'finger': traj_transferer = PoseTrajectoryTransferer(sim, args.eval.beta_pos, args.eval.beta_rot, args.eval.gamma, args.eval.use_collision_cost) if args.eval.transferopt == 'finger': traj_transferer = FingerTrajectoryTransferer(sim, args.eval.beta_pos, args.eval.gamma, args.eval.use_collision_cost, init_trajectory_transferer=traj_transferer) else: raise RuntimeError("Invalid transferopt option %s"%args.eval.transferopt) if args.eval.unified: reg_and_traj_transferer = UnifiedRegistrationAndTrajectoryTransferer(reg_factory, traj_transferer) else: reg_and_traj_transferer = TwoStepRegistrationAndTrajectoryTransferer(reg_factory, traj_transferer) return reg_and_traj_transferer
def test_tpsrpm_objective_monotonicity(self): n_iter = 10 em_iter = 10 reg_factory = TpsRpmRegistrationFactory( n_iter=n_iter, em_iter=em_iter, f_solver_factory=solver.AutoTpsSolverFactory(use_cache=False)) objs = np.zeros((n_iter, em_iter)) def callback(i, i_em, x_nd, y_md, xtarg_nd, wt_n, f, corr_nm, rad): objs[i, i_em] = TpsRpmRegistration.get_objective2( x_nd, y_md, f, corr_nm, rad).sum() reg = reg_factory.register(self.demos.values()[0], self.test_scene_state, callback=callback) print np.diff( objs, axis=1) <= 0 # TODO assert when monotonicity is more robust
def test_tps_objective(self): reg_factory = TpsRpmRegistrationFactory( {}, f_solver_factory=solver.CpuTpsSolverFactory(use_cache=False)) reg = reg_factory.register(self.demos.values()[0], self.test_scene_state) x_na = reg.f.x_na y_ng = reg.f.y_ng wt_n = reg.f.wt_n rot_coef = reg.f.rot_coef bend_coef = reg.f.bend_coef # code from tps_fit3 n, d = x_na.shape K_nn = tps.tps_kernel_matrix(x_na) Q = np.c_[np.ones((n, 1)), x_na, K_nn] rot_coefs = np.ones(d) * rot_coef if np.isscalar( rot_coef) else np.asarray(rot_coef) A = np.r_[np.zeros((d + 1, d + 1)), np.c_[np.ones((n, 1)), x_na]].T WQ = wt_n[:, None] * Q QWQ = Q.T.dot(WQ) H = QWQ H[d + 1:, d + 1:] += bend_coef * K_nn H[1:d + 1, 1:d + 1] += np.diag(rot_coefs) f = -WQ.T.dot(y_ng) f[1:d + 1, 0:d] -= np.diag(rot_coefs) # optimum point theta = np.r_[reg.f.trans_g[None, :], reg.f.lin_ag, reg.f.w_ng] # equality constraint self.assertTrue(np.allclose(A.dot(theta), np.zeros((4, 3)))) # objective obj = np.trace(theta.T.dot(H.dot(theta))) + 2*np.trace(f.T.dot(theta)) \ + np.trace(y_ng.T.dot(wt_n[:,None]*y_ng)) + rot_coefs.sum() # constant self.assertTrue(np.allclose(obj, reg.f.get_objective().sum()))
def test_tps_objective(self): reg_factory = TpsRpmRegistrationFactory({}, f_solver_factory=solver.CpuTpsSolverFactory(use_cache=False)) reg = reg_factory.register(self.demos.values()[0], self.test_scene_state) x_na = reg.f.x_na y_ng = reg.f.y_ng wt_n = reg.f.wt_n rot_coef = reg.f.rot_coef bend_coef = reg.f.bend_coef # code from tps_fit3 n,d = x_na.shape K_nn = tps.tps_kernel_matrix(x_na) Q = np.c_[np.ones((n,1)), x_na, K_nn] rot_coefs = np.ones(d) * rot_coef if np.isscalar(rot_coef) else np.asarray(rot_coef) A = np.r_[np.zeros((d+1,d+1)), np.c_[np.ones((n,1)), x_na]].T WQ = wt_n[:,None] * Q QWQ = Q.T.dot(WQ) H = QWQ H[d+1:,d+1:] += bend_coef * K_nn H[1:d+1, 1:d+1] += np.diag(rot_coefs) f = -WQ.T.dot(y_ng) f[1:d+1,0:d] -= np.diag(rot_coefs) # optimum point theta = np.r_[reg.f.trans_g[None,:], reg.f.lin_ag, reg.f.w_ng] # equality constraint self.assertTrue(np.allclose(A.dot(theta), np.zeros((4,3)))) # objective obj = np.trace(theta.T.dot(H.dot(theta))) + 2*np.trace(f.T.dot(theta)) \ + np.trace(y_ng.T.dot(wt_n[:,None]*y_ng)) + rot_coefs.sum() # constant self.assertTrue(np.allclose(obj, reg.f.get_objective().sum()))
def test_tps_rpm_solvers(self): tmp_cachedir = mkdtemp() reg_factory = TpsRpmRegistrationFactory(self.demos, f_solver_factory=None) sys.stdout.write("computing costs: no solver... ") sys.stdout.flush() start_time = time.time() costs = reg_factory.batch_cost(self.test_scene_state) print "done in {}s".format(time.time() - start_time) reg_factory_solver = TpsRpmRegistrationFactory(self.demos, f_solver_factory=solver.CpuTpsSolverFactory(cachedir=tmp_cachedir)) sys.stdout.write("computing costs: solver... ") sys.stdout.flush() start_time = time.time() costs_solver = reg_factory_solver.batch_cost(self.test_scene_state) print "done in {}s".format(time.time() - start_time) sys.stdout.write("computing costs: cached solver... ") sys.stdout.flush() start_time = time.time() costs_solver_cached = reg_factory_solver.batch_cost(self.test_scene_state) print "done in {}s".format(time.time() - start_time) if _has_cuda: reg_factory_gpu = TpsRpmRegistrationFactory(self.demos, f_solver_factory=solver.GpuTpsSolverFactory(cachedir=tmp_cachedir)) sys.stdout.write("computing costs: gpu solver... ") sys.stdout.flush() start_time = time.time() costs_gpu = reg_factory_gpu.batch_cost(self.test_scene_state) print "done in {}s".format(time.time() - start_time) sys.stdout.write("computing costs: cached gpu solver... ") sys.stdout.flush() start_time = time.time() costs_gpu_cached = reg_factory_gpu.batch_cost(self.test_scene_state) print "done in {}s".format(time.time() - start_time) else: print "couldn't run GPU tests since the GPU is not configured properly" for demo_name in self.demos.keys(): self.assertTrue(np.allclose(costs[demo_name], costs_solver[demo_name])) self.assertTrue(np.allclose(costs[demo_name], costs_solver_cached[demo_name])) if _has_cuda: self.assertTrue(np.allclose(costs[demo_name], costs_gpu[demo_name])) self.assertTrue(np.allclose(costs[demo_name], costs_gpu_cached[demo_name]))
def main(): # define simulation objects table_height = 0.77 sim_objs = [] sim_objs.append(XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)) sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False)) # initialize simulation world and environment sim = DynamicSimulationRobotWorld() sim.add_objects(sim_objs) sim.create_viewer() sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()]) sim.robot.SetDOFValues([1.25], [sim.robot.GetJoint('head_tilt_joint').GetJointIndex()]) # move head down so it can see the rope sim_util.reset_arms_to_side(sim) env = environment.LfdEnvironment(sim, sim, downsample_size=0.025) demo_rope_poss = np.array([[.2, -.2, table_height+0.006], [.8, -.2, table_height+0.006], [.8, .2, table_height+0.006], [.2, .2, table_height+0.006]]) demo = create_rope_demo(env, demo_rope_poss) test_rope_poss = np.array([[.2, -.2, table_height+0.006], [.5, -.4, table_height+0.006], [.8, .0, table_height+0.006], [.8, .2, table_height+0.006], [.6, .0, table_height+0.006], [.4, .2, table_height+0.006], [.2, .2, table_height+0.006]]) test_rope_sim_obj = create_rope(test_rope_poss) sim.add_objects([test_rope_sim_obj]) sim.settle() test_scene_state = env.observe_scene() reg_factory = TpsRpmRegistrationFactory() traj_transferer = FingerTrajectoryTransferer(sim) plot_cb = lambda i, i_em, x_nd, y_md, xtarg_nd, wt_n, f, corr_nm, rad: registration_plot_cb(sim, x_nd, y_md, f) reg_and_traj_transferer = TwoStepRegistrationAndTrajectoryTransferer(reg_factory, traj_transferer) test_aug_traj = reg_and_traj_transferer.transfer(demo, test_scene_state, callback=plot_cb, plotting=True) env.execute_augmented_trajectory(test_aug_traj)
viewer = trajoptpy.GetViewer(sim.env) camera_matrix = np.array([[ 0, 1, 0, 0], [-1, 0, 0.5, 0], [ 0.5, 0, 1, 0], [ 2.25, 0, 4.5, 1]]) viewer.SetWindowProp(0,0,1500,1500) viewer.SetCameraManipulatorMatrix(camera_matrix) def generate_cloud(x_center_pert=0, max_noise=0.02): # generates 40 cm by 60 cm cloud with optional pertubation along the x-axis grid = np.array(np.meshgrid(np.linspace(-.2,.2,21), np.linspace(-.3,.3,31))).T.reshape((-1,2)) grid = np.c_[grid, np.zeros(len(grid))] + np.array([.5, 0, table_height+max_noise]) cloud = grid + x_center_pert * np.c_[(0.3 - np.abs(grid[:,1]-0))/0.3, np.zeros((len(grid),2))] + (np.random.random((len(grid), 3)) - 0.5) * 2 * max_noise return cloud demos = {} for x_center_pert in np.arange(-0.1, 0.6, 0.1): demo_name = "demo_{}".format(x_center_pert) demo_cloud = generate_cloud(x_center_pert=x_center_pert) demo_scene_state = SceneState(demo_cloud, downsample_size=0.025) demo = Demonstration(demo_name, demo_scene_state, None) demos[demo_name] = demo test_cloud = generate_cloud(x_center_pert=0.2) test_scene_state = SceneState(test_cloud, downsample_size=0.025) plot_cb = lambda i, i_em, x_nd, y_md, xtarg_nd, wt_n, f, corr_nm, rad: registration_plot_cb(sim, x_nd, y_md, f) reg_factory = TpsRpmRegistrationFactory(demos) regs = reg_factory.batch_register(test_scene_state, callback=plot_cb)
def test_tps_rpm_solvers(self): tmp_cachedir = mkdtemp() reg_factory = TpsRpmRegistrationFactory(self.demos, f_solver_factory=None) sys.stdout.write("computing costs: no solver... ") sys.stdout.flush() start_time = time.time() costs = reg_factory.batch_cost(self.test_scene_state) print "done in {}s".format(time.time() - start_time) reg_factory_solver = TpsRpmRegistrationFactory( self.demos, f_solver_factory=solver.CpuTpsSolverFactory(cachedir=tmp_cachedir)) sys.stdout.write("computing costs: solver... ") sys.stdout.flush() start_time = time.time() costs_solver = reg_factory_solver.batch_cost(self.test_scene_state) print "done in {}s".format(time.time() - start_time) sys.stdout.write("computing costs: cached solver... ") sys.stdout.flush() start_time = time.time() costs_solver_cached = reg_factory_solver.batch_cost( self.test_scene_state) print "done in {}s".format(time.time() - start_time) if _has_cuda: reg_factory_gpu = TpsRpmRegistrationFactory( self.demos, f_solver_factory=solver.GpuTpsSolverFactory( cachedir=tmp_cachedir)) sys.stdout.write("computing costs: gpu solver... ") sys.stdout.flush() start_time = time.time() costs_gpu = reg_factory_gpu.batch_cost(self.test_scene_state) print "done in {}s".format(time.time() - start_time) sys.stdout.write("computing costs: cached gpu solver... ") sys.stdout.flush() start_time = time.time() costs_gpu_cached = reg_factory_gpu.batch_cost( self.test_scene_state) print "done in {}s".format(time.time() - start_time) else: print "couldn't run GPU tests since the GPU is not configured properly" for demo_name in self.demos.keys(): self.assertTrue( np.allclose(costs[demo_name], costs_solver[demo_name])) self.assertTrue( np.allclose(costs[demo_name], costs_solver_cached[demo_name])) if _has_cuda: self.assertTrue( np.allclose(costs[demo_name], costs_gpu[demo_name])) self.assertTrue( np.allclose(costs[demo_name], costs_gpu_cached[demo_name]))
def generate_cloud(x_center_pert=0, max_noise=0.02): # generates 40 cm by 60 cm cloud with optional pertubation along the x-axis grid = np.array( np.meshgrid(np.linspace(-.2, .2, 21), np.linspace(-.3, .3, 31))).T.reshape((-1, 2)) grid = np.c_[grid, np.zeros(len(grid))] + np.array( [.5, 0, table_height + max_noise]) cloud = grid + x_center_pert * np.c_[ (0.3 - np.abs(grid[:, 1] - 0)) / 0.3, np.zeros((len(grid), 2))] + (np.random.random( (len(grid), 3)) - 0.5) * 2 * max_noise return cloud demos = {} for x_center_pert in np.arange(-0.1, 0.6, 0.1): demo_name = "demo_{}".format(x_center_pert) demo_cloud = generate_cloud(x_center_pert=x_center_pert) demo_scene_state = SceneState(demo_cloud, downsample_size=0.025) demo = Demonstration(demo_name, demo_scene_state, None) demos[demo_name] = demo test_cloud = generate_cloud(x_center_pert=0.2) test_scene_state = SceneState(test_cloud, downsample_size=0.025) plot_cb = lambda i, i_em, x_nd, y_md, xtarg_nd, wt_n, f, corr_nm, rad: registration_plot_cb( sim, x_nd, y_md, f) reg_factory = TpsRpmRegistrationFactory(demos) regs = reg_factory.batch_register(test_scene_state, callback=plot_cb)