def spin(self):
# locate all markers on the object
if False:
max_angle = 60.0 / 180.0 * math.pi
min_z = math.cos(max_angle)
print "min_z: %s" % (min_z)
# big_box
# marker_list = [18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31]
# small_box
# marker_list = [32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45]
# table
# marker_list = [3,4,5,6]
# c-beam
marker_list = [46, 47, 48, 49, 50, 51, 52, 53, 54, 55]
mtrans_matrix = []
mtrans_weights_ori = []
mtrans_weights_pos = []
for i in range(0, len(marker_list)):
mtrans_matrix.append([])
mtrans_weights_ori.append([])
mtrans_weights_pos.append([])
for j in range(0, len(marker_list)):
if i > j:
mtrans_matrix[i].append([])
mtrans_weights_ori[i].append([])
mtrans_weights_pos[i].append([])
else:
mtrans_matrix[i].append(None)
mtrans_weights_ori[i].append(None)
mtrans_weights_pos[i].append(None)
marker_lock = Lock()
self.visible_markers = []
def markerCallback(data):
marker_lock.acquire()
self.visible_markers = []
for m in data.markers:
if m.id in marker_list:
self.visible_markers.append(
(m.id, pm.fromMsg(m.pose.pose)))
marker_lock.release()
rospy.Subscriber('/ar_pose_marker', AlvarMarkers, markerCallback)
while not rospy.is_shutdown():
marker_lock.acquire()
visible_markers_local = copy.copy(self.visible_markers)
marker_lock.release()
accepted_markers = []
accepted_markers_id = []
accepted_markers_weights_pos = []
accepted_markers_weights_ori = []
for m in visible_markers_local:
n = PyKDL.Frame(m[1].M) * PyKDL.Vector(0, 0, -1)
if n.z() > min_z:
accepted_markers.append(m)
accepted_markers_id.append(m[0])
# n.z() is in range (min_z, 1.0), min_z is the best for orientation and 1.0 i best for position
weight_pos = (n.z() - min_z) / (1.0 - min_z)
if weight_pos > 1.0 or weight_pos < 0.0:
print "error: weight==%s" % (weight_pos)
accepted_markers_weights_pos.append(weight_pos)
accepted_markers_weights_ori.append(1.0 - weight_pos)
print "visible: %s accepted markers: %s" % (
len(visible_markers_local), accepted_markers_id)
for i in range(0, len(accepted_markers)):
for j in range(i + 1, len(accepted_markers)):
if accepted_markers[i][0] > accepted_markers[j][0]:
idx1 = i
idx2 = j
else:
idx1 = j
idx2 = i
# print " %s with %s"%(accepted_markers[idx1][0], accepted_markers[idx2][0])
T_C_M1 = accepted_markers[idx1][1]
T_C_M2 = accepted_markers[idx2][1]
T_M1_M2 = T_C_M1.Inverse() * T_C_M2
marker_id1_index = marker_list.index(
accepted_markers[idx1][0])
marker_id2_index = marker_list.index(
accepted_markers[idx2][0])
mtrans_matrix[marker_id1_index][
marker_id2_index].append(T_M1_M2)
mtrans_weights_ori[marker_id1_index][
marker_id2_index].append(
accepted_markers_weights_ori[idx1] *
accepted_markers_weights_ori[idx2])
mtrans_weights_pos[marker_id1_index][
marker_id2_index].append(
accepted_markers_weights_pos[idx1] *
accepted_markers_weights_pos[idx2])
rospy.sleep(0.1)
G = {}
for i in range(0, len(marker_list)):
for j in range(0, len(marker_list)):
if mtrans_matrix[i][j] == None:
pass
# print "%s with %s: None"%(marker_list[i], marker_list[j])
elif len(mtrans_matrix[i][j]) < 10:
print "%s with %s: %s (ignoring)" % (
marker_list[i], marker_list[j],
len(mtrans_matrix[i][j]))
else:
score, R_M1_M2 = velmautils.meanOrientation(
mtrans_matrix[i][j], mtrans_weights_ori[i][j])
# ignore 20% the most distant measures from the mean
distances = []
for measure_idx in range(0, len(mtrans_matrix[i][j])):
diff = PyKDL.diff(
R_M1_M2,
mtrans_matrix[i][j][measure_idx]).rot.Norm()
distances.append([diff, measure_idx])
distances_sorted = sorted(distances,
key=operator.itemgetter(0))
mtrans = []
weights_ori = []
for measure_idx in range(
0, int(0.8 * len(mtrans_matrix[i][j]))):
mtrans.append(mtrans_matrix[i][j][
distances_sorted[measure_idx][1]])
weights_ori.append(mtrans_weights_ori[i][j][
distances_sorted[measure_idx][1]])
# score, R_M1_M2 = velmautils.meanOrientation(mtrans, weights_ori)
print "%s with %s: %s, score: %s" % (
marker_list[i], marker_list[j],
len(mtrans_matrix[i][j]), score)
P_M1_M2 = velmautils.meanPosition(
mtrans_matrix[i][j], mtrans_weights_pos[i][j])
print "P_M1_M2 before: %s" % (P_M1_M2)
# ignore 20% the most distant measures from the mean
distances = []
for measure_idx in range(0, len(mtrans_matrix[i][j])):
diff = PyKDL.diff(
R_M1_M2,
mtrans_matrix[i][j][measure_idx]).vel.Norm()
distances.append([diff, measure_idx])
distances_sorted = sorted(distances,
key=operator.itemgetter(0))
mtrans = []
weights_pos = []
for measure_idx in range(
0, int(0.8 * len(mtrans_matrix[i][j]))):
mtrans.append(mtrans_matrix[i][j][
distances_sorted[measure_idx][1]])
weights_pos.append(mtrans_weights_ori[i][j][
distances_sorted[measure_idx][1]])
# P_M1_M2 = velmautils.meanPosition(mtrans, weights_pos)
print "P_M1_M2 after: %s" % (P_M1_M2)
mtrans_matrix[i][j] = PyKDL.Frame(
copy.deepcopy(R_M1_M2.M), P_M1_M2)
neighbours = G.get(marker_list[i], {})
if score > 0.01:
score = 1000000.0
else:
score = 1.0
neighbours[marker_list[j]] = score
G[marker_list[i]] = neighbours
neighbours = G.get(marker_list[j], {})
neighbours[marker_list[i]] = score
G[marker_list[j]] = neighbours
frames = []
# get the shortest paths weighted by score from optimization
for marker_id in marker_list:
path = dijkstra.shortestPath(G, marker_id, marker_list[0])
print path
T_Ml_Mf = PyKDL.Frame()
for i in range(0, len(path) - 1):
if marker_list.index(path[i]) > marker_list.index(
path[i + 1]):
T_Mp_Mn = mtrans_matrix[marker_list.index(
path[i])][marker_list.index(path[i + 1])]
T_Ml_Mf = T_Ml_Mf * T_Mp_Mn
else:
T_Mn_Mp = mtrans_matrix[marker_list.index(
path[i + 1])][marker_list.index(path[i])]
T_Ml_Mf = T_Ml_Mf * T_Mn_Mp.Inverse()
frames.append(copy.deepcopy(T_Ml_Mf.Inverse()))
pub_marker = velmautils.MarkerPublisher()
rospy.sleep(1.0)
m_id = 0
marker_idx = 0
for fr in frames:
q = fr.M.GetQuaternion()
p = fr.p
print "[%s, PyKDL.Frame(PyKDL.Rotation.Quaternion(%s,%s,%s,%s),PyKDL.Vector(%s,%s,%s))]," % (
marker_list[marker_idx], q[0], q[1], q[2], q[3], p.x(),
p.y(), p.z())
m_id = pub_marker.publishFrameMarker(fr,
m_id,
scale=0.1,
frame='world',
namespace='default')
rospy.sleep(0.1)
marker_idx += 1
rospy.sleep(2.0)
exit(0)
else:
pub_marker = velmautils.MarkerPublisher()
rospy.sleep(1.0)
markers2 = [
[
46,
PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0, 0.0, 0.0, 1.0),
PyKDL.Vector(-0.0, -0.0, -0.0))
],
[
47,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.0161417497092,
-9.92379339669e-05,
-0.00603105214296,
0.999851519216),
PyKDL.Vector(-0.0987990318312, -0.000265582834399,
0.000544628226204))
],
[
48,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.706829255712,
0.00114672638679,
-0.707103949357,
0.0198769487466),
PyKDL.Vector(0.0427261427894, 0.00245374838957,
-0.116698579624))
],
[
49,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.706230029879,
-0.00660144281521,
-0.70769079068,
0.0192174565616),
PyKDL.Vector(0.0410352237403, 0.000595788239244,
-0.0336956438972))
],
[
50,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.0155276433453,
0.714580229904,
0.00743395758828,
0.699341635824),
PyKDL.Vector(-0.131991504795, -0.00410930997885,
-0.0916799439467))
],
[
51,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.706160148032,
0.0114839861434,
-0.707838133957,
0.0130820300375),
PyKDL.Vector(-0.150701418215, -0.000688426198715,
-0.035762545196))
],
[
52,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.705850422242,
0.00260066465892,
-0.708005925475,
0.022271673869),
PyKDL.Vector(-0.150663515172, -0.00196494029406,
-0.123356224597))
],
[
53,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.00630495805624,
-0.999979097775,
0.000308879730173,
0.00139860956497),
PyKDL.Vector(-0.0116053782242, 0.000352840524022,
-0.0267278839783))
],
[
54,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.00853722085061,
-0.999853611966,
0.00230495916657,
0.0146477869582),
PyKDL.Vector(-0.0949889134574, 0.00131718330416,
-0.0165548984986))
],
[
55,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.0208301706621,
-0.711731154203,
0.0052945617051,
0.702123091589),
PyKDL.Vector(0.0244779541548, 0.000463479220587,
-0.0804749548707))
],
]
markers = [
[
46,
PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0, 0.0, 0.0, 1.0),
PyKDL.Vector(-0.0, -0.0, -0.0))
],
[
47,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.00211977224419,
0.00133104595822,
-0.00433238039783,
0.999987482603),
PyKDL.Vector(-0.0995553679408, -0.000651966932258,
0.000444468330964))
],
[
48,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.705539374795,
0.00129956423061,
-0.708394191186,
0.0197527628665),
PyKDL.Vector(0.0433256677966, 0.00212664651843,
-0.116482343501))
],
[
49,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.704707757517,
-0.00628228374428,
-0.709255128279,
0.0174548680028),
PyKDL.Vector(0.0412709849952, 0.000494665961165,
-0.0338667341872))
],
[
50,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(-0.0117276773848,
0.706312439798,
0.00558379104701,
0.707781053891),
PyKDL.Vector(-0.131246837996, -0.00469319026484,
-0.0943814089463))
],
[
51,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.710112841604,
0.00963407546503,
-0.703895468304,
0.013345653983),
PyKDL.Vector(-0.149984963191, -0.00300459973807,
-0.0370193446712))
],
[
52,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.704691425649,
0.00497982940017,
-0.709159595229,
0.0218601100464),
PyKDL.Vector(-0.15277553848, -0.00431480401095,
-0.120995842686))
],
[
53,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.00984066000086,
-0.999945658681,
0.00299307949816,
0.00169781765667),
PyKDL.Vector(-0.0132269965227, -0.00110379001368,
-0.0274175040768))
],
[
54,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.00154140261629,
0.999633307109,
-0.00751679824708,
0.0259686953912),
PyKDL.Vector(-0.0974091549673, -0.000670004842722,
-0.0319416169884))
],
[
55,
PyKDL.Frame(
PyKDL.Rotation.Quaternion(0.0195903374145,
-0.713404165076,
0.00273342374532,
0.700473585745),
PyKDL.Vector(0.0250633176495, -0.00356911439713,
-0.0811403242495))
],
]
m_id = 0
for m in markers:
print m[0]
print m[1]
m_id = pub_marker.publishFrameMarker(m[1],
m_id,
scale=0.1,
frame='world',
namespace='default')
rospy.sleep(0.1)
def spin(self):
# locate all markers on the object
if False:
max_angle = 60.0/180.0*math.pi
min_z = math.cos(max_angle)
print "min_z: %s"%(min_z)
# big_box
# marker_list = [18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31]
# small_box
# marker_list = [32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45]
# table
# marker_list = [3,4,5,6]
# c-beam
marker_list = [46, 47, 48, 49, 50, 51, 52, 53, 54, 55]
mtrans_matrix = []
mtrans_weights_ori = []
mtrans_weights_pos = []
for i in range(0, len(marker_list)):
mtrans_matrix.append([])
mtrans_weights_ori.append([])
mtrans_weights_pos.append([])
for j in range(0, len(marker_list)):
if i > j:
mtrans_matrix[i].append([])
mtrans_weights_ori[i].append([])
mtrans_weights_pos[i].append([])
else:
mtrans_matrix[i].append(None)
mtrans_weights_ori[i].append(None)
mtrans_weights_pos[i].append(None)
marker_lock = Lock()
self.visible_markers = []
def markerCallback(data):
marker_lock.acquire()
self.visible_markers = []
for m in data.markers:
if m.id in marker_list:
self.visible_markers.append( (m.id, pm.fromMsg(m.pose.pose)) )
marker_lock.release()
rospy.Subscriber('/ar_pose_marker', AlvarMarkers, markerCallback)
while not rospy.is_shutdown():
marker_lock.acquire()
visible_markers_local = copy.copy(self.visible_markers)
marker_lock.release()
accepted_markers = []
accepted_markers_id = []
accepted_markers_weights_pos = []
accepted_markers_weights_ori = []
for m in visible_markers_local:
n = PyKDL.Frame(m[1].M) * PyKDL.Vector(0,0,-1)
if n.z() > min_z:
accepted_markers.append( m )
accepted_markers_id.append( m[0] )
# n.z() is in range (min_z, 1.0), min_z is the best for orientation and 1.0 i best for position
weight_pos = (n.z() - min_z)/(1.0-min_z)
if weight_pos > 1.0 or weight_pos < 0.0:
print "error: weight==%s"%(weight_pos)
accepted_markers_weights_pos.append(weight_pos)
accepted_markers_weights_ori.append(1.0-weight_pos)
print "visible: %s accepted markers: %s"%(len(visible_markers_local), accepted_markers_id)
for i in range(0, len(accepted_markers)):
for j in range(i+1, len(accepted_markers)):
if accepted_markers[i][0] > accepted_markers[j][0]:
idx1 = i
idx2 = j
else:
idx1 = j
idx2 = i
# print " %s with %s"%(accepted_markers[idx1][0], accepted_markers[idx2][0])
T_C_M1 = accepted_markers[idx1][1]
T_C_M2 = accepted_markers[idx2][1]
T_M1_M2 = T_C_M1.Inverse() * T_C_M2
marker_id1_index = marker_list.index(accepted_markers[idx1][0])
marker_id2_index = marker_list.index(accepted_markers[idx2][0])
mtrans_matrix[marker_id1_index][marker_id2_index].append(T_M1_M2)
mtrans_weights_ori[marker_id1_index][marker_id2_index].append(accepted_markers_weights_ori[idx1] * accepted_markers_weights_ori[idx2])
mtrans_weights_pos[marker_id1_index][marker_id2_index].append(accepted_markers_weights_pos[idx1] * accepted_markers_weights_pos[idx2])
rospy.sleep(0.1)
G = {}
for i in range(0, len(marker_list)):
for j in range(0, len(marker_list)):
if mtrans_matrix[i][j] == None:
pass
# print "%s with %s: None"%(marker_list[i], marker_list[j])
elif len(mtrans_matrix[i][j]) < 10:
print "%s with %s: %s (ignoring)"%(marker_list[i], marker_list[j], len(mtrans_matrix[i][j]))
else:
score, R_M1_M2 = velmautils.meanOrientation(mtrans_matrix[i][j], mtrans_weights_ori[i][j])
# ignore 20% the most distant measures from the mean
distances = []
for measure_idx in range(0, len(mtrans_matrix[i][j])):
diff = PyKDL.diff(R_M1_M2, mtrans_matrix[i][j][measure_idx]).rot.Norm()
distances.append([diff, measure_idx])
distances_sorted = sorted(distances, key=operator.itemgetter(0))
mtrans = []
weights_ori = []
for measure_idx in range(0, int(0.8*len(mtrans_matrix[i][j]))):
mtrans.append(mtrans_matrix[i][j][distances_sorted[measure_idx][1]])
weights_ori.append(mtrans_weights_ori[i][j][distances_sorted[measure_idx][1]])
# score, R_M1_M2 = velmautils.meanOrientation(mtrans, weights_ori)
print "%s with %s: %s, score: %s"%(marker_list[i], marker_list[j], len(mtrans_matrix[i][j]), score)
P_M1_M2 = velmautils.meanPosition(mtrans_matrix[i][j], mtrans_weights_pos[i][j])
print "P_M1_M2 before: %s"%(P_M1_M2)
# ignore 20% the most distant measures from the mean
distances = []
for measure_idx in range(0, len(mtrans_matrix[i][j])):
diff = PyKDL.diff(R_M1_M2, mtrans_matrix[i][j][measure_idx]).vel.Norm()
distances.append([diff, measure_idx])
distances_sorted = sorted(distances, key=operator.itemgetter(0))
mtrans = []
weights_pos = []
for measure_idx in range(0, int(0.8*len(mtrans_matrix[i][j]))):
mtrans.append(mtrans_matrix[i][j][distances_sorted[measure_idx][1]])
weights_pos.append(mtrans_weights_ori[i][j][distances_sorted[measure_idx][1]])
# P_M1_M2 = velmautils.meanPosition(mtrans, weights_pos)
print "P_M1_M2 after: %s"%(P_M1_M2)
mtrans_matrix[i][j] = PyKDL.Frame(copy.deepcopy(R_M1_M2.M), P_M1_M2)
neighbours = G.get(marker_list[i], {})
if score > 0.01:
score = 1000000.0
else:
score = 1.0
neighbours[marker_list[j]] = score
G[marker_list[i]] = neighbours
neighbours = G.get(marker_list[j], {})
neighbours[marker_list[i]] = score
G[marker_list[j]] = neighbours
frames = []
# get the shortest paths weighted by score from optimization
for marker_id in marker_list:
path = dijkstra.shortestPath(G,marker_id,marker_list[0])
print path
T_Ml_Mf = PyKDL.Frame()
for i in range(0, len(path)-1):
if marker_list.index(path[i]) > marker_list.index(path[i+1]):
T_Mp_Mn = mtrans_matrix[marker_list.index(path[i])][marker_list.index(path[i+1])]
T_Ml_Mf = T_Ml_Mf * T_Mp_Mn
else:
T_Mn_Mp = mtrans_matrix[marker_list.index(path[i+1])][marker_list.index(path[i])]
T_Ml_Mf = T_Ml_Mf * T_Mn_Mp.Inverse()
frames.append(copy.deepcopy(T_Ml_Mf.Inverse()))
pub_marker = velmautils.MarkerPublisher()
rospy.sleep(1.0)
m_id = 0
marker_idx = 0
for fr in frames:
q = fr.M.GetQuaternion()
p = fr.p
print "[%s, PyKDL.Frame(PyKDL.Rotation.Quaternion(%s,%s,%s,%s),PyKDL.Vector(%s,%s,%s))],"%(marker_list[marker_idx], q[0], q[1], q[2], q[3], p.x(), p.y(), p.z())
m_id = pub_marker.publishFrameMarker(fr, m_id, scale=0.1, frame='world', namespace='default')
rospy.sleep(0.1)
marker_idx += 1
rospy.sleep(2.0)
exit(0)
else:
pub_marker = velmautils.MarkerPublisher()
rospy.sleep(1.0)
markers2 = [
[46, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.0,1.0),PyKDL.Vector(-0.0,-0.0,-0.0))],
[47, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0161417497092,-9.92379339669e-05,-0.00603105214296,0.999851519216),PyKDL.Vector(-0.0987990318312,-0.000265582834399,0.000544628226204))],
[48, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.706829255712,0.00114672638679,-0.707103949357,0.0198769487466),PyKDL.Vector(0.0427261427894,0.00245374838957,-0.116698579624))],
[49, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.706230029879,-0.00660144281521,-0.70769079068,0.0192174565616),PyKDL.Vector(0.0410352237403,0.000595788239244,-0.0336956438972))],
[50, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0155276433453,0.714580229904,0.00743395758828,0.699341635824),PyKDL.Vector(-0.131991504795,-0.00410930997885,-0.0916799439467))],
[51, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.706160148032,0.0114839861434,-0.707838133957,0.0130820300375),PyKDL.Vector(-0.150701418215,-0.000688426198715,-0.035762545196))],
[52, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.705850422242,0.00260066465892,-0.708005925475,0.022271673869),PyKDL.Vector(-0.150663515172,-0.00196494029406,-0.123356224597))],
[53, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.00630495805624,-0.999979097775,0.000308879730173,0.00139860956497),PyKDL.Vector(-0.0116053782242,0.000352840524022,-0.0267278839783))],
[54, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00853722085061,-0.999853611966,0.00230495916657,0.0146477869582),PyKDL.Vector(-0.0949889134574,0.00131718330416,-0.0165548984986))],
[55, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0208301706621,-0.711731154203,0.0052945617051,0.702123091589),PyKDL.Vector(0.0244779541548,0.000463479220587,-0.0804749548707))],
]
markers = [
[46, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0,0.0,0.0,1.0),PyKDL.Vector(-0.0,-0.0,-0.0))],
[47, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.00211977224419,0.00133104595822,-0.00433238039783,0.999987482603),PyKDL.Vector(-0.0995553679408,-0.000651966932258,0.000444468330964))],
[48, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.705539374795,0.00129956423061,-0.708394191186,0.0197527628665),PyKDL.Vector(0.0433256677966,0.00212664651843,-0.116482343501))],
[49, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.704707757517,-0.00628228374428,-0.709255128279,0.0174548680028),PyKDL.Vector(0.0412709849952,0.000494665961165,-0.0338667341872))],
[50, PyKDL.Frame(PyKDL.Rotation.Quaternion(-0.0117276773848,0.706312439798,0.00558379104701,0.707781053891),PyKDL.Vector(-0.131246837996,-0.00469319026484,-0.0943814089463))],
[51, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.710112841604,0.00963407546503,-0.703895468304,0.013345653983),PyKDL.Vector(-0.149984963191,-0.00300459973807,-0.0370193446712))],
[52, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.704691425649,0.00497982940017,-0.709159595229,0.0218601100464),PyKDL.Vector(-0.15277553848,-0.00431480401095,-0.120995842686))],
[53, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.00984066000086,-0.999945658681,0.00299307949816,0.00169781765667),PyKDL.Vector(-0.0132269965227,-0.00110379001368,-0.0274175040768))],
[54, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.00154140261629,0.999633307109,-0.00751679824708,0.0259686953912),PyKDL.Vector(-0.0974091549673,-0.000670004842722,-0.0319416169884))],
[55, PyKDL.Frame(PyKDL.Rotation.Quaternion(0.0195903374145,-0.713404165076,0.00273342374532,0.700473585745),PyKDL.Vector(0.0250633176495,-0.00356911439713,-0.0811403242495))],
]
m_id = 0
for m in markers:
print m[0]
print m[1]
m_id = pub_marker.publishFrameMarker(m[1], m_id, scale=0.1, frame='world', namespace='default')
rospy.sleep(0.1)
def poseUpdaterThread(self, args, *args2):
index = 0
z_limit = 0.3
while not rospy.is_shutdown():
rospy.sleep(0.1)
if self.allow_update_objects_pose == None or not self.allow_update_objects_pose:
continue
for obj_name in self.dyn_obj_markers:
obj = self.dyn_obj_markers[obj_name]
visible_markers_Br_Co = []
visible_markers_weights_ori = []
visible_markers_weights_pos = []
visible_markers_idx = []
for marker in obj:#.markers:
T_Br_M = self.getMarkerPose(marker[0], wait = False, timeBack = 0.3)
if T_Br_M != None and self.velma != None:
T_B_C = self.getCameraPose()
T_C_M = T_B_C.Inverse() * T_Br_M
v = T_C_M * PyKDL.Vector(0,0,1) - T_C_M * PyKDL.Vector()
if v.z() > -z_limit:
continue
# v.z() is in range (-1.0, -0.3)
weight = ((-v.z()) - z_limit)/(1.0-z_limit)
if weight > 1.0 or weight < 0.0:
print "error: weight==%s"%(weight)
T_Co_M = marker[1]
T_Br_Co = T_Br_M * T_Co_M.Inverse()
visible_markers_Br_Co.append(T_Br_Co)
visible_markers_weights_ori.append(1.0-weight)
visible_markers_weights_pos.append(weight)
visible_markers_idx.append(marker[0])
if len(visible_markers_Br_Co) > 0:
# if obj.name == "object":
# print "vis: %s"%(visible_markers_idx)
# print "w_o: %s"%(visible_markers_weights_ori)
# print "w_p: %s"%(visible_markers_weights_pos)
# first calculate mean pose without weights
R_B_Co = velmautils.meanOrientation(visible_markers_Br_Co)[1]
p_B_Co = velmautils.meanPosition(visible_markers_Br_Co)
T_B_Co = PyKDL.Frame(copy.deepcopy(R_B_Co.M), copy.deepcopy(p_B_Co))
distances = []
for m_idx in range(0, len(visible_markers_Br_Co)):
diff = PyKDL.diff(T_B_Co, visible_markers_Br_Co[m_idx])
distances.append( [diff, m_idx] )
Br_Co_list = []
weights_ori = []
weights_pos = []
for d in distances:
if d[0].vel.Norm() > 0.04 or d[0].rot.Norm() > 15.0/180.0*math.pi:
continue
Br_Co_list.append( visible_markers_Br_Co[d[1]] )
weights_ori.append( visible_markers_weights_ori[d[1]] )
weights_pos.append( visible_markers_weights_pos[d[1]] )
if len(Br_Co_list) > 0:
R_B_Co = velmautils.meanOrientation(Br_Co_list, weights=weights_ori)[1]
p_B_Co = velmautils.meanPosition(Br_Co_list, weights=weights_pos)
# obj.updatePose( PyKDL.Frame(copy.deepcopy(R_B_Co.M), copy.deepcopy(p_B_Co)) )
self.openrave.updatePose(obj_name, T_Br_Co)
index += 1
if index >= 100:
index = 0
def poseUpdaterThread(self, args, *args2):
index = 0
z_limit = 0.3
while not rospy.is_shutdown():
rospy.sleep(0.1)
if self.allow_update_objects_pose == None or not self.allow_update_objects_pose:
continue
for obj in self.objects:
visible_markers_Br_Co = []
visible_markers_weights_ori = []
visible_markers_weights_pos = []
visible_markers_idx = []
for marker in obj.markers:
T_Br_M = self.getMarkerPose(marker[0],
wait=False,
timeBack=0.3)
if T_Br_M != None and self.velma != None:
T_B_C = self.velma.T_B_C #self.getCameraPose()
T_C_M = T_B_C.Inverse() * T_Br_M
v = T_C_M * PyKDL.Vector(0, 0,
1) - T_C_M * PyKDL.Vector()
if v.z() > -z_limit:
continue
# v.z() is in range (-1.0, -0.3)
weight = ((-v.z()) - z_limit) / (1.0 - z_limit)
if weight > 1.0 or weight < 0.0:
print "error: weight==%s" % (weight)
T_Co_M = marker[1]
T_Br_Co = T_Br_M * T_Co_M.Inverse()
visible_markers_Br_Co.append(T_Br_Co)
visible_markers_weights_ori.append(1.0 - weight)
visible_markers_weights_pos.append(weight)
visible_markers_idx.append(marker[0])
if len(visible_markers_Br_Co) > 0:
# if obj.name == "object":
# print "vis: %s"%(visible_markers_idx)
# print "w_o: %s"%(visible_markers_weights_ori)
# print "w_p: %s"%(visible_markers_weights_pos)
# first calculate mean pose without weights
R_B_Co = velmautils.meanOrientation(
visible_markers_Br_Co)[1]
p_B_Co = velmautils.meanPosition(visible_markers_Br_Co)
T_B_Co = PyKDL.Frame(copy.deepcopy(R_B_Co.M),
copy.deepcopy(p_B_Co))
distances = []
for m_idx in range(0, len(visible_markers_Br_Co)):
diff = PyKDL.diff(T_B_Co, visible_markers_Br_Co[m_idx])
distances.append([diff, m_idx])
Br_Co_list = []
weights_ori = []
weights_pos = []
for d in distances:
if d[0].vel.Norm() > 0.04 or d[0].rot.Norm(
) > 15.0 / 180.0 * math.pi:
continue
Br_Co_list.append(visible_markers_Br_Co[d[1]])
weights_ori.append(visible_markers_weights_ori[d[1]])
weights_pos.append(visible_markers_weights_pos[d[1]])
if len(Br_Co_list) > 0:
R_B_Co = velmautils.meanOrientation(
Br_Co_list, weights=weights_ori)[1]
p_B_Co = velmautils.meanPosition(Br_Co_list,
weights=weights_pos)
obj.updatePose(
PyKDL.Frame(copy.deepcopy(R_B_Co.M),
copy.deepcopy(p_B_Co)))
self.openrave.updatePose(obj.name, T_Br_Co)
index += 1
if index >= 100:
index = 0