def fitDrillBarrel ( drillPoints, forwardDirection, plane_origin, plane_normal):
''' Given a point cloud which ONLY contains points from a barrell drill, standing upright
and the equations of a table its resting on, and the general direction of the robot
Fit a barrell drill
'''
if not drillPoints.GetNumberOfPoints():
return
vis.updatePolyData(drillPoints, 'drill cluster', parent=getDebugFolder(), visible=False)
drillBarrelPoints = thresholdPoints(drillPoints, 'dist_to_plane', [0.177, 0.30])
if not drillBarrelPoints.GetNumberOfPoints():
return
# fit line to drill barrel points
linePoint, lineDirection, _ = applyLineFit(drillBarrelPoints, distanceThreshold=0.5)
if np.dot(lineDirection, forwardDirection) < 0:
lineDirection = -lineDirection
vis.updatePolyData(drillBarrelPoints, 'drill barrel points', parent=getDebugFolder(), visible=False)
pts = vtkNumpy.getNumpyFromVtk(drillBarrelPoints, 'Points')
dists = np.dot(pts-linePoint, lineDirection)
p1 = linePoint + lineDirection*np.min(dists)
p2 = linePoint + lineDirection*np.max(dists)
p1 = projectPointToPlane(p1, plane_origin, plane_normal)
p2 = projectPointToPlane(p2, plane_origin, plane_normal)
d = DebugData()
d.addSphere(p1, radius=0.01)
d.addSphere(p2, radius=0.01)
d.addLine(p1, p2)
vis.updatePolyData(d.getPolyData(), 'drill debug points', color=[0,1,0], parent=getDebugFolder(), visible=False)
drillToBasePoint = np.array([-0.07, 0.0 , -0.12])
zaxis = plane_normal
xaxis = lineDirection
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
t = getTransformFromAxes(xaxis, yaxis, zaxis)
t.PreMultiply()
t.Translate(-drillToBasePoint)
t.PostMultiply()
t.Translate(p1)
return t
def fitSwitchBox(self, polyData, points):
boxPosition = points[0]
wallPoint = points[1]
# find a frame that is aligned with wall
searchRadius = 0.2
planePoints, normal = segmentation.applyLocalPlaneFit(polyData, points[0], searchRadius=np.linalg.norm(points[1] - points[0]), searchRadiusEnd=1.0)
obj = vis.updatePolyData(planePoints, 'wall plane points', color=[0,1,0], visible=False)
obj.setProperty('Point Size', 7)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
if np.dot(normal, viewDirection) < 0:
normal = -normal
origin = segmentation.computeCentroid(planePoints)
zaxis = [0,0,1]
xaxis = normal
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
zaxis = np.cross(xaxis, yaxis)
zaxis /= np.linalg.norm(zaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
# translate that frame to the box position
t.PostMultiply()
t.Translate(boxPosition)
boxFrame = transformUtils.copyFrame(t)
self.switchPlanner.spawnBoxAffordanceAtFrame(boxFrame)
def run(self):
polyData = self.getPointCloud()
annotation = self.getAnnotationInput()
annotationPoint = annotation.annotationPoints[0]
planePoints, normal = segmentation.applyLocalPlaneFit(polyData, annotationPoint, searchRadius=0.1, searchRadiusEnd=0.2)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
if np.dot(normal, viewDirection) < 0:
normal = -normal
xaxis = normal
zaxis = [0, 0, 1]
yaxis = np.cross(zaxis, xaxis)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis /= np.linalg.norm(yaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
t.Translate(annotationPoint)
polyData = annotation.polyData
polyData = segmentation.transformPolyData(polyData, t.GetLinearInverse())
annotation.setProperty('Visible', False)
om.removeFromObjectModel(om.findObjectByName('wall'))
obj = vis.showPolyData(polyData, 'wall')
obj.actor.SetUserTransform(t)
vis.showFrame(t, 'wall frame', scale=0.2, parent=obj)
def run(self):
polyData = self.getPointCloud()
annotation = self.getAnnotationInput()
annotationPoint = annotation.annotationPoints[0]
planePoints, normal = segmentation.applyLocalPlaneFit(polyData, annotationPoint, searchRadius=0.1, searchRadiusEnd=0.2)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
if np.dot(normal, viewDirection) < 0:
normal = -normal
xaxis = normal
zaxis = [0, 0, 1]
yaxis = np.cross(zaxis, xaxis)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis /= np.linalg.norm(yaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
t.Translate(annotationPoint)
polyData = annotation.polyData
polyData = segmentation.transformPolyData(polyData, t.GetLinearInverse())
annotation.setProperty('Visible', False)
om.removeFromObjectModel(om.findObjectByName('wall'))
obj = vis.showPolyData(polyData, 'wall')
obj.actor.SetUserTransform(t)
vis.showFrame(t, 'wall frame', scale=0.2, parent=obj)
def computeRobotStanceFrame(self, objectTransform,
relativeStanceTransform):
'''
Given a robot model, determine the height of the ground
using an XY and Yaw standoff, combined to determine the relative 6DOF standoff
For a grasp or approach stance
'''
groundFrame = self.computeGroundFrame(self.robotModel)
groundHeight = groundFrame.GetPosition()[2]
graspPosition = np.array(objectTransform.GetPosition())
graspYAxis = [0.0, 1.0, 0.0]
graspZAxis = [0.0, 0.0, 1.0]
objectTransform.TransformVector(graspYAxis, graspYAxis)
objectTransform.TransformVector(graspZAxis, graspZAxis)
xaxis = graspYAxis
#xaxis = graspZAxis
zaxis = [0, 0, 1]
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
xaxis = np.cross(yaxis, zaxis)
graspGroundTransform = transformUtils.getTransformFromAxes(
xaxis, yaxis, zaxis)
graspGroundTransform.PostMultiply()
graspGroundTransform.Translate(graspPosition[0], graspPosition[1],
groundHeight)
robotStance = transformUtils.copyFrame(relativeStanceTransform)
robotStance.Concatenate(graspGroundTransform)
return robotStance
def computeGroundFrame(self, robotModel):
'''
Given a robol model, returns a vtkTransform at a position between
the feet, on the ground, with z-axis up and x-axis aligned with the
robot pelvis x-axis.
'''
t1 = robotModel.getLinkFrame(self.ikPlanner.leftFootLink)
t2 = robotModel.getLinkFrame(self.ikPlanner.rightFootLink)
pelvisT = robotModel.getLinkFrame(self.ikPlanner.pelvisLink)
xaxis = [1.0, 0.0, 0.0]
pelvisT.TransformVector(xaxis, xaxis)
xaxis = np.array(xaxis)
zaxis = np.array([0.0, 0.0, 1.0])
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
xaxis = np.cross(yaxis, zaxis)
stancePosition = (np.array(t2.GetPosition()) +
np.array(t1.GetPosition())) / 2.0
footHeight = 0.0811
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
t.Translate(stancePosition)
t.Translate([0.0, 0.0, -footHeight])
return t
def computeRobotStanceFrame(self, objectTransform, relativeStanceTransform ):
'''
Given a robot model, determine the height of the ground
using an XY and Yaw standoff, combined to determine the relative 6DOF standoff
For a grasp or approach stance
'''
groundFrame = self.computeGroundFrame(self.robotModel)
groundHeight = groundFrame.GetPosition()[2]
graspPosition = np.array(objectTransform.GetPosition())
graspYAxis = [0.0, 1.0, 0.0]
graspZAxis = [0.0, 0.0, 1.0]
objectTransform.TransformVector(graspYAxis, graspYAxis)
objectTransform.TransformVector(graspZAxis, graspZAxis)
xaxis = graspYAxis
#xaxis = graspZAxis
zaxis = [0, 0, 1]
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
xaxis = np.cross(yaxis, zaxis)
graspGroundTransform = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
graspGroundTransform.PostMultiply()
graspGroundTransform.Translate(graspPosition[0], graspPosition[1], groundHeight)
robotStance = transformUtils.copyFrame( relativeStanceTransform )
robotStance.Concatenate(graspGroundTransform)
return robotStance
def computeGroundFrame(self, robotModel):
'''
Given a robol model, returns a vtkTransform at a position between
the feet, on the ground, with z-axis up and x-axis aligned with the
robot pelvis x-axis.
'''
t1 = robotModel.getLinkFrame( self.ikPlanner.leftFootLink )
t2 = robotModel.getLinkFrame( self.ikPlanner.rightFootLink )
pelvisT = robotModel.getLinkFrame( self.ikPlanner.pelvisLink )
xaxis = [1.0, 0.0, 0.0]
pelvisT.TransformVector(xaxis, xaxis)
xaxis = np.array(xaxis)
zaxis = np.array([0.0, 0.0, 1.0])
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
xaxis = np.cross(yaxis, zaxis)
stancePosition = (np.array(t2.GetPosition()) + np.array(t1.GetPosition())) / 2.0
footHeight = 0.0811
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
t.Translate(stancePosition)
t.Translate([0.0, 0.0, -footHeight])
return t
def fitObjectFromPolyData(polyData):
origin, edges, wireframe = segmentation.getOrientedBoundingBox(polyData)
edgeLengths = [float(np.linalg.norm(edge)) for edge in edges]
axes = [edge / np.linalg.norm(edge) for edge in edges]
boxCenter = segmentation.computeCentroid(wireframe)
t = transformUtils.getTransformFromAxes(axes[0], axes[1], axes[2])
zAxisIndex, zAxis, zAxisSign = transformUtils.findTransformAxis(
t, [0, 0, 1.0])
xAxisIndex, xAxis, xAxisSign = transformUtils.findTransformAxis(
t, [1.0, 0, 0])
assert zAxisIndex != xAxisIndex
#zAxis = zAxis*zAxisSign
zAxis = [0, 0, 1.0]
xAxis = xAxis * xAxisSign
yAxis = np.cross(zAxis, xAxis)
xAxis = np.cross(yAxis, zAxis)
zLength = edgeLengths[zAxisIndex]
xLength = edgeLengths[xAxisIndex]
ids = [0, 1, 2]
ids.remove(zAxisIndex)
ids.remove(xAxisIndex)
yLength = edgeLengths[ids[0]]
t = transformUtils.getTransformFromAxes(xAxis, yAxis, zAxis)
t.PostMultiply()
t.Translate(boxCenter)
om.removeFromObjectModel(om.findObjectByName('box'))
obj = makeBox()
obj.setProperty('Dimensions', [xLength, yLength, zLength])
obj.getChildFrame().copyFrame(t)
#obj.getChildFrame().setProperty('Visible', True)
obj.setProperty('Surface Mode', 'Wireframe')
obj.setProperty('Color', [1, 0, 0])
return obj
def placeHandModel(displayPoint, view, side='left'):
obj, _ = vis.findPickedObject(displayPoint, view)
if isinstance(obj, vis.FrameItem):
_, handFrame = handFactory.placeHandModelWithTransform(
obj.transform, view, side=side, parent=obj.parent())
handFrame.frameSync = vis.FrameSync()
handFrame.frameSync.addFrame(obj)
handFrame.frameSync.addFrame(handFrame, ignoreIncoming=True)
return
pickedPointFields = vis.pickPoint(displayPoint,
view,
pickType='cells',
tolerance=0.0)
pickedPoint = pickedPointFields.pickedPoint
prop = pickedPointFields.pickedProp
obj = vis.getObjectByProp(prop)
if not obj:
return
yaxis = -normal
zaxis = [0, 0, 1]
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
zaxis = np.cross(xaxis, yaxis)
zaxis /= np.linalg.norm(zaxis)
t = transformUtils.getTransformFromAxes(-zaxis, yaxis, xaxis)
t.PostMultiply()
t.Translate(pickedPoint)
if side == 'right':
t.PreMultiply()
t.RotateY(180)
handObj, handFrame = handFactory.placeHandModelWithTransform(t,
view,
side=side,
parent=obj)
syncFrame = getChildFrame(obj)
if syncFrame:
handFrame.frameSync = vis.FrameSync()
handFrame.frameSync.addFrame(handFrame, ignoreIncoming=True)
handFrame.frameSync.addFrame(syncFrame)
def placeHandModel(displayPoint, view, side='left'):
obj, _ = vis.findPickedObject(displayPoint, view)
if isinstance(obj, vis.FrameItem):
_, handFrame = handFactory.placeHandModelWithTransform(obj.transform, view, side=side, parent=obj.parent())
handFrame.frameSync = vis.FrameSync()
handFrame.frameSync.addFrame(obj)
handFrame.frameSync.addFrame(handFrame, ignoreIncoming=True)
return
pickedPointFields = vis.pickPoint(displayPoint, view, pickType='cells', tolerance=0.0)
pickedPoint = pickedPointFields.pickedPoint
prop = pickedPointFields.pickedProp
obj = vis.getObjectByProp(prop)
if not obj:
return
yaxis = -normal
zaxis = [0,0,1]
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
zaxis = np.cross(xaxis, yaxis)
zaxis /= np.linalg.norm(zaxis)
t = transformUtils.getTransformFromAxes(-zaxis, yaxis, xaxis)
t.PostMultiply()
t.Translate(pickedPoint)
if side == 'right':
t.PreMultiply()
t.RotateY(180)
handObj, handFrame = handFactory.placeHandModelWithTransform(t, view, side=side, parent=obj)
syncFrame = getChildFrame(obj)
if syncFrame:
handFrame.frameSync = vis.FrameSync()
handFrame.frameSync.addFrame(handFrame, ignoreIncoming=True)
handFrame.frameSync.addFrame(syncFrame)
def createGoalSteps(self, model, pose):
distanceForward = 1.0
fr = model.getLinkFrame(_leftFootLink)
fl = model.getLinkFrame(_rightFootLink)
pelvisT = model.getLinkFrame(_pelvisLink)
xaxis = [1.0, 0.0, 0.0]
pelvisT.TransformVector(xaxis, xaxis)
xaxis = np.array(xaxis)
zaxis = np.array([0.0, 0.0, 1.0])
yaxis = np.cross(zaxis, xaxis)
xaxis = np.cross(yaxis, zaxis)
numGoalSteps = 3
is_right_foot = True
self.goalSteps = []
for i in range(numGoalSteps):
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
if is_right_foot:
t.Translate(fr.GetPosition())
else:
t.Translate(fl.GetPosition())
t.Translate(xaxis*distanceForward)
distanceForward += 0.15
is_right_foot = not is_right_foot
step = lcmdrc.footstep_t()
step.pos = positionMessageFromFrame(t)
step.is_right_foot = is_right_foot
step.params = self.getDefaultStepParams()
self.goalSteps.append(step)
request = self.constructFootstepPlanRequest(pose)
request.num_goal_steps = len(self.goalSteps)
request.goal_steps = self.goalSteps
self.sendFootstepPlanRequest(request)
def getOrientedBoundingBox(pd):
mesh = pd
origin, edges, wireframe = segmentation.getOrientedBoundingBox(mesh)
edgeLengths = np.array([np.linalg.norm(edge) for edge in edges])
axes = [edge / np.linalg.norm(edge) for edge in edges]
# find axis nearest to the +/- up vector
upVector = [0, 0, 1]
dotProducts = [np.abs(np.dot(axe, upVector)) for axe in axes]
zAxisIndex = np.argmax(dotProducts)
# re-index axes and edge lengths so that the found axis is the z axis
axesInds = [(zAxisIndex + 1) % 3, (zAxisIndex + 2) % 3, zAxisIndex]
axes = [axes[i] for i in axesInds]
edgeLengths = [edgeLengths[i] for i in axesInds]
# flip if necessary so that z axis points toward up
if np.dot(axes[2], upVector) < 0:
axes[1] = -axes[1]
axes[2] = -axes[2]
boxCenter = segmentation.computeCentroid(wireframe)
t = transformUtils.getTransformFromAxes(axes[0], axes[1], axes[2])
t.PostMultiply()
t.Translate(boxCenter)
pd = filterUtils.transformPolyData(pd, t.GetLinearInverse())
wireframe = filterUtils.transformPolyData(wireframe,
t.GetLinearInverse())
return FieldContainer(points=pd,
box=wireframe,
frame=t,
dims=edgeLengths,
axes=axes)
