Esempio n. 1
0
    def spin(self):

        parser = OptionParser(description='Openrave Velma interface')
        OpenRAVEGlobalArguments.addOptions(parser)
        (options, leftargs) = parser.parse_args()
        self.env = OpenRAVEGlobalArguments.parseAndCreate(options)#,defaultviewer=True)
        self.openrave_robot = self.env.ReadRobotXMLFile('robots/barretthand_ros.robot.xml')

        link = self.openrave_robot.GetLink("right_HandFingerOneKnuckleThreeLink")
        col = link.GetCollisionData()
        vertices = col.vertices
        faces = col.indices

        print "sampling the surface..."
        points = surfaceutils.sampleMeshDetailedRays(vertices, faces, 0.001)
        print "surface has %s points"%(len(points))

#        vertices, faces = surfaceutils.readStl("klucz_gerda_ascii.stl", scale=1.0)
#        points = surfaceutils.sampleMeshDetailedRays(vertices, faces, 0.002)

        print len(points)

        with open('points.txt', 'w') as f:
            for pt in points:
                p = pt.pos
                f.write(str(p.x()) + ' ' + str(p.y()) + ' ' + str(p.z()) + '\n')

        call(["touch","out_surf.off"])
        call(["touch","out_axis.off"])
        call(["/home/dseredyn/code/cocone/tightcocone/tcocone-linux", "-m", "points.txt", "out"])

        calc_radius = False
        points_medial = []
        with open('out_axis.off', 'r') as f:
            header = f.readline()
            header_s = header.split()
            p_count = int(header_s[1])
            for i in range(0, p_count):
                line = f.readline()
                values = line.split()
                center = PyKDL.Vector(float(values[0]), float(values[1]), float(values[2]))

                if calc_radius:
                    radius = None
                    for pt in points:
                        dist = (center-pt.pos).Norm()
                        if radius == None or radius > dist:
                            radius = dist
                    points_medial.append([center, radius*2.0])
                else:
                    points_medial.append([center, 0.001])

        print len(points_medial)
#        m_id = 0
#        for p in points:
#            m_id = self.pub_marker.publishSinglePointMarker(PyKDL.Vector(p[0], p[1], p[2]), m_id, r=0, g=1, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.005, 0.005, 0.005), T=None)

#        m_id = 0
#        for p in points_medial:
#            m_id = self.pub_marker.publishSinglePointMarker(p[0], m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(p[1],p[1],p[1]), T=None)
        m_id = 0
        m_id = self.pub_marker.publishMultiPointsMarkerWithSize(points_medial, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, T=None)
#        m_id = self.pub_marker.publishMultiPointsMarker(points_medial, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.0005, 0.0005, 0.0005), T=None)
        rospy.sleep(1)

        

        exit(0)
