def spin(self):
# load and init ik solver for right hand
velma_ikr = velmautils.VelmaIkSolver()
velma_ikr.initIkSolver()
# create Openrave interface
self.openrave = openraveinstance.OpenraveInstance(
PyKDL.Frame(PyKDL.Vector(0, 0, 0.1)))
self.openrave.startNewThread()
self.waitForOpenraveInit()
print "openrave initialised"
# create the robot interface for simulation
velma = VelmaSim(self.openrave, velma_ikr)
normals = velmautils.generateNormalsSphere(60.0 / 180.0 * math.pi)
frames = velmautils.generateFramesForNormals(60.0 / 180.0 * math.pi,
normals)
distance = 0.1
space = []
max_solutions = 0
for x in np.arange(0.0, 1.01, distance):
for y in np.arange(-1.0, 1.01, distance):
for z in np.arange(1.0, 2.01, distance):
solutions = 0
for fr in frames:
T_Br_E = PyKDL.Frame(PyKDL.Vector(x, y, z)) * fr
if self.openrave.findIkSolution(T_Br_E) != None:
solutions += 1
space.append([x, y, z, solutions])
if solutions > max_solutions:
max_solutions = solutions
print "x: %s" % (x)
print "points: %s" % (len(space))
m_id = 0
for s in space:
value = float(s[3]) / float(max_solutions)
m_id = publishSinglePointMarker(
PyKDL.Vector(s[0], s[1], s[2]),
m_id,
r=1,
g=value,
b=value,
namespace='default',
frame_id='world',
m_type=Marker.SPHERE,
scale=Vector3(distance * value, distance * value,
distance * value),
T=None)
if m_id % 100 == 0:
rospy.sleep(0.1)
print "done"
return
def spin(self):
# load and init ik solver for right hand
velma_ikr = velmautils.VelmaIkSolver()
velma_ikr.initIkSolver()
# create Openrave interface
self.openrave = openraveinstance.OpenraveInstance(PyKDL.Frame(PyKDL.Vector(0,0,0.1)))
self.openrave.startNewThread()
self.waitForOpenraveInit()
print "openrave initialised"
# create the robot interface for simulation
velma = VelmaSim(self.openrave, velma_ikr)
normals = velmautils.generateNormalsSphere(60.0/180.0*math.pi)
frames = velmautils.generateFramesForNormals(60.0/180.0*math.pi, normals)
distance = 0.1
space = []
max_solutions = 0
for x in np.arange(0.0, 1.01, distance):
for y in np.arange(-1.0, 1.01, distance):
for z in np.arange(1.0, 2.01, distance):
solutions = 0
for fr in frames:
T_Br_E = PyKDL.Frame(PyKDL.Vector(x,y,z)) * fr
if self.openrave.findIkSolution(T_Br_E) != None:
solutions += 1
space.append([x,y,z,solutions])
if solutions > max_solutions:
max_solutions = solutions
print "x: %s"%(x)
print "points: %s"%(len(space))
m_id = 0
for s in space:
value = float(s[3]) / float(max_solutions)
m_id = publishSinglePointMarker(PyKDL.Vector(s[0], s[1], s[2]), m_id, r=1, g=value, b=value, namespace='default', frame_id='world', m_type=Marker.SPHERE, scale=Vector3(distance*value, distance*value, distance*value), T=None)
if m_id%100 == 0:
rospy.sleep(0.1)
print "done"
return
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)