def escape(robot, dist_est, start_cfg):
N_WAYPOINTS = 20
UPDATE_STEPS = 200
safety_margin = -0.3
lr = 5e-2
path_history = []
init_path = start_cfg
p = init_path.requires_grad_(True)
opt = torch.optim.Adam([p], lr=lr)
for step in range(N_WAYPOINTS):
if step % 1 == 0:
path_history.append(p.data.clone())
opt.zero_grad()
collision_score = dist_est(p) - safety_margin
loss = collision_score
loss.backward()
opt.step()
p.data = utils.wrap2pi(p.data)
if collision_score <= 1e-4:
break
return torch.stack(path_history, dim=0)
def pre_process(p):
p = torch.DoubleTensor(p).reshape([-1, robot.dof])
p[:] = utils.wrap2pi(p)
p = torch.cat([init_path[:1], p, init_path[-1:]], dim=0)
return p
def gradient_free_traj_optimize(robot, checker, start_cfg, target_cfg, options=None):
N_WAYPOINTS = options['N_WAYPOINTS']
NUM_RE_TRIALS = options['NUM_RE_TRIALS']
MAXITER = options['MAXITER']
seed = options['seed']
torch.manual_seed(seed)
global cnt_check
cnt_check = 0
def pre_process(p):
p = torch.DoubleTensor(p).reshape([-1, robot.dof])
p[:] = utils.wrap2pi(p)
p = torch.cat([init_path[:1], p, init_path[-1:]], dim=0)
return p
def con_max_move(p):
p = pre_process(p)
control_points = robot.fkine(p)
return -torch.clamp_((control_points[1:]-control_points[:-1]).pow(2).sum(dim=2)-1.5**2, min=0).sum().numpy()
def con_collision_free(p):
global cnt_check
p = pre_process(p)
cnt_check += len(p)
return torch.sum(-torch.clamp_(checker(p), min=0)).numpy()
def con_joint_limit(p):
p = pre_process(p)
return -torch.sum(torch.clamp_(robot.limits[:, 0]-p, min=0) + torch.clamp_(p-robot.limits[:, 1], min=0)).numpy()
def cost(p):
p_tensor = pre_process(p)
control_points = robot.fkine(p_tensor)
diff = (control_points[1:]-control_points[:-1]).pow(2).sum()
return diff.numpy()
start_t = time()
success = False
for trial_time in range(NUM_RE_TRIALS):
if trial_time == 0:
if 'init_solution' in options:
assert isinstance(options['init_solution'], torch.Tensor)
init_path = options['init_solution']
else:
init_path = torch.from_numpy(np.linspace(start_cfg, target_cfg, num=N_WAYPOINTS, dtype=np.float64))
else:
init_path = (torch.rand(N_WAYPOINTS, robot.dof, dtype=torch.float64))*np.pi*2-np.pi
init_path[0] = start_cfg
init_path[-1] = target_cfg
res = fmin(cost, init_path[1:-1].reshape(-1).numpy(),
constraints=[
{'fun': con_max_move, 'type': 'ineq'},
{'fun': con_collision_free, 'type': 'ineq'},
{'fun': con_joint_limit, 'type': 'ineq'}
],
options={'maxiter': MAXITER, 'disp': False})
if res.success:
success = True
break
end_t = time()
res.x = res.x.reshape([-1, robot.dof])
res.x = utils.wrap2pi(pre_process(res.x))
rec = {
'start_cfg': start_cfg.numpy().tolist(),
'target_cfg': target_cfg.numpy().tolist(),
'cnt_check': cnt_check,
'cost': res.fun.item(),
'time': end_t - start_t,
'success': success,
'seed': seed,
'solution': res.x.numpy().tolist()
}
return rec
def pre_process(p):
global var_p
p = torch.DoubleTensor(p).reshape([-1, robot.dof])
p[:] = utils.wrap2pi(p)
var_p = torch.cat([init_path[:1], p, init_path[-1:]], dim=0).requires_grad_(True)
return var_p
def adam_traj_optimize(robot, dist_est, start_cfg, target_cfg, options):
N_WAYPOINTS = options['N_WAYPOINTS'] # 20
NUM_RE_TRIALS = options['NUM_RE_TRIALS'] # 10
MAXITER = options['MAXITER'] # 200
history = options['history']
dif_weight = 1 # This should NOT be changed
max_move_weight = 10
collision_weight = 10
joint_limit_weight = 10
safety_margin = options['safety_margin']
lr = 5e-1
seed = options['seed']
torch.manual_seed(seed)
lowest_loss_solution = None
lowest_loss = np.inf
lowest_loss_obj = np.inf
lowest_loss_trial = None
lowest_loss_step = None
best_valid_solution = None
best_valid_obj = np.inf
best_valid_step = None
best_valid_trial = None
trial_histories = []
cnt_check = 0
found = False
start_t = time()
for trial_time in range(NUM_RE_TRIALS):
path_history = []
if trial_time == 0:
if 'init_solution' in options:
assert isinstance(options['init_solution'], torch.Tensor)
init_path = options['init_solution']
else:
init_path = torch.from_numpy(np.linspace(start_cfg, target_cfg, num=N_WAYPOINTS)).double()
else:
init_path = torch.rand((N_WAYPOINTS, robot.dof)).double()
init_path = init_path * (robot.limits[:, 1]-robot.limits[:, 0]) + robot.limits[:, 0]
init_path[0] = start_cfg
init_path[-1] = target_cfg
p = init_path.requires_grad_(True)
opt = torch.optim.Adam([p], lr=lr)
for step in range(MAXITER):
opt.zero_grad()
collision_score = torch.clamp(dist_est(p)-safety_margin, min=0).sum()
cnt_check += len(p) # Counting collision checks
control_points = robot.fkine(p)
max_move_cost = torch.clamp((control_points[1:]-control_points[:-1]).pow(2).sum(dim=2)-1.5**2, min=0).sum()
joint_limit_cost = (
torch.clamp(robot.limits[:, 0]-p, min=0) + torch.clamp(p-robot.limits[:, 1], min=0)).sum()
diff = (control_points[1:]-control_points[:-1]).pow(2).sum()
constraint_loss = collision_weight * collision_score\
+ max_move_weight * max_move_cost + joint_limit_weight * joint_limit_cost
objective_loss = dif_weight * diff
loss = objective_loss + constraint_loss
loss.backward()
p.grad[[0, -1]] = 0.0
opt.step()
p.data = utils.wrap2pi(p.data)
if history:
path_history.append(p.data.clone())
if loss.data.numpy() < lowest_loss:
lowest_loss = loss.data.numpy()
lowest_loss_solution = p.data.clone()
lowest_loss_step = step
lowest_loss_trial = trial_time
lowest_loss_obj = objective_loss.data.numpy()
if constraint_loss <= 1e-2:
if objective_loss.data.numpy() < best_valid_obj:
best_valid_obj = objective_loss.data.numpy()
best_valid_solution = p.data.clone()
best_valid_step = step
best_valid_trial = trial_time
# if constraint_loss <= 1e-2 or step % (MAXITER/5) == 0 or step == MAXITER-1:
# print('Trial {}: Step {}, collision={:.3f}*{:.1f}, max_move={:.3f}*{:.1f}, diff={:.3f}*{:.1f}, Loss={:.3f}'.format(
# trial_time, step,
# collision_score.item(), collision_weight,
# max_move_cost.item(), max_move_weight,
# diff.item(), dif_weight,
# loss.item()))
if constraint_loss <= 1e-2 and torch.norm(p.grad) < 1e-4:
break
trial_histories.append(path_history)
if best_valid_solution is not None:
found = True
break
end_t = time()
if not found:
# print('Did not find a valid solution after {} trials!\
# Giving the lowest cost solution'.format(NUM_RE_TRIALS))
solution = lowest_loss_solution
solution_step = lowest_loss_step
solution_trial = lowest_loss_trial
solution_obj = lowest_loss_obj
else:
solution = best_valid_solution
solution_step = best_valid_step
solution_trial = best_valid_trial
solution_obj = best_valid_obj
path_history = trial_histories[solution_trial] # Could be empty when history = false
if not path_history:
path_history.append(solution)
else:
path_history = path_history[:(solution_step+1)]
rec = {
'start_cfg': start_cfg.numpy().tolist(),
'target_cfg': target_cfg.numpy().tolist(),
'cnt_check': cnt_check,
'cost': solution_obj.item(),
'time': end_t - start_t,
'success': found,
'seed': seed,
'solution': solution.numpy().tolist()
}
return 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 escape(robot, dist_est, start_cfg):
N_WAYPOINTS = 20
# NUM_RE_TRIALS = 10
UPDATE_STEPS = 200
# dif_weight = 1
# max_move_weight = 10
# collision_weight = 10
safety_margin = -0.3 #torch.FloatTensor([-2, -0.2])
lr = 5e-2
# seed = 19961221
# torch.manual_seed(seed)
# lowest_cost_solution = None
# lowest_cost = np.inf
# lowest_cost_trial = None
# lowest_cost_step = None
# best_valid_solution = None
# best_valid_cost = np.inf
# best_valid_step = None
# best_valid_trial = None
# trial_histories = []
# found = False
# p = torch.FloatTensor(np.concatenate([np.linspace(start_cfg, (-np.pi, 0), N_STEPS/2), np.linspace((np.pi, 0), target_cfg, N_STEPS/2)], axis=0)).requires_grad_(True)
# for trial_time in range(NUM_RE_TRIALS):
path_history = []
# if trial_time == 0:
# init_path = torch.from_numpy(np.linspace(start_cfg, target_cfg, num=UPDATE_STEPS))
# else:
# init_path = (torch.rand(N_WAYPOINTS, robot.dof))*np.pi*2-np.pi
init_path = start_cfg
# init_path[-1] = target_cfg
p = init_path.requires_grad_(True)
opt = torch.optim.Adam([p], lr=lr)
# opt = torch.optim.SGD([p], lr=lr, momentum=0.0)
for step in range(N_WAYPOINTS):
if step % 1 == 0:
path_history.append(p.data.clone())
opt.zero_grad()
collision_score = dist_est(p) - safety_margin #, min=0).sum()
# print(torch.clamp(dist_est(p)-safety_margin, min=0).max(dim=0).values.data)
# control_points = robot.fkine(p)
# max_move_cost = torch.clamp((control_points[1:]-control_points[:-1]).pow(2).sum(dim=2)-1.0**2, min=0).sum()
# diff = dif_weight * (control_points[1:]-control_points[:-1]).pow(2).sum()
# np.clip(1.5*float(i)/UPDATE_STEPS, 0, 1)**2 (float(i)/UPDATE_STEPS) *
# torch.clamp(utils.wrap2pi(p[1:]-p[:-1]).abs(), min=0.3).pow(2).sum()
# constraint_loss = collision_weight * collision_score + max_move_weight * max_move_cost
# objective_loss = dif_weight * diff
loss = collision_score #objective_loss + constraint_loss
loss.backward()
# p.grad[[0, -1]] = 0.0
opt.step()
p.data = utils.wrap2pi(p.data)
# if history:
# if loss.data.numpy() < lowest_cost:
# lowest_cost = loss.data.numpy()
# lowest_cost_solution = p.data.clone()
# lowest_cost_step = step
# lowest_cost_trial = trial_time
if collision_score <= 1e-4:
# if objective_loss.data.numpy() < best_valid_cost:
# best_valid_cost = objective_loss.data.numpy()
# best_valid_solution = p.data.clone()
# best_valid_step = step
# best_valid_trial = trial_time
break
# if constraint_loss <= 1e-2 or step % (UPDATE_STEPS/5) == 0 or step == UPDATE_STEPS-1:
# print('Trial {}: Step {}, collision={:.3f}*{:.1f}, max_move={:.3f}*{:.1f}, diff={:.3f}*{:.1f}, Loss={:.3f}'.format(
# trial_time, step,
# collision_score.item(), collision_weight,
# max_move_cost.item(), max_move_weight,
# diff.item(), dif_weight,
# loss.item()))
# trial_histories.append(path_history)
# if best_valid_solution is not None:
# found = True
# break
# if not found:
# print('Did not find a valid solution after {} trials!\
# Giving the lowest cost solution'.format(NUM_RE_TRIALS))
# solution = lowest_cost_solution
# solution_step = lowest_cost_step
# solution_trial = lowest_cost_trial
# else:
# solution = best_valid_solution
# solution_step = best_valid_step
# solution_trial = best_valid_trial
# path_history = trial_histories[solution_trial] # Could be empty when history = false
# if not path_history:
# path_history.append(solution)
# else:
# path_history = path_history[:(solution_step+1)]
return torch.stack(path_history, dim=0) # sum(trial_histories, []),
def traj_optimize(robot, dist_est, start_cfg, target_cfg, history=False):
N_WAYPOINTS = 20
NUM_RE_TRIALS = 5
UPDATE_STEPS = 200
dif_weight = 1
max_move_weight = 10
collision_weight = 10
safety_margin = torch.FloatTensor([0]) #-3, #-1 (shown)
lr = 5e-1
lowest_cost_solution = None
lowest_cost = np.inf
lowest_cost_trial = None
lowest_cost_step = None
best_valid_solution = None
best_valid_cost = np.inf
best_valid_step = None
best_valid_trial = None
trial_histories = []
found = False
# p = torch.FloatTensor(np.concatenate([np.linspace(start_cfg, (-np.pi, 0), N_STEPS/2), np.linspace((np.pi, 0), target_cfg, N_STEPS/2)], axis=0)).requires_grad_(True)
for trial_time in range(NUM_RE_TRIALS):
path_history = []
if trial_time == 0 and False: # Temp
init_path = torch.from_numpy(
np.linspace(start_cfg, target_cfg, num=N_WAYPOINTS))
else:
init_path = (torch.rand(N_WAYPOINTS,
robot.dof)) * np.pi * 2 - np.pi
init_path[0] = start_cfg
init_path[-1] = target_cfg
p = init_path.requires_grad_(True)
opt = torch.optim.Adam([p], lr=lr)
# opt = torch.optim.SGD([p], lr=lr, momentum=0.0)
for step in range(UPDATE_STEPS):
opt.zero_grad()
collision_score = torch.clamp(dist_est(p) - safety_margin,
min=0).sum()
# print(torch.clamp(dist_est(p)-safety_margin, min=0).max(dim=0).values.data)
control_points = robot.fkine(p)
max_move_cost = torch.clamp((control_points[1:, -1:]-control_points[:-1, -1:]).pow(2).sum(dim=2)-1**2, min=0).sum() \
+ torch.clamp((utils.wrap2pi(p[1:]-p[:-1])).pow(2).sum(dim=1)-(5*np.pi/180)**2, min=0).sum()
diff = (control_points[1:] - control_points[:-1]).pow(2).sum()
# np.clip(1.5*float(i)/UPDATE_STEPS, 0, 1)**2 (float(i)/UPDATE_STEPS) *
# torch.clamp(utils.wrap2pi(p[1:]-p[:-1]).abs(), min=0.3).pow(2).sum()
constraint_loss = collision_weight * collision_score + max_move_weight * max_move_cost
objective_loss = dif_weight * diff
loss = objective_loss + constraint_loss
loss.backward()
p.grad[[0, -1]] = 0.0
opt.step()
p.data = utils.wrap2pi(p.data)
if history:
path_history.append(p.data.clone())
# if loss.data.numpy() < lowest_cost:
if constraint_loss.data.numpy() < lowest_cost:
lowest_cost = loss.data.numpy()
lowest_cost_solution = p.data.clone()
lowest_cost_step = step
lowest_cost_trial = trial_time
if constraint_loss <= 1e-2:
if objective_loss.data.numpy() < best_valid_cost:
best_valid_cost = objective_loss.data.numpy()
best_valid_solution = p.data.clone()
best_valid_step = step
best_valid_trial = trial_time
if constraint_loss <= 1e-2 or step % (
UPDATE_STEPS / 5) == 0 or step == UPDATE_STEPS - 1:
print(
'Trial {}: Step {}, collision={:.3f}*{:.1f}, max_move={:.3f}*{:.1f}, diff={:.3f}*{:.1f}, Loss={:.3f}'
.format(trial_time, step,
collision_score.item(), collision_weight,
max_move_cost.item(), max_move_weight, diff.item(),
dif_weight, loss.item()))
trial_histories.append(path_history)
if best_valid_solution is not None:
found = True
break
if not found:
print('Did not find a valid solution after {} trials!\
Giving the lowest cost solution'.format(NUM_RE_TRIALS))
solution = lowest_cost_solution
solution_step = lowest_cost_step
solution_trial = lowest_cost_trial
else:
solution = best_valid_solution
solution_step = best_valid_step
solution_trial = best_valid_trial
path_history = trial_histories[
solution_trial] # Could be empty when history = false
if not path_history:
path_history.append(solution)
else:
path_history = path_history[:(solution_step + 1)]
return solution, path_history, solution_trial, solution_step # sum(trial_histories, []),
def traj_optimize(robot, dist_est, start_cfg, target_cfg, history=False):
# There is a slightly different version in speed_compare.py,
# which allows using SLSQP instead of Adam, allows
# inputting an initial solution other than straight line,
# and is better modularly written.
# That one with SLSQP is more recommended to use.
N_WAYPOINTS = 20
NUM_RE_TRIALS = 10
UPDATE_STEPS = 200
dif_weight = 1
max_move_weight = 10
collision_weight = 10
safety_margin = torch.FloatTensor([-12, -1.2]) #([-8.0, -0.8]) #
lr = 5e-1
seed = 19961221
torch.manual_seed(seed)
lowest_cost_solution = None
lowest_cost = np.inf
lowest_cost_trial = None
lowest_cost_step = None
best_valid_solution = None
best_valid_cost = np.inf
best_valid_step = None
best_valid_trial = None
trial_histories = []
found = False
for trial_time in range(NUM_RE_TRIALS):
path_history = []
if trial_time == 0:
init_path = torch.from_numpy(
np.linspace(start_cfg, target_cfg, num=N_WAYPOINTS))
else:
init_path = (torch.rand(N_WAYPOINTS,
robot.dof)) * np.pi * 2 - np.pi
init_path[0] = start_cfg
init_path[-1] = target_cfg
p = init_path.requires_grad_(True)
opt = torch.optim.Adam([p], lr=lr)
for step in range(UPDATE_STEPS):
opt.zero_grad()
collision_score = torch.clamp(dist_est(p) - safety_margin,
min=0).sum()
control_points = robot.fkine(p)
max_move_cost = torch.clamp(
(control_points[1:] - control_points[:-1]).pow(2).sum(dim=2) -
0.3**2,
min=0).sum()
diff = (control_points[1:] - control_points[:-1]).pow(2).sum()
constraint_loss = collision_weight * collision_score + max_move_weight * max_move_cost
objective_loss = dif_weight * diff
loss = objective_loss + constraint_loss
loss.backward()
p.grad[[0, -1]] = 0.0
opt.step()
p.data = utils.wrap2pi(p.data)
if history:
path_history.append(p.data.clone())
if loss.data.numpy() < lowest_cost:
lowest_cost = loss.data.numpy()
lowest_cost_solution = p.data.clone()
lowest_cost_step = step
lowest_cost_trial = trial_time
if constraint_loss <= 1e-2:
if objective_loss.data.numpy() < best_valid_cost:
best_valid_cost = objective_loss.data.numpy()
best_valid_solution = p.data.clone()
best_valid_step = step
best_valid_trial = trial_time
if constraint_loss <= 1e-2 or step % (
UPDATE_STEPS / 5) == 0 or step == UPDATE_STEPS - 1:
print(
'Trial {}: Step {}, collision={:.3f}*{:.1f}, max_move={:.3f}*{:.1f}, diff={:.3f}*{:.1f}, Loss={:.3f}'
.format(trial_time, step,
collision_score.item(), collision_weight,
max_move_cost.item(), max_move_weight, diff.item(),
dif_weight, loss.item()))
trial_histories.append(path_history)
if best_valid_solution is not None:
found = True
break
if not found:
print('Did not find a valid solution after {} trials!\
Giving the lowest cost solution'.format(NUM_RE_TRIALS))
solution = lowest_cost_solution
solution_step = lowest_cost_step
solution_trial = lowest_cost_trial
else:
solution = best_valid_solution
solution_step = best_valid_step
solution_trial = best_valid_trial
path_history = trial_histories[
solution_trial] # Could be empty when history = false
if not path_history:
path_history.append(solution)
else:
path_history = path_history[:(solution_step + 1)]
return solution, path_history, solution_trial, solution_step
def traj_optimize(robot,
start_cfg,
target_cfg,
dist_est,
initial_guess=None,
history=False):
N_WAYPOINTS = 50
NUM_RE_TRIALS = 1 #10
UPDATE_STEPS = 400
dif_weight = 5
max_move_weight = 10
collision_weight = 10
joint_limit_weight = 10
safety_margin = -1 # Corresponds to safety_bias in paper
lr = 1e-2
seed = 19961221
torch.manual_seed(seed)
lowest_cost_solution = None
lowest_cost = np.inf
lowest_cost_trial = None
lowest_cost_step = None
best_valid_solution = None
best_valid_cost = np.inf
best_valid_step = None
best_valid_trial = None
trial_histories = []
found = False
for trial_time in range(NUM_RE_TRIALS):
path_history = []
if trial_time == 0:
if initial_guess is None:
init_path = torch.from_numpy(
np.linspace(start_cfg, target_cfg, num=N_WAYPOINTS))
else:
init_path = initial_guess
else:
init_path = (torch.rand(N_WAYPOINTS,
robot.dof)) * np.pi * 2 - np.pi
init_path[0] = start_cfg
init_path[-1] = target_cfg
p = init_path.requires_grad_(True)
opt = torch.optim.Adam([p], lr=lr)
for step in range(UPDATE_STEPS):
opt.zero_grad()
assert p.dtype == torch.float
collision_score = torch.clamp(dist_est(p) - safety_margin,
min=0).sum()
control_points = robot.fkine(p, reuse=True)
max_move_cost = torch.clamp(
(control_points[1:] - control_points[:-1]).pow(2).sum(dim=2) -
0.03**2,
min=0).sum()
joint_limit_cost = (
torch.clamp(robot.limits[:, 0] - p, min=0) +
torch.clamp(p - robot.limits[:, 1], min=0)).sum()
diff = (control_points[1:] - control_points[:-1]).pow(2).sum() + (
utils.wrap2pi(p[1:] - p[:-1])).pow(2).sum()
constraint_loss = collision_weight * collision_score \
+ max_move_weight * max_move_cost + joint_limit_weight * joint_limit_cost
opt_loss = dif_weight * diff
loss = constraint_loss + opt_loss
loss.backward()
p.grad[[0, -1]] = 0.0 # Do not change start and goal configuration
opt.step()
p.data = utils.wrap2pi(
p.data) # Do not wrap if your configuration is not angular
if history:
path_history.append(p.data.clone())
if loss.data.numpy() < lowest_cost:
lowest_cost = loss.data.numpy()
lowest_cost_solution = p.data.clone()
lowest_cost_step = step
lowest_cost_trial = trial_time
if constraint_loss <= 1e-2:
if opt_loss.data.numpy() < best_valid_cost:
best_valid_cost = opt_loss.data.numpy()
best_valid_solution = p.data.clone()
best_valid_step = step
best_valid_trial = trial_time
if constraint_loss <= 1e-2 or step % (
UPDATE_STEPS / 5) == 0 or step == UPDATE_STEPS - 1:
print(
'Trial {}: Step {}, collision={:.3f}*{:.1f}, max_move={:.3f}*{:.1f}, joint_limit={:.3f}*{:.1f}, diff={:.3f}*{:.1f}, Loss={:.3f}'
.format(trial_time, step,
collision_score.item(), collision_weight,
max_move_cost.item(), max_move_weight,
joint_limit_cost.item(), joint_limit_weight,
diff.item(), dif_weight, loss.item()))
trial_histories.append(path_history)
if best_valid_solution is not None:
found = True
break
if not found:
print('Did not find a valid solution after {} trials!\
Giving the lowest cost solution'.format(NUM_RE_TRIALS))
solution = lowest_cost_solution
solution_step = lowest_cost_step
solution_trial = lowest_cost_trial
else:
solution = best_valid_solution
solution_step = best_valid_step
solution_trial = best_valid_trial
path_history = trial_histories[
solution_trial] # Could be empty when history = false
if not path_history:
path_history.append(solution)
else:
path_history = path_history[:(solution_step + 1)]
return solution, path_history, solution_trial, solution_step