def loadConfig(self, configFilename):
if configFilename is None:
configFilename = 'data_collection.yaml'
fullFilename = os.path.join(CorlUtils.getCorlBaseDir(), 'config',
configFilename)
self.config = CorlUtils.getDictFromYamlFilename(fullFilename)
def runSuperPCS4(modelName, sceneName):
modelPointCloud = om.findObjectByName(modelName).polyData
scenePointCloud = om.findObjectByName(sceneName).polyData
baseName = os.path.join(CorlUtils.getCorlDataDir(),
'registration-output/robot-scene')
modelFile = os.path.join(baseName, 'model_data_for_pcs4.ply')
sceneFile = os.path.join(baseName, 'scene_data_for_pcs4.ply')
ioUtils.writePolyData(modelPointCloud, modelFile)
ioUtils.writePolyData(scenePointCloud, sceneFile)
super4PCSBaseDir = CorlUtils.getSuper4PCSBaseDir()
registrationBin = os.path.join(super4PCSBaseDir, 'build/Super4PCS')
outputFile = os.path.join(CorlUtils.getCorlBaseDir(),
'sandbox/pcs4_mat_output.txt')
overlap = 0.9
distance = 0.02
timeout = 1000
numSamples = 200
registrationArgs = [
registrationBin,
'-i',
modelFile,
sceneFile,
'-o %f' % overlap,
'-d %f' % distance,
'-t %d' % timeout,
'-n %d' % numSamples,
'-r pcs4_transformed_output.ply',
'-m %s' % outputFile]
registrationArgs = ' '.join(registrationArgs).split()
if os.path.isfile(outputFile):
print 'removing', outputFile
os.remove(outputFile)
print 'calling super pcs4...'
print
print ' '.join(registrationArgs)
subprocess.check_call(registrationArgs)
print 'done.'
showAlignedData(sceneName, outputFile)
def __init__(self, robotSystem, openniDepthPointCloud, measurementPanel,
imageManager):
self.robotSystem = robotSystem
self.openniDepthPointCloud = openniDepthPointCloud
self.measurementPanel = measurementPanel
self.imageManager = imageManager
self.visFolder = om.getOrCreateContainer('data collection')
self.cameraName = 'OPENNI_FRAME_LEFT'
self.savedTransformFilename = os.path.join(
CorlUtils.getCorlBaseDir(), 'sandbox',
'reconstruction_robot_frame.yaml')
self.frustumVis = dict()
self.loadSavedData()
self.setupDevTools()
def loadData(self):
logFolder = 'logs_test/moving-camera'
self.pathDict = CorlUtils.getFilenames(logFolder)
if self.pathDict is None:
return
self.cameraPoses = CameraPoses(self.pathDict['cameraPoses'])
# load the elastic fusion reconstruction if we already know where to
# put it
self.savedTransformFilename = os.path.join(
CorlUtils.getCorlBaseDir(), 'sandbox',
'reconstruction_robot_frame.yaml')
if os.path.exists(self.savedTransformFilename):
firstFrameToWorld = CorlUtils.getFirstFrameToWorldTransform(
self.savedTransformFilename)
CorlUtils.loadElasticFusionReconstruction(
self.pathDict['reconstruction'], transform=firstFrameToWorld)
def makeTargetCameraFrames(self, filename=None):
self.targetFrames = []
if filename is None:
filename = 'data_collection.yaml'
fullFilename = os.path.join(CorlUtils.getCorlBaseDir(), 'config',
filename)
frameData = CorlUtils.getDictFromYamlFilename(fullFilename)['frames']
graspToHandLinkFrame = CorlUtils.getCameraToKukaEndEffectorFrame()
# d = dict()
# d['rotateX'] = -40
# d['rotateY'] = 30
# d['translateZ'] = -0.8
# d['numFrames'] = 4
# frameData.append(d)
rotationDirection = 1
for data in frameData:
rotateX = data['rotateX']
translateZ = data['translateZ']
numFrames = data['numFrames']
rotateY = data['rotateY']
rotateYGrid = np.linspace(rotateY['min'], rotateY['max'],
numFrames)
for idx in xrange(0, numFrames):
rotateY = rotateYGrid[idx] * rotationDirection
transform = self.makeTargetCameraTransform(
rotateX=rotateX,
rotateY=rotateY,
translateZ=translateZ,
visualize=False)
# check if feasible to reach that frame
ikData = self.runIK(transform,
makePlan=False,
graspToHandLinkFrame=graspToHandLinkFrame)
# if infeasible increase tolerance to 5 degs
if ikData['info'] != 1:
ikData = self.runIK(
transform,
makePlan=False,
graspToHandLinkFrame=graspToHandLinkFrame,
angleToleranceInDegrees=5.0)
if ikData['info'] == 1:
frameName = 'target frame ' + str(len(self.targetFrames))
frame = self.showTargetFrame(transform, frameName)
self.targetFrames.append(frame)
else:
print "\n\n----------"
print "infeasible frame"
print "rotateX = ", rotateX
print "rotateY = ", rotateY
print "translateZ = ", translateZ
print "-----------\n\n"
# alternate the rotation direction each time
rotationDirection = -rotationDirection