Esempio n. 2
0
    def spin(self):

        parser = OptionParser(description='Openrave Velma interface')
        OpenRAVEGlobalArguments.addOptions(parser)
        (options, leftargs) = parser.parse_args()
        self.env = OpenRAVEGlobalArguments.parseAndCreate(
            options)  #,defaultviewer=True)
        self.openrave_robot = self.env.ReadRobotXMLFile(
            'robots/barretthand_ros.robot.xml')

        link = self.openrave_robot.GetLink(
            "right_HandFingerOneKnuckleThreeLink")
        col = link.GetCollisionData()
        vertices = col.vertices
        faces = col.indices

        print "sampling the surface..."
        points = surfaceutils.sampleMeshDetailedRays(vertices, faces, 0.001)
        print "surface has %s points" % (len(points))

        #        vertices, faces = surfaceutils.readStl("klucz_gerda_ascii.stl", scale=1.0)
        #        points = surfaceutils.sampleMeshDetailedRays(vertices, faces, 0.002)

        print len(points)

        with open('points.txt', 'w') as f:
            for pt in points:
                p = pt.pos
                f.write(
                    str(p.x()) + ' ' + str(p.y()) + ' ' + str(p.z()) + '\n')

        call(["touch", "out_surf.off"])
        call(["touch", "out_axis.off"])
        call([
            "/home/dseredyn/code/cocone/tightcocone/tcocone-linux", "-m",
            "points.txt", "out"
        ])

        calc_radius = False
        points_medial = []
        with open('out_axis.off', 'r') as f:
            header = f.readline()
            header_s = header.split()
            p_count = int(header_s[1])
            for i in range(0, p_count):
                line = f.readline()
                values = line.split()
                center = PyKDL.Vector(float(values[0]), float(values[1]),
                                      float(values[2]))

                if calc_radius:
                    radius = None
                    for pt in points:
                        dist = (center - pt.pos).Norm()
                        if radius == None or radius > dist:
                            radius = dist
                    points_medial.append([center, radius * 2.0])
                else:
                    points_medial.append([center, 0.001])

        print len(points_medial)
        #        m_id = 0
        #        for p in points:
        #            m_id = self.pub_marker.publishSinglePointMarker(PyKDL.Vector(p[0], p[1], p[2]), m_id, r=0, g=1, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.005, 0.005, 0.005), T=None)

        #        m_id = 0
        #        for p in points_medial:
        #            m_id = self.pub_marker.publishSinglePointMarker(p[0], m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(p[1],p[1],p[1]), T=None)
        m_id = 0
        m_id = self.pub_marker.publishMultiPointsMarkerWithSize(
            points_medial,
            m_id,
            r=1,
            g=0,
            b=0,
            namespace='default',
            frame_id='world',
            m_type=Marker.SPHERE,
            T=None)
        #        m_id = self.pub_marker.publishMultiPointsMarker(points_medial, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(0.0005, 0.0005, 0.0005), T=None)
        rospy.sleep(1)

        exit(0)
Esempio n. 3
0
    def __init__(self, vol_radius, vol_samples_count, T_H_O, orientations_angle, vertices_obj, faces_obj):
        self.vol_radius = vol_radius
        self.vol_samples_count = vol_samples_count
        self.index_factor = float(self.vol_samples_count)/(2.0*self.vol_radius)
        self.vol_samples = []
        for x in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count):
            self.vol_samples.append([])
            for y in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count):
                self.vol_samples[-1].append([])
                for z in np.linspace(-self.vol_radius, self.vol_radius, self.vol_samples_count):
                    self.vol_samples[-1][-1].append([])
                    self.vol_samples[-1][-1][-1] = {}
        self.vol_sample_points = []
        for xi in range(self.vol_samples_count):
            for yi in range(self.vol_samples_count):
                for zi in range(self.vol_samples_count):
                    self.vol_sample_points.append( self.getVolPoint(xi,yi,zi) )
        self.T_H_O = T_H_O
        self.T_O_H = self.T_H_O.Inverse()

        # generate the set of orientations
        self.orientations_angle = orientations_angle
        normals_sphere = velmautils.generateNormalsSphere(self.orientations_angle)

        orientations1 = velmautils.generateFramesForNormals(self.orientations_angle, normals_sphere)
        orientations2 = []
        for ori in orientations1:
            x_axis = ori * PyKDL.Vector(1,0,0)
            if x_axis.z() > 0.0:
                orientations2.append(ori)
        self.orientations = {}
        for ori_idx in range(len(orientations2)):
            self.orientations[ori_idx] = orientations2[ori_idx]

        # generate a set of surface points
        self.surface_points_obj = surfaceutils.sampleMeshDetailedRays(vertices_obj, faces_obj, 0.0015)
        print "surface of the object has %s points"%(len(self.surface_points_obj))

        # disallow contact with the surface points beyond the key handle
        for p in self.surface_points_obj:
            if p.pos.x() > 0.0:
                p.allowed = False

        surface_points_obj_init = []
        surface_points2_obj_init = []
        for sp in self.surface_points_obj:
            if sp.allowed:
                surface_points_obj_init.append(sp)
            else:
                surface_points2_obj_init.append(sp)

        print "generating a subset of surface points of the object..."

        while True:
            p_idx = random.randint(0, len(self.surface_points_obj)-1)
            if self.surface_points_obj[p_idx].allowed:
                break
        p_dist = 0.003

        self.sampled_points_obj = []
        while True:
            self.sampled_points_obj.append(p_idx)
            surface_points_obj2 = []
            for sp in surface_points_obj_init:
                if (sp.pos-self.surface_points_obj[p_idx].pos).Norm() > p_dist:
                    surface_points_obj2.append(sp)
            if len(surface_points_obj2) == 0:
                break
            surface_points_obj_init = surface_points_obj2
            p_idx = surface_points_obj_init[0].id

        print "subset size: %s"%(len(self.sampled_points_obj))

        print "generating a subset of other surface points of the object..."

        p_dist2 = 0.006

        while True:
            p_idx = random.randint(0, len(self.surface_points_obj)-1)
            if not self.surface_points_obj[p_idx].allowed:
                break

        self.sampled_points2_obj = []
        while True:
            self.sampled_points2_obj.append(p_idx)
            surface_points2_obj2 = []
            for sp in surface_points2_obj_init:
                if (sp.pos-self.surface_points_obj[p_idx].pos).Norm() > p_dist2:
                    surface_points2_obj2.append(sp)
            if len(surface_points2_obj2) == 0:
                break
            surface_points2_obj_init = surface_points2_obj2
            p_idx = surface_points2_obj_init[0].id

        # test volumetric model
        if False:
            for pt_idx in self.sampled_points2_obj:
                pt = self.surface_points_obj[pt_idx]
                m_id = self.pub_marker.publishSinglePointMarker(pt.pos, m_id, r=1, g=0, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.003, 0.003, 0.003), T=None)
                rospy.sleep(0.001)

            for pt_idx in self.sampled_points_obj:
                pt = self.surface_points_obj[pt_idx]
                m_id = self.pub_marker.publishSinglePointMarker(pt.pos, m_id, r=0, g=1, b=0, namespace='default', frame_id='world', m_type=Marker.CUBE, scale=Vector3(0.003, 0.003, 0.003), T=None)
                rospy.sleep(0.001)

        print "subset size: %s"%(len(self.sampled_points2_obj))

        print "calculating surface curvature at sampled points of the obj..."
        m_id = 0
        planes = 0
        edges = 0
        points = 0
        for pt_idx in range(len(self.surface_points_obj)):
            indices, nx, pc1, pc2 = surfaceutils.pclPrincipalCurvaturesEstimation(self.surface_points_obj, pt_idx, 5, 0.003)
