def convex_hull(self):
""" Returns the convex hull of a mesh as a new mesh """
hull = cvh.ConvexHull(self.vertices_)
hull_tris = hull.vertices
cvh_mesh = Mesh3D(self.vertices_, hull_tris, self.normals_)
cvh_mesh.remove_unreferenced_vertices()
return cvh_mesh
def quickhull(data, n_com):
convexhull = convex_hull.ConvexHull(data)
res = []
anchor = []
for item in convexhull.simplices:
res.append(item.coords)
for i in range(n_com):
p = []
for j in range(n_com):
p.append(item.coords[i][j])
anchor.append(p)
return res, anchor
def ferrari_canny_L1(forces,
torques,
normals,
soft_fingers=False,
params=None):
""" The Ferrari-Canny L-infinity metric """
eps = 1e-2
if params is not None:
eps = params['eps']
# create grasp matrix
G = PointGraspMetrics3D.grasp_matrix(forces, torques, normals,
soft_fingers)
s = time.clock()
hull = cvh.ConvexHull(G.T, joggle=not soft_fingers)
e = time.clock()
logging.info('Convex hull took %f sec' % (e - s))
if len(hull.vertices) == 0:
logging.warning('Convex hull could not be computed')
return -sys.float_info.max
# determine whether or not zero is in the convex hull
min_norm_in_hull = PointGraspMetrics3D.min_norm_vector_in_facet(G)
# if norm is greater than 0 then forces are outside of hull
if min_norm_in_hull > eps:
return -min_norm_in_hull
# find minimum norm vector across all facets of convex hull
min_dist = sys.float_info.max
for v in hull.vertices:
facet = G[:, v]
dist = PointGraspMetrics3D.min_norm_vector_in_facet(facet)
if dist < min_dist:
min_dist = dist
return min_dist
def ferrari_canny_L1(forces, torques, normals, soft_fingers=False, params=None,
wrench_norm_thresh=1e-3,
wrench_regularizer=1e-10):
""" Ferrari & Canny's L1 metric. Also known as the epsilon metric.
Parameters
----------
forces : 3xN :obj:`numpy.ndarray`
set of forces on object in object basis
torques : 3xN :obj:`numpy.ndarray`
set of torques on object in object basis
normals : 3xN :obj:`numpy.ndarray`
surface normals at the contact points
soft_fingers : bool
whether or not to use the soft finger contact model
params : :obj:`GraspQualityConfig`
set of parameters for grasp matrix and contact model
wrench_norm_thresh : float
threshold to use to determine equivalence of target wrenches
wrench_regularizer : float
small float to make quadratic program positive semidefinite
Returns
-------
float : value of metric
"""
if params is not None and 'wrench_norm_thresh' in params.keys():
wrench_norm_thresh = params.wrench_norm_thresh
if params is not None and 'wrench_regularizer' in params.keys():
wrench_regularizer = params.wrench_regularizer
# create grasp matrix
G = PointGraspMetrics3D.grasp_matrix(forces, torques, normals,
soft_fingers, params=params)
s = time.time()
# center grasp matrix for better convex hull comp
hull = cvh.ConvexHull(G.T)
# TODO: suppress ridiculous amount of output for perfectly valid input to qhull
e = time.time()
logging.debug('CVH took %.3f sec' %(e - s))
debug = False
if debug:
fig = plt.figure()
torques = G[3:,:].T
ax = Axes3D(fig)
ax.scatter(torques[:,0], torques[:,1], torques[:,2], c='b', s=50)
ax.scatter(0, 0, 0, c='k', s=80)
ax.set_xlim3d(-1.5, 1.5)
ax.set_ylim3d(-1.5, 1.5)
ax.set_zlim3d(-1.5, 1.5)
ax.set_xlabel('tx')
ax.set_ylabel('ty')
ax.set_zlabel('tz')
plt.show()
if len(hull.vertices) == 0:
logging.warning('Convex hull could not be computed')
return 0.0
# determine whether or not zero is in the convex hull
s = time.time()
min_norm_in_hull, v = PointGraspMetrics3D.min_norm_vector_in_facet(G, wrench_regularizer=wrench_regularizer)
e = time.time()
logging.debug('Min norm took %.3f sec' %(e - s))
# if norm is greater than 0 then forces are outside of hull
if min_norm_in_hull > wrench_norm_thresh:
logging.debug('Zero not in convex hull')
return 0.0
# if there are fewer nonzeros than D-1 (dim of space minus one)
# then zero is on the boundary and therefore we do not have
# force closure
if np.sum(v > 1e-4) <= G.shape[0]-1:
logging.debug('Zero not in interior of convex hull')
return 0.0
# find minimum norm vector across all facets of convex hull
s = time.time()
min_dist = sys.float_info.max
closest_facet = None
for v in hull.vertices:
if np.max(np.array(v)) < G.shape[1]: # because of some occasional odd behavior from pyhull
facet = G[:, v]
dist, _ = PointGraspMetrics3D.min_norm_vector_in_facet(facet, wrench_regularizer=wrench_regularizer)
if dist < min_dist:
min_dist = dist
closest_facet = v
e = time.time()
logging.debug('Min dist took %.3f sec for %d vertices' %(e - s, len(hull.vertices)))
return min_dist
from matplotlib import pyplot as plt
import numpy as np
from sklearn import decomposition
# data = [[-0.5, -0.5], [-0.5, 0.5], [0.5, -0.5], [0, 0], [0.3, 0.25]]
# tnse = TSNE(n_components=2)
# a1 = tnse.fit_transform(a)
# print "TNSE"
# print a1
# print '\n'
#
# pca = decomposition.PCA(n_components=2)
# a2 = pca.fit_transform(a)
# print "PCA"
# print a2
size = 20
x_axis = np.random.rand(size)
y_axis = np.random.rand(size)
data = []
for i in range(0, size):
data.append([x_axis[i], y_axis[i]])
convex_hull = convex_hull.ConvexHull(data)
print "Convex hull"
print convex_hull.dim
for item in convex_hull.simplices:
c = item.coords
plt.plot([c[0][0], c[1][0]], [c[0][1], c[1][1]], color='r')
plt.scatter(x_axis, y_axis)
plt.show()
def ferrari_canny_L1(forces,
torques,
normals,
soft_fingers=False,
params=None,
wrench_norm_thresh=1e-3,
wrench_regularizer=1e-10):
# print ('gggggggggggggggggggggg')
""" Ferrari & Canny's L1 metric. Also known as the epsilon metric.
Parameters
----------
forces : 3xN :obj:`numpy.ndarray`
set of forces on object in object basis
torques : 3xN :obj:`numpy.ndarray`
set of torques on object in object basis
normals : 3xN :obj:`numpy.ndarray`
surface normals at the contact points
soft_fingers : bool
whether or not to use the soft finger contact model
params : :obj:`GraspQualityConfig`
set of parameters for grasp matrix and contact model
wrench_norm_thresh : float
threshold to use to determine equivalence of target wrenches
wrench_regularizer : float
small float to make quadratic program positive semidefinite
Returns
-------
float : value of metric
"""
if params is not None and 'wrench_norm_thresh' in params.keys():
wrench_norm_thresh = params.wrench_norm_thresh
if params is not None and 'wrench_regularizer' in params.keys():
wrench_regularizer = params.wrench_regularizer
s_all = time.time()
# create grasp matrix
G = PointGraspMetrics3D.grasp_matrix(forces,
torques,
normals,
soft_fingers,
params=params)
s = time.time()
# center grasp matrix for better convex hull comp
hull = cvh.ConvexHull(G.T)
# TODO: suppress ridiculous amount of output for perfectly valid input to qhull
e = time.time()
logging.debug('CVH took %.3f sec' % (e - s))
debug = False
if debug:
fig = plt.figure()
torques = G[3:, :].T
ax = Axes3D(fig)
ax.scatter(torques[:, 0],
torques[:, 1],
torques[:, 2],
c='b',
s=50)
ax.scatter(0, 0, 0, c='k', s=80)
ax.set_xlim3d(-1.5, 1.5)
ax.set_ylim3d(-1.5, 1.5)
ax.set_zlim3d(-1.5, 1.5)
ax.set_xlabel('tx')
ax.set_ylabel('ty')
ax.set_zlabel('tz')
plt.savefig("filename" + str(e - s) + ".png")
# plt.show()
if len(hull.vertices) == 0:
logging.warning('Convex hull could not be computed')
return -99
# determine whether or not zero is in the convex hull
s = time.time()
min_norm_in_hull, v = PointGraspMetrics3D.min_norm_vector_in_facet(
G, wrench_regularizer=wrench_regularizer)
# 这个是个难点，用的是cvx优化包来做的。
#求的是方程：
#最小化 0.5 x'Px + q'x subject to Gx <= h, Ax = b
#的x的最优解
#此处P为之前的G^T*G,q趋于0，G为负单位阵，h为0，A为单位阵，b为1
#故本质就是求使得|Gx|最小的x系数（x必须满足凸组合且大于0）
#（为什么要满足满足凸组合？要保证Gx在G的凸包边界上）！！！！！！！！
#所以这个方程的本质含义是什么？就是求在G的凸包边界上距离原点的最小值！！！！！！！！！！！
e = time.time()
logging.debug('Min norm took %.3f sec' % (e - s))
# if norm is greater than 0 then forces are outside of hull ??????????????这个有问题，应该是在凸包外，这个没有对其进行判断的部分，而是直接设定人为阈值
minuskey = 0
if min_norm_in_hull > wrench_norm_thresh:
logging.debug('Zero not in convex hull')
minuskey = 1
return 0.0
# if there are fewer nonzeros than D-1 (dim of space minus one)
# then zero is on the boundary and therefore we do not have
# force closure
if np.sum(v > 1e-4) <= G.shape[0] - 1:
logging.debug('Zero not in interior of convex hull')
return 0.0
# find minimum norm vector across all facets of convex hull
s = time.time()
min_dist = sys.float_info.max
closest_facet = None
for v in hull.vertices:
if np.max(np.array(v)) < G.shape[
1]: # because of some occasional odd behavior from pyhull
facet = G[:, v] #G凸包的G中的角点组成的面片？？
dist, _ = PointGraspMetrics3D.min_norm_vector_in_facet(
facet,
wrench_regularizer=wrench_regularizer) #求这个面片到原点的最小值
if dist < min_dist:
min_dist = dist
closest_facet = v
e = time.time()
logging.debug('Min dist took %.3f sec for %d vertices' %
(e - s, len(hull.vertices)))
e_all = time.time()
# if minuskey==1:
# min_dist=-min_dist
return min_dist
def ferrari_canny_L1(forces,
torques,
normals,
soft_fingers=False,
params=None,
wrench_norm_thresh=1e-3,
wrench_regularizer=1e-10):
""" The Ferrari-Canny L1 metric """
if params is not None and 'wrench_norm_thresh' in params.keys():
wrench_norm_thresh = params.wrench_norm_thresh
if params is not None and 'wrench_regularizer' in params.keys():
wrench_regularizer = params.wrench_regularizer
# create grasp matrix
G = PointGraspMetrics3D.grasp_matrix(forces,
torques,
normals,
soft_fingers,
params=params)
s = time.clock()
# center grasp matrix for better convex hull comp
hull = cvh.ConvexHull(G.T)
# TODO: suppress ridiculous amount of output for perfectly valid input to qhull
e = time.clock()
debug = False
if debug:
fig = plt.figure()
torques = G[3:, :].T
ax = Axes3D(fig)
ax.scatter(torques[:, 0],
torques[:, 1],
torques[:, 2],
c='b',
s=50)
ax.scatter(0, 0, 0, c='k', s=80)
ax.set_xlim3d(-1.5, 1.5)
ax.set_ylim3d(-1.5, 1.5)
ax.set_zlim3d(-1.5, 1.5)
ax.set_xlabel('tx')
ax.set_ylabel('ty')
ax.set_zlabel('tz')
plt.show()
if len(hull.vertices) == 0:
logging.warning('Convex hull could not be computed')
return 0.0
# determine whether or not zero is in the convex hull
min_norm_in_hull, v = PointGraspMetrics3D.min_norm_vector_in_facet(
G, wrench_regularizer=wrench_regularizer)
# if norm is greater than 0 then forces are outside of hull
if min_norm_in_hull > wrench_norm_thresh:
logging.debug('Zero not in convex hull')
return 0.0
if np.sum(v > 1e-4) <= G.shape[0]:
logging.debug('Zero not in interior of convex hull')
return 0.0
# find minimum norm vector across all facets of convex hull
min_dist = sys.float_info.max
closest_facet = None
for v in hull.vertices:
if np.max(np.array(v)) < G.shape[
1]: # because of some occasional odd behavior from pyhull
facet = G[:, v]
dist, _ = PointGraspMetrics3D.min_norm_vector_in_facet(
facet, wrench_regularizer=wrench_regularizer)
if dist < min_dist:
min_dist = dist
closest_facet = v
return min_dist