def updateMouseHitPoint(self, event):
displayPoint = self.getMousePositionInView(event)
camera = self.view.camera()
actors = self.view.renderer().GetActors()
actors = [
actors.GetItemAsObject(i) for i in range(actors.GetNumberOfItems())
]
pickableSettings = [actor.GetPickable() for actor in actors]
for actor in actors:
if actor != self.cameraCenterObj.actor:
actor.SetPickable(True)
#res = vis.pickPoint(displayPoint, self.view, pickType='render')
#if res.pickedProp:
# print('got pick with hardware')
#if not res.pickedProp:
res = vis.pickPoint(displayPoint,
self.view,
pickType='points',
tolerance=0.0005)
if not res.pickedProp:
res = vis.pickPoint(displayPoint,
self.view,
pickType='points',
tolerance=0.001)
if not res.pickedProp:
res = vis.pickPoint(displayPoint, self.view, pickType='cells')
for actor, pickable in zip(actors, pickableSettings):
actor.SetPickable(pickable)
if res.pickedProp is not None:
worldPoint = res.pickedPoint
else:
pt1, pt2 = vis.getRayFromDisplayPoint(self.view, displayPoint)
if self.lastHitPoint is None:
worldPoint = projectPointToLine(
pt1, pt2, np.array(camera.GetFocalPoint()))
else:
projectedWorldPoint, pcoord = projectPointToLineParam(
pt1, pt2, self.lastHitPoint)
if pcoord >= 0.1:
worldPoint = projectedWorldPoint
else:
worldPoint = projectPointToLine(
pt1, pt2, np.array(camera.GetFocalPoint()))
# don't use last hit point
# self.lastHitPoint = np.array(worldPoint)
t = vtk.vtkTransform()
t.Translate(worldPoint[:3])
self.cameraCenterObj.actor.SetUserTransform(t)
self.showOnMove = True
self.style.SetCustomCenterOfRotation(worldPoint[:3])
def newWalkingGoal(displayPoint, view):
footFrame = footstepsDriver.getFeetMidPoint(robotModel)
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)
footFrame.Translate(np.array(selectedGroundPoint) - np.array(footFrame.GetPosition()))
footstepsdriverpanel.panel.onNewWalkingGoal(footFrame)
def newWalkingGoal(displayPoint, view):
footFrame = footstepsDriver.getFeetMidPoint(robotModel)
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()))
footstepsdriverpanel.panel.onNewWalkingGoal(walkingTarget)
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 getCameraRay(displayPoint):
_, ray = vis.getRayFromDisplayPoint(view, displayPoint)
ray = ray - origin
ray /= np.linalg.norm(ray)
return ray
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)
def updateMouseHitPoint(self, event):
displayPoint = self.getMousePositionInView(event)
camera = self.view.camera()
if self.lastHitPoint is not None:
newFocalPoint = projectPointToLine(
np.array(camera.GetPosition()),
np.array(camera.GetFocalPoint()),
self.lastHitPoint)
camera.SetFocalPoint(newFocalPoint)
actors = self.view.renderer().GetActors()
actors = [actors.GetItemAsObject(i) for i in range(actors.GetNumberOfItems())]
pickableSettings = [actor.GetPickable() for actor in actors]
for actor in actors:
if actor != self.cameraCenterObj.actor:
actor.SetPickable(True)
#res = vis.pickPoint(displayPoint, self.view, pickType='render')
#if res.pickedProp:
# print('got pick with hardware')
#if not res.pickedProp:
res = vis.pickPoint(displayPoint, self.view, pickType='points', tolerance=0.0005)
if not res.pickedProp:
res = vis.pickPoint(displayPoint, self.view, pickType='points', tolerance=0.001)
if res.pickedProp is None:
res = vis.pickPoint(displayPoint, self.view, pickType='cells')
for actor, pickable in zip(actors, pickableSettings):
actor.SetPickable(pickable)
if res.pickedProp is not None:
worldPoint = res.pickedPoint
else:
pt1, pt2 = vis.getRayFromDisplayPoint(self.view, displayPoint)
rayOrigin = pt1
ray = pt2 - pt1
pt1 = np.array(camera.GetPosition())
pt2 = pt1 + ray
#print('cam to ray origin:', rayOrigin - pt1)
if self.lastHitPoint is None:
worldPoint = projectPointToLine(pt1, pt2, np.array(camera.GetFocalPoint()))
else:
#print('no prop was picked, finding a point on the pick ray...')
projectedWorldPoint, pcoord = projectPointToLineParam(pt1, pt2, self.lastHitPoint)
#print('pcoord of last hit point:', pcoord)
if pcoord >= 0.1:
#print('using last hit point projected to pick ray')
worldPoint = projectedWorldPoint
else:
#print('pcoord was bad so using current focal projected to pick ray')
worldPoint = projectPointToLine(pt1, pt2, np.array(camera.GetFocalPoint()))
self.lastHitPoint = np.array(worldPoint)
t = vtk.vtkTransform()
t.Translate(worldPoint[:3])
self.cameraCenterObj.actor.SetUserTransform(t)
self.showOnMove = True
self.style.SetCustomCenterOfRotation(worldPoint[:3])
def getCameraRay(displayPoint):
_, ray = vis.getRayFromDisplayPoint(view, displayPoint)
ray = ray-origin
ray /= np.linalg.norm(ray)
return ray