def supmap(self, v):
tf = self.get_tf_world()
x0 = self.x - 2 * self.margin()
y0 = self.y - 2 * self.margin()
z0 = self.z - 2 * self.margin()
# x_max_dot = -np.Inf
# x_max = np.zeros((3,1))
# for x in [-x0/2, x0/2]:
# for y in [-y0/2, y0/2]:
# for z in [-z0/2, z0/2]:
# pt = np.array([x, y, z])
# pt = SE3.transform_point(tf, pt.reshape((3,1)))
# if x_max_dot < np.dot(v.T, pt).item():
# x_max_dot = np.dot(v.T, pt).item()
# x_max = pt
v = SO3.transform_point_by_inverse(tf.R, v)
x = np.multiply(np.sign(v),
np.array([x0 / 2, y0 / 2, z0 / 2]).reshape((3, 1)))
x = SE3.transform_point(tf, x)
# v = SO3.transform_point(tf.R, v)
# x_dot = (v.T @ x).item()
# assert(np.linalg.norm(x_dot - x_max_dot) < 1e-9)
return x
def closest_points(self, points, normals, tangents, tf):
if DEBUG:
print(tf)
# Transform object points.
points = SE3.transform_point(tf, points)
if DEBUG:
print('points')
print(points)
# Compute closest points.
n_pts = points.shape[1]
n_pairs = len(self.pairs)
dists = np.zeros((n_pts, ))
frame_centers = np.zeros((3, n_pts))
for i in range(n_pairs):
p = self.pairs[i]
sphere = p[0]
sphere.get_tf_world().set_translation(points[:, i, None])
obstacle = p[1]
manifold = gjk(obstacle, sphere)
if DEBUG:
print(manifold.pts_A)
print(manifold.pts_B)
print(manifold.normal)
print(manifold.dist)
points[:, i, None] = manifold.pts_A
normals[:, i, None] = manifold.normal
dists[i] = manifold.dist + sphere.margin()
return points, normals, tangents, dists
def supmap(self, v, use_margin=False):
"""Returns the support mapping, i.e. argmax v⋅x, x ∈ g⋅V.
Arguments:
v {np.ndarray} -- Input vector (3x1).
Keyword Arguments:
use_margin {bool} -- Use the objects margin for V. (default: {False})
Returns:
tuple -- (x, vertex)
"""
g = self.o2w
x_max_dot = -np.Inf
x_max = np.zeros((3, 1))
vert = self.vertices[0]
while True:
adj_closer = False
for v_adj in vert.adjacent_vertices():
x = SE3.transform_point(g, v_adj.position)
if x_max_dot < np.dot(v.T, x).item():
x_max_dot = np.dot(v.T, x).item()
x_max = x
adj_closer = True
vert = v_adj
if not adj_closer:
return x_max
def vertex_positions(self):
n = len(self.vertices)
V = np.zeros((3, n))
tf = self.get_tf_world()
for i, v in zip(range(n), self.vertices):
V[:, i, None] = SE3.transform_point(tf, v.position)
return V
def closest_points(self, points, normals, tangents, tf):
# Transform object points.
points = SE3.transform_point(tf, points)
# Transform object normals.
normals = SO3.transform_point(tf.R, normals)
# Transform object tangents.
n_pts = points.shape[1]
for i in range(n_pts):
tangents[:, i, 0] = SO3.transform_point(tf.R,
tangents[:, i,
0]).flatten()
tangents[:, i, 1] = SO3.transform_point(tf.R,
tangents[:, i,
1]).flatten()
# Get distances.
dists = np.zeros((n_pts, ))
return points, normals, tangents, dists
def position(self, v):
return SE3.transform_point(self.get_tf_world(), v.position)
