def init(view=None, robotName=""): global imageManager if not imageManager: imageManager = ImageManager() global cameraViews if not cameraViews: cameraViews = {} cameraViews[robotName] = CameraView(imageManager, view, robotName) directorConfig = drcargs.getRobotConfig(robotName) _modelName = directorConfig["modelName"] cameraNames = imageManager.images[robotName] cameras = [ camera["name"] for camera in directorConfig["sensors"]["camera"]["color"] ] for camera in cameras: if camera in cameraNames: print("will add {} to view".format(camera)) imageManager.addImage(camera, robotName) view = CameraImageView(imageManager, camera, camera, robotName=robotName) global views if robotName not in views: views[robotName] = {} views[robotName][camera] = view
def __init__(self, handType, robotModel): """ handType is of the form 'left_robotiq' or 'right_valkyrie' """ def toFrame(xyzrpy): rpy = [math.degrees(rad) for rad in xyzrpy[3:]] return transformUtils.frameFromPositionAndRPY(xyzrpy[:3], rpy) self.side, self.handType = handType.split("_") assert self.side in ("left", "right") thisCombination = None handCombinations = drcargs.getRobotConfig( robotModel.robotName)["handCombinations"] numberOfHands = len(handCombinations) for i in range(0, numberOfHands): if handCombinations[i]["side"] == self.side: thisCombination = handCombinations[i] break assert thisCombination is not None self.handLinkName = thisCombination["handLinkName"] self.handUrdf = thisCombination["handUrdf"] self.loadHandModel() self.initHandTransforms(robotModel, thisCombination)
def __init__(self, imageManager, view=None, robotName=""): self.imageManager = imageManager self.updateUtimes = {} self.robotModel = None self.sphereObjects = {} self.robotName = robotName try: if self.robotName: self.images = [ camera["name"] for camera in drcargs.getRobotConfig( self.robotName)["sensors"]["camera"]["color"] ] except KeyError as e: raise Exception( "CameraView requires color cameras to be defined at sensors/camera/color." " Check your director config.") for name in self.images: imageManager.addImage(name, self.robotName) self.updateUtimes[name] = 0 self.initView(view) self.initEventFilter() self.rayCallback = rayDebug self.timerCallback = TimerCallback() self.timerCallback.targetFps = 60 self.timerCallback.callback = self.updateView self.timerCallback.start()
def getHeadLink(self): headLink = drcargs.getRobotConfig(self.robotName).get("headLink") if not headLink: import warnings warnings.warn( "headLink is not defined in director config - certain features will be broken" ) return headLink
def init(view, robotStateJointController): global _multisenseItem sensorsFolder = om.getOrCreateContainer( "sensors", om.getOrCreateContainer(robotStateJointController.robotName) ) config = drcargs.getRobotConfig(robotStateJointController.robotName)[ "perceptionSources" ] validSourceTypes = ["gridMap", "depthImagePointCloud", "pointCloud"] perceptionSources = {} for sourceType in config: if sourceType not in validSourceTypes: raise ValueError( "Source type {} is not a recognised perception source. Valid types are {}. Check your" " director configuration.".format(sourceType, validSourceTypes) ) # TODO might be nice to avoid the if statement in the loop by having a function to create each source for sourceConfig in config[sourceType]: if sourceType == "gridMap": source = RosGridMap( robotStateJointController, sourceConfig["name"], callbackFunc=view.render, ) source.addToView(view) om.addToObjectModel(source, sensorsFolder) if sourceType == "depthImagePointCloud": source = DepthImagePointCloudSource( sourceConfig["name"], sourceConfig["sensor"], None, robotStateJointController, ) source.addToView(view) om.addToObjectModel(source, sensorsFolder) if sourceType == "pointCloud": source = PointCloudSource( sourceConfig["name"], robotStateJointController, callbackFunc=view.render, ) source.addToView(view) om.addToObjectModel(source, sensorsFolder) if "properties" in sourceConfig: for prop, value in sourceConfig["properties"].items(): source.setProperty(prop, value) perceptionSources[sourceConfig["robotSystemKey"]] = source return perceptionSources
def __init__(self, robotModel, defaultLeftHandType=None, defaultRightHandType=None): self.robotModel = robotModel self.defaultHandTypes = {} self.loaders = {} handCombinations = drcargs.getRobotConfig(None).get("handCombinations", []) for description in handCombinations: handType = description["handType"] side = description["side"] self.defaultHandTypes[side] = handType
def resetCameraToRobotAbove(self, view): link = drcargs.getRobotConfig(self.robotName)["pelvisLink"] t = self.robotModel.getLinkFrame(link) if t is None: t = vtk.vtkTransform() focalPoint = [2, 0.0, 0.25] position = [1, 0.0, 15.25] # to avoid singularities t.TransformPoint(focalPoint, focalPoint) t.TransformPoint(position, position) flyer = cameracontrol.Flyer(view) flyer.zoomTo(focalPoint, position)
def resetCameraToRobot(self, view): link = drcargs.getRobotConfig(self.robotName)["pelvisLink"] t = self.robotModel.getLinkFrame(link) if t is None: t = vtk.vtkTransform() focalPoint = [0.0, 0.0, 0.25] position = [-4.0, -2.0, 2.25] t.TransformPoint(focalPoint, focalPoint) t.TransformPoint(position, position) flyer = cameracontrol.Flyer(view) flyer.zoomTo(focalPoint, position)
def initDirectorConfig(self, robotSystem): directorConfig = drcargs.getRobotConfig(robotSystem.robotName) if "colorMode" not in directorConfig: defaultColorMode = "URDF Colors" directorConfig["colorMode"] = defaultColorMode assert directorConfig["colorMode"] in [ "URDF Colors", "Solid Color", "Textures" ] return FieldContainer(directorConfig=directorConfig, robotType=directorConfig["modelName"])
def resetCameraToHeadView(self, view): head = self.robotModel.getLinkFrame( drcargs.getRobotConfig(self.robotName)["headLink"]) pelvis = self.robotModel.getLinkFrame( drcargs.getRobotConfig(self.robotName)["pelvisLink"]) viewDirection = np.array([1.0, 0.0, 0.0]) pelvis.TransformVector(viewDirection, viewDirection) cameraPosition = np.array(head.GetPosition()) + 0.10 * viewDirection camera = view.camera() focalOffset = np.array(camera.GetFocalPoint()) - np.array( camera.GetPosition()) focalOffset /= np.linalg.norm(focalOffset) camera.SetPosition(cameraPosition) camera.SetFocalPoint(cameraPosition + focalOffset * 0.03) camera.SetViewUp([0, 0, 1]) camera.SetViewAngle(90) view.render()
def newDrivingGoal(self, displayPoint, view): # Places the driving goal on the plane of the root link current yaw # for husky: the bottom of the wheels. # for hyq/anymal the midpoint of the trunk # TODO: read the link from the director config mainLink = drcargs.getRobotConfig(self.robotName)["pelvisLink"] footFrame = self.robotModel.getLinkFrame(mainLink) worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint) groundOrigin = footFrame.GetPosition() groundNormal = [0.0, 0.0, 1.0] selectedGroundPoint = [0.0, 0.0, 0.0] t = vtk.mutable(0.0) vtk.vtkPlane.IntersectWithLine( worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint ) footFrameRPY = transformUtils.rollPitchYawFromTransform(footFrame) drivingTarget = transformUtils.frameFromPositionAndRPY( selectedGroundPoint, [0, 0, footFrameRPY[2] * 180.0 / np.pi] ) # Create the widget and send a message: # walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel) frameObj = vis.updateFrame( drivingTarget, "driving goal", parent="planning", scale=0.25 ) frameObj.setProperty("Edit", True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() frameObj.connectFrameModified(onNewDrivingGoal) onNewDrivingGoal(frameObj)
def __init__( self, model=None, visible=True, color="#ff0000", colorMode=0, alpha=1.0, robotName="", ): if model: alpha = model.alpha() visible = model.visible() color = model.color() filename = model.filename() self.initialised = True else: filename = "" self.initialised = False self.robotName = robotName directorConfig = drcargs.getRobotConfig(self.robotName) self._modelName = directorConfig["modelName"] om.ObjectModelItem.__init__(self, self._modelName, om.Icons.Robot) self.views = [] self.model = None self.callbacks.addSignal(self.MODEL_CHANGED_SIGNAL) self.addProperty("Filename", filename) self.addProperty("Visible", visible) self.addProperty( "Alpha", alpha, attributes=om.PropertyAttributes(decimals=2, minimum=0, maximum=1.0, singleStep=0.1, hidden=False), ) self.addProperty( "Color Mode", colorMode, attributes=om.PropertyAttributes( enumNames=["Solid Color", "Textures", "URDF Colors"]), ) self.addProperty("Color", color) if model: self.setModel(model) self.pelvisLink = "" # pelvis self.leftFootLink = "" # l_foot self.rightFootLink = "" # r_foot self.leftHandLink = "" self.rightHandLink = "" self.quadruped = False if "pelvisLink" in directorConfig: self.pelvisLink = directorConfig["pelvisLink"] if "leftFootLink" in directorConfig: self.leftFootLink = directorConfig["leftFootLink"] self.rightFootLink = directorConfig["rightFootLink"] if "quadruped" in directorConfig: self.quadruped = True # Using 'hands' to signify quadruped front feet, for now: # Note: there has not been the use of leftHandLink for previous bipeds if "leftHandLink" in directorConfig: self.leftHandLink = directorConfig["leftHandLink"] self.rightHandLink = directorConfig["rightHandLink"]
from director import segmentation from director import segmentationroutines from director import applogic from director import visualization as vis from director import drcargs from director import roboturdf app = ConsoleApp() # create a view view = app.createView() segmentation._defaultSegmentationView = view segmentation.initAffordanceManager(view) robot_config = drcargs.getRobotConfig() urdf_path = os.path.join(robot_config.dirname, robot_config['urdfConfig']['default']) robotStateModel, robotStateJointController = roboturdf.loadRobotModel( 'robot state model', view, parent='sensors', urdfFile=urdf_path, color=roboturdf.getRobotGrayColor(), visible=True) segmentationroutines.SegmentationContext.initWithRobot(robotStateModel) # load poly data dataDir = app.getTestingDataDirectory() polyData = ioutils.readPolyData( os.path.join(dataDir, 'misc/valve/valve-sparse-stereo.pcd'))
if not useLightColorScheme: viewBackgroundLightHandler.action.trigger() cameraBooksmarksPanel = camerabookmarks.init(view) cameraControlPanel = cameracontrolpanel.CameraControlPanel(view) app.addWidgetToDock(cameraControlPanel.widget, action=None).hide() app.setCameraTerrainModeEnabled(view, True) app.resetCamera(viewDirection=[-1, 0, 0], view=view) for robotSystem in robotSystems: selector.addRobot(robotSystem.robotName) selector.associateViewBehaviorWithRobot(robotSystem.viewBehaviors, robotSystem.robotName) directorConfig = drcargs.getRobotConfig(robotSystem.robotName) usePerception = True useDataFiles = True useSkybox = False useFeetlessRobot = False poseCollection = PythonQt.dd.ddSignalMap() costCollection = PythonQt.dd.ddSignalMap() if "disableComponents" in directorConfig: for component in directorConfig["disableComponents"]: print "Disabling", component locals()[component] = False
def getStateJointNames(self): if not self._jointNames: self._jointNames = drcargs.getRobotConfig( self.robotName)["jointNames"] return self._jointNames
def newWalkingGoal(self, displayPoint, view): # put walking goal at robot's base mainLink = drcargs.getRobotConfig(self.robotName)["pelvisLink"] footFrame = self.robotModel.getLinkFrame(mainLink) if not footFrame: print( "ERROR: The link '{}' provided for the key 'pelvisLink' in the configuration file does not exist in " "the robot's URDF. Cannot place walking goal.".format(mainLink) ) return worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint) groundOrigin = footFrame.GetPosition() groundNormal = [0.0, 0.0, 1.0] selectedGroundPoint = [0.0, 0.0, 0.0] t = vtk.mutable(0.0) vtk.vtkPlane.IntersectWithLine( worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint ) walkingTarget = transformUtils.frameFromPositionAndRPY( selectedGroundPoint, np.array(footFrame.GetOrientation()) ) frameObj = vis.updateFrame( walkingTarget, self.robotName + " walking goal", parent="planning", scale=0.25, ) frameObj.setProperty("Edit", True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() terrain = om.findObjectByName("HEIGHT_MAP_SCENE") if terrain: pos = np.array(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]) frameObj.transform.Translate( pos - np.array(frameObj.transform.GetPosition()) ) d = DebugData() d.addSphere((0, 0, 0), radius=0.03) handle = vis.showPolyData( d.getPolyData(), "walking goal terrain handle " + self.robotName, parent=frameObj, visible=True, color=[1, 1, 0], ) handle.actor.SetUserTransform(frameObj.transform) placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain) def onFramePropertyModified(propertySet, propertyName): if propertyName == "Edit": if propertySet.getProperty(propertyName): placer.start() else: placer.stop() frameObj.properties.connectPropertyChanged(onFramePropertyModified) onFramePropertyModified(frameObj, "Edit") frameObj.connectFrameModified(self.onWalkingGoalModified)