def onHideBDIButton(self):
self.driver.showBDIPlan = False
self.driver.bdiRobotModel.setProperty('Visible', False)
folder = om.getOrCreateContainer("BDI footstep plan")
om.removeFromObjectModel(folder)
folder = om.getOrCreateContainer("BDI adj footstep plan")
om.removeFromObjectModel(folder)
def _remove_subscriber(self):
if (self._subscriber is None):
return
lcmUtils.removeSubscriber(self._subscriber)
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
self._subscriber = None
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 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 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 _addPlanItem(plan, name, itemClass):
assert plan is not None
item = itemClass(name)
item.plan = plan
om.removeFromObjectModel(om.findObjectByName(name))
om.addToObjectModel(item, om.getOrCreateContainer('segmentation'))
return item
def _onPropertyChanged(self, propertySet, propertyName):
PolyDataItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == 'Scale':
scale = self.getProperty(propertyName)
self.rep.SetWorldSize(scale)
self._updateAxesGeometry()
elif propertyName == 'Edit':
view = app.getCurrentRenderView()
if view not in self.views:
view = self.views[0]
self.widget.SetInteractor(view.renderWindow().GetInteractor())
self.widget.SetEnabled(self.getProperty(propertyName))
isEditing = self.getProperty(propertyName)
if isEditing:
frameupdater.registerFrame(self)
elif propertyName == 'Trace':
trace = self.getProperty(propertyName)
if trace and not self.traceData:
self.traceData = FrameTraceVisualizer(self)
elif not trace and self.traceData:
om.removeFromObjectModel(self.traceData.getTraceData())
self.traceData = None
elif propertyName == 'Tube':
self.properties.setPropertyAttribute('Tube Width', 'hidden', not self.getProperty(propertyName))
self._updateAxesGeometry()
def promotePolyDataItem(cls, obj):
parent = obj.parent()
view = obj.views[0]
name = obj.getProperty('Name')
polyData = obj.polyData
props = obj.properties._properties
childFrame = obj.getChildFrame()
if childFrame:
t = transformUtils.copyFrame(childFrame.transform)
else:
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(polyData))
polyData = filterUtils.transformPolyData(polyData, t.GetLinearInverse())
children = [c for c in obj.children() if c is not childFrame]
meshId = cls.getMeshManager().add(polyData)
om.removeFromObjectModel(obj)
obj = MeshAffordanceItem(name, view)
obj.setProperty('Filename', meshId)
om.addToObjectModel(obj, parentObj=parent)
frame = vis.addChildFrame(obj)
frame.copyFrame(t)
for child in children:
om.addToObjectModel(child, parentObj=obj)
obj.syncProperties(props)
return obj
def computeGraspFrameSamplesBoard(self):
affordanceFrame = self.getAffordanceFrame()
additionalOffset = 0.0
yoffset = 0.5*self.affordance.params['ywidth'] + additionalOffset
xoffset = 0.5*self.affordance.params['xwidth'] + additionalOffset
frames = [
[[0.0, yoffset, 0.0], [0.0, 90, 180.0]],
[[0.0, yoffset, 0.0], [0.0, -90, 180.0]],
[[0.0, -yoffset, 0.0], [0.0, 90, 0.0]],
[[0.0, -yoffset, 0.0], [0.0, -90, 0.0]],
[[xoffset, 0.0, 0.0], [-90, -90, 180.0]],
[[xoffset, 0.0, 0.0], [90, 90, 180.0]],
[[-xoffset, 0.0, 0.0], [90, -90, 180.0]],
[[-xoffset, 0.0, 0.0], [-90, 90, 180.0]],
]
for i, frame in enumerate(frames):
pos, rpy = frame
t = transformUtils.frameFromPositionAndRPY(pos, rpy)
t.Concatenate(affordanceFrame.transform)
name = 'sample grasp frame %d' % i
om.removeFromObjectModel(self.findAffordanceChild(name))
vis.showFrame(copyFrame(t), name, parent=self.affordance, visible=False, scale=0.2)
def addGeometry(self, path, geomItems):
folder = self.getPathFolder(path)
ancestors = findPathToAncestor(folder, self.getRootFolder())
geomTransform = vtk.vtkTransform()
for item in reversed(ancestors):
if not hasattr(item, "transform"):
item.transform = vtk.vtkTransform()
item.transform.PostMultiply()
geomTransform.Concatenate(item.transform)
for geom in geomItems:
existing_item = self.getItemByPath(path + ["geometry"])
item = geom.polyDataItem
if existing_item is not None:
for prop in existing_item.propertyNames():
item.setProperty(prop, existing_item.getProperty(prop))
om.removeFromObjectModel(existing_item)
else:
item.setProperty("Point Size", 2)
for colorBy in ["rgb", "intensity"]:
try:
item.setProperty("Color By", colorBy)
except ValueError:
pass
else:
break
item.addToView(self.view)
om.addToObjectModel(item, parentObj=folder)
item.actor.SetUserTransform(geomTransform)
return path
def projectDrillDemoInCamera():
q = om.findObjectByName('camera overlay')
om.removeFromObjectModel(q)
imageView = cameraview.views['CAMERA_LEFT']
imageView.imageActor.SetOpacity(.2)
drawFrameInCamera(drillDemo.drill.frame.transform, 'drill frame',visible=False)
tf = transformUtils.copyFrame( drillDemo.drill.frame.transform )
tf.PreMultiply()
tf.Concatenate( drillDemo.drill.drillToButtonTransform )
drawFrameInCamera(tf, 'drill button')
tf2 = transformUtils.copyFrame( tf )
tf2.PreMultiply()
tf2.Concatenate( transformUtils.frameFromPositionAndRPY( [0,0,0] , [180,0,0] ) )
drawFrameInCamera(tf2, 'drill button flip')
drawObjectInCamera('drill',visible=False)
drawObjectInCamera('sensed pointer tip')
obj = om.findObjectByName('sensed pointer tip frame')
if (obj is not None):
drawFrameInCamera(obj.transform, 'sensed pointer tip frame',visible=False)
#drawObjectInCamera('left robotiq',visible=False)
#drawObjectInCamera('right pointer',visible=False)
v = imageView.view
v.render()
def initPlanning(self, robotSystem):
from director import objectmodel as om
from director import planningutils
from director import roboturdf
from director import ikplanner
directorConfig = robotSystem.directorConfig
ikRobotModel, ikJointController = roboturdf.loadRobotModel('ik model', urdfFile=directorConfig['urdfConfig']['ik'], parent=None)
om.removeFromObjectModel(ikRobotModel)
ikJointController.addPose('q_end', ikJointController.getPose('q_nom'))
ikJointController.addPose('q_start', ikJointController.getPose('q_nom'))
handFactory = roboturdf.HandFactory(robotSystem.robotStateModel)
handModels = []
for side in ['left', 'right']:
if side in handFactory.defaultHandTypes:
handModels.append(handFactory.getLoader(side))
ikPlanner = ikplanner.IKPlanner(ikRobotModel, ikJointController, handModels)
planningUtils = planningutils.PlanningUtils(robotSystem.robotStateModel, robotSystem.robotStateJointController)
return FieldContainer(
ikRobotModel=ikRobotModel,
ikJointController=ikJointController,
handFactory=handFactory,
handModels=handModels,
ikPlanner=ikPlanner,
planningUtils=planningUtils
)
def add_target(self, target):
data = DebugData()
center = [target[0], target[1], 1]
axis = [0, 0, 1] # Upright cylinder.
data.addCylinder(center, axis, 2, 3)
om.removeFromObjectModel(om.findObjectByName("target"))
self._add_polydata(data.getPolyData(), "target", [0, 0.8, 0])
def resetTargetPath(self):
for obj in om.getObjects():
if obj.getProperty('Name') == 'target frame desired':
om.removeFromObjectModel(obj)
for obj in om.getObjects():
if obj.getProperty('Name') == 'target frame desired path':
om.removeFromObjectModel(obj)
def testAffordanceToUrdf():
affs = [func() for func in newSphere, newBox, newCylinder, newCapsule, newMesh]
print affordanceurdf.urdfStringFromAffordances(affs)
for aff in affs:
om.removeFromObjectModel(aff)
def onExecClicked(self):
self.commitFootstepPlan(self.lastFootstepPlan)
om.removeFromObjectModel(om.findObjectByName('footstep widget'))
walkGoal = om.findObjectByName('walking goal')
if walkGoal:
walkGoal.setProperty('Edit', False)
self.execButton.setEnabled(False)
def removeHandFrames(self):
sides = ['left', 'right']
for side in sides:
linkName = self.ikPlanner.getHandLink(side)
frameName = '%s constraint frame' % linkName
frame = om.findObjectByName(frameName)
if frame:
om.removeFromObjectModel(frame)
def remove_subscriber(self):
if self._sub is None:
return
lcmUtils.removeSubscriber(self._sub)
self._sub = None
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
print self._name + " subscriber removed."
def _remove_subscriber(self):
if (self._subscriber is None):
return
lcmUtils.removeSubscriber(self._subscriber)
for frame_channel in self._frame_channels:
frame_channel.remove_folder()
self._frame_channels.clear()
self._subscriber = None
om.removeFromObjectModel(self._get_folder())
def ungraspAffordance(self, affordanceName):
try:
del self.frameSyncs[affordanceName]
del self.attachedAffordances[affordanceName]
except KeyError:
pass
if not self.frameSyncs:
om.removeFromObjectModel(om.findObjectByName('l_hand frame'))
om.removeFromObjectModel(om.findObjectByName('r_hand frame'))
def clear_visualization(self):
"""
Clears the visualization
:return:
:rtype:
"""
container_name = "ROS"
self._vis_container = om.getOrCreateContainer(container_name)
om.removeFromObjectModel(self._vis_container)
self._vis_container = om.getOrCreateContainer(container_name)
def testAffordanceToUrdf():
affs = [
func()
for func in (newSphere, newBox, newCylinder, newCapsule, newMesh)
]
print(affordanceurdf.urdfStringFromAffordances(affs))
for aff in affs:
om.removeFromObjectModel(aff)
def _clear_visualization(self):
"""
Delete the 'object manipulation' vis container, create a new one with the same name
:return:
:rtype:
"""
self._vis_container = om.getOrCreateContainer("object_manipulation")
om.removeFromObjectModel(self._vis_container)
self._vis_container = om.getOrCreateContainer("object_manipulation")
self._vis_dict = dict()
def set_enabled(self, enable, clear=True):
print("DeformableVisualizer set_enabled", enable)
self._enabled = enable
if enable:
self.add_subscriber()
else:
self.remove_subscriber()
# Removes the folder completely and resets the known meshes.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
self._poly_item_list = []
def spawnBox():
t = transformUtils.frameFromPositionAndRPY([0.5, 0.0, 0.5], [0, 0, 0])
om.removeFromObjectModel(om.findObjectByName('box'))
obj = makeBox()
obj.setProperty('Dimensions', [0.06, 0.04, 0.12])
#obj.setProperty('Surface Mode', 'Wireframe')
obj.setProperty('Color', [1, 0, 0])
obj.getChildFrame().copyFrame(t)
def selectCandidate(self, selectedObj, candidates):
if self.properties.getProperty('Delete candidates'):
for obj in candidates:
if obj != selectedObj:
om.removeFromObjectModel(obj)
newName = self.properties.getProperty('New name')
if newName:
selectedObj.rename(newName)
def set_enabled(self, enable):
self._enabled = enable
if enable:
self.add_subscriber()
self.configure_action.setEnabled(True)
else:
self.remove_subscriber()
self.configure_action.setEnabled(False)
# Removes the folder completely.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
def removeStaleVisObjects(self):
keysToRemove = []
for key, val in self.visObjectDrawTime.iteritems():
elapsed = time.time() - val
if elapsed > self.timeout:
keysToRemove.append(key)
for key in keysToRemove:
om.removeFromObjectModel(om.findObjectByName(key))
del self.visObjectDrawTime[key]
def ungraspAffordance(self, affordanceName):
try:
del self.frameSyncs[affordanceName]
del self.attachedAffordances[affordanceName]
except KeyError:
pass
if not self.frameSyncs:
om.removeFromObjectModel(om.findObjectByName('l_hand frame'))
om.removeFromObjectModel(om.findObjectByName('r_hand frame'))
def clear_visualation(self):
"""
Clear the vis container
:return:
:rtype:
"""
container_name = "Category Manipulation"
c = om.getOrCreateContainer(container_name)
om.removeFromObjectModel(c)
self._vis_container = om.getOrCreateContainer(container_name)
def drawModel(self, model):
modelFolder = om.getOrCreateContainer(model.name,
parentObj=self.getRootFolder())
markerFolder = om.getOrCreateContainer('markers',
parentObj=modelFolder)
modelName = model.name
markerObjects = markerFolder.children()
if len(markerObjects) != model.nummarkers:
for obj in markerObjects:
om.removeFromObjectModel(obj)
modelColor = vis.getRandomColor()
markerObjects = self.createMarkerObjects(model.nummarkers,
markerFolder, modelName,
modelColor)
self.models[modelName] = markerObjects
if len(markerObjects):
modelColor = markerObjects[0].getProperty('Color')
for i, marker in enumerate(model.markers):
xyz = np.array(marker.xyz) * self.unitConversion
markerFrame = vtk.vtkTransform()
markerFrame.Translate(xyz)
markerObjects[i].getChildFrame().copyFrame(markerFrame)
if self.drawEdges:
d = DebugData()
for m1 in model.markers:
xyz = np.array(m1.xyz) * self.unitConversion
for m2 in model.markers:
xyz2 = np.array(m2.xyz) * self.unitConversion
d.addLine(xyz, xyz2)
edges = shallowCopy(d.getPolyData())
vis.updatePolyData(edges,
modelName + ' edges',
color=modelColor,
parent=modelFolder)
else:
edgesObj = om.findObjectByName(modelName + ' edges')
om.removeFromObjectModel(edgesObj)
computeModelFrame = True
if computeModelFrame:
pos = np.array(model.segments[0].T) * self.unitConversion
rpy = np.array(model.segments[0].A)
modelFrame = transformUtils.frameFromPositionAndRPY(
pos, np.degrees(rpy))
vis.updateFrame(modelFrame,
modelName + ' frame',
parent=modelFolder,
scale=0.1)
def initialize(self):
self.dt = self.options['dt']
self.controllerTypeOrder = [
'defaultRandom', 'learnedRandom', 'learned', 'default'
]
self.setDefaultOptions()
self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'],
numRays=self.options['Sensor']['numRays'])
self.Controller = ControllerObj(self.Sensor)
self.Car = CarPlant(controller=self.Controller,
velocity=self.options['Car']['velocity'])
self.Reward = Reward(
self.Sensor,
collisionThreshold=self.collisionThreshold,
actionCost=self.options['Reward']['actionCost'],
collisionPenalty=self.options['Reward']['collisionPenalty'],
raycastCost=self.options['Reward']['raycastCost'])
self.setSARSA()
# create the things needed for simulation
om.removeFromObjectModel(om.findObjectByName('world'))
self.world = World.buildCircleWorld(
percentObsDensity=self.options['World']['percentObsDensity'],
circleRadius=self.options['World']['circleRadius'],
nonRandom=self.options['World']['nonRandomWorld'],
scale=self.options['World']['scale'],
randomSeed=self.options['World']['randomSeed'],
obstaclesInnerFraction=self.options['World']
['obstaclesInnerFraction'])
om.removeFromObjectModel(om.findObjectByName('robot'))
self.robot, self.frame = World.buildRobot()
self.locator = World.buildCellLocator(self.world.visObj.polyData)
self.Sensor.setLocator(self.locator)
self.frame = self.robot.getChildFrame()
self.frame.setProperty('Scale', 3)
self.frame.setProperty('Edit', True)
self.frame.widget.HandleRotationEnabledOff()
rep = self.frame.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
self.supervisedTrainingTime = self.options['runTime'][
'supervisedTrainingTime']
self.learningRandomTime = self.options['runTime']['learningRandomTime']
self.learningEvalTime = self.options['runTime']['learningEvalTime']
self.defaultControllerTime = self.options['runTime'][
'defaultControllerTime']
self.Car.setFrame(self.frame)
print "Finished initialization"
def showHandSamples(self, numberOfSamples=15):
om.removeFromObjectModel(om.findObjectByName('sampled hands'))
handFolder = om.getOrCreateContainer('sampled hands', parentObj=om.getOrCreateContainer('debug'))
for i in xrange(numberOfSamples):
t = self.splineInterp(i/float(numberOfSamples-1))
handObj, f = self.handFactory.placeHandModelWithTransform(t, self.view, side=self.side, name='sample %d' % i, parent=handFolder)
handObj.setProperty('Alpha', 0.3)
handFolder.setProperty('Visible', False)
def reset(self, robot, frame_name):
self._tick_count = 0
robot.init_angle()
self.clear_locator()
om.removeFromObjectModel(om.findObjectByName("line1"))
om.removeFromObjectModel(om.findObjectByName("line2"))
self._play_count += 1
self.set_safe_position(robot)
print("count:", self._play_count)
self._update_moving_object(robot, frame_name)
robot._ctrl.save()
def setBoxGraspTarget(position, rpy, dimensions):
rot_quat = transformUtils.rollPitchYawToQuaternion(
[math.radians(x) for x in rpy])
t = transformUtils.transformFromPose(position, rot_quat)
om.removeFromObjectModel(om.findObjectByName('box'))
obj = makeBox()
obj.setProperty('Dimensions', dimensions)
obj.getChildFrame().copyFrame(t)
obj.setProperty('Surface Mode', 'Wireframe')
obj.setProperty('Color', [1, 0, 0])
def drawBDIFootstepPlanAdjusted(self):
if (self.bdi_plan_adjusted is None):
return
folder = om.getOrCreateContainer('BDI adj footstep plan')
om.removeFromObjectModel(folder)
folder = om.getOrCreateContainer('BDI adj footstep plan')
folder.setIcon(om.Icons.Feet)
om.collapse(folder)
self.drawFootstepPlan(self.bdi_plan_adjusted, folder, [1.0, 1.0, 0.0] , [0.0, 1.0, 1.0])
def testCollection():
affordanceCollection = affordanceManager.collection
assert len(affordanceCollection.collection) == 0
aff = newBox()
assert len(affordanceCollection.collection) == 1
assert aff.getProperty('uuid') in affordanceCollection.collection
om.removeFromObjectModel(aff)
assert len(affordanceCollection.collection) == 0
def _init(self, size):
items = self._getPolyDataItems()
for item in items:
om.removeFromObjectModel(item)
for i in range(0, size):
name = self.name if i == 0 else (self.name + str(i))
item = PolyDataItem(name, vtk.vtkPolyData(), view=None)
item.setProperty("Visible", self.getProperty("Visible"))
for view in self.views:
item.addToView(view)
om.addToObjectModel(item, self)
def testCollection():
affordanceCollection = affordanceManager.collection
assert len(affordanceCollection.collection) == 0
aff = newBox()
assert len(affordanceCollection.collection) == 1
assert aff.getProperty('uuid') in affordanceCollection.collection
om.removeFromObjectModel(aff)
assert len(affordanceCollection.collection) == 0
def selectCandidate(self, selectedObj, candidates):
if self.properties.getProperty('Delete candidates'):
for obj in candidates:
if obj != selectedObj:
om.removeFromObjectModel(obj)
newName = self.properties.getProperty('New name')
if newName:
oldName = selectedObj.getProperty('Name')
selectedObj.setProperty('Name', newName)
for child in selectedObj.children():
child.setProperty('Name', child.getProperty('Name').replace(oldName, newName))
def run(self):
polyData = self.getPointCloud()
annotation = self.getAnnotationInput()
annotationPoint = annotation.annotationPoints[0]
mesh = segmentation.fitShelfItem(polyData, annotationPoint, clusterTolerance=self.properties.getProperty('Cluster tolerance'))
annotation.setProperty('Visible', False)
om.removeFromObjectModel(om.findObjectByName('shelf item'))
obj = vis.showPolyData(mesh, 'shelf item', color=[0,1,0])
t = transformUtils.frameFromPositionAndRPY(segmentation.computeCentroid(mesh), [0,0,0])
segmentation.makeMovable(obj, t)
def run(self):
polyData = self.getPointCloud()
annotation = self.getAnnotationInput()
annotationPoint = annotation.annotationPoints[0]
mesh = segmentation.fitShelfItem(polyData, annotationPoint, clusterTolerance=self.properties.getProperty('Cluster tolerance'))
annotation.setProperty('Visible', False)
om.removeFromObjectModel(om.findObjectByName('shelf item'))
obj = vis.showPolyData(mesh, 'shelf item', color=[0,1,0])
t = transformUtils.frameFromPositionAndRPY(segmentation.computeCentroid(mesh), [0,0,0])
segmentation.makeMovable(obj, t)
def selectLink(self, displayPoint, view):
if not self.enabled():
return False
robotModel, _ = vis.findPickedObject(displayPoint, view)
try:
robotModel.model.getLinkNameForMesh
except AttributeError:
return False
model = robotModel.model
pickedPoint, _, polyData = vis.pickProp(displayPoint, view)
linkName = model.getLinkNameForMesh(polyData)
if not linkName:
return False
fadeValue = 1.0 if linkName == self.selectedLink else 0.05
for name in model.getLinkNames():
linkColor = model.getLinkColor(name)
linkColor.setAlphaF(fadeValue)
model.setLinkColor(name, linkColor)
if linkName == self.selectedLink:
self.selectedLink = None
vis.hideCaptionWidget()
om.removeFromObjectModel(
om.findObjectByName("selected link frame"))
else:
self.selectedLink = linkName
linkColor = model.getLinkColor(self.selectedLink)
linkColor.setAlphaF(1.0)
model.setLinkColor(self.selectedLink, linkColor)
vis.showCaptionWidget(
robotModel.getLinkFrame(self.selectedLink).GetPosition(),
self.selectedLink,
view=view,
)
vis.updateFrame(
robotModel.getLinkFrame(self.selectedLink),
"selected link frame",
scale=0.2,
parent=robotModel,
)
return True
def initialize(self):
self.dt = self.options["dt"]
self.controllerTypeOrder = ["default"]
self.setDefaultOptions()
self.Sensor = SensorObj(
rayLength=self.options["Sensor"]["rayLength"], numRays=self.options["Sensor"]["numRays"]
)
self.SensorApproximator = SensorApproximatorObj(
numRays=self.options["Sensor"]["numRays"], circleRadius=self.options["World"]["circleRadius"]
)
self.Controller = ControllerObj(self.Sensor, self.SensorApproximator)
self.Car = CarPlant(controller=self.Controller, velocity=self.options["Car"]["velocity"])
self.Controller.initializeVelocity(self.Car.v)
self.ActionSet = ActionSetObj()
# create the things needed for simulation
om.removeFromObjectModel(om.findObjectByName("world"))
self.world = World.buildLineSegmentTestWorld(
percentObsDensity=self.options["World"]["percentObsDensity"],
circleRadius=self.options["World"]["circleRadius"],
nonRandom=self.options["World"]["nonRandomWorld"],
scale=self.options["World"]["scale"],
randomSeed=self.options["World"]["randomSeed"],
obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"],
)
om.removeFromObjectModel(om.findObjectByName("robot"))
self.robot, self.frame = World.buildRobot()
self.frame = self.robot.getChildFrame()
self.frame.setProperty("Scale", 3)
# self.frame.setProperty('Visible', False)
# self.frame.setProperty('Edit', True)
self.frame.widget.HandleRotationEnabledOff()
rep = self.frame.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
self.defaultControllerTime = self.options["runTime"]["defaultControllerTime"]
self.Car.setFrame(self.frame)
print "Finished initialization"
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 initialize(self):
self.dt = self.options['dt']
self.controllerTypeOrder = ['default']
self.setDefaultOptions()
self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'],
numRays=self.options['Sensor']['numRays'])
self.SensorManual = SensorObjManual(rayLength=self.options['Sensor']['rayLength'],
numRays=self.options['Sensor']['numRays'])
self.SensorApproximator = SensorApproximatorObj(numRays=self.options['Sensor']['numRays'], circleRadius=self.options['World']['circleRadius'], )
self.Controller = ControllerObj(self.Sensor, self.SensorApproximator)
self.Car = CarPlant(controller=self.Controller,
velocity=self.options['Car']['velocity'])
self.Controller.initializeVelocity(self.Car.v)
# create the things needed for simulation
om.removeFromObjectModel(om.findObjectByName('world'))
self.world = World.buildLineSegmentTestWorld(percentObsDensity=self.options['World']['percentObsDensity'],
circleRadius=self.options['World']['circleRadius'],
nonRandom=self.options['World']['nonRandomWorld'],
scale=self.options['World']['scale'],
randomSeed=self.options['World']['randomSeed'],
obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction'])
om.removeFromObjectModel(om.findObjectByName('robot'))
self.robot, self.frame = World.buildRobot()
self.frame = self.robot.getChildFrame()
self.frame.setProperty('Scale', 3)
#self.frame.setProperty('Visible', False)
#self.frame.setProperty('Edit', True)
self.frame.widget.HandleRotationEnabledOff()
rep = self.frame.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
self.defaultControllerTime = self.options['runTime']['defaultControllerTime']
self.Car.setFrame(self.frame)
print "Finished initialization"
def handle_message(self, msg):
# Limits the rate of message handling, since redrawing is done in the
# message handler.
self._sub.setSpeedLimit(30)
# Removes the folder completely.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
# Recreates folder.
folder = om.getOrCreateContainer(self._folder_name)
# A map from pair of body names to a list of contact forces
collision_pair_to_forces = {}
for contact in msg.contact_info:
point = np.array([
contact.contact_point[0], contact.contact_point[1],
contact.contact_point[2]
])
force = np.array([
contact.contact_force[0], contact.contact_force[1],
contact.contact_force[2]
])
mag = np.linalg.norm(force)
if mag > 1e-4:
mag = 0.3 / mag
key1 = (str(contact.body1_name), str(contact.body2_name))
key2 = (str(contact.body2_name), str(contact.body1_name))
if key1 in collision_pair_to_forces:
collision_pair_to_forces[key1].append(
(point, point + mag * force))
elif key2 in collision_pair_to_forces:
collision_pair_to_forces[key2].append(
(point, point + mag * force))
else:
collision_pair_to_forces[key1] = [(point, point + mag * force)]
for key, list_of_forces in collision_pair_to_forces.iteritems():
d = DebugData()
for force_pair in list_of_forces:
d.addArrow(start=force_pair[0],
end=force_pair[1],
tubeRadius=0.005,
headRadius=0.01)
vis.showPolyData(d.getPolyData(),
str(key),
parent=folder,
color=[0, 1, 0])
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 _handle_message(self, msg):
# Removes the folder completely. This is the clearest and easiest way
# of doing update. Otherwise we need to store the FrameItem returned
# by vis.showFrame, and update its transform. Also need to handle
# enable / disable.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
# Recreates folder.
folder = om.getOrCreateContainer(self._folder_name)
for i in range(0, msg.num_links):
name = msg.link_name[i]
transform = transformUtils.transformFromPose(
msg.position[i], msg.quaternion[i])
vis.showFrame(transform, name, parent=folder, scale=0.1)
def _handle_message(self, msg):
# Removes the folder completely. This is the clearest and easiest way
# of doing update. Otherwise we need to store the FrameItem returned
# by vis.showFrame, and update its transform. Also need to handle
# enable / disable.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
# Recreates folder.
folder = om.getOrCreateContainer(self._folder_name)
for i in range(0, msg.num_links):
name = msg.link_name[i]
transform = transformUtils.transformFromPose(
msg.position[i], msg.quaternion[i])
vis.showFrame(transform, name, parent=folder, scale=0.1);
def _add_polydata(self, polydata, frame_name, color):
"""Adds polydata to the simulation.
Args:
polydata: Polydata.
frame_name: Frame name.
color: Color of object.
Returns:
Frame.
"""
om.removeFromObjectModel(om.findObjectByName(frame_name))
frame = vis.showPolyData(polydata, frame_name, color=color)
vis.addChildFrame(frame)
return frame
def spawnTargetFrame(self):
debugFolder = om.getOrCreateContainer('debug')
om.removeFromObjectModel('target frame')
handLink = str(self.robotSystem.ikPlanner.getHandLink())
handFrame = transformUtils.copyFrame(
self.robotSystem.robotStateModel.getLinkFrame(handLink))
handFrame.PreMultiply()
handFrame.Translate(0.02, 0, 0)
self.targetFrame = vis.updateFrame(handFrame,
'target frame',
parent=debugFolder,
scale=0.15)
return self.targetFrame
def loadHandheldScannerMesh(affordanceManager, filename='oil_bottle.obj', name='oil_bottle', scaleDown=True):
filename = os.path.join(getLabelFusionDataDir(),'object-meshes/handheld-scanner', filename)
print filename
pose = [[0,0,0],[1,0,0,0]]
visObj = loadAffordanceModel(affordanceManager, name, filename, pose)
t = visObj.getChildFrame().transform
center = visObj.polyData.GetCenter()
translation = -np.array(center)
t.Translate(translation)
scale = 0.001
t.Scale(scale, scale, scale)
polyData = filterUtils.transformPolyData(visObj.polyData, t)
om.removeFromObjectModel(visObj)
scaledVisObj = vis.showPolyData(polyData, name+'_small')
vis.addChildFrame(scaledVisObj)
def showHandSamples(self, numberOfSamples=15):
om.removeFromObjectModel(om.findObjectByName('sampled hands'))
handFolder = om.getOrCreateContainer(
'sampled hands', parentObj=om.getOrCreateContainer('debug'))
for i in xrange(numberOfSamples):
t = self.splineInterp(i / float(numberOfSamples - 1))
handObj, f = self.handFactory.placeHandModelWithTransform(
t,
self.view,
side=self.side,
name='sample %d' % i,
parent=handFolder)
handObj.setProperty('Alpha', 0.3)
handFolder.setProperty('Visible', False)
def _handle_message(self, msg):
if set(self._link_name_published) != set(msg.link_name):
# Removes the folder completely.
# TODO(eric.cousineau): Consider only removing frames that are in
# the set difference.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
self._link_name_published = msg.link_name
# Recreates folder.
folder = om.getOrCreateContainer(self._folder_name)
for i in range(0, msg.num_links):
name = msg.link_name[i]
transform = transformUtils.transformFromPose(
msg.position[i], msg.quaternion[i])
# `vis.updateFrame` will either create or update the frame
# according to its name within its parent folder.
vis.updateFrame(transform, name, parent=folder, scale=0.1);
def _handleMarkerSets(self, marker_sets):
# Get the list of existing marker sets so we can track any
# which disappear.
remaining_set_names = set(
[x.getProperty('Name') for x in self.marker_sets.children()])
for marker_set in marker_sets:
set_name = 'Marker set ' + marker_set.name
remaining_set_names.discard(set_name)
set_folder = om.getOrCreateContainer(set_name,
parentObj=self.marker_sets)
marker_ids = range(marker_set.num_markers)
self._updateMarkerCollection(marker_set.name + '.', set_folder,
marker_ids, marker_set.xyz)
for remaining_set in remaining_set_names:
obj = om.findObjectByName(remaining_set, self.marker_sets)
om.removeFromObjectModel(obj)
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 makePostGraspFrame(obj, graspFrameName):
graspFrame = om.findObjectByName(graspFrameName).transform
goalTransform = vtk.vtkTransform()
goalTransform.Translate(0, 0, 0.10)
# Copy the resulting matrix. graspFrame can move around if the
# robot's frame moves when the gripper occludes the mocap.
goalTransform.SetMatrix(
transformUtils.concatenateTransforms([graspFrame,
goalTransform]).GetMatrix())
goalFrameName = "postgrasp to world"
om.removeFromObjectModel(om.findObjectByName(goalFrameName))
goalFrame = vis.showFrame(goalTransform,
goalFrameName,
scale=0.1,
parent=obj,
visible=False)
obj.getChildFrame().getFrameSync().addFrame(goalFrame, ignoreIncoming=True)