Esempio n. 1
0
 def setRegion(self, safe_region):
     debug = DebugData()
     pos = safe_region.point
     try:
         xy_verts = safe_region.xy_polytope()
         if xy_verts.shape[1] == 0:
             raise QhullError("No points returned")
         xyz_verts = np.vstack((xy_verts, pos[2] + 0.02 + np.zeros((1, xy_verts.shape[1]))))
         xyz_verts = np.hstack((xyz_verts, np.vstack((xy_verts, pos[2] + 0.015 + np.zeros((1, xy_verts.shape[1]))))))
         # print xyz_verts.shape
         polyData = vnp.getVtkPolyDataFromNumpyPoints(xyz_verts.T.copy())
         vol_mesh = filterUtils.computeDelaunay3D(polyData)
         for j in range(xy_verts.shape[1]):
             z = pos[2] + 0.005
             p1 = np.hstack((xy_verts[:,j], z))
             if j < xy_verts.shape[1] - 1:
                 p2 = np.hstack((xy_verts[:,j+1], z))
             else:
                 p2 = np.hstack((xy_verts[:,0], z))
             debug.addLine(p1, p2, color=[.7,.7,.7], radius=0.003)
         debug.addPolyData(vol_mesh)
         # self.setPolyData(vol_mesh)
         self.setPolyData(debug.getPolyData())
         self.safe_region = safe_region
     except QhullError:
         print "Could not generate convex hull (polytope is likely unbounded)."
Esempio n. 2
0
 def setRegion(self, safe_region):
     debug = DebugData()
     pos = safe_region.point
     try:
         xy_verts = safe_region.xy_polytope()
         if xy_verts.shape[1] == 0:
             raise QhullError("No points returned")
         xyz_verts = np.vstack((xy_verts, pos[2] + 0.02 + np.zeros(
             (1, xy_verts.shape[1]))))
         xyz_verts = np.hstack((xyz_verts,
                                np.vstack(
                                    (xy_verts, pos[2] + 0.015 + np.zeros(
                                        (1, xy_verts.shape[1]))))))
         # print xyz_verts.shape
         polyData = vnp.getVtkPolyDataFromNumpyPoints(xyz_verts.T.copy())
         vol_mesh = filterUtils.computeDelaunay3D(polyData)
         for j in range(xy_verts.shape[1]):
             z = pos[2] + 0.005
             p1 = np.hstack((xy_verts[:, j], z))
             if j < xy_verts.shape[1] - 1:
                 p2 = np.hstack((xy_verts[:, j + 1], z))
             else:
                 p2 = np.hstack((xy_verts[:, 0], z))
             debug.addLine(p1, p2, color=[.7, .7, .7], radius=0.003)
         debug.addPolyData(vol_mesh)
         # self.setPolyData(vol_mesh)
         self.setPolyData(debug.getPolyData())
         self.safe_region = safe_region
     except QhullError:
         print(
             "Could not generate convex hull (polytope is likely unbounded)."
         )
Esempio n. 3
0
 def drawContactVolumes(self, footstepTransform, color):
     volFolder = getWalkingVolumesFolder()
     for zs, xy in self.contact_slices.iteritems():
         points0 = np.vstack((xy, zs[0] + np.zeros((1,xy.shape[1]))))
         points1 = np.vstack((xy, zs[1] + np.zeros((1,xy.shape[1]))))
         points = np.hstack((points0, points1))
         points = points + np.array([[0.05],[0],[-0.0811]])
         points = points.T
         polyData = vnp.getVtkPolyDataFromNumpyPoints(points.copy())
         vol_mesh = filterUtils.computeDelaunay3D(polyData)
         obj = vis.showPolyData(vol_mesh, 'walking volume', parent=volFolder, alpha=0.5, visible=self.show_contact_slices, color=color)
         obj.actor.SetUserTransform(footstepTransform)
