def exercise_1_1(B): M = sp.copy(B) M[:, 0] = 2 * M[:, 0] # double column 1 M[2] = 1 / 2 * M[2] # halve row 3 M[0] = M[2] + M[0] # add row 3 to row 1 M[:, [0, 3]] = M[:, [3, 0]] # interchange columns 1 and 4 M[[0, 2, 3]] = M[[0, 2, 3]] - M[1] # subtract row 2 from each of the other rows M[:, 3] = M[:, 2] # replace column 4 by column 3 M[:, 0] = 0 # delete column 1 O1 = sp.matrix([[2, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) O2 = sp.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1 / 2, 0], [0, 0, 0, 1]]) O3 = sp.matrix([[1, 0, 1, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) O4 = sp.matrix([[0, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0], [1, 0, 0, 0]]) O5 = sp.matrix([[1, -1, 0, 0], [0, 1, 0, 0], [0, -1, 1, 0], [0, -1, 0, 1]]) O6 = sp.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 0]]) O7 = sp.matrix([[0, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) A = sp.matrix([[1, -1, 1 / 2, 0], [0, 1, 0, 0], [0, -1, 1 / 2, 0], [0, -1, 0, 1]]) C = sp.matrix([[0, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 0]]) print(M) print(O5 * O3 * O2 * B * O1 * O4 * O6 * O7) print(A * B * C) print( sp.array_equal(M, O5 * O3 * O2 * B * O1 * O4 * O6 * O7) and sp.array_equal(M, A * B * C))
def kmeans(feat_mat, freqs, c1=-1, c2=-1, min_size=50, max_iter=20, spherical=True): if c1 == -1: c1, c2 = sp.random.randint(feat_mat.shape[0]), sp.random.randint( 1, feat_mat.shape[0]) c1, c2 = feat_mat[c1], feat_mat[(c1 + c2) % feat_mat.shape[0]] old_indexer = sp.ones(feat_mat.shape[0]) * -1 for _ in range(max_iter): scores = sp.squeeze(sp.asarray(feat_mat.multiply(c1 - c2).sum(1))) if freqs is None: indexer = get_split_wo_freq(scores=scores, min_size=min_size) else: indexer = get_split_w_freq(scores=scores, min_size=min_size, freqs=freqs) if sp.array_equal(indexer, old_indexer): break old_indexer = indexer c1 = feat_mat[indexer].sum(0) c2 = feat_mat[~indexer].sum(0) if spherical: c1 = normalize(c1) c2 = normalize(c2) return indexer
def _run_once(self): if self._ran_once: return try: self._h5 = h5py.File(self.filename, "r") except IOError as e: raise IOError("Missing or unopenable file '{0}' -- Native error message: {1}".format(self.filename,e)) row_key,col_key,val_key,row_property_key,col_property_key = self._find_vocab() self._row = PstData._fixup_input(self._h5[row_key]) self._col = PstData._fixup_input(self._h5[col_key]) if self._row.dtype == self._col.dtype and np.array_equal(self._row,self._col): #If it's square, mark it so by making the col and row the same object self._row = self._col self._row_property = PstData._fixup_input(self._h5[row_property_key] if row_property_key else None,count=len(self._row)) #Extra "if ... else" for backwards compatibility. self._col_property = PstData._fixup_input(self._h5[col_property_key],count=len(self._col)) self.val_in_file = self._h5[val_key] self.is_col_major = None if "col-major" in self.val_in_file.attrs: self.is_col_major = self.val_in_file.attrs["col-major"] elif "SNP-major" in self.val_in_file.attrs: self.is_col_major = self.val_in_file.attrs["SNP-major"] assert self.is_col_major is not None, "In Hdf5 the 'val' matrix must have a Boolean 'col-major' (or 'SNP-major') attribute" S_original = len(self._col) N_original = len(self._row) if self.is_col_major: if not self.val_in_file.shape == (S_original, N_original) : raise Exception("In Hdf5, the val matrix dimensions don't match those of 'row' and 'col'") else: if not self.val_in_file.shape == (N_original, S_original) : raise Exception("In Hdf5, the val matrix dimensions don't match those of 'row' and 'col'") self._ran_once = True
def kmeans(points, labels, k, maxiter): """Cluster points into k groups. Return a dictionary mapping each cluster ID (from 0 through k-1) to a list of the labels of the points that belong to that cluster. """ numvecs, numdims = points.shape # initialize means by picking first k vectors means = points[0:k, :] # assign points to nearest mean assignments = assign(points, means) # repeat till convergence (no points change assignments), for maxiter runs for i in range(maxiter): # compute new means from assignments means = scipy.zeros((k, numdims)) for m in range(k): # get indices that have m in assignment indices = scipy.where(assignments == m)[0] if len(indices)==0: # empty cluster # make the mean the mth point means[m, :] = points[m, :] print 'Empty cluster: assigning point to centroid' else: means[m, :] = scipy.mean(points[indices, :], axis=0) # get new assignments new_assignments = assign(points, means) if scipy.array_equal(assignments, new_assignments): print 'Converged in', i+1, 'iterations' break assignments = new_assignments # now assign corresponding words to clusters clusters = defaultdict(list) for i, c in enumerate(assignments): clusters[c].append(labels[i]) return clusters
def kmeans(points, labels, k, maxiter): """Cluster points into k groups. Return a dictionary mapping each cluster ID (from 0 through k-1) to a list of the labels of the points that belong to that cluster. """ numvecs, numdims = points.shape # initialize means by picking first k vectors means = points[0:k, :] # assign points to nearest mean assignments = assign(points, means) # repeat till convergence (no points change assignments), for maxiter runs for i in range(maxiter): # compute new means from assignments means = scipy.zeros((k, numdims)) for m in range(k): # get indices that have m in assignment indices = scipy.where(assignments == m)[0] if len(indices) == 0: # empty cluster # make the mean the mth point means[m, :] = points[m, :] print 'Empty cluster: assigning point to centroid' else: means[m, :] = scipy.mean(points[indices, :], axis=0) # get new assignments new_assignments = assign(points, means) if scipy.array_equal(assignments, new_assignments): print 'Converged in', i + 1, 'iterations' break assignments = new_assignments # now assign corresponding words to clusters clusters = defaultdict(list) for i, c in enumerate(assignments): clusters[c].append(labels[i]) return clusters
def calcProblemActivation(self): """Returns a value of activation for the weighted problem input.""" # calculation of sum of weighted inputs (positive weights) if self.exactInputMatch: if len(self.problemWeights) > 0: return int(np.array_equal(self.problemState, self.problemWeights)) return 0 wInSum = weightedSum(self.problemState, self.problemWeights, norm=True) return wInSum
def calcGoalMismatch(self): """Returns a value of goal state mismatch.""" if len(self.goalState) == 0: return 0 if self.exactInputMatch: self.mismatch = float(np.array_equal(self.goalState, self.goalWeights)) return self.mismatch self.mismatch = weightedSum(self.goalState, self.goalWeights, norm=True) return self.mismatch
def calcProblemActivation(self): """Returns a value of activation for the weighted problem input.""" # calculation of sum of weighted inputs (positive weights) if self.exactInputMatch: if len(self.problemWeights) > 0: return int( np.array_equal(self.problemState, self.problemWeights)) return 0 wInSum = weightedSum(self.problemState, self.problemWeights, norm=True) return wInSum
def update_particle_types(particles: Tissue, startidx: int) -> Tissue: """ Assign the correct type for cell or bound particles. """ types = sp.array(range( particles.num_types)) + startidx # Correct types. if not sp.array_equal(particles.types, types): ntypes = particles.type.map( lambda ptype: types[sp.where(particles.types == ptype)[0][0]]) return particles.assign(type=ntypes) else: return particles
def contains(self, p): #Checks whether a point is on self. #point is inside tri iff ABxAP.unit==BCxBP.unit==CAxCP.unit #Defining points just to make it neater. p1 = self.points[0] p2 = self.points[1] p3 = self.points[2] #Define the 6 vectors AB = sp.array([p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2]]) BC = sp.array([p2[0] - p3[0], p2[1] - p3[1], p2[2] - p3[2]]) CA = sp.array([p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2]]) AP = sp.array([p1[0] - p[0], p1[1] - p[1], p1[2] - p[2]]) BP = sp.array([p2[0] - p[0], p2[1] - p[1], p2[2] - p[2]]) CP = sp.array([p3[0] - p[0], p3[1] - p[1], p3[2] - p[2]]) #Find cross products c1 = unit(sp.cross(AB, AP)) c2 = unit(sp.cross(BC, BP)) c3 = unit(sp.cross(CA, CP)) return sp.array_equal(c1, c2) and sp.array_equal(c1, c3)
def contains(self,p): #Checks whether a point is on self. #point is inside tri iff ABxAP.unit==BCxBP.unit==CAxCP.unit #Defining points just to make it neater. p1 = self.points[0] p2 = self.points[1] p3 = self.points[2] #Define the 6 vectors AB=sp.array([ p1[0]-p2[0], p1[1]-p2[1], p1[2]-p2[2]]) BC=sp.array([ p2[0]-p3[0], p2[1]-p3[1], p2[2]-p3[2]]) CA=sp.array([ p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]]) AP=sp.array([ p1[0]-p[0], p1[1]-p[1], p1[2]-p[2]]) BP=sp.array([ p2[0]-p[0], p2[1]-p[1], p2[2]-p[2]]) CP=sp.array([ p3[0]-p[0], p3[1]-p[1], p3[2]-p[2]]) #Find cross products c1 = unit(sp.cross(AB,AP)) c2 = unit(sp.cross(BC,BP)) c3 = unit(sp.cross(CA,CP)) return sp.array_equal(c1,c2) and sp.array_equal(c1,c3)
def H(G1, G2, k, T): q = (G1 - G2).dot(G1 - G2) if scipy.array_equal(G1, G2): h0 = (hbar * hbar / (2 * m0)) * 2 * 4.72 * (G2 + k).dot(G2 + k) else: h0 = 0 hS = Vfs(q) * cos((G2 - G1).dot(T)) hA = sqrt(-1) * (Vfa(q) * sin((G2 - G1).dot(T))) return h0 + hS + hA
def diag_normal_pdf(x, variance, num_samples): """ Sample from diagonal multivariate normal distribution with mean zero. """ if sp.array_equal(variance, sp.ones(sp.shape(x)[0])): pdf_tmp = sp.exp(-(x**2.0)/2.).T/(2.*sp.pi)**.5 else: var_batch = (sp.ones((sp.shape(x)[0], num_samples)).T*variance).T pdf_tmp = sp.exp(-(x**2.0)/2./var_batch).T/(2.*sp.pi*variance)**.5 return (sp.prod(pdf_tmp, axis=1))
def calcGoalMismatch(self): """Returns a value of goal state mismatch.""" if len(self.goalState) == 0: return 0 if self.exactInputMatch: self.mismatch = float( np.array_equal(self.goalState, self.goalWeights)) return self.mismatch self.mismatch = weightedSum(self.goalState, self.goalWeights, norm=True) return self.mismatch
def compare_attributes(atts1,atts2): self.assertEqual(len(atts1.keys()),len(atts2.keys()),"{}".format(nameRun)) self.assertListEqual(sorted(atts1.keys()),sorted(atts2.keys()),"{}".format(nameRun)) for item in atts1: nequal = True if isinstance(atts1[item],numpy.ndarray): nequal = sp.logical_not(sp.array_equal(atts1[item],atts2[item])) else: nequal = atts1[item]!=atts2[item] if nequal: print("WARNING: {}: not exactly equal, using allclose for {}".format(nameRun,item)) print(atts1[item],atts2[item]) allclose = sp.allclose(atts1[item],atts2[item]) self.assertTrue(allclose,"{}".format(nameRun)) return
def _run_once(self): if self._ran_once: return try: self._h5 = h5py.File(self.filename, "r") except IOError as e: raise IOError( "Missing or unopenable file '{0}' -- Native error message: {1}" .format(self.filename, e)) row_key, col_key, val_key, row_property_key, col_property_key = self._find_vocab( ) self._row = PstData._fixup_input(self._h5[row_key]) self._col = PstData._fixup_input(self._h5[col_key]) if np.array_equal( self._row, self._col ): #If it's square, mark it so by making the col and row the same object self._row = self._col self._row_property = PstData._fixup_input( self._h5[row_property_key] if row_property_key else None, count=len( self._row)) #Extra "if ... else" for backwards compatibility. self._col_property = PstData._fixup_input(self._h5[col_property_key], count=len(self._col)) self.val_in_file = self._h5[val_key] self.is_col_major = None if "col-major" in self.val_in_file.attrs: self.is_col_major = self.val_in_file.attrs["col-major"] elif "SNP-major" in self.val_in_file.attrs: self.is_col_major = self.val_in_file.attrs["SNP-major"] assert self.is_col_major is not None, "In Hdf5 the 'val' matrix must have a Boolean 'col-major' (or 'SNP-major') attribute" S_original = len(self._col) N_original = len(self._row) if self.is_col_major: if not self.val_in_file.shape == (S_original, N_original): raise Exception( "In Hdf5, the val matrix dimensions don't match those of 'row' and 'col'" ) else: if not self.val_in_file.shape == (N_original, S_original): raise Exception( "In Hdf5, the val matrix dimensions don't match those of 'row' and 'col'" ) self._ran_once = True
def compare_fits(self,path1,path2,nameRun=""): print("\n") m = fitsio.FITS(path1) self.assertTrue(os.path.isfile(path2),"{}".format(nameRun)) b = fitsio.FITS(path2) self.assertEqual(len(m),len(b),"{}".format(nameRun)) for i,_ in enumerate(m): ### r_m = m[i].read_header().records() ld_m = [] for el in r_m: name = el['name'] if len(name)>5 and name[:5]=="TTYPE": ld_m += [el['value'].replace(" ","")] ### r_b = b[i].read_header().records() ld_b = [] for el in r_b: name = el['name'] if len(name)>5 and name[:5]=="TTYPE": ld_b += [el['value'].replace(" ","")] self.assertListEqual(ld_m,ld_b,"{}".format(nameRun)) for k in ld_m: d_m = m[i][k][:] d_b = b[i][k][:] if d_m.dtype in ['<U23','S23']: ### for fitsio old version compatibility d_m = sp.char.strip(d_m) if d_b.dtype in ['<U23','S23']: ### for fitsio old version compatibility d_b = sp.char.strip(d_b) self.assertEqual(d_m.size,d_b.size,"{}: Header key is {}".format(nameRun,k)) if not sp.array_equal(d_m,d_b): print("WARNING: {}: Header key is {}, arrays are not exactly equal, using allclose".format(nameRun,k)) diff = d_m-d_b w = d_m!=0. diff[w] = sp.absolute( diff[w]/d_m[w] ) allclose = sp.allclose(d_m,d_b) self.assertTrue(allclose,"{}: Header key is {}, maximum relative difference is {}".format(nameRun,k,diff.max())) m.close() b.close() return
def test_load_success(self): # Load JSON file and ensure project integrity path = Path(os.path.realpath(__file__), '../../../fixtures/JSONGraphFormat') filename = Path(path.resolve(), 'valid.json') project = op.io.JSONGraphFormat.load(filename) assert len(project) == 1 # Ensure overal network properties net = project.network assert net.Np == 2 assert net.Nt == 1 # Ensure existence of pore properties pore_props = { 'pore.index', 'pore.coords', 'pore.diameter', 'pore.area', 'pore.volume' } assert pore_props.issubset(net.props()) # Ensure correctness of pore properties assert sp.array_equal(net['pore.area'], sp.array([0, 0])) assert sp.array_equal(net['pore.index'], sp.array([0, 1])) assert sp.array_equal(net['pore.volume'], sp.array([0, 0])) assert sp.array_equal(net['pore.diameter'], sp.array([0, 0])) assert sp.array_equal(net['pore.coords'][0], sp.array([0, 0, 0])) assert sp.array_equal(net['pore.coords'][1], sp.array([1, 1, 1])) # Ensure existence of throat properties throat_props = { 'throat.length', 'throat.conns', 'throat.diameter', 'throat.area', 'throat.volume', 'throat.perimeter', 'throat.surface_area' } assert throat_props.issubset(net.props()) # Ensure correctness of throat properties length = 1.73205080757 squared_radius = 5.169298742047715 assert net['throat.length'] == length assert net['throat.area'] == sp.pi * squared_radius assert sp.array_equal(net['throat.conns'], sp.array([[0, 1]])) assert net['throat.diameter'] == 2.0 * sp.sqrt(squared_radius) assert net['throat.volume'] == sp.pi * squared_radius * length assert net['throat.perimeter'] == 2.0 * sp.pi * sp.sqrt(squared_radius) assert net['throat.surface_area'] == 2.0 * \ sp.sqrt(squared_radius) * sp.pi * length
def test_load_success(self): # Load JSON file and ensure project integrity path = Path(os.path.realpath(__file__), '../../../fixtures/JSONGraphFormat') filename = Path(path.resolve(), 'valid.json') project = op.io.JSONGraphFormat.load(filename) assert len(project) == 1 # Ensure overal network properties net = project.network assert net.Np == 2 assert net.Nt == 1 # Ensure existence of pore properties pore_props = {'pore.index', 'pore.coords', 'pore.diameter', 'pore.area', 'pore.volume'} assert pore_props.issubset(net.props()) # Ensure correctness of pore properties assert sp.array_equal(net['pore.area'], sp.array([0, 0])) assert sp.array_equal(net['pore.index'], sp.array([0, 1])) assert sp.array_equal(net['pore.volume'], sp.array([0, 0])) assert sp.array_equal(net['pore.diameter'], sp.array([0, 0])) assert sp.array_equal(net['pore.coords'][0], sp.array([0, 0, 0])) assert sp.array_equal(net['pore.coords'][1], sp.array([1, 1, 1])) # Ensure existence of throat properties throat_props = {'throat.length', 'throat.conns', 'throat.diameter', 'throat.area', 'throat.volume', 'throat.perimeter', 'throat.surface_area'} assert throat_props.issubset(net.props()) # Ensure correctness of throat properties length = 1.73205080757 squared_radius = 5.169298742047715 assert net['throat.length'] == length assert net['throat.area'] == sp.pi * squared_radius assert sp.array_equal(net['throat.conns'], sp.array([[0, 1]])) assert net['throat.diameter'] == 2.0 * sp.sqrt(squared_radius) assert net['throat.volume'] == sp.pi * squared_radius * length assert net['throat.perimeter'] == 2.0 * sp.pi * sp.sqrt(squared_radius) assert net['throat.surface_area'] == 2.0 * \ sp.sqrt(squared_radius) * sp.pi * length
def path_to_ufsr_dict(input_path, start_radius=3, max_radius=10, number_min=4, number_of_moments=3): """ Get the ufsr of the different pockets, sorted by ligands, in a neighborhood :param input_path: use the preprocessed pdb :param start_radius: :param max_radius: :param number_min: :param number_of_moments: :return: dict of ligand - array of coordinates """ parser = Bio.PDB.FastMMCIFParser(QUIET=True) name = input_path[-8:-4] structure = parser.get_structure(name, input_path) ligands = find_ligands_residue(structure) output = {} for ligand in ligands: # get the corresponding array and make it a USFR vector binding_pocket = find_spatial_binding_pocket(structure, ligand, start_radius, max_radius, number_min) if not binding_pocket: continue arr = pocket_to_array(binding_pocket) usfr_vector = ufsr.encode(arr, number_of_moments=number_of_moments) # check for duplicate pockets duplicate = False for key in output.keys(): if key.get_resname() == ligand.get_resname(): if np.array_equal(output[key], usfr_vector): duplicate = True break if not duplicate: output[ligand] = usfr_vector return output
def __init__(self, data: pd.DataFrame, family="gaussian", link_function=Link.identity, model="clustering", n_states=2, graph=None, sampler="nuts"): self._data = data self._data = self._data.sort_values([GENE, CONDITION, INTERVENTION]) self._set_link(link_function) self._set_family(family) self._set_data() super().__init__(model=model, n_states=n_states, graph=graph, sampler=sampler) if graph: d_genes = sp.sort(sp.unique(self._data.gene.values)) if not sp.array_equal(d_genes, self.node_labels): raise ValueError("Graph nodes != data genes")
def equivalent(self, e): #Checks if e shares both endpoints with self. return (sp.array_equal(self.a, e.a) and sp.array_equal(self.b, e.b)) or (sp.array_equal( self.a, e.b) and sp.array_equal(self.b, e.a))
def test_cvmodel(): """ ConstanVelocity Transition Model test """ # State related variables state_vec = sp.array([[3.0], [1.0]]) old_timestamp = datetime.datetime.now() timediff = 1 # 1sec new_timestamp = old_timestamp + datetime.timedelta(seconds=timediff) time_interval = new_timestamp - old_timestamp # Model-related components noise_diff_coeff = 0.001 # m/s^2 F = sp.array([[1, timediff], [0, 1]]) Q = sp.array([[sp.power(timediff, 3) / 3, sp.power(timediff, 2) / 2], [sp.power(timediff, 2) / 2, timediff]]) * noise_diff_coeff # Create and a Constant Velocity model object cv = ConstantVelocity(noise_diff_coeff=noise_diff_coeff) # Ensure ```cv.transfer_function(time_interval)``` returns F assert sp.array_equal( F, cv.matrix(timestamp=new_timestamp, time_interval=time_interval)) # Ensure ```cv.covar(time_interval)``` returns Q assert sp.array_equal( Q, cv.covar(timestamp=new_timestamp, time_interval=time_interval)) # Propagate a state vector throught the model # (without noise) new_state_vec_wo_noise = cv.function(state_vec, timestamp=new_timestamp, time_interval=time_interval, noise=0) assert sp.array_equal(new_state_vec_wo_noise, F @ state_vec) # Evaluate the likelihood of the predicted state, given the prior # (without noise) prob = cv.pdf(new_state_vec_wo_noise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert sp.array_equal( prob, multivariate_normal.pdf(new_state_vec_wo_noise.T, mean=sp.array(F @ state_vec).ravel(), cov=Q).T) # Propagate a state vector throught the model # (with internal noise) new_state_vec_w_inoise = cv.function(state_vec, timestamp=new_timestamp, time_interval=time_interval) assert not sp.array_equal(new_state_vec_w_inoise, F @ state_vec) # Evaluate the likelihood of the predicted state, given the prior # (with noise) prob = cv.pdf(new_state_vec_w_inoise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert sp.array_equal( prob, multivariate_normal.pdf(new_state_vec_w_inoise.T, mean=sp.array(F @ state_vec).ravel(), cov=Q).T) # Propagate a state vector throught the model # (with external noise) noise = cv.rvs(timestamp=new_timestamp, time_interval=time_interval) new_state_vec_w_enoise = cv.function(state_vec, timestamp=new_timestamp, time_interval=time_interval, noise=noise) assert sp.array_equal(new_state_vec_w_enoise, F @ state_vec + noise) # Evaluate the likelihood of the predicted state, given the prior # (with noise) prob = cv.pdf(new_state_vec_w_enoise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert sp.array_equal( prob, multivariate_normal.pdf(new_state_vec_w_enoise.T, mean=sp.array(F @ state_vec).ravel(), cov=Q).T)
def compare_values(val1,val2): if not sp.array_equal(val1,val2): print("WARNING: {}: not exactly equal, using allclose".format(nameRun)) allclose = sp.allclose(val1,val2) self.assertTrue(allclose,"{}".format(nameRun)) return
def grasp_callback(my_grasp): my_pose = geometry_msgs.msg.PoseStamped() my_pose.header.stamp = my_grasp.markers[0].header.stamp my_pose.header.frame_id = "/xtion_rgb_optical_frame" pose_target = geometry_msgs.msg.Pose() pose_target.position.x = my_grasp.markers[0].points[0].x pose_target.position.y = my_grasp.markers[0].points[0].y pose_target.position.z = my_grasp.markers[0].points[0].z ## Convert to quaternion u = [1, 0, 0] norm = linalg.norm([ my_grasp.markers[i].points[0].x - my_grasp.markers[0].points[1].x, my_grasp.markers[0].points[0].y - my_grasp.markers[0].points[1].y, my_grasp.markers[0].points[0].z - my_grasp.markers[0].points[1].z ]) v = asarray([ my_grasp.markers[0].points[0].x - my_grasp.markers[0].points[1].x, my_grasp.markers[0].points[0].y - my_grasp.markers[0].points[1].y, my_grasp.markers[0].points[0].z - my_grasp.markers[0].points[1].z ]) / norm if (array_equal(u, v)): pose_target.orientation.w = 1 pose_target.orientation.x = 0 pose_target.orientation.y = 0 pose_target.orientation.z = 0 elif (array_equal(u, negative(v))): pose_target.orientation.w = 0 pose_target.orientation.x = 0 pose_target.orientation.y = 0 pose_target.orientation.z = 1 else: half = [u[0] + v[0], u[1] + v[1], u[2] + v[2]] pose_target.orientation.w = dot(u, half) temp = cross(u, half) pose_target.orientation.x = temp[0] pose_target.orientation.y = temp[1] pose_target.orientation.z = temp[2] norm = math.sqrt(pose_target.orientation.x * pose_target.orientation.x + pose_target.orientation.y * pose_target.orientation.y + pose_target.orientation.z * pose_target.orientation.z + pose_target.orientation.w * pose_target.orientation.w) if norm == 0: norm = 1 my_pose.pose.orientation.x = pose_target.orientation.x / norm my_pose.pose.orientation.y = pose_target.orientation.y / norm my_pose_.pose.orientation.z = pose_target.orientation.z / norm my_pose.pose.orientation.w = pose_target.orientation.w / norm pose_target_trans = geometry_msgs.msg.PoseStamped() pose_target_trans.header.stamp = pose_target.header.stamp pose_target_trans.header.frame_id = "/map" now = rospy.Time.now() listener.waitForTransform("/map", "/xtion_rgb_optical_frame", now, rospy.Duration(1.0)) pose_target_trans = listener.transformPose("/map", pose_target) my_grasp_pub.publish(pose_target_trans)
def base(model, state_vec, noise_diff_coeffs, turn_rate): """ Base test for n-dimensional ConstantAcceleration Transition Models """ # Create an ConstantTurn model object model = model model_obj = model(noise_diff_coeffs=noise_diff_coeffs, turn_rate=turn_rate) # State related variables state_vec = state_vec old_timestamp = datetime.datetime.now() timediff = 1 # 1sec new_timestamp = old_timestamp + datetime.timedelta(seconds=timediff) time_interval = new_timestamp - old_timestamp # Model-related components noise_diff_coeffs = noise_diff_coeffs # m/s^3 turn_rate = turn_rate turn_ratedt = turn_rate * timediff F = sp.array([[ 1, sp.sin(turn_ratedt) / turn_rate, 0, -(1 - sp.cos(turn_ratedt)) / turn_rate ], [0, sp.cos(turn_ratedt), 0, -sp.sin(turn_ratedt)], [ 0, (1 - sp.cos(turn_ratedt)) / turn_rate, 1, sp.sin(turn_ratedt) / turn_rate ], [0, sp.sin(turn_ratedt), 0, sp.cos(turn_ratedt)]]) qx = noise_diff_coeffs[0] qy = noise_diff_coeffs[1] Q = sp.array([[ sp.power(qx, 2) * sp.power(timediff, 3) / 3, sp.power(qx, 2) * sp.power(timediff, 2) / 2, 0, 0 ], [ sp.power(qx, 2) * sp.power(timediff, 2) / 2, sp.power(qx, 2) * timediff, 0, 0 ], [ 0, 0, sp.power(qy, 2) * sp.power(timediff, 3) / 3, sp.power(qy, 2) * sp.power(timediff, 2) / 2 ], [ 0, 0, sp.power(qy, 2) * sp.power(timediff, 2) / 2, sp.power(qy, 2) * timediff ]]) # Ensure ```model_obj.transfer_function(time_interval)``` returns F assert sp.array_equal( F, model_obj.matrix(timestamp=new_timestamp, time_interval=time_interval)) # Ensure ```model_obj.covar(time_interval)``` returns Q assert sp.array_equal( Q, model_obj.covar(timestamp=new_timestamp, time_interval=time_interval)) # Propagate a state vector through the model # (without noise) new_state_vec_wo_noise = model_obj.function(state_vec, noise=0, timestamp=new_timestamp, time_interval=time_interval) assert sp.array_equal(new_state_vec_wo_noise, F @ state_vec) # Evaluate the likelihood of the predicted state, given the prior # (without noise) prob = model_obj.pdf(new_state_vec_wo_noise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert approx(prob) == multivariate_normal.pdf(new_state_vec_wo_noise.T, mean=sp.array( F @ state_vec).ravel(), cov=Q) # Propagate a state vector throughout the model # (with internal noise) new_state_vec_w_inoise = model_obj.function(state_vec, timestamp=new_timestamp, time_interval=time_interval) assert not sp.array_equal(new_state_vec_w_inoise, F @ state_vec) # Evaluate the likelihood of the predicted state, given the prior # (with noise) prob = model_obj.pdf(new_state_vec_w_inoise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert approx(prob) == multivariate_normal.pdf(new_state_vec_w_inoise.T, mean=sp.array( F @ state_vec).ravel(), cov=Q) # Propagate a state vector through the model # (with external noise) noise = model_obj.rvs(timestamp=new_timestamp, time_interval=time_interval) new_state_vec_w_enoise = model_obj.function(state_vec, timestamp=new_timestamp, time_interval=time_interval, noise=noise) assert sp.array_equal(new_state_vec_w_enoise, F @ state_vec + noise) # Evaluate the likelihood of the predicted state, given the prior # (with noise) prob = model_obj.pdf(new_state_vec_w_enoise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert approx(prob) == multivariate_normal.pdf(new_state_vec_w_enoise.T, mean=sp.array( F @ state_vec).ravel(), cov=Q)
class PstHdf5(PstReader): ''' A :class:`.PstReader` for reading \*.pst.hdf5 files from disk. See :class:`.PstReader` for general examples of using PstReaders. The general HDF5 format is described in http://www.hdfgroup.org/HDF5/. The PstHdf5 format stores val, row, col, row_property, and col_property information in Hdf5 format. **Constructor:** :Parameters: * **filename** (*string*) -- The PstHdf5 file to read. :Example: >>> from pysnptools.pstreader import PstHdf5 >>> on_disk = PstHdf5('../examples/toydata.iidmajor.snp.hdf5') # PstHdf5 can load .pst.hdf5, .snp.hdf5, and kernel.hdf5 >>> print on_disk.row_count 500 **Methods beyond** :class:`.PstReader` ''' def __init__(self, filename): super(PstHdf5, self).__init__() #We know PstReader doesn't want the file name self._block_size = 5000 self._ran_once = False self._h5 = None self.filename = filename def __repr__(self): return "{0}('{1}')".format( self.__class__.__name__, self.filename) #!!LATER print non-default values, too def copyinputs(self, copier): copier.input(self.filename) @property def row(self): self._run_once() return self._row @property def col(self): self._run_once() return self._col @property def row_property(self): self._run_once() return self._row_property @property def col_property(self): self._run_once() return self._col_property def _find_vocab(self): vocab_list = [['row', 'col', 'val', 'row_property', 'col_property'], ['iid', 'sid', 'val', None, 'pos'], ['iid', 'rs', 'snps', None, 'pos']] for vocab in vocab_list: if all((key is None or key in self._h5) for key in vocab): return vocab raise Exception("Don't know how to read HDF5 with these keys: " + ",".join(self._h5.iterkeys())) def _run_once(self): if self._ran_once: return try: self._h5 = h5py.File(self.filename, "r") except IOError, e: raise IOError( "Missing or unopenable file '{0}' -- Native error message: {1}" .format(self.filename, e)) row_key, col_key, val_key, row_property_key, col_property_key = self._find_vocab( ) self._row = PstData._fixup_input(self._h5[row_key]) self._col = PstData._fixup_input(self._h5[col_key]) if np.array_equal( self._row, self._col ): #If it's square, mark it so by making the col and row the same object self._row = self._col self._row_property = PstData._fixup_input( self._h5[row_property_key] if row_property_key else None, count=len( self._row)) #Extra "if ... else" for backwards compatibility. self._col_property = PstData._fixup_input(self._h5[col_property_key], count=len(self._col)) self.val_in_file = self._h5[val_key] self.is_col_major = None if "col-major" in self.val_in_file.attrs: self.is_col_major = self.val_in_file.attrs["col-major"] elif "SNP-major" in self.val_in_file.attrs: self.is_col_major = self.val_in_file.attrs["SNP-major"] assert self.is_col_major is not None, "In Hdf5 the 'val' matrix must have a Boolean 'col-major' (or 'SNP-major') attribute" S_original = len(self._col) N_original = len(self._row) if self.is_col_major: if not self.val_in_file.shape == (S_original, N_original): raise Exception( "In Hdf5, the val matrix dimensions don't match those of 'row' and 'col'" ) else: if not self.val_in_file.shape == (N_original, S_original): raise Exception( "In Hdf5, the val matrix dimensions don't match those of 'row' and 'col'" ) self._ran_once = True
def equivalent(self,e): #Checks if e shares both endpoints with self. return (sp.array_equal(self.a,e.a) and sp.array_equal(self.b,e.b)) or (sp.array_equal(self.a,e.b) and sp.array_equal(self.b,e.a))
def isideal(segment): # BS(2, 1): [[0, 1, 0], [1, 1, 1], [0, 1, 0]] ideal = iterate_structure(BS(2, 1), (min(segment.shape) - 1) // 2) return array_equal(segment, ideal)
# float test flt = 5.0 flt_array = aerotbx.utils.to_ndarray(flt) bflt = aerotbx.utils.from_ndarray(*flt_array) # list test lst = [1, 4, 5] lst_array = aerotbx.utils.to_ndarray(lst) blst = aerotbx.utils.from_ndarray(*lst_array) # tuple test tup = (9,8,7) tup_array = aerotbx.utils.to_ndarray(tup) btup = aerotbx.utils.from_ndarray(*tup_array) # array test arr = sp.array([2,4]) arr_array = aerotbx.utils.to_ndarray(arr) barr = aerotbx.utils.from_ndarray(*arr_array) assert binteg==integ, "function 'from_ndarray' failed on %s" % type(integ) assert bflt==flt, "function 'from_ndarray' failed on %s" % type(flt) assert blst==lst, "function 'from_ndarray' failed on %s" % type(lst) assert btup==tup, "function 'from_ndarray' failed on %s" % type(tup) assert sp.array_equal(arr, barr), "function 'from_ndarray' failed on %s" % type(arr) print("All tests pass.") # passes
def test_oumodel(): """ OrnsteinUhlenbeck Transition Model test """ # State related variables state_vec = sp.array([[3.0], [1.0]]) old_timestamp = datetime.datetime.now() timediff = 1 # 1sec new_timestamp = old_timestamp + datetime.timedelta(seconds=timediff) time_interval = new_timestamp - old_timestamp # Model-related components q = 0.001 # m/s^2 k = 0.1 dt = time_interval.total_seconds() exp_kdt = sp.exp(-k*dt) exp_2kdt = sp.exp(-2*k*dt) F = sp.array([[1, (1 - exp_kdt)/k], [0, exp_kdt]]) q11 = q/k ** 2*(dt - 2/k*(1 - exp_kdt) + 1/(2*k)*(1 - exp_2kdt)) q12 = q/k*((1 - exp_kdt)/k - 1/(2*k)*(1 - exp_2kdt)) q22 = q/(2*k)*(1 - exp_2kdt) Q = sp.array([[q11, q12], [q12, q22]]) # Create and a Constant Velocity model object ou = OrnsteinUhlenbeck(noise_diff_coeff=q, damping_coeff=k) # Ensure ```ou.transfer_function(time_interval)``` returns F assert sp.array_equal(F, ou.matrix( timestamp=new_timestamp, time_interval=time_interval)) # Ensure ```ou.covar(time_interval)``` returns Q assert sp.array_equal(Q, ou.covar( timestamp=new_timestamp, time_interval=time_interval)) # Propagate a state vector throught the model # (without noise) new_state_vec_wo_noise = ou.function( state_vec, timestamp=new_timestamp, time_interval=time_interval, noise=0) assert sp.array_equal(new_state_vec_wo_noise, F @ state_vec) # Evaluate the likelihood of the predicted state, given the prior # (without noise) prob = ou.pdf(new_state_vec_wo_noise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert sp.array_equal(prob, multivariate_normal.pdf( new_state_vec_wo_noise.T, mean=sp.array(F @ state_vec).ravel(), cov=Q).T) # Propagate a state vector throught the model # (with internal noise) new_state_vec_w_inoise = ou.function( state_vec, timestamp=new_timestamp, time_interval=time_interval) assert not sp.array_equal(new_state_vec_w_inoise, F @ state_vec) # Evaluate the likelihood of the predicted state, given the prior # (with noise) prob = ou.pdf(new_state_vec_w_inoise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert sp.array_equal(prob, multivariate_normal.pdf( new_state_vec_w_inoise.T, mean=sp.array(F @ state_vec).ravel(), cov=Q).T) # Propagate a state vector throught the model # (with external noise) noise = ou.rvs(timestamp=new_timestamp, time_interval=time_interval) new_state_vec_w_enoise = ou.function( state_vec, timestamp=new_timestamp, time_interval=time_interval, noise=noise) assert sp.array_equal(new_state_vec_w_enoise, F @ state_vec + noise) # Evaluate the likelihood of the predicted state, given the prior # (with noise) prob = ou.pdf(new_state_vec_w_enoise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert sp.array_equal(prob, multivariate_normal.pdf( new_state_vec_w_enoise.T, mean=sp.array(F @ state_vec).ravel(), cov=Q).T)
import scipy as s def are_matrix_equal(m1, m2): return s.equal(m1, m2).all() # Test matrices multiplication m1 = s.matrix([[1, 2], [3, 4]]) m2 = s.matrix("0 1; 1 0") m3 = s.matrix([[2, 1], [4, 3]]) print "m1 = ", m1 print "m2 = ", m2 print are_matrix_equal(m1*m2, m3) # Check the matrix size m4 = s.matrix([[1,2],[3,4],[5,6]]) height, width = m4.shape print (height, width) == (3,2) print height*width == m4.size # Multiply matrix and vector m1 = s.matrix("1 2; 3 4") x1 = s.array([5,6]) r1 = m1.dot(x1) # works, but returns a matrix a1 = r1.A[0] # now it's a vector print s.array_equal(a1, s.array([17,39]))
Those are the same as python types and may be interchanged. Fixed widths: - int32 Integer (-2147483648 to 2147483647) - uint32 Unsigned integer (0 to 4294967295) - float32 Single precision float: sign bit, 8 bits exponent, 23 bits mantissa - float64 Double precision float: sign bit, 11 bits exponent, 52 bits mantissa - complex64 Complex number, represented by two 32-bit floats (real and imaginary components) Those have fixed width on all systems, and may not be compatible with the python types. """ assert sp.array_equal( sp.array([1, 2, 3], dtype = sp.int_), sp.int_([1, 2, 3]) ) # Different types evaluate to equal sp.arrays assert sp.array_equal( sp.array([1, 2, 3], dtype = sp.int_ ), sp.array([1, 2, 3], dtype = sp.float_) ) # Get type v = sp.array([1,2], dtype = sp.int32) assert v.dtype == sp.int32 # Subtype:
def __eq__(self, e): return sp.array_equal(e.a, self.a) and sp.array_equal(e.b, self.b)
def base(state_vec, noise_diff_coeffs): """ Base test for n-dimensional ConstantAcceleration Transition Models """ # Create a 1D ConstantAcceleration or an n-dimensional # CombinedLinearGaussianTransitionModel object dim = len(state_vec) // 3 # pos, vel, acc for each dimension if dim == 1: model_obj = ConstantAcceleration(noise_diff_coeff=noise_diff_coeffs[0]) else: model_list = [ ConstantAcceleration(noise_diff_coeff=noise_diff_coeffs[i]) for i in range(0, dim) ] model_obj = CombinedLinearGaussianTransitionModel(model_list) # State related variables state_vec = state_vec old_timestamp = datetime.datetime.now() timediff = 1 # 1sec new_timestamp = old_timestamp + datetime.timedelta(seconds=timediff) time_interval = new_timestamp - old_timestamp # Model-related components noise_diff_coeffs = noise_diff_coeffs # m/s^3 base_mat = sp.array([[1, timediff, sp.power(timediff, 2) / 2], [0, 1, timediff], [0, 0, 1]]) mat_list = [base_mat for num in range(0, dim)] F = sp.linalg.block_diag(*mat_list) base_covar = sp.array( [[ sp.power(timediff, 5) / 20, sp.power(timediff, 4) / 8, sp.power(timediff, 3) / 6 ], [ sp.power(timediff, 4) / 8, sp.power(timediff, 3) / 3, sp.power(timediff, 2) / 2 ], [sp.power(timediff, 3) / 6, sp.power(timediff, 2) / 2, timediff]]) covar_list = [ base_covar * sp.power(noise_diff_coeffs[i], 2) for i in range(0, dim) ] Q = sp.linalg.block_diag(*covar_list) # Ensure ```model_obj.transfer_function(time_interval)``` returns F assert sp.array_equal( F, model_obj.matrix(timestamp=new_timestamp, time_interval=time_interval)) # Ensure ```model_obj.covar(time_interval)``` returns Q assert sp.array_equal( Q, model_obj.covar(timestamp=new_timestamp, time_interval=time_interval)) # Propagate a state vector throught the model # (without noise) new_state_vec_wo_noise = model_obj.function(state_vec, timestamp=new_timestamp, time_interval=time_interval, noise=0) assert sp.array_equal(new_state_vec_wo_noise, F @ state_vec) # Evaluate the likelihood of the predicted state, given the prior # (without noise) prob = model_obj.pdf(new_state_vec_wo_noise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert sp.array_equal( prob, multivariate_normal.pdf(new_state_vec_wo_noise.T, mean=sp.array(F @ state_vec).ravel(), cov=Q).T) # Propagate a state vector throughout the model # (with internal noise) new_state_vec_w_inoise = model_obj.function(state_vec, timestamp=new_timestamp, time_interval=time_interval) assert not sp.array_equal(new_state_vec_w_inoise, F @ state_vec) # Evaluate the likelihood of the predicted state, given the prior # (with noise) prob = model_obj.pdf(new_state_vec_w_inoise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert sp.array_equal( prob, multivariate_normal.pdf(new_state_vec_w_inoise.T, mean=sp.array(F @ state_vec).ravel(), cov=Q).T) # Propagate a state vector throught the model # (with external noise) noise = model_obj.rvs(timestamp=new_timestamp, time_interval=time_interval) new_state_vec_w_enoise = model_obj.function(state_vec, timestamp=new_timestamp, time_interval=time_interval, noise=noise) assert sp.array_equal(new_state_vec_w_enoise, F @ state_vec + noise) # Evaluate the likelihood of the predicted state, given the prior # (with noise) prob = model_obj.pdf(new_state_vec_w_enoise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert sp.array_equal( prob, multivariate_normal.pdf(new_state_vec_w_enoise.T, mean=sp.array(F @ state_vec).ravel(), cov=Q).T)
def grasp_callback(my_grasp): ## Planning to a Pose goal ## ^^^^^^^^^^^^^^^^^^^^^^^ ## We can plan a motion for this group to a desired pose for the ## end-effector pose_target = geometry_msgs.msg.PoseStamped() pose_target.header.stamp = my_grasp.markers[0].header.stamp.secs pose_target.header.frame_id = "/camera_rgb_optical_frame" pose_target.pose.position.x = my_grasp.markers[0].points[1].x pose_target.pose.position.y = my_grasp.markers[0].points[1].y pose_target.pose.position.z = my_grasp.markers[0].points[1].z ## Convert to quaternion u = [1, 0, 0] norm = linalg.norm([ my_grasp.markers[0].points[0].x - my_grasp.markers[0].points[1].x, my_grasp.markers[0].points[0].y - my_grasp.markers[0].points[1].y, my_grasp.markers[0].points[0].z - my_grasp.markers[0].points[1].z ]) v = asarray([ my_grasp.markers[0].points[0].x - my_grasp.markers[0].points[1].x, my_grasp.markers[0].points[0].y - my_grasp.markers[0].points[1].y, my_grasp.markers[0].points[0].z - my_grasp.markers[0].points[1].z ]) / norm if (array_equal(u, v)): pose_target.pose.orientation.w = 1 pose_target.pose.orientation.x = 0 pose_target.pose.orientation.y = 0 pose_target.pose.orientation.z = 0 elif (array_equal(u, negative(v))): pose_target.pose.orientation.w = 0 pose_target.pose.orientation.x = 0 pose_target.pose.orientation.y = 0 pose_target.pose.orientation.z = 1 else: half = [u[0] + v[0], u[1] + v[1], u[2] + v[2]] pose_target.pose.orientation.w = dot(u, half) temp = cross(u, half) pose_target.pose.orientation.x = temp[0] pose_target.pose.orientation.y = temp[1] pose_target.pose.orientation.z = temp[2] norm = math.sqrt( pose_target.pose.orientation.x * pose_target.pose.orientation.x + pose_target.pose.orientation.y * pose_target.pose.orientation.y + pose_target.pose.orientation.z * pose_target.pose.orientation.z + pose_target.pose.orientation.w * pose_target.pose.orientation.w) if norm == 0: norm = 1 pose_target.pose.orientation.x = pose_target.pose.orientation.x / norm pose_target.pose.orientation.y = pose_target.pose.orientation.y / norm pose_target.pose.orientation.z = pose_target.pose.orientation.z / norm pose_target.pose.orientation.w = pose_target.pose.orientation.w / norm print "Timestamp: %d." % pose_target.header.stamp print "Position X: %f." % pose_target.pose.position.x print "Position Y: %f." % pose_target.pose.position.y print "Position Z: %f." % pose_target.pose.position.z print "Orientation X: %f." % pose_target.pose.orientation.x print "Orientation Y: %f." % pose_target.pose.orientation.y print "Orientation Z: %f." % pose_target.pose.orientation.z print "Orientation W: %f." % pose_target.pose.orientation.w ## Broadcast transform br = tf.TransformBroadcaster() br.sendTransform( (pose_target.pose.position.x, pose_target.pose.position.y, pose_target.pose.position.z), (pose_target.pose.orientation.x, pose_target.pose.orientation.y, pose_target.pose.orientation.z, pose_target.pose.orientation.w), my_grasp.markers[0].header.stamp, "/grasping_target", "/camera_rgb_optical_frame") #br.sendTransform((pose_target.pose.position.x, pose_target.pose.position.y, pose_target.pose.position.z), tf.transformations.quaternion_from_euler(my_grasp.grasps[0].axis.x, my_grasp.grasps[0].axis.y, my_grasp.grasps[0].axis.z), my_grasp.header.stamp, "/grasping_target", "/camera_rgb_optical_frame") ## Listening to transform (trans, rot) = listener.lookupTransform('/world', '/grasping_target', rospy.Time(0)) ## Build new pose pose_target_trans = geometry_msgs.msg.PoseStamped() pose_target_trans.header.frame_id = "/world" pose_target_trans.header.stamp = my_grasp.markers[0].header.stamp pose_target_trans.pose.position.x = trans[0] pose_target_trans.pose.position.y = trans[1] pose_target_trans.pose.position.z = trans[2] pose_target_trans.pose.orientation.x = rot[0] pose_target_trans.pose.orientation.y = rot[1] pose_target_trans.pose.orientation.z = rot[2] pose_target_trans.pose.orientation.w = rot[3] print "NEW POSITION." print "Position X: %f." % pose_target_trans.pose.position.x print "Position Y: %f." % pose_target_trans.pose.position.y print "Position Z: %f." % pose_target_trans.pose.position.z print "Orientation X: %f." % pose_target_trans.pose.orientation.x print "Orientation Y: %f." % pose_target_trans.pose.orientation.y print "Orientation Z: %f." % pose_target_trans.pose.orientation.z print "Orientation W: %f." % pose_target_trans.pose.orientation.w my_grasp_pub.publish(pose_target_trans) group.set_pose_target(pose_target_trans.pose, end_effector_link="my_eef") ## Now, we call the planner to compute the plan ## and visualize it if successful ## Note that we are just planning, not asking move_group ## to actually move the robot # group.set_planner_id("RRTstarkConfigDefault") # group.allow_replanning(True) ## Planning with collision detection can be slow. Lets set the planning time ## to be sure the planner has enough time to plan around the box. 10 seconds ## should be plenty. # group.set_planning_time(5.0) plan1 = group.plan() ## Moving to a pose goal ## ^^^^^^^^^^^^^^^^^^^^^ ## ## Moving to a pose goal is similar to the step above ## except we now use the go() function. Note that ## the pose goal we had set earlier is still active ## and so the robot will try to move to that goal. We will ## not use that function in this tutorial since it is ## a blocking function and requires a controller to be active ## and report success on execution of a trajectory. # Uncomment below line when working with a real robot # group.go(wait=True) print "============ DONE PLANNING ============" sys.exit("DONE PLANNING") ## Sleep to give Rviz time to visualize the plan. */ rospy.sleep(5) group.clear_pose_targets()
def __eq__(self,e): return sp.array_equal(e.a,self.a) and sp.array_equal(e.b,self.b)
- complex_ Shorthand for complex128. Those are the same as python types and may be interchanged. Fixed widths: - int32 Integer (-2147483648 to 2147483647) - uint32 Unsigned integer (0 to 4294967295) - float32 Single precision float: sign bit, 8 bits exponent, 23 bits mantissa - float64 Double precision float: sign bit, 11 bits exponent, 52 bits mantissa - complex64 Complex number, represented by two 32-bit floats (real and imaginary components) Those have fixed width on all systems, and may not be compatible with the python types. """ assert sp.array_equal(sp.array([1, 2, 3], dtype=sp.int_), sp.int_([1, 2, 3])) # Different types evaluate to equal sp.arrays assert sp.array_equal(sp.array([1, 2, 3], dtype=sp.int_), sp.array([1, 2, 3], dtype=sp.float_)) # Get type v = sp.array([1, 2], dtype=sp.int32) assert v.dtype == sp.int32 # Subtype: sp.issubdtype(sp.int32, sp.int_)
def base(state_vec, noise_diff_coeffs, recips_decorr_times, timediff=1.0): """ Base test for n-dimensional ConstantAcceleration Transition Models """ # Create a 1D Singer or an n-dimensional # CombinedLinearGaussianTransitionModel object dim = len(state_vec) // 3 # pos, vel, acc for each dimension if dim == 1: model_obj = Singer(noise_diff_coeff=noise_diff_coeffs[0], recip_decorr_time=recips_decorr_times[0]) else: model_list = [ Singer(noise_diff_coeff=noise_diff_coeffs[i], recip_decorr_time=recips_decorr_times[i]) for i in range(0, dim) ] model_obj = CombinedLinearGaussianTransitionModel(model_list) # State related variables state_vec = state_vec old_timestamp = datetime.datetime.now() new_timestamp = old_timestamp + datetime.timedelta(seconds=timediff) time_interval = new_timestamp - old_timestamp # Model-related components noise_diff_coeffs = noise_diff_coeffs # m/s^3 mat_list = [] covar_list = [] for i in range(0, dim): recip_decorr_time = recips_decorr_times[i] recip_decorr_timedt = recip_decorr_time * timediff noise_diff_coeff = noise_diff_coeffs[i] mat_list.append( sp.array([[ 1, timediff, (recip_decorr_timedt - 1 + sp.exp(-recip_decorr_timedt)) / sp.power(recip_decorr_time, 2) ], [0, 1, (1 - sp.exp(-recip_decorr_timedt)) / recip_decorr_time], [0, 0, sp.exp(-recip_decorr_timedt)]])) alpha_time = recip_decorr_time * timediff e_neg_at = sp.exp(-alpha_time) e_neg2_at = sp.exp(-2 * alpha_time) covar_list.append( sp.array([[( (1 - e_neg2_at) + 2 * alpha_time + (2 * sp.power(alpha_time, 3)) / 3 - 2 * sp.power(alpha_time, 2) - 4 * alpha_time * e_neg_at) / (2 * sp.power(recip_decorr_time, 5)), sp.power(alpha_time - (1 - e_neg_at), 2) / (2 * sp.power(recip_decorr_time, 4)), ((1 - e_neg2_at) - 2 * alpha_time * e_neg_at) / (2 * sp.power(recip_decorr_time, 3))], [ sp.power(alpha_time - (1 - e_neg_at), 2) / (2 * sp.power(recip_decorr_time, 4)), (2 * alpha_time - 4 * (1 - e_neg_at) + (1 - e_neg2_at)) / (2 * sp.power(recip_decorr_time, 3)), sp.power(1 - e_neg_at, 2) / (2 * sp.power(recip_decorr_time, 2)) ], [((1 - e_neg2_at) - 2 * alpha_time * e_neg_at) / (2 * sp.power(recip_decorr_time, 3)), sp.power(1 - e_neg_at, 2) / (2 * sp.power(recip_decorr_time, 2)), (1 - e_neg2_at) / (2 * recip_decorr_time)]]) * noise_diff_coeff) F = sp.linalg.block_diag(*mat_list) Q = sp.linalg.block_diag(*covar_list) # Ensure ```model_obj.transfer_function(time_interval)``` returns F assert sp.array_equal( F, model_obj.matrix(timestamp=new_timestamp, time_interval=time_interval)) # Ensure ```model_obj.covar(time_interval)``` returns Q assert sp.array_equal( Q, model_obj.covar(timestamp=new_timestamp, time_interval=time_interval)) # Propagate a state vector throught the model # (without noise) new_state_vec_wo_noise = model_obj.function(state_vec, timestamp=new_timestamp, time_interval=time_interval, noise=0) assert sp.array_equal(new_state_vec_wo_noise, F @ state_vec) # Evaluate the likelihood of the predicted state, given the prior # (without noise) prob = model_obj.pdf(new_state_vec_wo_noise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert sp.array_equal( prob, multivariate_normal.pdf(new_state_vec_wo_noise.T, mean=sp.array(F @ state_vec).ravel(), cov=Q).T) # Propagate a state vector throughout the model # (with internal noise) new_state_vec_w_inoise = model_obj.function(state_vec, timestamp=new_timestamp, time_interval=time_interval) assert not sp.array_equal(new_state_vec_w_inoise, F @ state_vec) # Evaluate the likelihood of the predicted state, given the prior # (with noise) prob = model_obj.pdf(new_state_vec_w_inoise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert sp.array_equal( prob, multivariate_normal.pdf(new_state_vec_w_inoise.T, mean=sp.array(F @ state_vec).ravel(), cov=Q).T) # Propagate a state vector throught the model # (with external noise) noise = model_obj.rvs(timestamp=new_timestamp, time_interval=time_interval) new_state_vec_w_enoise = model_obj.function(state_vec, timestamp=new_timestamp, time_interval=time_interval, noise=noise) assert sp.array_equal(new_state_vec_w_enoise, F @ state_vec + noise) # Evaluate the likelihood of the predicted state, given the prior # (with noise) prob = model_obj.pdf(new_state_vec_w_enoise, state_vec, timestamp=new_timestamp, time_interval=time_interval) assert sp.array_equal( prob, multivariate_normal.pdf(new_state_vec_w_enoise.T, mean=sp.array(F @ state_vec).ravel(), cov=Q).T)