def __init__(self):
GLRealtimeProgram.__init__(self,"GLRotationTest")
Ra = so3.rotation((0,1,0),math.pi*-0.9)
Rb = so3.rotation((0,1,0),math.pi*0.9)
print "Angle between"
print so3.matrix(Ra)
print "and"
print so3.matrix(Rb)
print "is",so3.distance(Ra,Rb)
self.Ta = (Ra,(-1,0,1))
self.Tb = (Rb,(1,0,1))
self.T = self.Ta
self.clearColor = [1,1,1,0]
def __init__(self):
GLRealtimeProgram.__init__(self, "GLRotationTest")
Ra = so3.rotation((0, 1, 0), math.pi * -0.9)
Rb = so3.rotation((0, 1, 0), math.pi * 0.9)
print "Angle between"
print so3.matrix(Ra)
print "and"
print so3.matrix(Rb)
print "is", so3.distance(Ra, Rb)
self.Ta = (Ra, (-1, 0, 1))
self.Tb = (Rb, (1, 0, 1))
self.T = self.Ta
self.clearColor = [1, 1, 1, 0]
def shelf_subtract(shelf_cloud, camera_cloud, shelf_model, bounds, downsample=100):
# generate the shelf kdtree
mark = time()
shelf_tree = scipy.spatial.KDTree(shelf_cloud)
logger.info('generated shelf tree in {:.3f}s'.format(time() - mark))
# align the camera cloud with ICP
mark = time()
diff_xform = icp.match(camera_cloud[::downsample], shelf_tree, initial_threshold=0.05)
camera_cloud_aligned = camera_cloud.dot(diff_xform[0].T) + diff_xform[1]
logger.info('aligned camera cloud in {:.3f}s'.format(time() - mark))
# remove all points near the shelf
mark = time()
# _, ind = shelf_tree.query(camera_cloud_aligned, distance_upper_bound=0.005)
# mask = ind >= len(shelf_tree.data)
# segmented_cloud = camera_cloud_aligned[mask]
test_point = GeometricPrimitive()
test_point.setPoint((0, 0, 0))
test_geom = Geometry3D()
test_geom.setGeometricPrimitive(test_point)
def collides(point):
test_geom.setCurrentTransform(so3.identity(), point)
return test_geom.withinDistance(shelf_model.geometry(), 0.01)
mask = map(lambda p: not collides(p), camera_cloud_aligned)
# segmented_cloud = [ p for (i, p) in enumerate(camera_cloud_aligned) if mask[i] ]
camera_cloud_aligned_local = (camera_cloud_aligned - shelf_model.getTransform()[1]).dot(so3.matrix(shelf_model.getTransform()[0]))
mask2 = reduce(numpy.bitwise_and, [ (bounds[0][i] < camera_cloud_aligned_local[:, i]) & (bounds[1][i] > camera_cloud_aligned_local[:, i]) for i in range(3) ])
segmented_cloud = camera_cloud_aligned[mask & mask2]
logger.info('segmented camera cloud in {:.3f}s: {} -> {}'.format(time() - mark, len(camera_cloud_aligned), len(segmented_cloud)))
return diff_xform, camera_cloud_aligned, segmented_cloud, mask & mask2
(rx, ry, rz) = map(lambda a: float(a) * 3.141592/180, args) + ([0] * (3 - len(args)))
xform = master.knowledge_base.shelf_xform
master.knowledge_base.shelf_xform = (
reduce(so3.mul, [ so3.rotation(v, a) for (v, a) in zip([ [1,0,0], [0,1,0], [0,0,1] ], [ rx, ry, rz ]) ]),
xform[1]
)
elif cmd == 'rotl' and len(args) <= 3:
(rx, ry, rz) = map(float, args) + ([0] * (3 - len(args)))
xform = master.knowledge_base.shelf_xform
master.knowledge_base.shelf_xform = (
reduce(so3.mul, [ xform[0] ] + [ so3.rotation(v, a) for (v, a) in zip([ [1,0,0], [0,1,0], [0,0,1] ], [ rx, ry, rz ]) ]),
xform[1]
)
print so3.matrix(master.knowledge_base.shelf_xform[0]), master.knowledge_base.shelf_xform[1]
elif cmd == 'reset' and len(args) == 1:
if args[0] == 'control':
print master.manager._restart_control()
elif args[0] == 'order':
print master._load_apc_order(apc_order)
elif args[0] == 'kb':
print master._create_knowledge_base()
print master._load_apc_order(apc_order)
elif cmd == 'run' and len(args) == 2:
print master.run(args[0], args[1])
for request in master.order:
print ' {}: {}'.format(request['bin'], request['item'])
