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