def main(DOF, env_name, lmbda=10):
dataset = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name))
cfgs = dataset['data']
labels = dataset['label']
dists = dataset['dist']
obstacles = dataset['obs']
robot = dataset['robot'](*dataset['rparam'])
train_num = 6000
indices = torch.LongTensor(
np.random.choice(len(cfgs), train_num, replace=False))
fkine = robot.fkine
'''
checker = DiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(lmbda)), beta=1.0)
# checker = MultiDiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0)
keep_all = False
if 'compare' not in env_name:
checker.train(cfgs[:train_num], labels[:train_num], max_iteration=len(cfgs[:train_num]), distance=dists[:train_num],
keep_all=keep_all)
else:
checker.train(cfgs[indices], labels[indices], max_iteration=len(cfgs[indices]), distance=dists[indices],
keep_all=keep_all)
# Check DiffCo test ACC
test_preds = (checker.score(cfgs[train_num:]) > 0) * 2 - 1
test_acc = torch.sum(test_preds == labels[train_num:], dtype=torch.float32)/len(test_preds.view(-1))
test_tpr = torch.sum(test_preds[labels[train_num:]==1] == 1, dtype=torch.float32) / len(test_preds[labels[train_num:]==1])
test_tnr = torch.sum(test_preds[labels[train_num:]==-1] == -1, dtype=torch.float32) / len(test_preds[labels[train_num:]==-1])
print('Test acc: {}, TPR {}, TNR {}'.format(test_acc, test_tpr, test_tnr))
print(len(checker.gains), 'Support Points')
# assert(test_acc > 0.9)
fitting_target = 'label' # {label, dist, hypo}
Epsilon = 0.01
checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon), target=fitting_target, fkine=fkine) # epsilon=Epsilon,
# checker.fit_poly(kernel_func=kernel.MultiQuadratic(Epsilon), target=fitting_target, fkine=fkine)
# checker.fit_full_poly(epsilon=Epsilon, target=fitting_target, fkine=fkine) #, lmbd=10)
dist_est = checker.rbf_score
# = checker.score
# dist_est = checker.poly_score
'''
''' #==================3-figure compare (work, c space 1, c space 2)==========
size = [400, 400]
env_name_gt = env_name if 'compare' in env_name else env_name+'_for_compare'
gt_grid = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name_gt))['dist']
grid_points = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name_gt))['data']
raw_grid_score = checker.score(grid_points)
raw_grid_score = torch.from_numpy(ndimage.gaussian_filter(raw_grid_score, 1))
use3d = False
dpi=100
est, c_axes = create_plots(robot, obstacles, None, None, use3d=use3d) # raw_grid_score)#gt_grid)
plt.tight_layout()
plt.show()
plt.savefig('figs/original_DiffCo_score_compared_{}_dpi{}.jpg'.format('3d' if use3d else '2d', dpi), dpi=dpi, bbox_inches='tight')
plt.savefig('figs/robot_gallery/2d_{}dof_{}.jpg'.format(DOF, env_name), dpi=dpi, bbox_inches='tight')
plt.savefig('figs/vis_{}.png'.format(env_name), dpi=500)
'''
'''# =============== test error ============
# est = est / est.std() * gt_grid.std()
# print('{:.4f}, {:.4f}, {:.4f}'.format(
# (est-gt_grid).mean(), (est-gt_grid).std(), gt_grid.std()))
'''
# ''' new diffco 3-figure compare (work, c space 1, c space 2)==========
from diffco import DiffCo
checker = DiffCo(
obstacles,
kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)),
rbf_kernel=kernel.Polyharmonic(1, epsilon=1)
) #kernel.Polyharmonic(1, epsilon=1)) kernel.MultiQuadratic(epsilon=1)
checker.train(cfgs[:train_num],
dists[:train_num],
fkine=fkine,
max_iteration=int(1e4),
n_left_out_points=300,
dtol=1e-1)
checker.gains = checker.gains.reshape(-1, 1)
dist_est = checker.rbf_score
size = [400, 400]
env_name_gt = env_name if 'compare' in env_name else env_name + '_for_compare'
# gt_grid = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name_gt))['dist']
# grid_points = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name_gt))['data']
# raw_grid_score = checker.rbf_score(grid_points)
# raw_grid_score = torch.from_numpy(ndimage.gaussian_filter(raw_grid_score, 1))
# use3d = False
# # dpi=100
# # est, c_axes = create_plots(robot, obstacles, None, None, use3d=use3d) # raw_grid_score)#gt_grid)
# est, c_axes = create_plots(robot, obstacles, checker.rbf_score, gt_grid, use3d=use3d) # raw_grid_score)#gt_grid)
# c_support_points = checker.support_points
# c_axes[1].scatter(c_support_points[:, 0], c_support_points[:, 1], marker='.', c='black', s=1.5)
# # plt.tight_layout()
# plt.show()
# plt.savefig('figs/original_DiffCo_score_compared_{}_dpi{}.jpg'.format('3d' if use3d else '2d', dpi), dpi=dpi, bbox_inches='tight')
# plt.savefig('figs/robot_gallery/2d_{}dof_{}.jpg'.format(DOF, env_name), dpi=dpi, bbox_inches='tight')
# plt.savefig('figs/vis_{}.png'.format(env_name), dpi=500)
# plt.savefig('figs/new_diffco_vis.png')
# '''
# ''' #===============================
# ''' =============== correlation ==============
# gt_grid = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name))['dist']
gt_grid = checker.distance
# yy, xx = torch.meshgrid(torch.linspace(-np.pi, np.pi, size[0]), torch.linspace(-np.pi, np.pi, size[1]))
# grid_points = torch.stack([xx, yy], axis=2).reshape((-1, 2))
# est_grid = dist_est(cfgs[train_num:])
est_grid = dist_est(checker.support_points)
# indices = np.random.choice(range(len(est_grid)), size=400, replace=False)
# gt_grid = gt_grid[train_num:]
# est_grid = est_grid[indices]
fig = plt.figure(figsize=(5, 5)) # temp
plt.rcParams.update({
"text.usetex": True,
"font.family": "sans-serif",
"font.sans-serif": ["Helvetica"]
})
# gs = fig.add_gridspec(1, 3)
ax = fig.add_subplot(111)
# ax.set_aspect('equal', adjustable='box')
# ax.axis('equal')
# ax.set_xlim((-4, 4))
# ax.set_ylim((-3, 3))
ax.scatter(gt_grid, est_grid, s=5)
xlim_max = torch.FloatTensor(ax.get_xlim()).abs().max()
ax.set_xlim(-xlim_max, xlim_max)
ylim_max = torch.FloatTensor(ax.get_ylim()).abs().max()
ax.set_ylim(-ylim_max, ylim_max)
ax.axhline(0, linestyle='-', color='gray', alpha=0.5)
ax.axvline(0, linestyle='-', color='gray', alpha=0.5)
# ax.spines['left'].set_position('center')
# ax.spines['bottom'].set_position('center')
# ax.
from scipy import stats
slope, intercept, r_value, p_value, std_err = stats.linregress(
est_grid.numpy().reshape(-1),
gt_grid.numpy().reshape(-1))
print('{}DOF, environment {}, with FK {}, r-squared: {}'.format(
DOF, env_name, checker.fkine is not None, r_value**2))
ax.text(xlim_max / 4,
-7 * ylim_max / 8,
'$\\mathrm{R}^2=' + ('{:.4f}$'.format(r_value**2)),
fontsize=15,
bbox=dict(boxstyle='round', facecolor='wheat',
alpha=1)) #, fontdict={"family": "Times New Roman",})
ax.set_title('{} original supports, {} random samples'.format(
checker.num_origin_supports, checker.n_left_out_points))
# plt.show()
plt.savefig(
'figs/correlation/training_{}dof_{}_{}_{}ransample_rsquare.png'.format(
DOF, env_name, 'hybriddiffco', checker.n_left_out_points))
# plt.savefig('figs/correlation/{}dof_{}_{}.pdf'.format(DOF, env_name, fitting_target))#, dpi=500)
# plt.savefig('figs/correlation/{}dof_{}_{}_{}_rsquare.png'.format(DOF, env_name, fitting_target, 'woFK' if checker.fkine is None else 'withFK'), dpi=300)
# '''
''' timing
# times = []
# st = time()
# # for i, cfg in enumerate(cfgs):
# # st1 = time()
# # dist_est(cfg)
# # end1 = time()
# # times.append(end1-st1)
# dist_est(cfgs)
# end = time()
# times = np.array(times)
# print('std: {}, mean {}, avg {}'.format(times.std(), times.mean(), (end-st)/len(cfgs)))
'''
''' decomposition
def test_one_env(env_name, method, folder, args: ExpConfigs, prev_rec={}):
assert (args.use_previous_solution and prev_rec != {}) or \
((not args.use_previous_solution) and prev_rec == {}), \
"args.use_previous_solution does not match the existence of prev_rec."
print(env_name, method, 'Begin')
# Prepare distance estimator ====================
dataset = torch.load('{}/{}.pt'.format(folder, env_name))
cfgs = dataset['data'].double()
labels = dataset['label'].double() #.max(1).values
dists = dataset['dist'].double() #.reshape(-1, 1) #.max(1).values
obstacles = dataset['obs']
# obstacles = [obs+(i, ) for i, obs in enumerate(obstacles)]
robot = dataset['robot'](*dataset['rparam'])
width = robot.link_width
train_num = 6000
fkine = robot.fkine
train_t = time()
checker = DiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0)
# checker = MultiDiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0)
if not args.use_previous_solution:
checker.train(cfgs[:train_num], labels[:train_num], max_iteration=len(cfgs[:train_num]), distance=dists[:train_num])
# Check DiffCo test ACC
# test_preds = (checker.score(cfgs[train_num:]) > 0) * 2 - 1
# test_acc = torch.sum(test_preds == labels[train_num:], dtype=torch.float32)/len(test_preds.view(-1))
# test_tpr = torch.sum(test_preds[labels[train_num:]==1] == 1, dtype=torch.float32) / len(test_preds[labels[train_num:]==1])
# test_tnr = torch.sum(test_preds[labels[train_num:]==-1] == -1, dtype=torch.float32) / len(test_preds[labels[train_num:]==-1])
# print('Test acc: {}, TPR {}, TNR {}'.format(test_acc, test_tpr, test_tnr))
# if test_acc < 0.9:
# print('test acc is only {}'.format(test_acc))
fitting_target = 'label' # {label, dist, hypo}
Epsilon = 1 #0.01
checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon), target=fitting_target, fkine=fkine)#, reg=0.09) # epsilon=Epsilon,
# checker.fit_full_poly(epsilon=Epsilon, target=fitting_target, fkine=fkine)#, lmbd=80)
# ========================
# ONLY for additional training timing exp
# fcl_obs = [FCLObstacle(*param) for param in obstacles]
# fcl_collision_obj = [fobs.cobj for fobs in fcl_obs]
# obs_managers = [fcl.DynamicAABBTreeCollisionManager()]
# obs_managers[0].registerObjects(fcl_collision_obj)
# obs_managers[0].setup()
# robot_links = robot.update_polygons(cfgs[0])
# robot_manager = fcl.DynamicAABBTreeCollisionManager()
# robot_manager.registerObjects(robot_links)
# robot_manager.setup()
# for mng in obs_managers:
# mng.setup()
# gt_checker = FCLChecker(obstacles, robot, robot_manager, obs_managers)
# gt_checker.predict(cfgs[:train_num], distance=False)
# return time() - train_t
# END ========================
dist_est = checker.rbf_score
# dist_est = checker.poly_score
min_score = dist_est(cfgs[train_num:]).min().item()
# print('MIN_SCORE = {:.6f}'.format(min_score))
else:
dist_est = None
min_score = 0
# ==============================================
# FCL checker =====================
fcl_obs = [FCLObstacle(*param) for param in obstacles]
fcl_collision_obj = [fobs.cobj for fobs in fcl_obs]
label_type = 'binary'
num_class = 1
if label_type == 'binary':
obs_managers = [fcl.DynamicAABBTreeCollisionManager()]
obs_managers[0].registerObjects(fcl_collision_obj)
obs_managers[0].setup()
elif label_type == 'instance':
obs_managers = [fcl.DynamicAABBTreeCollisionManager() for _ in fcl_obs]
for mng, cobj in zip(obs_managers, fcl_collision_obj):
mng.registerObjects([cobj])
elif label_type == 'class':
obs_managers = [fcl.DynamicAABBTreeCollisionManager() for _ in range(num_class)]
obj_by_cls = [[] for _ in range(num_class)]
for obj in fcl_obs:
obj_by_cls[obj.category].append(obj.cobj)
for mng, obj_group in zip(obs_managers, obj_by_cls):
mng.registerObjects(obj_group)
else:
raise NotImplementedError('Unsupported label_type {}'.format(label_type))
robot_links = robot.update_polygons(cfgs[0])
robot_manager = fcl.DynamicAABBTreeCollisionManager()
robot_manager.registerObjects(robot_links)
robot_manager.setup()
for mng in obs_managers:
mng.setup()
fcl_checker = FCLChecker(obstacles, robot, robot_manager, obs_managers)
# =================================
options = {
'N_WAYPOINTS': 20,
'NUM_RE_TRIALS': 3,
'MAXITER': 200,
'safety_margin': max(1/5*min_score, -0.5),
'seed': 1234,
'history': False
}
repair_options = {
'N_WAYPOINTS': 20,
'NUM_RE_TRIALS': 1, # just one trial
'MAXITER': 200,
'seed': 1234, # actually not used due to only one trial
'history': False,
}
test_rec = {
'start_cfg': [],
'target_cfg': [],
'cnt_check': [],
'repair_cnt_check': [],
'cost': [],
'repair_cost': [],
'time': [],
'val_time': [],
'repair_time': [],
'success': [],
'repair_success': [],
'seed': [],
'solution': [],
'repair_solution': [],
}
with open('{}/{}_testcfgs.json'.format(folder, env_name), 'r') as f:
test_cfg_dataset = json.load(f)
s_cfgs = torch.FloatTensor(test_cfg_dataset['start_cfgs'])[:10]
t_cfgs = torch.FloatTensor(test_cfg_dataset['target_cfgs'])[:10]
assert env_name == test_cfg_dataset['env_name']
if prev_rec != {}:
rec_s_cfgs = torch.FloatTensor(prev_rec['solution'])[:, 0]
rec_t_cfgs = torch.FloatTensor(prev_rec['solution'])[:, -1]
assert torch.all(torch.isclose(rec_s_cfgs, s_cfgs[:len(rec_s_cfgs)])) and torch.all(torch.isclose(rec_t_cfgs, t_cfgs[:len(rec_t_cfgs)]))
for test_it, (start_cfg, target_cfg) in tqdm(enumerate(zip(s_cfgs, t_cfgs)), desc='Test Query'):
options['seed'] += 1 # Otherwise the random initialization will stay the same every problem
if prev_rec != {} and test_it < len(prev_rec['success']):
print('using saved solution {}'.format(test_it))
tmp_rec = {k: prev_rec[k][test_it] for k in prev_rec}
elif method == 'fclgradfree':
print('solving query {} with fclgradfree'.format(test_it))
tmp_rec = gradient_free_traj_optimize(robot, lambda cfg: fcl_checker.predict(cfg, distance=False), start_cfg, target_cfg, options=options)
elif method == 'fcldist':
tmp_rec = gradient_free_traj_optimize(robot, fcl_checker.score, start_cfg, target_cfg, options=options)
elif method == 'adamdiffco':
tmp_rec = adam_traj_optimize(robot, dist_est, start_cfg, target_cfg, options=options)
elif method == 'bidiffco':
tmp_rec = gradient_free_traj_optimize(robot, lambda cfg: 2*(dist_est(cfg)>=0).type(torch.FloatTensor)-1, start_cfg, target_cfg, options=options)
elif method == 'diffcogradfree':
with torch.no_grad():
tmp_rec = gradient_free_traj_optimize(robot, dist_est, start_cfg, target_cfg, options=options)
elif method == 'margindiffcogradfree':
with torch.no_grad():
tmp_rec = gradient_free_traj_optimize(robot, lambda cfg: dist_est(cfg)-options['safety_margin'], start_cfg, target_cfg, options=options)
elif method == 'givengrad':
tmp_rec = givengrad_traj_optimize(robot, dist_est, start_cfg, target_cfg, options=options)
else:
raise NotImplementedError('Method = {} not implemented'.format(method))
# Verification
# if tmp_rec['success']:
def con_max_move(p):
control_points = robot.fkine(p)
return torch.all((control_points[1:]-control_points[:-1]).pow(2).sum(dim=2)-1.5**2 <= 0).item()
def con_collision_free(p):
return torch.all(fcl_checker.predict(p, distance=False) < 0).item()
def con_dist_collision_free(p):
return torch.all(fcl_checker.score(p) < 0).item()
# return True
def con_joint_limit(p):
return (torch.all(robot.limits[:, 0]-p <= 0) and torch.all(p-robot.limits[:, 1] <= 0)).item()
def validate(solution, method=None):
veri_cfgs = [utils.anglin(q1, q2, args.validate_density, endpoint=False)\
for q1, q2 in zip(solution[:-1], solution[1:])]
veri_cfgs = torch.cat(veri_cfgs, 0)
collision_free = con_collision_free(veri_cfgs) if method != 'fcldist' else con_dist_collision_free(veri_cfgs)
sol_tensor = torch.FloatTensor(solution)
within_jointlimit = con_joint_limit(sol_tensor)
within_movelimit = con_max_move(sol_tensor)
return collision_free and within_jointlimit and within_movelimit
if 'fcl' in method and args.validate_density == 1: # skip validation if using fcl and density is only 1
val_t = 0
# tmp_rec['success'] = validate(tmp_rec['solution'], method=method) # This is only temporary
else:
val_t = time()
tmp_rec['success'] = validate(tmp_rec['solution'])
val_t = time() - val_t
tmp_rec['val_time'] = val_t
for k in tmp_rec:
if 'repair_' in k:
continue
test_rec[k].append(tmp_rec[k])
# Repair
if not tmp_rec['success'] and 'fcl' not in method:
repair_rec = gradient_free_traj_optimize(robot, fcl_checker.score, start_cfg, target_cfg,
options={**repair_options, 'init_solution': torch.DoubleTensor(tmp_rec['solution'])})
# repair_rec['success'] = validate(repair_rec['solution']) # validation not needed
else:
repair_rec = {
'cnt_check': 0,
'cost': tmp_rec['cost'],
'time': 0,
'success': tmp_rec['success'],
'solution': tmp_rec['solution'],
}
for k in ['cnt_check', 'cost', 'time', 'success', 'solution']:
test_rec['repair_'+k].append(repair_rec[k])
# ================Visualize for debugging purposes===================
# cfg_path_plots = []
# if robot.dof > 2:
# fig, ax, link_plot, joint_plot, eff_plot = create_plots(robot, obstacles, dist_est, checker)
# elif robot.dof == 2:
# fig, ax, link_plot, joint_plot, eff_plot, cfg_path_plots = create_plots(robot, obstacles, dist_est, checker)
# single_plot(robot, torch.FloatTensor(test_rec['repair_solution'][-1]), fig, link_plot, joint_plot, eff_plot, cfg_path_plots=cfg_path_plots, ax=ax)
# debug_dir = join('debug', exp_name, method)
# if not isdir(debug_dir):
# os.makedirs(debug_dir)
# plt.savefig(join(debug_dir, 'debug_view_{}.png'.format(test_it)), dpi=500)
# plt.close()
# break # debugging
return test_rec
def main(checking_method='diffco'):
DOF = 2
env_name = '1rect_active' # '2rect' # '1rect_1circle' '1rect' 'narrow' '2instance'
dataset = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name))
cfgs = dataset['data'].double()
labels = dataset['label'].reshape(-1, 1).double() #.max(1).values
dists = dataset['dist'].reshape(-1, 1).double() #.max(1).values
obstacles = dataset['obs']
obstacles = [list(o) for o in obstacles]
robot = dataset['robot'](*dataset['rparam'])
width = robot.link_width
#=================================================================================================================================
fcl_obs = [FCLObstacle(*param) for param in obstacles]
fcl_collision_obj = [fobs.cobj for fobs in fcl_obs]
label_type = 'binary'
num_class = 1
T = 11
nu = 5 #5
kai = 1500
sigma = 0.3
seed = 1918
torch.manual_seed(seed)
np.random.seed(seed)
num_init_points = 8000
if label_type == 'binary':
obs_managers = [fcl.DynamicAABBTreeCollisionManager()]
obs_managers[0].registerObjects(fcl_collision_obj)
obs_managers[0].setup()
elif label_type == 'instance':
obs_managers = [fcl.DynamicAABBTreeCollisionManager() for _ in fcl_obs]
for mng, cobj in zip(obs_managers, fcl_collision_obj):
mng.registerObjects([cobj])
elif label_type == 'class':
obs_managers = [
fcl.DynamicAABBTreeCollisionManager() for _ in range(num_class)
]
obj_by_cls = [[] for _ in range(num_class)]
for obj in fcl_obs:
obj_by_cls[obj.category].append(obj.cobj)
for mng, obj_group in zip(obs_managers, obj_by_cls):
mng.registerObjects(obj_group)
robot_links = robot.update_polygons(cfgs[0])
robot_manager = fcl.DynamicAABBTreeCollisionManager()
robot_manager.registerObjects(robot_links)
robot_manager.setup()
for mng in obs_managers:
mng.setup()
gt_checker = FCLChecker(obstacles, robot, robot_manager, obs_managers)
train_num = 6000
fkine = robot.fkine
# checker = DiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0)
from time import time
init_train_t = time()
checker = MultiDiffCo(obstacles,
kernel_func=kernel.FKKernel(fkine,
kernel.RQKernel(10)),
beta=1.0)
labels, dists = gt_checker.predict(cfgs[:train_num], distance=True)
labels = labels.double()
dists = dists.double()
checker.train(cfgs[:train_num],
labels[:train_num],
max_iteration=len(cfgs[:train_num]),
distance=dists[:train_num])
# Check DiffCo test ACC
# test_preds = (checker.score(cfgs[train_num:]) > 0) * 2 - 1
# test_acc = torch.sum(test_preds == labels[train_num:], dtype=torch.float32)/len(test_preds.view(-1))
# test_tpr = torch.sum(test_preds[labels[train_num:]==1] == 1, dtype=torch.float32) / len(test_preds[labels[train_num:]==1])
# test_tnr = torch.sum(test_preds[labels[train_num:]==-1] == -1, dtype=torch.float32) / len(test_preds[labels[train_num:]==-1])
# print('Test acc: {}, TPR {}, TNR {}'.format(test_acc, test_tpr, test_tnr))
# assert(test_acc > 0.9)
fitting_target = 'dist' # {label, dist, hypo}
Epsilon = 0.01
checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon),
target=fitting_target,
fkine=fkine) # epsilon=Epsilon,
# checker.fit_poly(kernel_func=kernel.MultiQuadratic(Epsilon), target=fitting_target, fkine=fkine)
# checker.fit_full_poly(epsilon=Epsilon, target=fitting_target, fkine=fkine, lmbd=10)
dist_est = checker.rbf_score
init_train_t = time() - init_train_t
# dist_est = checker.score
# dist_est = checker.poly_score
print('MIN_SCORE = {:.6f}'.format(dist_est(cfgs[train_num:]).min()))
positions = torch.FloatTensor(np.linspace(obstacles[0][1], [4, 3], T))
start_cfg = torch.zeros(robot.dof,
dtype=cfgs.dtype) # free_cfgs[indices[0]] #
target_cfg = torch.zeros(robot.dof,
dtype=cfgs.dtype) # free_cfgs[indices[1]] #
start_cfg[0] = np.pi / 2 # -np.pi/16
start_cfg[1] = -np.pi / 6
target_cfg[0] = 0 # -np.pi/2 # -15*np.pi/16
target_cfg[1] = np.pi / 7
update_ts = []
plan_ts = []
for t, trans in zip(range(T), positions):
ut = time()
fcl_collision_obj[0].setTransform(
fcl.Transform(
# Rotation.from_rotvec([0, 0, angle]).as_quat()[[3,0,1,2]],
[trans[0], trans[1], 0]))
for obs_mng in obs_managers:
obs_mng.update()
# if checking_method == 'diffco':
exploit_samples = torch.randn(nu,
len(checker.gains),
robot.dof,
dtype=checker.support_points.dtype
) * sigma + checker.support_points
exploit_samples = utils.wrap2pi(exploit_samples).reshape(-1, robot.dof)
explore_samples = torch.rand(
kai, robot.dof,
dtype=checker.support_points.dtype) * 2 * np.pi - np.pi
cfgs = torch.cat(
[exploit_samples, explore_samples, checker.support_points])
labels, dists = gt_checker.predict(cfgs, distance=True)
dists = dists.double()
print('Collision {}, Free {}\n'.format((labels == 1).sum(),
(labels == -1).sum()))
gains = torch.cat([
torch.zeros(len(exploit_samples) + len(explore_samples),
checker.num_class,
dtype=checker.gains.dtype), checker.gains
]) #None #
#TODO: bug: not calculating true hypothesis for new points
added_hypothesis = checker.score(cfgs[:-len(checker.support_points)])
hypothesis = torch.cat(
[added_hypothesis, checker.hypothesis]
) # torch.cat([torch.zeros(len(exploit_samples)+len(explore_samples), checker.num_class), checker.hypothesis]) # None #
# kernel_matrix = torch.zeros(len(cfgs), len(cfgs)) #None #
# kernel_matrix[-len(checker.kernel_matrix):, -len(checker.kernel_matrix):] = checker.kernel_matrix
checker.train(cfgs,
labels,
gains=gains,
hypothesis=hypothesis,
distance=dists) #, kernel_matrix=kernel_matrix
print('Num of support points {}'.format(len(checker.support_points)))
checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon),
target=fitting_target,
fkine=fkine,
reg=0.1)
update_ts.append(time() - ut)
if checking_method == 'fcl':
fcl_options = {
'N_WAYPOINTS': 20,
'NUM_RE_TRIALS': 5, # Debugging
'MAXITER': 200,
'seed': seed,
'history': False
}
elif checking_method == 'diffco':
diffco_options = {
'N_WAYPOINTS': 20,
'NUM_RE_TRIALS': 5, # Debugging
'MAXITER': 200,
'safety_margin': -0.5, #max(1/5*min_score, -0.5),
'seed': seed,
'history': False
}
print('t = {}'.format(t))
if t % 1 == 0 and not torch.any(
checker.predict(torch.stack([start_cfg, target_cfg], dim=0)) ==
1):
obstacles[0][1] = (trans[0], trans[1])
cfg_path_plots = []
if robot.dof > 2:
fig, ax, link_plot, joint_plot, eff_plot = create_plots(
robot, obstacles, dist_est, checker)
elif robot.dof == 2:
fig, ax, link_plot, joint_plot, eff_plot, cfg_path_plots = create_plots(
robot, obstacles, dist_est, checker)
ot = time()
# Begin optimization==========
if checking_method == 'diffco':
# p, path_history, num_trial, num_step = traj_optimize(
# robot, dist_est, start_cfg, target_cfg, history=False)
solution_rec = givengrad_traj_optimize(robot,
dist_est,
start_cfg,
target_cfg,
options=diffco_options)
p = torch.FloatTensor(solution_rec['solution'])
elif checking_method == 'fcl':
solution_rec = gradient_free_traj_optimize(
robot,
lambda cfg: gt_checker.predict(cfg, distance=False),
start_cfg,
target_cfg,
options=fcl_options)
p = torch.FloatTensor(solution_rec['solution'])
# ============================
plan_ts.append(time() - ot)
# path_dir = 'results/active_learning/path_2d_{}dof_{}_seed{}_step{:02d}.json'.format(robot.dof, env_name, seed, t) # DEBUGGING
path_dir = 'results/active_learning/path_2d_{}dof_{}_checker={}_seed{}_step{:02d}.json'.format(
robot.dof, env_name, checking_method, seed, t)
with open(path_dir, 'w') as f:
json.dump({
'path': p.data.numpy().tolist(),
}, f, indent=1)
print('Plan recorded in {}'.format(f.name))
# Use saved path ======
# if not isfile(path_dir):
# continue
# with open(path_dir, 'r') as f:
# path_dict = json.load(f)
# p = torch.FloatTensor(path_dict['path'])
# =====================
p = utils.make_continue(p)
#animation
# vid_name = None #'results/maual_trajopt_2d_{}dof_{}_fitting_{}_eps_{}_dif_{}_updates_{}_steps_{}.mp4'.format(
# # robot.dof, env_name, fitting_target, Epsilon, dif_weight, UPDATE_STEPS, N_STEPS)
# if robot.dof == 2:
# animation_demo(
# robot, p, fig, link_plot, joint_plot, eff_plot,
# cfg_path_plots=cfg_path_plots, path_history=path_history, save_dir=vid_name)
# elif robot.dof == 7:
# animation_demo(robot, p, fig, link_plot, joint_plot, eff_plot, save_dir=vid_name)
# single shot
single_plot(robot,
p,
fig,
link_plot,
joint_plot,
eff_plot,
cfg_path_plots=cfg_path_plots,
ax=ax)
# plt.show()
# plt.savefig('figs/path_2d_{}dof_{}.png'.format(robot.dof, env_name), dpi=500)
# plt.savefig('figs/active_2d_{}dof_{}_{}'.format(robot.dof, env_name, t), dpi=500)
fig_dir = 'figs/active/{random_seed}/{checking_method}'.format(
random_seed=seed, checking_method=checking_method)
if not isdir(fig_dir):
makedirs(fig_dir)
plt.savefig(join(
fig_dir,
'2d_{DOF}dof_{ename}_{checking_method}_{step:02d}'.format(
DOF=robot.dof,
ename=env_name,
checking_method=checking_method,
step=t)),
dpi=300)
print('{} summary'.format(checking_method))
print('Initial training {} sec.'.format(init_train_t))
print('Update {} sec.'.format(update_ts))
print('Planning {} sec.'.format(plan_ts))
def main():
DOF = 2
env_name = '2instance'
dataset = torch.load('data/2d_{}dof_{}.pt'.format(DOF, env_name))
cfgs = dataset['data']
labels = dataset['label']
dists = dataset['dist']
obstacles = dataset['obs']
obstacles = [obs + (i, ) for i, obs in enumerate(obstacles)]
print(obstacles)
robot = dataset['robot'](*dataset['rparam'])
width = robot.link_width
train_num = 6000
fkine = robot.fkine
# checker = DiffCo(obstacles, kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)), beta=1.0)
checker = MultiDiffCo(obstacles,
kernel_func=kernel.FKKernel(fkine,
kernel.RQKernel(10)),
beta=1.0)
checker.train(cfgs[:train_num],
labels[:train_num],
max_iteration=len(cfgs[:train_num]),
distance=dists[:train_num])
# Check DiffCo test ACC
test_preds = (checker.score(cfgs[train_num:]) > 0) * 2 - 1
test_acc = torch.sum(test_preds == labels[train_num:],
dtype=torch.float32) / len(test_preds.view(-1))
test_tpr = torch.sum(test_preds[labels[train_num:] == 1] == 1,
dtype=torch.float32) / len(
test_preds[labels[train_num:] == 1])
test_tnr = torch.sum(test_preds[labels[train_num:] == -1] == -1,
dtype=torch.float32) / len(
test_preds[labels[train_num:] == -1])
print('Test acc: {}, TPR {}, TNR {}'.format(test_acc, test_tpr, test_tnr))
assert (test_acc > 0.9)
fitting_target = 'label' # {label, dist, hypo}
Epsilon = 1 #0.01
checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon),
target=fitting_target,
fkine=fkine) #, reg=0.09) # epsilon=Epsilon,
dist_est = checker.rbf_score
print('MIN_SCORE = {:.6f}'.format(dist_est(cfgs[train_num:]).min()))
# return # DEBUGGING
cfg_path_plots = []
if robot.dof > 2:
fig, ax, link_plot, joint_plot, eff_plot = create_plots(
robot, obstacles, dist_est, checker)
elif robot.dof == 2:
fig, ax, link_plot, joint_plot, eff_plot, cfg_path_plots = create_plots(
robot, obstacles, dist_est, checker)
# Begin optimization
# free_cfgs = cfgs[labels == -1]
# indices = torch.randint(0, len(free_cfgs), (2, ))
# while indices[0] == indices[1]:
# indices = torch.randint(0, len(free_cfgs), (2, ))
start_cfg = torch.zeros(robot.dof,
dtype=torch.float32) # free_cfgs[indices[0]] #
target_cfg = torch.zeros(robot.dof,
dtype=torch.float32) # free_cfgs[indices[1]] #
start_cfg[0] = -np.pi / 4 #np.pi/2 #-np.pi/16 # # # #0 # # #
# start_cfg[1] = -np.pi/6
target_cfg[0] = 3 * np.pi / 4 #-np.pi/2 # -15*np.pi/16 # # # #np.pi# # #
# target_cfg[1] = np.pi/5
## This is for doing traj optimization
p, path_history, num_trial, num_step = traj_optimize(robot,
dist_est,
start_cfg,
target_cfg,
history=True)
with open('results/path_2d_{}dof_{}.json'.format(robot.dof, env_name),
'w') as f:
json.dump(
{
'path': p.data.numpy().tolist(),
'path_history':
[tmp.data.numpy().tolist() for tmp in path_history],
'trial': num_trial,
'step': num_step
},
f,
indent=1)
print('Plan recorded in {}'.format(f.name))
## This for doing the escaping-from-collision experiment
# p = escape(robot, dist_est, start_cfg)
# with open('results/esc_2d_{}dof_{}.json'.format(robot.dof, env_name), 'w') as f:
# json.dump({'path': p.data.numpy().tolist(), },f, indent=1)
# print('Plan recorded in {}'.format(f.name))
## This is for loading previously computed trajectory
# with open('results/path_2d_{}dof_{}.json'.format(robot.dof, env_name), 'r') as f:
# path_dict = json.load(f)
# p = torch.FloatTensor(path_dict['path'])
# path_history = [torch.FloatTensor(shot) for shot in path_dict['path_history']] #[p] #
## This produces an animation for the trajectory
# vid_name = None #'results/maual_trajopt_2d_{}dof_{}_fitting_{}_eps_{}_dif_{}_updates_{}_steps_{}.mp4'.format(
# # robot.dof, env_name, fitting_target, Epsilon, dif_weight, UPDATE_STEPS, N_STEPS)
# if robot.dof == 2:
# animation_demo(
# robot, p, fig, link_plot, joint_plot, eff_plot,
# cfg_path_plots=cfg_path_plots, path_history=path_history, save_dir=vid_name)
# elif robot.dof == 7:
# animation_demo(robot, p, fig, link_plot, joint_plot, eff_plot, save_dir=vid_name)
# (Recommended) This produces a single shot of the planned trajectory
single_plot(robot,
p,
fig,
link_plot,
joint_plot,
eff_plot,
cfg_path_plots=cfg_path_plots,
ax=ax)
# plt.show()
# plt.savefig('figs/path_2d_{}dof_{}.png'.format(robot.dof, env_name), dpi=500)
# plt.savefig('figs/2d_{}dof_{}_equalmargin'.format(robot.dof, env_name), dpi=500) #_equalmargin.png
plt.tight_layout()
plt.savefig('figs/opening_contourline.png', dpi=500, bbox_inches='tight')
# obstacles = [
# ('circle', (6, 2), 1.5),
# ('rect', (2, 6), (1.5, 1.5))]
obstacles = [
('circle', (6, 2), 2),
# ('circle', (2, 7), 1),
('rect', (3.5, 6), (2, 1)),
('rect', (4, 7), 1),
('rect', (5, 8), (10, 1)),
('rect', (7.5, 6), (2, 1)),
('rect', (8, 7), 1),
]
obstacles = [Obstacle(*param) for param in obstacles]
# kernel = kernel.CauchyKernel(100)
# k = kernel.TangentKernel(0.8, 0)
k = kernel.RQKernel(5)
checker = DiffCo(obstacles, kernel_func=k, beta=20)
gt_checker = CollisionChecker(obstacles)
np.random.seed(1917)
torch.random.manual_seed(1917)
# checker.initialize(3000)
# checker.train(200000, method='original')
cfgs = torch.rand((3000, 2)) * 10
labels = torch.tensor(gt_checker.predict(cfgs) * 2 - 1)
assert labels.max() == 1 and labels.min() == -1
checker.train(cfgs, labels, method='original')
rbfi = Rbf(checker.support_points[:, 0].numpy(),
checker.support_points[:, 1].numpy(),
def main():
robot_name = 'baxter'
env_name = 'catontable' #'2objontable' #'complex' # 2objontable' # 'table'
dataset = torch.load('data/3d_{}_{}.pt'.format(robot_name, env_name))
cfgs = dataset['data']
cfgs = cfgs.type(torch.float)
labels = dataset['label']
# dists = dataset['dist']
obstacles = dataset['obs']
robot = dataset['robot']()
train_num = int(len(cfgs) * 0.5)
fkine = robot.fkine
# '''
#====
checker = DiffCo(obstacles,
kernel_func=kernel.FKKernel(fkine, kernel.RQKernel(10)),
beta=1.0)
checker.train(cfgs[:train_num],
labels[:train_num],
max_iteration=len(cfgs[:train_num]))
with open('results/checker_3d_{}_{}.p'.format(robot_name, env_name),
'wb') as f:
pickle.dump(checker, f)
print('checker saved: {}'.format(f.name))
#==== The following can be used alone to save training time in debugging
with open('results/checker_3d_{}_{}.p'.format(robot_name, env_name),
'rb') as f:
checker = pickle.load(f)
print('checker loaded: {}'.format(f.name))
# Check DiffCo test ACC
test_preds = (checker.score(cfgs[train_num:]) > 0) * 2 - 1
test_acc = torch.sum(test_preds == labels[train_num:],
dtype=torch.float32) / len(test_preds)
test_tpr = torch.sum(test_preds[labels[train_num:] == 1] == 1,
dtype=torch.float32) / len(
test_preds[labels[train_num:] == 1])
test_tnr = torch.sum(test_preds[labels[train_num:] == -1] == -1,
dtype=torch.float32) / len(
test_preds[labels[train_num:] == -1])
print('Test acc: {}, TPR {}, TNR {}'.format(test_acc, test_tpr, test_tnr))
assert (test_acc > 0.8)
fitting_target = 'label' # {label, dist, hypo}
Epsilon = 0.01
checker.fit_poly(kernel_func=kernel.Polyharmonic(1, Epsilon),
target=fitting_target,
fkine=fkine)
dist_est = checker.rbf_score
MIN_SCORE = dist_est(cfgs[train_num:]).min().item()
print('MIN_SCORE = {}'.format(MIN_SCORE))
free_cfgs = cfgs[labels == -1]
indices = torch.randint(0, len(free_cfgs), (2, ))
while indices[0] == indices[1]:
indices = torch.randint(0, len(free_cfgs), (2, ))
## Setting start and target configurations
# start_cfg = torch.FloatTensor([-39, 40, -111, 81, 4, 29, -136])/180*pi # free_cfgs[indices[0]]
# start_cfg = torch.FloatTensor([-41, 27, -88, 23, -166, 67, 168])/180*pi # 2obj scene, start from between the objs
# torch.FloatTensor([25, 31, -120, 58, -66, -8, 116])/180*pi # medium scene
# torch.FloatTensor([5, 51, -126, 58, -66, -8, 116])/180*pi # complex scene, start below objects
# torch.FloatTensor([-5, 49, -146, 41, -107, -20, 168])/180*pi #2obj scene, start from left side of the objects
start_cfg = torch.FloatTensor([-11, -1, -64, 60, 11, 36, 147
]) / 180 * pi # medium scene
#
# torch.FloatTensor([-27, 34, -92, 34, -174, -50, -19]) /180*pi #start from high-risk (basic scene)
target_cfg = torch.FloatTensor([
13, 31, -88, 16, -160, -27, 169
]) / 180 * pi # 2obj scene, stop beside table
# torch.FloatTensor([-48, 59, -147, 50, -170, -30, 169])/180*pi #complex scene, end below objects; medium scene
# torch.FloatTensor([-22, 28, -87, 45, -170, -30, 169])/180*pi #2obj scene, stop on the right of the objects
#
# torch.FloatTensor([4, 29, -86, 44, 3, 16, -146])/180*pi
## This is for trajectory optimization with or w/o an initial guess
# with open('data/{}_success_1.json'.format(env_name), 'r') as f:
# init_guess = torch.FloatTensor(json.load(f)['path'])
# p, path_history, num_trial, num_step = traj_optimize(robot, start_cfg, target_cfg, dist_est, init_guess, history=True)
# p, path_history, num_trial, num_step = traj_optimize(robot, start_cfg, target_cfg, dist_est, history=True)
# with open('results/path_3d_{}_{}_2.json'.format(robot_name, env_name), 'w') as f:
# json.dump(
# {
# 'path': p.data.numpy().tolist(),
# 'path_history': [tmp.data.numpy().tolist() for tmp in path_history],
# 'trial': num_trial,
# 'step': num_step
# },
# f, indent=1)
# print('Plan recorded in {}'.format(f.name))
## This is for escaping from high-risk region
p = escape(robot, dist_est, start_cfg)
with open('results/path_3d_{}_{}_escape.json'.format(robot_name, env_name),
'w') as f:
json.dump({
'path': p.data.numpy().tolist(),
}, f, indent=1)
print('Plan recorded in {}'.format(f.name))
## This is for loading previous trajectories
# with open('results/path_3d_{}_{}.json'.format(robot_name, env_name), 'r') as f:
# p = torch.FloatTensor(json.load(f)['path'])
# with open('data/medium_success_2.json', 'r') as f:
# p = torch.FloatTensor(json.load(f)['path'])
try:
raw_input = input
except NameError:
pass
try:
print("Press Ctrl-D to exit at any time")
print("")
print("============ Press `Enter` to visualize the path generated")
raw_input()
box_names = []
# box_names, exp = animate_path(p, obstacles)
box_names, exp = single_shot(p, obstacles)
except rospy.ROSInterruptException:
pass
except KeyboardInterrupt:
pass
for box_name in box_names:
exp.scene.remove_world_object(box_name)
print('Tried removing {}!'.format(box_name))
wait_for_state_update(exp.scene,
box_name,
box_is_attached=False,
box_is_known=False)