def makeDebugRegions(self):
stepWidth = (15 + 3/8.0) * 0.0254
stepLength = (15 + 5/8.0) * 0.0254
stepHeight = (5 + 5/8.0) * 0.0254
stepPoints = np.array([
[-stepLength/2.0, -stepWidth/2.0, 0.0],
[-stepLength/2.0, stepWidth/2.0, 0.0],
[stepLength/2.0, stepWidth/2.0, 0.0],
[stepLength/2.0, -stepWidth/2.0, 0.0]
])
t = vtk.vtkTransform()
t.Translate(0.0, 0.0, 0.0)
t.RotateZ(4.5)
for i in xrange(len(stepPoints)):
stepPoints[i] = np.array(t.TransformPoint(stepPoints[i]))
stepOffset = np.array([stepLength, 0.0, stepHeight])
numSteps = 5
goalFrame = transformUtils.frameFromPositionAndRPY([0.4, 0.0, 0.1], [0,0,0])
vis.showFrame(goalFrame, 'goal frame', scale=0.2)
rpySeed = np.radians(goalFrame.GetOrientation())
for i in xrange(numSteps):
step = stepPoints + (i+1)*stepOffset
self.convertStepToSafeRegion(step, rpySeed)
self.footstepsPanel.onNewWalkingGoal(goalFrame)
def makeDebugRegions(self):
stepWidth = (15 + 3/8.0) * 0.0254
stepLength = (15 + 5/8.0) * 0.0254
stepHeight = (5 + 5/8.0) * 0.0254
stepPoints = np.array([
[-stepLength/2.0, -stepWidth/2.0, 0.0],
[-stepLength/2.0, stepWidth/2.0, 0.0],
[stepLength/2.0, stepWidth/2.0, 0.0],
[stepLength/2.0, -stepWidth/2.0, 0.0]
])
t = vtk.vtkTransform()
t.Translate(0.0, 0.0, 0.0)
t.RotateZ(4.5)
for i in xrange(len(stepPoints)):
stepPoints[i] = np.array(t.TransformPoint(stepPoints[i]))
stepOffset = np.array([stepLength, 0.0, stepHeight])
numSteps = 5
goalFrame = transformUtils.frameFromPositionAndRPY([0.4, 0.0, 0.1], [0,0,0])
vis.showFrame(goalFrame, 'goal frame', scale=0.2)
rpySeed = np.radians(goalFrame.GetOrientation())
for i in xrange(numSteps):
step = stepPoints + (i+1)*stepOffset
self.convertStepToSafeRegion(step, rpySeed)
self.footstepsPanel.onNewWalkingGoal(goalFrame)
def showFusedMaps(self):
om.removeFromObjectModel(om.findObjectByName('stereo'))
om.getOrCreateContainer('stereo')
q = imageManager.queue
cameraToLocalNow = vtk.vtkTransform()
utime = q.getCurrentImageTime('CAMERA_TSDF')
q.getTransform('CAMERA_LEFT', 'local', utime, cameraToLocalNow)
cameraToLocalFusedNow = vtk.vtkTransform()
q.getTransform('CAMERA_LEFT_ALT', 'local', utime,
cameraToLocalFusedNow)
for i in range(len(self.pointClouds)):
fusedNowToLocalNow = vtk.vtkTransform()
fusedNowToLocalNow.PreMultiply()
fusedNowToLocalNow.Concatenate(cameraToLocalNow)
fusedNowToLocalNow.Concatenate(
cameraToLocalFusedNow.GetLinearInverse())
fusedTransform = vtk.vtkTransform()
fusedTransform.PreMultiply()
fusedTransform.Concatenate(fusedNowToLocalNow)
fusedTransform.Concatenate(self.cameraToLocalFusedTransforms[i])
pd = filterUtils.transformPolyData(self.pointClouds[i],
fusedTransform)
vis.showFrame(fusedTransform, ('cloud frame ' + str(i)),
visible=True,
scale=0.2,
parent='stereo')
vis.showPolyData(pd, ('stereo ' + str(i)),
parent='stereo',
colorByName='rgb_colors')
def showFusedMaps(self):
om.removeFromObjectModel(om.findObjectByName('stereo'))
om.getOrCreateContainer('stereo')
q = imageManager.queue
cameraToLocalNow = vtk.vtkTransform()
utime = q.getCurrentImageTime('CAMERA_TSDF')
q.getTransform('CAMERA_LEFT','local', utime,cameraToLocalNow)
cameraToLocalFusedNow = vtk.vtkTransform()
q.getTransform('CAMERA_LEFT_ALT','local', utime,cameraToLocalFusedNow)
for i in range(len(self.pointClouds)):
fusedNowToLocalNow = vtk.vtkTransform()
fusedNowToLocalNow.PreMultiply()
fusedNowToLocalNow.Concatenate( cameraToLocalNow)
fusedNowToLocalNow.Concatenate( cameraToLocalFusedNow.GetLinearInverse() )
fusedTransform = vtk.vtkTransform()
fusedTransform.PreMultiply()
fusedTransform.Concatenate( fusedNowToLocalNow)
fusedTransform.Concatenate( self.cameraToLocalFusedTransforms[i] )
pd = filterUtils.transformPolyData(self.pointClouds[i], fusedTransform)
vis.showFrame(fusedTransform, ('cloud frame ' + str(i)), visible=True, scale=0.2, parent='stereo')
vis.showPolyData(pd, ('stereo ' + str(i)), parent='stereo', colorByName='rgb_colors')
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 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 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 spawnBoardAffordance(self, randomize=False):
self.boardLength = 1.5
if randomize:
position = [random.uniform(0.5, 0.8), random.uniform(-0.2, 0.2), random.uniform(0.5, 0.8)]
rpy = [random.choice((random.uniform(-35, 35), random.uniform(70, 110))), random.uniform(-10, 10), random.uniform(-5, 5)]
zwidth = random.uniform(0.5, 1.0)
else:
if self.b.val:
position = [0.4, 0.0, 1.]
else:
position = [0.6, 0.0, 1.]
rpy = [90, 1, 0]
zwidth = self.boardLength
xwidth = 3.75 * .0254
ywidth = 1.75 * .0254
t = transformUtils.frameFromPositionAndRPY(position, rpy)
t.Concatenate(self.b.computeGroundFrame(self.b.robotModel))
xaxis = [1,0,0]
yaxis = [0,1,0]
zaxis = [0,0,1]
for axis in (xaxis, yaxis, zaxis):
t.TransformVector(axis, axis)
self.affordance = segmentation.createBlockAffordance(t.GetPosition(), xaxis, yaxis, zaxis, xwidth, ywidth, zwidth, 'board', parent='affordances')
self.affordance.setProperty('Color', QtGui.QColor(200, 150, 100))
t = self.affordance.actor.GetUserTransform()
self.frame = vis.showFrame(t, 'board frame', parent=self.affordance, visible=False, scale=0.2)
def makeGoalFrames(self):
for linkName, positionGoal, _ in self.positionCosts:
orientationGoal = [0.0, 0.0, 0.0]
t = transformUtils.frameFromPositionAndRPY(positionGoal, orientationGoal)
f = vis.showFrame(t, '%s position goal' % linkName)
f.connectFrameModified(self.onGoalFrameModified)
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False):
obj, _ = vis.findPickedObject(displayPoint, view)
if not obj:
return False
name = obj.getProperty('Name')
if name in ('footstep widget', 'footstep widget frame'):
om.removeFromObjectModel(om.findObjectByName('footstep widget'))
return True
match = re.match('^step (\d+)$', name)
if not match:
return False
stepIndex = int(match.group(1))
existingWidget = om.findObjectByName('footstep widget')
if existingWidget:
previousStep = existingWidget.stepIndex
print 'have existing widget for step:', stepIndex
om.removeFromObjectModel(existingWidget)
if previousStep == stepIndex:
print 'returning because widget was for selected step'
return True
footMesh = shallowCopy(obj.polyData)
footFrame = transformUtils.copyFrame(obj.getChildFrame().transform)
if useHorizontalWidget:
rpy = [0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2]]
footFrame = transformUtils.frameFromPositionAndRPY(footFrame.GetPosition(), np.degrees(rpy))
footObj = vis.showPolyData(footMesh, 'footstep widget', parent='planning', alpha=0.2)
footObj.stepIndex = stepIndex
frameObj = vis.showFrame(footFrame, 'footstep widget frame', parent=footObj, scale=0.2)
footObj.actor.SetUserTransform(frameObj.transform)
footObj.setProperty('Color', obj.getProperty('Color'))
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
walkGoal = om.findObjectByName('walking goal')
if walkGoal:
walkGoal.setProperty('Edit', False)
def onFootWidgetChanged(frame):
footstepsDriver.onStepModified(stepIndex - 1, frame)
frameObj.connectFrameModified(onFootWidgetChanged)
return True
def newPolyData(self, name, view, parent=None):
self.handModel.model.setJointPositions(
np.zeros(self.handModel.model.numberOfJoints()))
polyData = vtk.vtkPolyData()
self.handModel.model.getModelMesh(polyData)
polyData = filterUtils.transformPolyData(polyData, self.modelToPalm)
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
color = [1.0, 1.0, 0.0]
if self.side == 'right':
color = [0.33, 1.0, 0.0]
obj = vis.showPolyData(polyData,
name,
view=view,
color=color,
visible=False,
parent=parent)
obj.side = self.side
frame = vtk.vtkTransform()
frame.PostMultiply()
obj.actor.SetUserTransform(frame)
frameObj = vis.showFrame(frame,
'%s frame' % name,
view=view,
scale=0.2,
visible=False,
parent=obj)
return obj
def newPolyData(self, name, view, parent=None):
polyData = self.getNewHandPolyData()
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
color = [1.0, 1.0, 0.0]
if self.side == 'right':
color = [0.33, 1.0, 0.0]
obj = vis.showPolyData(polyData,
name,
view=view,
color=color,
visible=False,
parent=parent)
obj.side = self.side
frame = vtk.vtkTransform()
frame.PostMultiply()
obj.actor.SetUserTransform(frame)
frameObj = vis.showFrame(frame,
'%s frame' % name,
view=view,
scale=0.2,
visible=False,
parent=obj)
return obj
def __init__(self, jointController):
self.jointController = jointController
pos, rpy = jointController.q[:3], jointController.q[3:6]
t = transformUtils.frameFromPositionAndRPY(pos, np.degrees(rpy))
self.frame = vis.showFrame(t, 'mover widget', scale=0.3)
self.frame.setProperty('Edit', True)
self.frame.connectFrameModified(self.onFrameModified)
def run(self):
radius = self.properties.getProperty('Radius')
thickness = 0.03
folder = om.getOrCreateContainer('affordances')
frame = self.computeValveFrame()
d = DebugData()
d.addLine(np.array([0, 0, -thickness / 2.0]),
np.array([0, 0, thickness / 2.0]),
radius=radius)
mesh = d.getPolyData()
params = dict(radius=radius,
length=thickness,
xwidth=radius,
ywidth=radius,
zwidth=thickness,
otdf_type='steering_cyl',
friendly_name='valve')
affordance = vis.showPolyData(mesh,
'valve',
color=[0.0, 1.0, 0.0],
cls=affordanceitems.FrameAffordanceItem,
parent=folder,
alpha=1.0)
frame = vis.showFrame(frame,
'valve frame',
parent=affordance,
visible=False,
scale=radius)
affordance.actor.SetUserTransform(frame.transform)
affordance.setAffordanceParams(params)
affordance.updateParamsFromActorTransform()
def makeSplineGraspConstraints(self, ikPlanner, positionTolerance=0.03, angleToleranceInDegrees=15):
params = self.computeHandleParameterization()
segments = zip(params, params[1:])
#times = [np.linspace(segment[0], segment[1], 6) for segment in segments]
#times = [[0.0, 0.3, 0.5], np.linspace(params[-2], params[-1], 6)]
times = np.linspace(params[-2], params[-1], 6)
times = np.hstack(times)
times = np.unique(times)
frames = []
for t in times:
frames.append(self.splineInterp(t))
folder = om.getOrCreateContainer('constraint spline samples', parentObj=om.getOrCreateContainer('debug'))
for f in frames:
vis.showFrame(f, 'frame', scale=0.1)
side = self.side
graspToPalm = vtk.vtkTransform()
graspToHand = ikPlanner.newGraspToHandFrame(side, graspToPalm)
constraints = []
for f, t in zip(frames[:-1], times[:-1]):
graspToWorld = f
p, q = ikPlanner.createPositionOrientationGraspConstraints(side, graspToWorld, graspToHand)
p.lowerBound = np.tile(-positionTolerance, 3)
p.upperBound = np.tile(positionTolerance, 3)
q.angleToleranceInDegrees = angleToleranceInDegrees
if t >= params[-2]:
q.angleToleranceInDegrees = 0
p.lowerBound = np.tile(-0.0, 3)
p.upperBound = np.tile(0.0, 3)
constraints.append(q)
p.tspan = [t, t]
q.tspan = [t, t]
constraints.append(p)
return constraints
def makeGoalFrames(self):
for linkName, positionGoal, _ in self.positionCosts:
orientationGoal = [0.0, 0.0, 0.0]
t = transformUtils.frameFromPositionAndRPY(positionGoal,
orientationGoal)
f = vis.showFrame(t, '%s position goal' % linkName)
f.connectFrameModified(self.onGoalFrameModified)
def getRoomSweepFrames(self, rotateHandFrame=False):
topFrame = transformUtils.frameFromPositionAndRPY([0.65, 0.0, 0.8],
[160, 0, 90])
yawFrame = transformUtils.frameFromPositionAndRPY(
[0, 0.0, 0], [0, 0, self.currentYawDegrees])
if rotateHandFrame:
fixHandFrame = transformUtils.frameFromPositionAndRPY([0, 0.0, 0],
[0, -90, 0])
topFrame.PreMultiply()
topFrame.Concatenate(fixHandFrame)
topFrame.PostMultiply()
topFrame.Concatenate(yawFrame)
bottomFrame = transformUtils.frameFromPositionAndRPY([0.6, 0.0, 0.4],
[210, 0, 90])
yawFrame = transformUtils.frameFromPositionAndRPY(
[0, 0.0, 0], [0, 0, self.currentYawDegrees])
if rotateHandFrame:
bottomFrame.PreMultiply()
bottomFrame.Concatenate(fixHandFrame)
bottomFrame.PostMultiply()
bottomFrame.Concatenate(yawFrame)
if (self.fromTop):
self.startFrame = vis.showFrame(topFrame,
'frame start',
visible=False,
scale=0.1,
parent=self.mapFolder)
self.endFrame = vis.showFrame(bottomFrame,
'frame end',
visible=False,
scale=0.1,
parent=self.mapFolder)
else:
self.startFrame = vis.showFrame(bottomFrame,
'frame start',
visible=False,
scale=0.1,
parent=self.mapFolder)
self.endFrame = vis.showFrame(topFrame,
'frame end',
visible=False,
scale=0.1,
parent=self.mapFolder)
def planReachToTableObjectCollisionFree(self, side ='left'):
# Hard-coded demonstration of collision reaching to object on table
# Using RRT Connect
goalFrame = transformUtils.frameFromPositionAndRPY([1.05,0.4,1],[0,90,-90])
vis.showFrame(goalFrame,'goal frame')
frameObj = om.findObjectByName( 'goal frame')
startPose = self.getPlanningStartPose()
self.constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, side, frameObj.transform, lockBase=self.lockBase, lockBack=self.lockBack)
self.constraintSet.runIk()
print 'planning reach to planReachToTableObjectCollisionFree'
self.constraintSet.ikParameters.usePointwise = False
self.constraintSet.ikParameters.useCollision = True
self.teleopPanel.endEffectorTeleop.updateCollisionEnvironment()
plan = self.constraintSet.runIkTraj()
self.addPlan(plan)
def __init__(self, uid, view, seed_pose, irisDriver, existing_region=None):
d = DebugData()
self.uid = uid
vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid), d.getPolyData(), view)
self.transform = seed_pose
d.addSphere((0,0,0), radius=0.02)
self.seedObj = vis.showPolyData(d.getPolyData(), 'region seed', parent=om.getOrCreateContainer('IRIS region seeds'))
self.seedObj.actor.SetUserTransform(self.transform)
self.frameObj = vis.showFrame(self.transform, 'region seed frame',
scale=0.2,
visible=False,
parent=self.seedObj)
self.frameObj.setProperty('Edit', True)
self.frameObj.widget.HandleRotationEnabledOff()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
rep = self.frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
pos = np.array(self.frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1])
polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2])
self.frameObj.transform.Translate(pos - np.array(self.frameObj.transform.GetPosition()))
self.placer = PlacerWidget(view, self.seedObj, terrain)
self.placer.start()
else:
self.frameObj.setProperty('Edit', True)
self.frameObj.setProperty('Visible', True)
self.driver = irisDriver
self.safe_region = None
self.addProperty('Visible', True)
self.addProperty('Enabled for Walking', True)
self.addProperty('Alpha', 1.0)
self.addProperty('Color', QtGui.QColor(200,200,20))
self.frameObj.connectFrameModified(self.onFrameModified)
if existing_region is None:
self.onFrameModified(self.frameObj)
else:
self.setRegion(existing_region)
self.setProperty('Alpha', 0.5)
self.setProperty('Color', QtGui.QColor(220,220,220))
def spawnBoardAffordance(self, randomize=False):
self.boardLength = 1.5
if randomize:
position = [
random.uniform(0.5, 0.8),
random.uniform(-0.2, 0.2),
random.uniform(0.5, 0.8)
]
rpy = [
random.choice((random.uniform(-35,
35), random.uniform(70, 110))),
random.uniform(-10, 10),
random.uniform(-5, 5)
]
zwidth = random.uniform(0.5, 1.0)
else:
if self.b.val:
position = [0.4, 0.0, 1.]
else:
position = [0.6, 0.0, 1.]
rpy = [90, 1, 0]
zwidth = self.boardLength
xwidth = 3.75 * .0254
ywidth = 1.75 * .0254
t = transformUtils.frameFromPositionAndRPY(position, rpy)
t.Concatenate(self.b.computeGroundFrame(self.b.robotModel))
xaxis = [1, 0, 0]
yaxis = [0, 1, 0]
zaxis = [0, 0, 1]
for axis in (xaxis, yaxis, zaxis):
t.TransformVector(axis, axis)
self.affordance = segmentation.createBlockAffordance(
t.GetPosition(),
xaxis,
yaxis,
zaxis,
xwidth,
ywidth,
zwidth,
'board',
parent='affordances')
self.affordance.setProperty('Color', QtGui.QColor(200, 150, 100))
t = self.affordance.actor.GetUserTransform()
self.frame = vis.showFrame(t,
'board frame',
parent=self.affordance,
visible=False,
scale=0.2)
def computeNextRoomFrame(self):
assert self.targetAffordance
t = transformUtils.frameFromPositionAndRPY(self.nextPosition,
[0, 0, 0])
self.faceTransformLocal = transformUtils.copyFrame(t) # copy required
t.Concatenate(self.targetFrame)
self.faceFrameDesired = vis.showFrame(t,
'target frame desired',
parent=self.targetAffordance,
visible=False,
scale=0.2)
def computeBinStanceFrame(self):
assert self.binFrame
zGround = 0.0
binHeight = self.binFrame.transform.GetPosition()[2] - zGround
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(-0.45, 0.1, -binHeight)
t.Concatenate(self.binFrame.transform)
self.binStanceFrame = vis.showFrame(t, 'bin stance frame', parent=None, scale=0.2)
t = vtk.vtkTransform()
t.PostMultiply()
t.RotateZ(30)
t.Translate(-0.8, 0.4, -binHeight)
t.Concatenate(self.binFrame.transform)
self.startStanceFrame = vis.showFrame(t, 'start stance frame', parent=None, scale=0.2)
def run(self):
folder = om.getOrCreateContainer('affordances')
frame = self.computeAffordanceFrame()
mesh = segmentation.getDrillMesh()
params = segmentation.getDrillAffordanceParams(np.array(frame.GetPosition()), [1,0,0], [0,1,0], [0,0,1])
affordance = vis.showPolyData(mesh, 'drill', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder)
frame = vis.showFrame(frame, 'drill frame', parent=affordance, visible=False, scale=0.2)
affordance.actor.SetUserTransform(frame.transform)
affordance.setAffordanceParams(params)
affordance.updateParamsFromActorTransform()
def addNewFrame():
t = transformUtils.copyFrame(affordanceObj.getChildFrame().transform)
t.PostMultiply()
t.Translate(np.array(pickedPoint) - np.array(t.GetPosition()))
newFrame = vis.showFrame(
t,
'%s frame %d' %
(affordanceObj.getProperty('Name'), len(affordanceObj.children())),
scale=0.2,
parent=affordanceObj)
affordanceObj.getChildFrame().getFrameSync().addFrame(
newFrame, ignoreIncoming=True)
def drawFrameInCamera(t, frameName='new frame',visible=True):
v = imageView.view
q = cameraview.imageManager.queue
localToCameraT = vtk.vtkTransform()
q.getTransform('local', 'CAMERA_LEFT', localToCameraT)
res = vis.showFrame( vtk.vtkTransform() , 'temp',view=v, visible=True, scale = 0.2)
om.removeFromObjectModel(res)
pd = res.polyData
pd = filterUtils.transformPolyData(pd, t)
pd = filterUtils.transformPolyData(pd, localToCameraT)
q.projectPoints('CAMERA_LEFT', pd )
vis.showPolyData(pd, ('overlay ' + frameName), view=v, colorByName='Axes',parent='camera overlay',visible=visible)
def computeTableStanceFrame(self):
assert self.tableData
zGround = 0.0
tableHeight = self.tableData.frame.GetPosition()[2] - zGround
t = transformUtils.copyFrame(self.tableData.frame)
t.PreMultiply()
t1 = transformUtils.frameFromPositionAndRPY([-x/2 for x in self.tableData.dims],[0,0,0])
t.Concatenate(t1)
t2 = transformUtils.frameFromPositionAndRPY([-0.35, self.tableData.dims[1]*0.5, -tableHeight],[0,0,0])
t.Concatenate(t2)
self.tableStanceFrame = vis.showFrame(t, 'table stance frame', parent=self.tableObj, scale=0.2)
def getRoomSweepFrames(self, rotateHandFrame=False):
topFrame = transformUtils.frameFromPositionAndRPY([0.65,0.0,0.8],[160,0,90])
yawFrame = transformUtils.frameFromPositionAndRPY([0,0.0,0],[0,0,self.currentYawDegrees])
if rotateHandFrame:
fixHandFrame = transformUtils.frameFromPositionAndRPY([0,0.0,0],[0,-90,0])
topFrame.PreMultiply()
topFrame.Concatenate( fixHandFrame )
topFrame.PostMultiply()
topFrame.Concatenate( yawFrame )
bottomFrame = transformUtils.frameFromPositionAndRPY([0.6,0.0,0.4],[210,0,90])
yawFrame = transformUtils.frameFromPositionAndRPY([0,0.0,0],[0,0,self.currentYawDegrees])
if rotateHandFrame:
bottomFrame.PreMultiply()
bottomFrame.Concatenate( fixHandFrame )
bottomFrame.PostMultiply()
bottomFrame.Concatenate( yawFrame )
if (self.fromTop):
self.startFrame = vis.showFrame(topFrame, 'frame start', visible=False, scale=0.1,parent=self.mapFolder)
self.endFrame = vis.showFrame(bottomFrame, 'frame end', visible=False, scale=0.1,parent=self.mapFolder)
else:
self.startFrame = vis.showFrame(bottomFrame, 'frame start', visible=False, scale=0.1,parent=self.mapFolder)
self.endFrame = vis.showFrame(topFrame, 'frame end', visible=False, scale=0.1,parent=self.mapFolder)
def newPolyData(self, name, view, parent=None):
polyData = self.getNewHandPolyData()
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
color = [1.0, 1.0, 0.0]
if self.side == 'right':
color = [0.33, 1.0, 0.0]
obj = vis.showPolyData(polyData, name, view=view, color=color, visible=False, parent=parent)
obj.side = self.side
frame = vtk.vtkTransform()
frame.PostMultiply()
obj.actor.SetUserTransform(frame)
frameObj = vis.showFrame(frame, '%s frame' % name, view=view, scale=0.2, visible=False, parent=obj)
return obj
def run(self):
radius = self.properties.getProperty('Radius')
thickness = 0.03
folder = om.getOrCreateContainer('affordances')
frame = self.computeValveFrame()
d = DebugData()
d.addLine(np.array([0, 0, -thickness/2.0]), np.array([0, 0, thickness/2.0]), radius=radius)
mesh = d.getPolyData()
params = dict(radius=radius, length=thickness, xwidth=radius, ywidth=radius, zwidth=thickness, otdf_type='steering_cyl', friendly_name='valve')
affordance = vis.showPolyData(mesh, 'valve', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder, alpha=1.0)
frame = vis.showFrame(frame, 'valve frame', parent=affordance, visible=False, scale=radius)
affordance.actor.SetUserTransform(frame.transform)
affordance.setAffordanceParams(params)
affordance.updateParamsFromActorTransform()
def onSegmentTable(self, p1, p2):
print p1
print p2
self.picker.stop()
om.removeFromObjectModel(self.picker.annotationObj)
self.picker = None
om.removeFromObjectModel(om.findObjectByName('table demo'))
self.tableData = segmentation.segmentTableEdge(self.getInputPointCloud(), p1, p2)
self.tableObj = vis.showPolyData(self.tableData.mesh, 'table', parent='table demo', color=[0,1,0])
self.tableFrame = vis.showFrame(self.tableData.frame, 'table frame', parent=self.tableObj, scale=0.2)
self.tableBox = vis.showPolyData(self.tableData.box, 'table box', parent=self.tableObj, color=[0,1,0], visible=False)
self.tableObj.actor.SetUserTransform(self.tableFrame.transform)
self.tableBox.actor.SetUserTransform(self.tableFrame.transform)
if self.useCollisionEnvironment:
self.addCollisionObject(self.tableObj)
def spawnTargetAffordance(self):
for obj in om.getObjects():
if obj.getProperty('Name') == 'target':
om.removeFromObjectModel(obj)
targetFrame = transformUtils.frameFromPositionAndRPY([0.6, 0.2, 0.6],
[180, 0, 90])
folder = om.getOrCreateContainer('affordances')
z = DebugData()
z.addLine(np.array([0, 0, 0]),
np.array([-self.boxLength, 0, 0]),
radius=0.02) # main bar
z.addLine(np.array([-self.boxLength, 0, 0]),
np.array([-self.boxLength, 0, self.boxLength]),
radius=0.02) # main bar
z.addLine(np.array([-self.boxLength, 0, self.boxLength]),
np.array([0, 0, self.boxLength]),
radius=0.02) # main bar
z.addLine(np.array([0, 0, self.boxLength]),
np.array([0, 0, 0]),
radius=0.02) # main bar
targetMesh = z.getPolyData()
self.targetAffordance = vis.showPolyData(
targetMesh,
'target',
color=[0.0, 1.0, 0.0],
cls=affordanceitems.FrameAffordanceItem,
parent=folder,
alpha=0.3)
self.targetAffordance.actor.SetUserTransform(targetFrame)
self.targetFrame = vis.showFrame(targetFrame,
'target frame',
parent=self.targetAffordance,
visible=False,
scale=0.2)
self.targetFrame = self.targetFrame.transform
params = dict(length=self.boxLength,
otdf_type='target',
friendly_name='target')
self.targetAffordance.setAffordanceParams(params)
self.targetAffordance.updateParamsFromActorTransform()
def planRoomSweep(self):
self.initConstraintSet()
faceFrameDesired = transformUtils.frameInterpolate(
self.startFrame.transform, self.endFrame.transform, 0)
vis.showFrame(faceFrameDesired,
'frame 0',
visible=True,
scale=0.1,
parent=self.mapFolder)
self.addConstraintForTargetFrame(faceFrameDesired, 0)
faceFrameDesired = transformUtils.frameInterpolate(
self.startFrame.transform, self.endFrame.transform, 1.0 / 3.0)
vis.showFrame(faceFrameDesired,
'frame 1',
visible=True,
scale=0.1,
parent=self.mapFolder)
self.addConstraintForTargetFrame(faceFrameDesired, 1)
faceFrameDesired = transformUtils.frameInterpolate(
self.startFrame.transform, self.endFrame.transform, 2.0 / 3.0)
vis.showFrame(faceFrameDesired,
'frame 2',
visible=True,
scale=0.1,
parent=self.mapFolder)
self.addConstraintForTargetFrame(faceFrameDesired, 2)
faceFrameDesired = transformUtils.frameInterpolate(
self.startFrame.transform, self.endFrame.transform, 3.0 / 3.0)
vis.showFrame(faceFrameDesired,
'frame 3',
visible=True,
scale=0.1,
parent=self.mapFolder)
self.addConstraintForTargetFrame(faceFrameDesired, 3)
#self.ikPlanner.ikServer.maxDegreesPerSecond = self.speedLow
self.planTrajectory()
def planRoomSweep(self):
self.initConstraintSet()
faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 0)
vis.showFrame(faceFrameDesired, 'frame 0', visible=True, scale=0.1,parent=self.mapFolder)
self.addConstraintForTargetFrame(faceFrameDesired, 0)
faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 1.0/3.0)
vis.showFrame(faceFrameDesired, 'frame 1', visible=True, scale=0.1,parent=self.mapFolder)
self.addConstraintForTargetFrame(faceFrameDesired, 1)
faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 2.0/3.0)
vis.showFrame(faceFrameDesired, 'frame 2', visible=True, scale=0.1,parent=self.mapFolder)
self.addConstraintForTargetFrame(faceFrameDesired, 2)
faceFrameDesired = transformUtils.frameInterpolate(self.startFrame.transform , self.endFrame.transform, 3.0/3.0)
vis.showFrame(faceFrameDesired, 'frame 3', visible=True, scale=0.1,parent=self.mapFolder)
self.addConstraintForTargetFrame(faceFrameDesired, 3)
#self.ikPlanner.ikServer.maxDegreesPerSecond = self.speedLow
self.planTrajectory()
def newPolyData(self, name, view, parent=None):
self.handModel.model.setJointPositions(np.zeros(self.handModel.model.numberOfJoints()))
polyData = vtk.vtkPolyData()
self.handModel.model.getModelMesh(polyData)
polyData = filterUtils.transformPolyData(polyData, self.modelToPalm)
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
color = [1.0, 1.0, 0.0]
if self.side == 'right':
color = [0.33, 1.0, 0.0]
obj = vis.showPolyData(polyData, name, view=view, color=color, visible=False, parent=parent)
obj.side = self.side
frame = vtk.vtkTransform()
frame.PostMultiply()
obj.actor.SetUserTransform(frame)
frameObj = vis.showFrame(frame, '%s frame' % name, view=view, scale=0.2, visible=False, parent=obj)
return obj
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 run(self):
folder = om.getOrCreateContainer('affordances')
frame = self.computeAffordanceFrame()
mesh = segmentation.getDrillMesh()
params = segmentation.getDrillAffordanceParams(
np.array(frame.GetPosition()), [1, 0, 0], [0, 1, 0], [0, 0, 1])
affordance = vis.showPolyData(mesh,
'drill',
color=[0.0, 1.0, 0.0],
cls=affordanceitems.FrameAffordanceItem,
parent=folder)
frame = vis.showFrame(frame,
'drill frame',
parent=affordance,
visible=False,
scale=0.2)
affordance.actor.SetUserTransform(frame.transform)
affordance.setAffordanceParams(params)
affordance.updateParamsFromActorTransform()
def spawnTargetAffordance(self):
for obj in om.getObjects():
if obj.getProperty('Name') == 'target':
om.removeFromObjectModel(obj)
targetFrame = transformUtils.frameFromPositionAndRPY([0.6,0.2,0.6],[180,0,90])
folder = om.getOrCreateContainer('affordances')
z = DebugData()
z.addLine(np.array([0,0,0]), np.array([-self.boxLength,0,0]), radius=0.02) # main bar
z.addLine(np.array([-self.boxLength,0,0]), np.array([-self.boxLength,0,self.boxLength]), radius=0.02) # main bar
z.addLine(np.array([-self.boxLength,0,self.boxLength]), np.array([0,0,self.boxLength]), radius=0.02) # main bar
z.addLine(np.array([0,0,self.boxLength]), np.array([0,0,0]), radius=0.02) # main bar
targetMesh = z.getPolyData()
self.targetAffordance = vis.showPolyData(targetMesh, 'target', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder, alpha=0.3)
self.targetAffordance.actor.SetUserTransform(targetFrame)
self.targetFrame = vis.showFrame(targetFrame, 'target frame', parent=self.targetAffordance, visible=False, scale=0.2)
self.targetFrame = self.targetFrame.transform
params = dict(length=self.boxLength, otdf_type='target', friendly_name='target')
self.targetAffordance.setAffordanceParams(params)
self.targetAffordance.updateParamsFromActorTransform()
def __init__(self, uid, view, seed_pose, irisDriver, existing_region=None):
d = DebugData()
self.uid = uid
vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid),
d.getPolyData(), view)
self.transform = seed_pose
d.addSphere((0, 0, 0), radius=0.02)
self.seedObj = vis.showPolyData(
d.getPolyData(),
'region seed',
parent=om.getOrCreateContainer('IRIS region seeds'))
self.seedObj.actor.SetUserTransform(self.transform)
self.frameObj = vis.showFrame(self.transform,
'region seed frame',
scale=0.2,
visible=False,
parent=self.seedObj)
self.frameObj.setProperty('Edit', True)
self.frameObj.widget.HandleRotationEnabledOff()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
rep = self.frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
pos = np.array(self.frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(
polyData, pos, pos + [0, 0, 1])
polyData = segmentation.thresholdPoints(
polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(
vnp.getNumpyFromVtk(polyData, 'Points')[:, 2])
self.frameObj.transform.Translate(
pos - np.array(self.frameObj.transform.GetPosition()))
self.placer = PlacerWidget(view, self.seedObj, terrain)
self.placer.start()
else:
self.frameObj.setProperty('Edit', True)
self.frameObj.setProperty('Visible', True)
self.driver = irisDriver
self.safe_region = None
self.addProperty('Visible', True)
self.addProperty('Enabled for Walking', True)
self.addProperty('Alpha', 1.0)
self.addProperty('Color', QtGui.QColor(200, 200, 20))
self.frameObj.connectFrameModified(self.onFrameModified)
if existing_region is None:
self.onFrameModified(self.frameObj)
else:
self.setRegion(existing_region)
self.setProperty('Alpha', 0.5)
self.setProperty('Color', QtGui.QColor(220, 220, 220))
def addFrame(self, frameName):
t = self.getFrameTransform(frameName)
folder = self.getFramesFolder()
frame = vis.showFrame(t, frameName, parent=folder, scale=0.2)
frame.setProperty('Trace', True)
def computeNextRoomFrame(self):
assert self.targetAffordance
t = transformUtils.frameFromPositionAndRPY(self.nextPosition, [0, 0, 0])
self.faceTransformLocal = transformUtils.copyFrame(t) # copy required
t.Concatenate(self.targetFrame)
self.faceFrameDesired = vis.showFrame(t, 'target frame desired', parent=self.targetAffordance, visible=False, scale=0.2)
def addNewFrame():
t = transformUtils.copyFrame(affordanceObj.getChildFrame().transform)
t.PostMultiply()
t.Translate(np.array(pickedPoint) - np.array(t.GetPosition()))
newFrame = vis.showFrame(t, '%s frame %d' % (affordanceObj.getProperty('Name'), len(affordanceObj.children())), scale=0.2, parent=affordanceObj)
affordanceObj.getChildFrame().getFrameSync().addFrame(newFrame, ignoreIncoming=True)
def drawFootstepPlan(self, msg, folder, left_color=None, right_color=None, alpha=1.0):
for step in folder.children():
om.removeFromObjectModel(step)
allTransforms = []
volFolder = getWalkingVolumesFolder()
map(om.removeFromObjectModel, volFolder.children())
slicesFolder = getTerrainSlicesFolder()
map(om.removeFromObjectModel, slicesFolder.children())
for i, footstep in enumerate(msg.footsteps):
trans = footstep.pos.translation
trans = [trans.x, trans.y, trans.z]
quat = footstep.pos.rotation
quat = [quat.w, quat.x, quat.y, quat.z]
footstepTransform = transformUtils.transformFromPose(trans, quat)
allTransforms.append(footstepTransform)
if i < 2:
continue
if footstep.is_right_foot:
mesh = getRightFootMesh()
if (right_color is None):
color = getRightFootColor()
else:
color = right_color
else:
mesh = getLeftFootMesh()
if (left_color is None):
color = getLeftFootColor()
else:
color = left_color
# add gradual shading to steps to indicate destination
frac = float(i)/ float(msg.num_steps-1)
this_color = [0,0,0]
this_color[0] = 0.25*color[0] + 0.75*frac*color[0]
this_color[1] = 0.25*color[1] + 0.75*frac*color[1]
this_color[2] = 0.25*color[2] + 0.75*frac*color[2]
if self.show_contact_slices:
self.drawContactVolumes(footstepTransform, color)
contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()
if footstep.is_right_foot:
sole_offset = np.mean(contact_pts_right, axis=0)
else:
sole_offset = np.mean(contact_pts_left, axis=0)
t_sole_prev = transformUtils.frameFromPositionMessage(msg.footsteps[i-2].pos)
t_sole_prev.PreMultiply()
t_sole_prev.Translate(sole_offset)
t_sole = transformUtils.copyFrame(footstepTransform)
t_sole.Translate(sole_offset)
yaw = np.arctan2(t_sole.GetPosition()[1] - t_sole_prev.GetPosition()[1],
t_sole.GetPosition()[0] - t_sole_prev.GetPosition()[0])
T_terrain_to_world = transformUtils.frameFromPositionAndRPY([t_sole_prev.GetPosition()[0], t_sole_prev.GetPosition()[1], 0],
[0, 0, math.degrees(yaw)])
path_dist = np.array(footstep.terrain_path_dist)
height = np.array(footstep.terrain_height)
# if np.any(height >= trans[2]):
terrain_pts_in_local = np.vstack((path_dist, np.zeros(len(footstep.terrain_path_dist)), height))
d = DebugData()
for j in range(terrain_pts_in_local.shape[1]-1):
d.addLine(terrain_pts_in_local[:,j], terrain_pts_in_local[:,j+1], radius=0.01)
obj = vis.showPolyData(d.getPolyData(), 'terrain slice', parent=slicesFolder, visible=slicesFolder.getProperty('Visible'), color=[.8,.8,.3])
obj.actor.SetUserTransform(T_terrain_to_world)
renderInfeasibility = False
if renderInfeasibility and footstep.infeasibility > 1e-6:
d = DebugData()
start = allTransforms[i-1].GetPosition()
end = footstepTransform.GetPosition()
d.addArrow(start, end, 0.02, 0.005,
startHead=True,
endHead=True)
vis.showPolyData(d.getPolyData(), 'infeasibility %d -> %d' % (i-2, i-1), parent=folder, color=[1, 0.2, 0.2])
stepName = 'step %d' % (i-1)
obj = vis.showPolyData(mesh, stepName, color=this_color, alpha=alpha, parent=folder)
obj.setIcon(om.Icons.Feet)
frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False)
obj.actor.SetUserTransform(footstepTransform)
obj.addProperty('Support Contact Groups', footstep.params.support_contact_groups, attributes=om.PropertyAttributes(enumNames=['Whole Foot', 'Front 2/3', 'Back 2/3']))
obj.properties.setPropertyIndex('Support Contact Groups', 0)
obj.footstep_index = i
obj.footstep_property_callback = obj.properties.connectPropertyChanged(functools.partial(self.onFootstepPropertyChanged, obj))
self.drawContactPts(obj, footstep, color=this_color)
def drawFootstepPlan(self, msg, folder, left_color=None, right_color=None, alpha=1.0):
for step in folder.children():
om.removeFromObjectModel(step)
allTransforms = []
volFolder = getWalkingVolumesFolder()
map(om.removeFromObjectModel, volFolder.children())
slicesFolder = getTerrainSlicesFolder()
map(om.removeFromObjectModel, slicesFolder.children())
for i, footstep in enumerate(msg.footsteps):
trans = footstep.pos.translation
trans = [trans.x, trans.y, trans.z]
quat = footstep.pos.rotation
quat = [quat.w, quat.x, quat.y, quat.z]
footstepTransform = transformUtils.transformFromPose(trans, quat)
allTransforms.append(footstepTransform)
if i < 2:
continue
if footstep.is_right_foot:
mesh = getRightFootMesh()
if (right_color is None):
color = getRightFootColor()
else:
color = right_color
else:
mesh = getLeftFootMesh()
if (left_color is None):
color = getLeftFootColor()
else:
color = left_color
# add gradual shading to steps to indicate destination
frac = float(i)/ float(msg.num_steps-1)
this_color = [0,0,0]
this_color[0] = 0.25*color[0] + 0.75*frac*color[0]
this_color[1] = 0.25*color[1] + 0.75*frac*color[1]
this_color[2] = 0.25*color[2] + 0.75*frac*color[2]
if self.show_contact_slices:
self.drawContactVolumes(footstepTransform, color)
contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()
if footstep.is_right_foot:
sole_offset = np.mean(contact_pts_right, axis=0)
else:
sole_offset = np.mean(contact_pts_left, axis=0)
t_sole_prev = transformUtils.frameFromPositionMessage(msg.footsteps[i-2].pos)
t_sole_prev.PreMultiply()
t_sole_prev.Translate(sole_offset)
t_sole = transformUtils.copyFrame(footstepTransform)
t_sole.Translate(sole_offset)
yaw = np.arctan2(t_sole.GetPosition()[1] - t_sole_prev.GetPosition()[1],
t_sole.GetPosition()[0] - t_sole_prev.GetPosition()[0])
T_terrain_to_world = transformUtils.frameFromPositionAndRPY([t_sole_prev.GetPosition()[0], t_sole_prev.GetPosition()[1], 0],
[0, 0, math.degrees(yaw)])
path_dist = np.array(footstep.terrain_path_dist)
height = np.array(footstep.terrain_height)
# if np.any(height >= trans[2]):
terrain_pts_in_local = np.vstack((path_dist, np.zeros(len(footstep.terrain_path_dist)), height))
d = DebugData()
for j in range(terrain_pts_in_local.shape[1]-1):
d.addLine(terrain_pts_in_local[:,j], terrain_pts_in_local[:,j+1], radius=0.01)
obj = vis.showPolyData(d.getPolyData(), 'terrain slice', parent=slicesFolder, visible=slicesFolder.getProperty('Visible'), color=[.8,.8,.3])
obj.actor.SetUserTransform(T_terrain_to_world)
renderInfeasibility = False
if renderInfeasibility and footstep.infeasibility > 1e-6:
d = DebugData()
start = allTransforms[i-1].GetPosition()
end = footstepTransform.GetPosition()
d.addArrow(start, end, 0.02, 0.005,
startHead=True,
endHead=True)
vis.showPolyData(d.getPolyData(), 'infeasibility %d -> %d' % (i-2, i-1), parent=folder, color=[1, 0.2, 0.2])
stepName = 'step %d' % (i-1)
obj = vis.showPolyData(mesh, stepName, color=this_color, alpha=alpha, parent=folder)
obj.setIcon(om.Icons.Feet)
frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False)
obj.actor.SetUserTransform(footstepTransform)
obj.addProperty('Support Contact Groups', footstep.params.support_contact_groups, attributes=om.PropertyAttributes(enumNames=['Whole Foot', 'Front 2/3', 'Back 2/3']))
obj.properties.setPropertyIndex('Support Contact Groups', 0)
obj.footstep_index = i
obj.footstep_property_callback = obj.properties.connectPropertyChanged(functools.partial(self.onFootstepPropertyChanged, obj))
self.drawContactPts(obj, footstep, color=this_color)
def addFrame(self, frameName):
t = self.getFrameTransform(frameName)
folder = self.getFramesFolder()
frame = vis.showFrame(t, frameName, parent=folder, scale=0.2)
frame.setProperty('Trace', True)