def sample(self, size=1): """ Sample random variables from the model. Parameters ---------- size : int number of sample to take Returns ------- :obj:`list` of :obj:`ParallelJawPtGrasp3D` sampled grasps in various poses """ samples = [] for i in range(size): # sample random pose xi = self.r_xi_rv_.rvs(size=1) S_xi = skew(xi) axis_sigma = self.R_sample_sigma_.T.dot(self.grasp_.axis) v = self.R_sample_sigma_.dot(scipy.linalg.expm(S_xi).dot(axis_sigma)) t = self.R_sample_sigma_.dot(self.t_rv_.rvs(size=1).T).T open_width = max(self.open_width_rv_.rvs(size=1), 0) close_width = max(self.close_width_rv_.rvs(size=1), 0) approach = self.approach_rv_.rvs(size=1) # transform grasp by pose grasp_sample = ParallelJawPtGrasp3D(ParallelJawPtGrasp3D.configuration_from_params(t, v, open_width, approach, self.grasp_.jaw_width, close_width)) samples.append(grasp_sample) if size == 1: return samples[0] return samples
def sample(self, size=1): """ Sample rigid transform random variables. Parameters ---------- size : int number of sample to take Returns ------- :obj:`list` of :obj:`RigidTransform` sampled rigid transformations """ samples = [] for i in range(size): # sample random pose xi = self._r_xi_rv.rvs(size=1) S_xi = skew(xi) R_sample = scipy.linalg.expm(S_xi) t_sample = self._t_rv.rvs(size=1) samples.append( RigidTransform(rotation=R_sample, translation=t_sample, from_frame=self._from_frame, to_frame=self._to_frame)) # not a list if only 1 sample if size == 1 and len(samples) > 0: return samples[0] return samples
def sample(self, size=1): """ Sample random variables from the model. Parameters ---------- size : int number of sample to take Returns ------- :obj:`list` of :obj:`GraspableObject3D` sampled graspable objects from the pose random variable """ samples = [] for i in range(size): num_consecutive_failures = 0 prev_len = len(samples) while len(samples) == prev_len: try: # sample random pose obj_copy = copy.deepcopy(self.obj_) xi = self.r_xi_rv_.rvs(size=1) S_xi = skew(xi) R = self.R_sample_sigma_.dot( scipy.linalg.expm(S_xi).dot( self.R_sample_sigma_.T.dot( self.mean_T_obj_world_.rotation))) s = max(self.s_rv_.rvs(size=1)[0], 0) t = self.R_sample_sigma_.dot(self.t_rv_.rvs(size=1).T).T z = self.R_sample_sigma_.dot(self.com_rv_.rvs(size=1)) sample_tf = SimilarityTransform(rotation=R.T, translation=t, scale=s) z_tf = sample_tf * Point(z, frame=sample_tf.from_frame) z_tf = z_tf.data # transform object by pose obj_sample = obj_copy.transform(sample_tf) obj_sample.mesh.center_of_mass = z_tf samples.append(obj_sample) except Exception as e: num_consecutive_failures += 1 if num_consecutive_failures > 3: raise # not a list if only 1 sample if size == 1 and len(samples) > 0: return samples[0] return samples