def drawForce(self, name, linkName, forceLocation, force, color, key=None):
forceDirection = force / np.linalg.norm(force)
# om.removeFromObjectModel(om.findObjectByName(name))
linkToWorld = self.robotStateModel.getLinkFrame(linkName)
forceLocationInWorld = np.array(
linkToWorld.TransformPoint(forceLocation))
forceDirectionInWorld = np.array(
linkToWorld.TransformDoubleVector(forceDirection))
# point = forceLocationInWorld - 0.1*forceDirectionInWorld
# d = DebugData()
# # d.addArrow(point, forceLocationInWorld, headRadius=0.025, tubeRadius=0.005, color=color)
# d.addSphere(forceLocationInWorld, radius=0.01)
# d.addLine(point, forceLocationInWorld, radius=0.005)
transformForVis = transformUtils.getTransformFromOriginAndNormal(
forceLocationInWorld, forceDirectionInWorld)
obj = vis.updatePolyData(self.plungerPolyData, name, color=color)
obj.actor.SetUserTransform(transformForVis)
if key is not None and om.findObjectByName(name) is not None:
obj.addProperty('magnitude',
self.externalForces[key]['forceMagnitude'])
obj.addProperty('linkName', linkName)
obj.addProperty('key', key)
obj.connectRemovedFromObjectModel(self.removeForceFromFrameObject)
obj.properties.connectPropertyChanged(
functools.partial(self.onPropertyChanged, obj))
return obj
def fitObjectsOnShelf(polyData, maxHeight = 0.25):
# find the shelf plane:
polyDataWithoutFront, _ = segmentation.removeMajorPlane(polyData, distanceThreshold=0.02)
polyDataPlaneFit, origin, normal = segmentation.applyPlaneFit(polyDataWithoutFront, expectedNormal=np.array([0.0,0.0,1.0]), perpendicularAxis=np.array([0.0,0.0,1.0]), returnOrigin=True)
vis.updatePolyData(polyDataPlaneFit, 'polyDataPlaneFit', parent='segmentation', visible=False)
shelfSurfacePoints = segmentation.thresholdPoints(polyDataPlaneFit, 'dist_to_plane', [-0.01, 0.01])
shelfCenter = segmentation.computeCentroid(shelfSurfacePoints)
shelfFrame = transformUtils.getTransformFromOriginAndNormal(shelfCenter, normal, normalAxis=2)
vis.showFrame(shelfFrame, 'shelfFrame', parent='segmentation', scale=0.15 , visible=False)
# find the points near to the shelf plane and find objects on it:
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
shelfPoints = segmentation.thresholdPoints(polyData, 'dist_to_plane', [-0.01, maxHeight])
vis.updatePolyData(shelfPoints, 'shelf', parent='segmentation', visible=False)
data = segmentation.segmentTableScene(shelfPoints, shelfCenter, filterClustering = False )
vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')
# remove the points that we considered from the orginal cloud
dists = vnp.getNumpyFromVtk(polyData, 'dist_to_plane')
diffShelf = ( ((dists > maxHeight) + (dists < -0.01))) + 0.1 -0.1
vnp.addNumpyToVtk(polyData, diffShelf, 'diff_shelf')
polyData = segmentation.thresholdPoints(polyData, 'diff_shelf', [1, 1])
vis.updatePolyData(polyData, 'rest', parent='segmentation', visible=False)
return polyData
def removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95):
yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis]
backY = np.percentile(yvalues, percentile)
if ( percentile > 50):
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf])
else:
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1])
vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False)
# find the plane of the back wall, remove it and the points behind it:
_, origin, normal = segmentation.applyPlaneFit(searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True)
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2)
vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15 , visible=False)
vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False)
polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange)
vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False)
return polyData
def fitObjectsOnShelf(polyData, maxHeight = 0.25):
# find the shelf plane:
polyDataWithoutFront, _ = segmentation.removeMajorPlane(polyData, distanceThreshold=0.02)
polyDataPlaneFit, origin, normal = segmentation.applyPlaneFit(polyDataWithoutFront, expectedNormal=np.array([0.0,0.0,1.0]), perpendicularAxis=np.array([0.0,0.0,1.0]), returnOrigin=True)
vis.updatePolyData(polyDataPlaneFit, 'polyDataPlaneFit', parent='segmentation', visible=False)
shelfSurfacePoints = segmentation.thresholdPoints(polyDataPlaneFit, 'dist_to_plane', [-0.01, 0.01])
shelfCenter = segmentation.computeCentroid(shelfSurfacePoints)
shelfFrame = transformUtils.getTransformFromOriginAndNormal(shelfCenter, normal, normalAxis=2)
vis.showFrame(shelfFrame, 'shelfFrame', parent='segmentation', scale=0.15 , visible=False)
# find the points near to the shelf plane and find objects on it:
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
shelfPoints = segmentation.thresholdPoints(polyData, 'dist_to_plane', [-0.01, maxHeight])
vis.updatePolyData(shelfPoints, 'shelf', parent='segmentation', visible=False)
data = segmentation.segmentTableScene(shelfPoints, shelfCenter, filterClustering = False )
vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')
# remove the points that we considered from the orginal cloud
dists = vnp.getNumpyFromVtk(polyData, 'dist_to_plane')
diffShelf = ( ((dists > maxHeight) + (dists < -0.01))) + 0.1 -0.1
vnp.addNumpyToVtk(polyData, diffShelf, 'diff_shelf')
polyData = segmentation.thresholdPoints(polyData, 'diff_shelf', [1, 1])
vis.updatePolyData(polyData, 'rest', parent='segmentation', visible=False)
return polyData
def removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95):
yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis]
backY = np.percentile(yvalues, percentile)
if ( percentile > 50):
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf])
else:
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1])
vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False)
# find the plane of the back wall, remove it and the points behind it:
_, origin, normal = segmentation.applyPlaneFit(searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True)
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2)
vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15 , visible=False)
vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False)
polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange)
vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False)
return polyData
def computeGraspPlan(self,
targetFrame,
graspToHandFrame,
inLine=False,
ikParameters=None):
startPose = self.getPlanningStartPose()
endPose, constraintSet = self.computeGraspPose(startPose, targetFrame)
if ikParameters:
constraintSet.ikParameters = ikParameters
constraintSet.ikParameters.usePointwise = False
if inLine:
handLinkName = self.ikPlanner.getHandLink(self.graspingHand)
graspToHand = graspToHandFrame
handToWorld1 = self.ikPlanner.getLinkFrameAtPose(
handLinkName, startPose)
handToWorld2 = self.ikPlanner.getLinkFrameAtPose(
handLinkName, endPose)
handToWorld1 = transformUtils.concatenateTransforms(
[graspToHand, handToWorld1])
handToWorld2 = transformUtils.concatenateTransforms(
[graspToHand, handToWorld2])
motionVector = np.array(handToWorld2.GetPosition()) - np.array(
handToWorld1.GetPosition())
motionTargetFrame = transformUtils.getTransformFromOriginAndNormal(
np.array(handToWorld2.GetPosition()), motionVector)
#vis.updateFrame(motionTargetFrame, 'motion target frame', scale=0.1)
#d = DebugData()
#d.addLine(np.array(handToWorld2.GetPosition()), np.array(handToWorld2.GetPosition()) - motionVector)
#vis.updatePolyData(d.getPolyData(), 'motion vector', visible=False)
p = self.ikPlanner.createLinePositionConstraint(
handLinkName,
graspToHand,
motionTargetFrame,
lineAxis=2,
bounds=[-np.linalg.norm(motionVector), 0.001],
positionTolerance=0.001)
p.tspan = np.linspace(0, 1, 5)
constraintSet.constraints.append(p)
newPlan = constraintSet.runIkTraj()
else:
newPlan = self.ikPlanner.computePostureGoal(startPose, endPose)
return newPlan
def rotateReconstructionToStandardOrientation(self,
pointCloud=None,
savePoseToFile=True,
filename=None):
"""
Rotates the reconstruction to right side up and saves the transform to a file
:param pointcloud:
:return:
"""
if pointCloud is None:
pointCloud = om.findObjectByName('reconstruction').polyData
returnData = self.segmentTable(scenePolyData=pointCloud,
visualize=False)
normal = returnData['normal']
polyData = returnData['polyData']
# get transform that rotates normal --> [0,0,1]
origin = returnData['pointOnTable'] - 0.5 * normal
pointCloudToWorldTransform = transformUtils.getTransformFromOriginAndNormal(
origin, normal).GetLinearInverse()
rotatedPolyData = filterUtils.transformPolyData(
polyData, pointCloudToWorldTransform)
parent = om.getOrCreateContainer('segmentation')
vis.updatePolyData(rotatedPolyData,
'reconstruction',
colorByName='RGB',
parent=parent)
if savePoseToFile:
if filename is None:
assert self.pathDict is not None
filename = self.pathDict['transforms']
(pos, quat
) = transformUtils.poseFromTransform(pointCloudToWorldTransform)
d = dict()
d['firstFrameToWorld'] = [pos.tolist(), quat.tolist()]
utils.saveDictToYaml(d, filename)
return pointCloudToWorldTransform
def computeGraspPlan(self, targetFrame, graspToHandFrame, inLine=False, ikParameters=None):
startPose = self.getPlanningStartPose()
endPose, constraintSet = self.computeGraspPose(startPose, targetFrame)
if ikParameters:
constraintSet.ikParameters = ikParameters
constraintSet.ikParameters.usePointwise = False
if inLine:
handLinkName = self.ikPlanner.getHandLink(self.graspingHand)
graspToHand = graspToHandFrame
handToWorld1 = self.ikPlanner.getLinkFrameAtPose(handLinkName, startPose)
handToWorld2 = self.ikPlanner.getLinkFrameAtPose(handLinkName, endPose)
handToWorld1 = transformUtils.concatenateTransforms([graspToHand, handToWorld1])
handToWorld2 = transformUtils.concatenateTransforms([graspToHand, handToWorld2])
motionVector = np.array(handToWorld2.GetPosition()) - np.array(handToWorld1.GetPosition())
motionTargetFrame = transformUtils.getTransformFromOriginAndNormal(np.array(handToWorld2.GetPosition()), motionVector)
#vis.updateFrame(motionTargetFrame, 'motion target frame', scale=0.1)
#d = DebugData()
#d.addLine(np.array(handToWorld2.GetPosition()), np.array(handToWorld2.GetPosition()) - motionVector)
#vis.updatePolyData(d.getPolyData(), 'motion vector', visible=False)
p = self.ikPlanner.createLinePositionConstraint(handLinkName, graspToHand, motionTargetFrame,
lineAxis=2, bounds=[-np.linalg.norm(motionVector), 0.001], positionTolerance=0.001)
p.tspan = np.linspace(0, 1, 5)
constraintSet.constraints.append(p)
newPlan = constraintSet.runIkTraj()
else:
newPlan = self.ikPlanner.computePostureGoal(startPose, endPose)
return newPlan