Esempio n. 4
0
    def update(self, snapshot=False):
        """
        Visualizes the pointclouds set to true. This should be called from the main thread
        :return:
        :rtype:
        """

        for topic, data in self._subscribers.iteritems():

            if not data['visualize']:
                continue

            msg = data['subscriber'].lastMsg
            if msg is None:
                continue

            # get frame
            # TransformStamped
            # this might need to be called in thread
            try:
                T_W_pointcloud_stamped = self._tf_buffer.lookup_transform(
                    self._expressed_in_frame, msg.header.frame_id,
                    msg.header.stamp)
            except:
                continue

            T_W_pointcloud = ros_numpy.numpify(
                T_W_pointcloud_stamped.transform)
            T_W_pointcloud_vtk = transformUtils.getTransformFromNumpy(
                T_W_pointcloud)
            pointcloud_numpy = DirectorROSVisualizer.numpy_from_pointcloud2_msg(
                msg)
            pointcloud_vtk = vnp.getVtkPolyDataFromNumpyPoints(
                pointcloud_numpy)
            pointcloud_vtk = filterUtils.transformPolyData(
                pointcloud_vtk, T_W_pointcloud_vtk)

            data['pointcloud'] = pointcloud_vtk
            if snapshot:
                name = data["name"] + " snapshot"
                vis.showPolyData(pointcloud_vtk,
                                 name,
                                 parent=self._vis_container)
            else:
                vis.updatePolyData(pointcloud_vtk,
                                   data['name'],
                                   parent=self._vis_container)
    def fitRunningBoardAtFeet(self):

        # get stance frame
        startPose = self.getPlanningStartPose()
        stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(
            self.robotSystem.robotStateModel, useWorldZ=False)
        stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)

        # get pointcloud and extract search region covering the running board
        polyData = segmentation.getCurrentRevolutionData()
        polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
        _, polyData = segmentation.removeGround(polyData)
        polyData = segmentation.cropToBox(polyData, stanceFrame,
                                          [1.0, 1.0, 0.1])

        if not polyData.GetNumberOfPoints():
            print('empty search region point cloud')
            return

        vis.updatePolyData(polyData,
                           'running board search points',
                           parent=segmentation.getDebugFolder(),
                           color=[0, 1, 0],
                           visible=False)

        # extract maximal points along the stance x axis
        perpAxis = stanceFrameAxes[0]
        edgeAxis = stanceFrameAxes[1]
        edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
        edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
        vis.updatePolyData(edgePoints,
                           'edge points',
                           parent=segmentation.getDebugFolder(),
                           visible=True)

        # ransac fit a line to the edge points
        linePoint, lineDirection, fitPoints = segmentation.applyLineFit(
            edgePoints)
        if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
            lineDirection = -lineDirection

        linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels',
                                                  [1.0, 1.0])
        dists = np.dot(
            vnp.getNumpyFromVtk(linePoints, 'Points') - linePoint,
            lineDirection)
        p1 = linePoint + lineDirection * np.min(dists)
        p2 = linePoint + lineDirection * np.max(dists)
        vis.updatePolyData(fitPoints,
                           'line fit points',
                           parent=segmentation.getDebugFolder(),
                           colorByName='ransac_labels',
                           visible=False)

        # compute a new frame that is in plane with the stance frame
        # and matches the orientation and position of the detected edge
        origin = np.array(stanceFrame.GetPosition())
        normal = np.array(stanceFrameAxes[2])

        # project stance origin to edge, then back to foot frame
        originProjectedToEdge = linePoint + lineDirection * np.dot(
            origin - linePoint, lineDirection)
        originProjectedToPlane = segmentation.projectPointToPlane(
            originProjectedToEdge, origin, normal)
        zaxis = np.array(stanceFrameAxes[2])
        yaxis = np.array(lineDirection)
        xaxis = np.cross(yaxis, zaxis)
        xaxis /= np.linalg.norm(xaxis)
        yaxis = np.cross(zaxis, xaxis)
        yaxis /= np.linalg.norm(yaxis)

        d = DebugData()
        d.addSphere(p1, radius=0.005)
        d.addSphere(p2, radius=0.005)
        d.addLine(p1, p2)
        d.addSphere(originProjectedToEdge, radius=0.001, color=[1, 0, 0])
        d.addSphere(originProjectedToPlane, radius=0.001, color=[0, 1, 0])
        d.addLine(originProjectedToPlane, origin, color=[0, 1, 0])
        d.addLine(originProjectedToEdge, origin, color=[1, 0, 0])
        vis.updatePolyData(d.getPolyData(),
                           'running board edge',
                           parent=segmentation.getDebugFolder(),
                           colorByName='RGB255',
                           visible=False)

        # update the running board box affordance position and orientation to
        # fit the detected edge
        box = self.spawnRunningBoardAffordance()
        boxDimensions = box.getProperty('Dimensions')
        t = transformUtils.getTransformFromAxesAndOrigin(
            xaxis, yaxis, zaxis, originProjectedToPlane)
        t.PreMultiply()
        t.Translate(-boxDimensions[0] / 2.0, 0.0, -boxDimensions[2] / 2.0)
        box.getChildFrame().copyFrame(t)

        self.initialize()
    def fitRunningBoardAtFeet(self):

        # get stance frame
        startPose = self.getPlanningStartPose()
        stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel, useWorldZ=False)
        stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)

        # get pointcloud and extract search region covering the running board
        polyData = segmentation.getCurrentRevolutionData()
        polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
        _, polyData = segmentation.removeGround(polyData)
        polyData = segmentation.cropToBox(polyData, stanceFrame, [1.0, 1.0, 0.1])

        if not polyData.GetNumberOfPoints():
            print 'empty search region point cloud'
            return

        vis.updatePolyData(polyData, 'running board search points', parent=segmentation.getDebugFolder(), color=[0,1,0], visible=False)

        # extract maximal points along the stance x axis
        perpAxis = stanceFrameAxes[0]
        edgeAxis = stanceFrameAxes[1]
        edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
        edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
        vis.updatePolyData(edgePoints, 'edge points', parent=segmentation.getDebugFolder(), visible=True)

        # ransac fit a line to the edge points
        linePoint, lineDirection, fitPoints = segmentation.applyLineFit(edgePoints)
        if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
            lineDirection = -lineDirection

        linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels', [1.0, 1.0])
        dists = np.dot(vnp.getNumpyFromVtk(linePoints, 'Points')-linePoint, lineDirection)
        p1 = linePoint + lineDirection*np.min(dists)
        p2 = linePoint + lineDirection*np.max(dists)
        vis.updatePolyData(fitPoints, 'line fit points', parent=segmentation.getDebugFolder(), colorByName='ransac_labels', visible=False)


        # compute a new frame that is in plane with the stance frame
        # and matches the orientation and position of the detected edge
        origin = np.array(stanceFrame.GetPosition())
        normal = np.array(stanceFrameAxes[2])

        # project stance origin to edge, then back to foot frame
        originProjectedToEdge = linePoint + lineDirection*np.dot(origin - linePoint, lineDirection)
        originProjectedToPlane = segmentation.projectPointToPlane(originProjectedToEdge, origin, normal)
        zaxis = np.array(stanceFrameAxes[2])
        yaxis = np.array(lineDirection)
        xaxis = np.cross(yaxis, zaxis)
        xaxis /= np.linalg.norm(xaxis)
        yaxis = np.cross(zaxis, xaxis)
        yaxis /= np.linalg.norm(yaxis)

        d = DebugData()
        d.addSphere(p1, radius=0.005)
        d.addSphere(p2, radius=0.005)
        d.addLine(p1, p2)
        d.addSphere(originProjectedToEdge, radius=0.001, color=[1,0,0])
        d.addSphere(originProjectedToPlane, radius=0.001, color=[0,1,0])
        d.addLine(originProjectedToPlane, origin, color=[0,1,0])
        d.addLine(originProjectedToEdge, origin, color=[1,0,0])
        vis.updatePolyData(d.getPolyData(), 'running board edge', parent=segmentation.getDebugFolder(), colorByName='RGB255', visible=False)

        # update the running board box affordance position and orientation to
        # fit the detected edge
        box = self.spawnRunningBoardAffordance()
        boxDimensions = box.getProperty('Dimensions')
        t = transformUtils.getTransformFromAxesAndOrigin(xaxis, yaxis, zaxis, originProjectedToPlane)
        t.PreMultiply()
        t.Translate(-boxDimensions[0]/2.0, 0.0, -boxDimensions[2]/2.0)
        box.getChildFrame().copyFrame(t)

        self.initialize()