def findClosestPointSingleLink(self, linkName, pointInLinkFrame):
data = self.locatorData[linkName]
pointInWorldFrame = data['meshToWorld'].TransformPoint(pointInLinkFrame)
cellId = vtk.mutable(0)
subId = vtk.mutable(0)
dist2 = vtk.mutable(0)
closestPoint = [0.0, 0.0, 0.0]
data['locator'].FindClosestPoint(pointInWorldFrame, closestPoint, cellId, subId, dist2)
# this is a hack to get the normals to turn out correctly
normal = -np.array(data['normals'].GetTuple(cellId))
# want to convert all quantities back to linkFrame so that they are portable across different robot poses
worldToMesh = data['worldToMesh']
closestPointLinkFrame = worldToMesh.TransformPoint(closestPoint)
normalLinkFrame = worldToMesh.TransformVector(normal)
closestPointData = {'linkName': linkName, 'closestPoint': closestPointLinkFrame, 'cellId': cellId,
'normal': normalLinkFrame, 'dist2': dist2}
return closestPointData
def raycastAgainstLinkMesh(self, linkName, rayOrigin, rayEnd):
meshToWorld = self.linkMeshData[linkName]['transform']
rayOriginInWorld = np.array(meshToWorld.TransformPoint(rayOrigin))
rayEndInWorld = np.array(meshToWorld.TransformPoint(rayEnd))
# ### DEBUGGING
# if self.showContactRay:
# d = DebugData()
# d.addLine(rayOriginInWorld, rayEndInWorld, radius=0.005)
# color=[1,0,0]
# obj = vis.updatePolyData(d.getPolyData(), "raycast ray in mesh frame", color=color)
tolerance = 0.0 # intersection tolerance
pt = [0.0, 0.0, 0.0] # data coordinate where intersection occurs
lineT = vtk.mutable(
0.0
) # parametric distance along line segment where intersection occurs
pcoords = [
0.0, 0.0, 0.0
] # parametric location within cell (triangle) where intersection occurs
subId = vtk.mutable(0) # sub id of cell intersection
result = self.linkMeshData[linkName]['locator'].IntersectWithLine(
rayOriginInWorld, rayEndInWorld, tolerance, lineT, pt, pcoords,
subId)
# this means we didn't find an intersection
if not result:
return None
# otherwise we need to transform it back to linkFrame
worldToMesh = meshToWorld.GetLinearInverse()
ptInLinkFrame = worldToMesh.TransformPoint(pt)
return ptInLinkFrame
def raycast(self, locator, rayOrigin, rayEnd):
tolerance = 0.0 # intersection tolerance
pt = [0.0, 0.0, 0.0] # data coordinate where intersection occurs
lineT = vtk.mutable(0.0) # parametric distance along line segment where intersection occurs
pcoords = [0.0, 0.0, 0.0] # parametric location within cell (triangle) where intersection occurs
subId = vtk.mutable(0) # sub id of cell intersection
result = locator.IntersectWithLine(rayOrigin, rayEnd, tolerance, lineT, pt, pcoords, subId)
return pt if result else None
def raycast(locator, rayOrigin, rayEnd):
tolerance = 0.0 # intersection tolerance
pt = [0.0, 0.0, 0.0] # data coordinate where intersection occurs
lineT = vtk.mutable(
0.0
) # parametric distance along line segment where intersection occurs
pcoords = [
0.0, 0.0, 0.0
] # parametric location within cell (triangle) where intersection occurs
subId = vtk.mutable(0) # sub id of cell intersection
result = locator.IntersectWithLine(rayOrigin, rayEnd, tolerance, lineT,
pt, pcoords, subId)
return pt if result else None
def computePointToSurfaceDistance(pointsPolyData, meshPolyData):
cl = vtk.vtkCellLocator()
cl.SetDataSet(meshPolyData)
cl.BuildLocator()
points = vnp.getNumpyFromVtk(pointsPolyData, 'Points')
dists = np.zeros(len(points))
closestPoint = np.zeros(3)
closestPointDist = vtk.mutable(0.0)
cellId = vtk.mutable(0)
subId = vtk.mutable(0)
for i in xrange(len(points)):
cl.FindClosestPoint(points[i], closestPoint, cellId, subId,
closestPointDist)
dists[i] = closestPointDist
return np.sqrt(dists)
def _cast_ray(self, start, end):
"""Casts a ray and determines intersections and distances.
Args:
start: Origin of the ray.
end: End point of the ray.
Returns:
Tuple of (whether it intersected, distance, intersection).
"""
tolerance = 0.0 # intersection tolerance
pt = [0.0, 0.0, 0.0] # coordinate of intersection
distance = vtk.mutable(0.0) # distance of intersection
pcoords = [0.0, 0.0, 0.0] # location within intersected cell
subID = vtk.mutable(0) # subID of intersected cell
hit = self._locator.IntersectWithLine(start, end, tolerance,
distance, pt, pcoords, subID)
return hit, distance, pt
def _cast_ray(self, start, end):
"""Casts a ray and determines intersections and distances.
Args:
start: Origin of the ray.
end: End point of the ray.
Returns:
Tuple of (whether it intersected, distance, intersection).
"""
tolerance = 0.0 # intersection tolerance
pt = [0.0, 0.0, 0.0] # coordinate of intersection
distance = vtk.mutable(0.0) # distance of intersection
pcoords = [0.0, 0.0, 0.0] # location within intersected cell
subID = vtk.mutable(0) # subID of intersected cell
hit = self._locator.IntersectWithLine(start, end, tolerance, distance,
pt, pcoords, subID)
return hit, distance, pt
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 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)
