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 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)