#            m_id = self.pub_marker.publishVectorMarker(self.T_W_O * self.surface_points_obj[pt_idx].pos, self.T_W_O * (self.surface_points_obj[pt_idx].pos + nx*0.004), m_id, 1, 0, 0, frame='world', namespace='default', scale=0.0002)
#            m_id = self.pub_marker.publishVectorMarker(self.T_W_O * self.surface_points_obj[pt_idx].pos, self.T_W_O * (self.surface_points_obj[pt_idx].pos + self.surface_points_obj[pt_idx].normal*0.004), m_id, 0, 0, 1, frame='world', namespace='default', scale=0.0002)
            self.surface_points_obj[pt_idx].frame = PyKDL.Frame(PyKDL.Rotation(nx, self.surface_points_obj[pt_idx].normal * nx, self.surface_points_obj[pt_idx].normal), self.surface_points_obj[pt_idx].pos)
            self.surface_points_obj[pt_idx].pc1 = pc1
            self.surface_points_obj[pt_idx].pc2 = pc2
            if pc1 < 0.2:
                self.surface_points_obj[pt_idx].is_plane = True
                self.surface_points_obj[pt_idx].is_edge = False
                self.surface_points_obj[pt_idx].is_point = False
                planes += 1
            elif pc2 < 0.2:
                self.surface_points_obj[pt_idx].is_plane = False
                self.surface_points_obj[pt_idx].is_edge = True
                self.surface_points_obj[pt_idx].is_point = False
                edges += 1
            else:
                self.surface_points_obj[pt_idx].is_plane = False
                self.surface_points_obj[pt_idx].is_edge = False
                self.surface_points_obj[pt_idx].is_point = True
                points += 1

        print "obj planes: %s  edges: %s  points: %s"%(planes, edges, points)