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 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 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 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
pickedPoint, prop, _, normal = vis.pickPoint(displayPoint, view, pickType='cells', tolerance=0.0, returnNormal=True)
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)
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 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 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 onSegmentBin(self, p1, p2):
print p1
print p2
self.picker.stop()
om.removeFromObjectModel(self.picker.annotationObj)
self.picker = None
om.removeFromObjectModel(om.findObjectByName('bin frame'))
binEdge = p2 - p1
zaxis = [0.0, 0.0, 1.0]
xaxis = np.cross(binEdge, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
t.Translate(p1)
self.binFrame = vis.showFrame(t, 'bin frame', parent=None, scale=0.2)
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 = transformUtils.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 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 = transformUtils.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 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
