def features(self, state, **kwargs):
self.tgt_cld = state.cloud
self.tgt_ctx.set_cld(self.tgt_cld)
self.costs = batch_tps_rpm_bij(self.src_ctx,
self.tgt_ctx,
component_cost=True)
return np.c_[self.costs, self.indicators]
def features(self, state, **kwargs):
self.tgt_cld = state.cloud
self.tgt_ctx.set_cld(self.tgt_cld)
rloc = state.cloud[
0] # the location to set if the left/right gripper doesn't close for an action
self.costs = batch_tps_rpm_bij(
self.src_ctx, self.tgt_ctx,
component_cost=True)[:, :SimpleMulFeats.N_costs]
#ipdb.set_trace()
self.src_ctx.get_unscaled_trajs(self.tgt_ctx)
l_gripper_locs = [
self.src_ctx.l_traj_w[i].get()[self.l_ind[i]]
if self.l_ind[i] >= 0 else rloc for i in range(self.src_ctx.N)
]
l_gripper_dists = np.min(cdist(np.asarray(l_gripper_locs),
np.asarray(state.cloud)),
axis=1)
r_gripper_locs = [
self.src_ctx.r_traj_w[i].get()[self.r_ind[i]]
if self.r_ind[i] >= 0 else rloc for i in range(self.src_ctx.N)
]
r_gripper_dists = np.min(cdist(np.asarray(r_gripper_locs),
np.asarray(state.cloud)),
axis=1)
dist_to_rope = np.max(np.array([r_gripper_dists, l_gripper_dists]),
axis=0)[:, None]
return np.c_[self.costs, self.indicators, dist_to_rope]
def features(self, state, **kwargs):
self.tgt_cld = state.cloud
self.tgt_ctx.set_cld(self.tgt_cld)
costs = batch_tps_rpm_bij(self.src_ctx, self.tgt_ctx, component_cost=True)[:, :SimpleMulFeats.N_costs]
self.costs = np.zeros((self.N, QuadSimpleMulFeats.N_feats))
for i in range(self.N):
self.costs[i, :] = get_quad_terms(costs[i])
return np.c_[self.costs, self.indicators]
def features(self, state, **kwargs):
self.tgt_cld = state.cloud
self.tgt_ctx.set_cld(self.tgt_cld)
self.costs = batch_tps_rpm_bij(self.src_ctx, self.tgt_ctx, component_cost=True)[:, :SimpleMulFeats.N_costs]
for i in range(self.N):
self.regind_feats[i,:] = self.indicators[i]*self.costs[i,0]
return np.c_[self.costs, self.indicators, self.regind_feats]
def features(self, state, **kwargs):
mul_feats = MulFeats.features(self, state)
self.landmark_targ_ctx.set_cld(state.cloud)
landmark_feats = batch_tps_rpm_bij(self.landmark_ctx, self.landmark_targ_ctx)
landmark_feats = np.exp(-landmark_feats)
landmark_feats /= np.sum(landmark_feats)
self.costs = np.c_[mul_feats, np.tile(landmark_feats, (self.src_ctx.N, 1))]
return self.costs
def features(self, state, **kwargs):
self.tgt_cld = state.cloud
self.tgt_ctx.set_cld(self.tgt_cld)
costs = batch_tps_rpm_bij(
self.src_ctx, self.tgt_ctx,
component_cost=True)[:, :SimpleMulFeats.N_costs]
self.costs = np.zeros((self.N, QuadSimpleMulFeats.N_feats))
for i in range(self.N):
self.costs[i, :] = get_quad_terms(costs[i])
return np.c_[self.costs, self.indicators]
def features(self, state, **kwargs):
self.tgt_cld = state.cloud
self.tgt_ctx.set_cld(self.tgt_cld)
self.costs = batch_tps_rpm_bij(
self.src_ctx, self.tgt_ctx,
component_cost=True)[:, :SimpleMulFeats.N_costs]
for i in range(self.N):
self.regind_feats[i, :] = self.indicators[i] * self.costs[i, 0]
return np.c_[self.costs, self.indicators, self.regind_feats]
def features(self, state, **kwargs):
mul_feats = MulFeats.features(self, state)
self.landmark_targ_ctx.set_cld(state.cloud)
landmark_feats = batch_tps_rpm_bij(self.landmark_ctx,
self.landmark_targ_ctx)
landmark_feats = np.exp(-landmark_feats)
landmark_feats /= np.sum(landmark_feats)
self.costs = np.c_[mul_feats,
np.tile(landmark_feats, (self.src_ctx.N, 1))]
return self.costs
def features(self, state, **kwargs):
self.tgt_cld = state.cloud
self.tgt_ctx.set_cld(self.tgt_cld)
rloc = state.cloud[0] # the location to set if the left/right gripper doesn't close for an action
self.costs = batch_tps_rpm_bij(self.src_ctx, self.tgt_ctx, component_cost=True)[:, :SimpleMulFeats.N_costs]
#ipdb.set_trace()
self.src_ctx.get_unscaled_trajs(self.tgt_ctx)
l_gripper_locs = [self.src_ctx.l_traj_w[i].get()[self.l_ind[i]] if self.l_ind[i]>=0 else rloc for i in range(self.src_ctx.N)]
l_gripper_dists = np.min(cdist(np.asarray(l_gripper_locs), np.asarray(state.cloud)), axis=1)
r_gripper_locs = [self.src_ctx.r_traj_w[i].get()[self.r_ind[i]] if self.r_ind[i]>=0 else rloc for i in range(self.src_ctx.N)]
r_gripper_dists = np.min(cdist(np.asarray(r_gripper_locs), np.asarray(state.cloud)), axis=1)
dist_to_rope = np.max(np.array([r_gripper_dists,l_gripper_dists]), axis=0)[:,None]
return np.c_[self.costs, self.indicators, dist_to_rope]
def batch_cost(self, test_scene_state):
if not(self.actionfile):
raise ValueError('No actionfile provided for gpu context')
tgt_ctx = TgtContext(self.src_ctx)
cloud = test_scene_state.cloud
cloud = self._clip_cloud(cloud)
tgt_ctx.set_cld(cloud)
cost_array = batch_tps_rpm_bij(self.src_ctx, tgt_ctx,
T_init=self.rad_init, T_final=self.rad_final,
outlierfrac=self.outlierfrac, outlierprior=self.outlierprior,
outliercutoff=settings.OUTLIER_CUTOFF,
em_iter=self.em_iter,
component_cost=True)
costs = dict(zip(self.src_ctx.seg_names, cost_array))
return costs
def features(self, state, **kwargs):
self.tgt_cld = state.cloud
self.tgt_ctx.set_cld(self.tgt_cld)
self.costs = batch_tps_rpm_bij(self.src_ctx, self.tgt_ctx, component_cost=True)
return np.c_[self.costs, self.indicators]
