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)
