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)