def set_neighbors(self):
self.neighbors = utils.find_neighbors(utils.get_host(), self.port,
NEIGHBORS_IP_RANGE_NUM[0],
NEIGHBORS_IP_RANGE_NUM[1],
BLOCKCHAIN_PORT_RAMGE[0],
BLOCKCHAIN_PORT_RAMGE[1])
logger.info({'action': 'set_neighbors', 'neighbors': self.neighbors})
def run(self):
for i in range(self.threadID, self.N, self.tsum):
neighs = find_neighbors(i, self.Dsize[0], self.Dsize[1])
neighs_len = len(neighs)
for j in range(0, self.n):
for idx in neighs:
try:
self.res[i][j] += self.x[idx][j]
except:
print("Errror")
self.res[i][j] -= (neighs_len * self.x[i][j])
def generate_matrix_D(lvl):
size = calculate_N_from_level(lvl)
(m, n) = calc_D_size(lvl)
res = np.zeros((size, size), dtype=int)
for i in range(0, size):
neighbors = find_neighbors(i, m, n)
for pos in neighbors:
res[i][pos] = 1
res[i][i] = -1 * len(neighbors)
return res
def get_hsps(seq1_dict, seq2_dict, K, scoring_matrix, T):
hsps = []
for key, val in seq1_dict.items():
neighbors = utils.find_neighbors(key, scoring_matrix, ALPHABET, T)
for neighbor in neighbors:
if neighbor in seq2_dict.keys():
score = utils.align(key, neighbor, scoring_matrix)
for seq1_start in val:
seq1_end = seq1_start + K - 1
for seq2_start in seq2_dict[neighbor]:
seq2_end = seq2_start + K - 1
hsp = HSP(seq1_start, seq1_end, seq2_start, seq2_end,
score)
hsps.append(hsp)
return hsps
def getstate(self):
rospy.wait_for_service('/gazebo/get_link_state')
torso_pose = self.get_link_state("humanoid::Torso_link",
"world").link_state.pose
T = tf.transformations.quaternion_matrix([
torso_pose.orientation.x, torso_pose.orientation.y,
torso_pose.orientation.z, torso_pose.orientation.w
])
# rotation in /world frame, which is [0, 0, -0.93] to /base frame
# target_line_start-target_pos_start is the original position of human in /world frame
self.target_line = np.dot(T[:3, :3], (self.target_line_start - self.target_pos_start).T).T + \
[torso_pose.position.x, torso_pose.position.y, torso_pose.position.z] + \
[0, 0, -0.93]
right_limb_pose, right_joint_pos = limbPose(self.kdl_tree,
self.base_link,
self.right_limb_interface,
'right')
left_limb_pose, left_joint_pos = limbPose(self.kdl_tree,
self.base_link,
self.left_limb_interface,
'left')
right_joint = [
right_joint_pos[0], right_joint_pos[1], right_joint_pos[2],
right_joint_pos[3], right_joint_pos[4], right_joint_pos[5],
right_joint_pos[6]
]
left_joint = [
left_joint_pos[0], left_joint_pos[1], left_joint_pos[2],
left_joint_pos[3], left_joint_pos[4], left_joint_pos[5],
left_joint_pos[6]
]
# right limb joint positions
state1 = np.asarray([right_joint, left_joint]).flatten()
# right limb link cartesian positions
state2 = np.asarray([right_limb_pose[5:],
left_limb_pose[5:]]).flatten()
# hugging target -- cylinder
# aa = np.asarray([self.cylinder_radius])
# bb = np.asarray([self.cylinder1, self.cylinder1 + asarray([0, 0, self.cylinder_height])]).flatten()
# state3 = np.concatenate((aa, bb), axis=0)
state3 = self.target_line.flatten()
# ####################### writhe matrix ###########################
writhe = np.empty((len(self.target_line) - 2, 14))
for idx_target in range(10):
for idx_robot in range(5, 12):
writhe[idx_target, idx_robot-5] = \
GLI(self.target_line[idx_target], self.target_line[idx_target+1],
right_limb_pose[idx_robot], right_limb_pose[idx_robot+1])[0]
writhe[idx_target, idx_robot-5+7] = \
GLI(self.target_line[idx_target], self.target_line[idx_target+1],
left_limb_pose[idx_robot], left_limb_pose[idx_robot+1])[0]
for idx_target in range(11, 21):
for idx_robot in range(5, 12):
writhe[idx_target-1, idx_robot-5] = \
GLI(self.target_line[idx_target], self.target_line[idx_target+1],
right_limb_pose[idx_robot], right_limb_pose[idx_robot+1])[0]
writhe[idx_target-1, idx_robot-5+7] = \
GLI(self.target_line[idx_target], self.target_line[idx_target+1],
left_limb_pose[idx_robot], left_limb_pose[idx_robot+1])[0]
state4 = writhe.flatten()
# ############################### interaction mesh ##################################
graph_points = np.concatenate(
(right_limb_pose[5:], left_limb_pose[5:], self.target_line), 0)
InterMesh = np.empty(graph_points.shape)
mesh1 = []
mesh2 = []
for idx, point in enumerate(graph_points):
neighbor_index = find_neighbors(idx, self.triangulation)
W = 0
Lap = point
# calculate normalization constant
for nei_point in graph_points[neighbor_index]:
W = W + 1.0 / math.sqrt((nei_point[0] - point[0])**2 +
(nei_point[1] - point[1])**2 +
(nei_point[2] - point[2])**2)
# calculate Laplace coordinates
for nei_point in graph_points[neighbor_index]:
mesh1.append(point)
mesh2.append(nei_point)
dis_nei = math.sqrt((nei_point[0] - point[0])**2 +
(nei_point[1] - point[1])**2 +
(nei_point[2] - point[2])**2)
Lap = Lap - nei_point / (dis_nei * W)
InterMesh[idx] = Lap
state5 = InterMesh.flatten()
# DisMesh = np.empty([limb_pose.shape[0], self.target_line.shape[0]])
# for i in range(DisMesh.shape[0]):
# for j in range(DisMesh.shape[1]):
# DisMesh[i][j] = math.sqrt( (limb_pose[i][0] - self.target_line[j][0]) ** 2 +
# (limb_pose[i][1] - self.target_line[j][1]) ** 2 +
# (limb_pose[i][2] - self.target_line[j][2]) ** 2)
# state5 = DisMesh.flatten()
#
# DisMesh = np.empty(limb_pose.shape)
# for i in range(DisMesh.shape[0]):
# W = 0
# Lap = limb_pose[i]
# # calculate normalization constant
# for target_point in self.target_line:
# W = W + 1.0 / math.sqrt((limb_pose[i][0] - target_point[0])**2 +
# (limb_pose[i][1] - target_point[1])**2 +
# (limb_pose[i][2] - target_point[2])**2)
# for target_point in self.target_line:
# dis = math.sqrt((limb_pose[i][0] - target_point[0])**2 +
# (limb_pose[i][1] - target_point[1])**2 +
# (limb_pose[i][2] - target_point[2])**2)
# Lap = Lap - target_point / (dis * W)
# DisMesh[i] = Lap
# state5 = DisMesh.flatten()
state = [state1, state2, state3, state4, state5]
return state, writhe, InterMesh, [mesh1, mesh2]
# 无标记数据
# u_index = np.arange(5000, 60000)
u_index = np.random.randint(5000, 60000, size=2000)
uindex_dataset = DataSet(u_index, labels[u_index])
sample_data = np.vstack((samples[l_index], samples[u_index]))
sample_label = np.vstack((labels[l_index], labels[u_index]))
# 测试数据
test_dataset = DataSet(samples[60000:], labels[60000:])
neighborIndex, remoteIndex, RBF_matrix = find_neighbors(
samples,
l_index,
u_index,
num_n=k_of_knn,
num_r=m_of_knn,
neigh_file_path=None,
sigma=3.8
# neigh_file_path='./neigh_core5k_around_5k-60k.txt'
)
input_data_shape = (None, image_size, image_size, 1)
input_neighbor_shape = (None, image_size, image_size, 1)
input_remote_shape = (None, image_size, image_size, 1)
label_shape = (None, num_classes)
wij_nshape = (None, k_of_knn, 1)
wij_rshape = (None, m_of_knn, 1)
xl = tf.placeholder(tf.float32, input_data_shape, name="labeled_data_input")
xn = tf.placeholder(tf.float32,
input_neighbor_shape,
name="neighbor_data_input")
def getstate(self):
right_limb_pose, right_joint_pos = limbPose(self.kdl_tree,
self.base_link,
self.right_limb_interface,
'right')
left_limb_pose, left_joint_pos = limbPose(self.kdl_tree,
self.base_link,
self.left_limb_interface,
'left')
right_joint = [
right_joint_pos[0], right_joint_pos[1], right_joint_pos[2],
right_joint_pos[3], right_joint_pos[4], right_joint_pos[5],
right_joint_pos[6]
]
left_joint = [
left_joint_pos[0], left_joint_pos[1], left_joint_pos[2],
left_joint_pos[3], left_joint_pos[4], left_joint_pos[5],
left_joint_pos[6]
]
# right limb joint positions
state1 = np.asarray([right_joint, left_joint]).flatten()
# right limb link cartesian positions
state2 = np.asarray([right_limb_pose[5:],
left_limb_pose[5:]]).flatten()
# hugging target -- cylinder
# aa = np.asarray([self.cylinder_radius])
# bb = np.asarray([self.cylinder1, self.cylinder1 + asarray([0, 0, self.cylinder_height])]).flatten()
# state3 = np.concatenate((aa, bb), axis=0)
state3 = self.target_line.flatten()
# ####################### writhe matrix ###########################
writhe = np.empty((len(self.target_line) - 2, 14))
for idx_target in range(10):
for idx_robot in range(5, 12):
writhe[idx_target, idx_robot-5] = \
GLI(self.target_line[idx_target], self.target_line[idx_target+1],
right_limb_pose[idx_robot], right_limb_pose[idx_robot+1])[0]
writhe[idx_target, idx_robot-5+7] = \
GLI(self.target_line[idx_target], self.target_line[idx_target+1],
left_limb_pose[idx_robot], left_limb_pose[idx_robot+1])[0]
for idx_target in range(11, 21):
for idx_robot in range(5, 12):
writhe[idx_target-1, idx_robot-5] = \
GLI(self.target_line[idx_target], self.target_line[idx_target+1],
right_limb_pose[idx_robot], right_limb_pose[idx_robot+1])[0]
writhe[idx_target-1, idx_robot-5+7] = \
GLI(self.target_line[idx_target], self.target_line[idx_target+1],
left_limb_pose[idx_robot], left_limb_pose[idx_robot+1])[0]
state4 = writhe.flatten()
# ############################### interaction mesh ##################################
graph_points = np.concatenate(
(right_limb_pose[5:], left_limb_pose[5:], self.target_line), 0)
InterMesh = np.empty(graph_points.shape)
for idx, point in enumerate(graph_points):
neighbor_index = find_neighbors(idx, self.triangulation)
W = 0
Lap = point
# calculate normalization constant
for nei_point in graph_points[neighbor_index]:
W = W + 1.0 / math.sqrt((nei_point[0] - point[0])**2 +
(nei_point[1] - point[1])**2 +
(nei_point[2] - point[2])**2)
# calculate Laplace coordinates
for nei_point in graph_points[neighbor_index]:
dis_nei = math.sqrt((nei_point[0] - point[0])**2 +
(nei_point[1] - point[1])**2 +
(nei_point[2] - point[2])**2)
Lap = Lap - nei_point / (dis_nei * W)
InterMesh[idx] = Lap
state5 = InterMesh.flatten()
state = [state1, state2, state3, state4, state5]
return state, writhe, InterMesh
