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)
