Beispiel #1
0
 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]
Beispiel #2
0
 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]
Beispiel #3
0
 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]
Beispiel #4
0
    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]
Beispiel #5
0
 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
Beispiel #6
0
 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]
Beispiel #7
0
    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]
Beispiel #8
0
 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
Beispiel #9
0
 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]
Beispiel #10
0
 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
Beispiel #11
0
 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]